Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 21 additions & 14 deletions ExtLibs/ArduPilot/CurrentState.cs
Original file line number Diff line number Diff line change
Expand Up @@ -181,6 +181,8 @@ public bool prearmstatus
get => connected && (sensors_health.prearm || !sensors_enabled.prearm);
}

private readonly PrearmFailureTracker _prearmFailureTracker = new PrearmFailureTracker();

private bool useLocation;

/// <summary>
Expand Down Expand Up @@ -1274,10 +1276,10 @@ public string messageHigh
if (value == null || value == "")
return;
// check against get
_messageHighTime = DateTime.Now;
if (messageHigh == value)
return;
log.Info("messageHigh " + value);
_messageHighTime = DateTime.Now;
_messagehigh = value;
messageHighSeverity = MAVLink.MAV_SEVERITY.EMERGENCY;
}
Expand Down Expand Up @@ -2967,16 +2969,17 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi

safetyactive = !sensors_enabled.motor_control;

string latestPrearmFailure = _prearmFailureTracker.Update(
sensors_health.prearm,
sensors_enabled.prearm,
sensors_present.prearm,
messages,
DateTime.Now);

if (errors_count1 > 0 || errors_count2 > 0)
{
messageHigh = "InternalError 0x" + (errors_count1 + (errors_count2 << 16)).ToString("X");
}

if (!sensors_health.prearm && sensors_enabled.prearm && sensors_present.prearm)
{
messageHigh = messages.LastOrDefault(a => a.message.ToLower().Contains("prearm")).message
?.ToString();
}
else if (!sensors_health.gps && sensors_enabled.gps && sensors_present.gps)
{
messageHigh = Strings.BadGPSHealth;
Expand Down Expand Up @@ -3053,6 +3056,10 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi
{
messageHigh = Strings.BadAirspeed;
}
else if (!string.IsNullOrEmpty(latestPrearmFailure))
{
messageHigh = latestPrearmFailure;
}
}

break;
Expand Down Expand Up @@ -4056,11 +4063,11 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi
case (uint)MAVLink.MAVLINK_MSG_ID.HIGHRES_IMU:
{
const ushort HIGHRES_IMU_UPDATED_XACC = 0x01;
const ushort HIGHRES_IMU_UPDATED_XGYRO = 0x08;
const ushort HIGHRES_IMU_UPDATED_XMAG = 0x40;
const ushort HIGHRES_IMU_UPDATED_ABS_PRESSURE = 0x200;
const ushort HIGHRES_IMU_UPDATED_PRESSURE_ALT = 0x800;
const ushort HIGHRES_IMU_UPDATED_TEMPERATURE = 0x1000;
const ushort HIGHRES_IMU_UPDATED_XGYRO = 0x08;
const ushort HIGHRES_IMU_UPDATED_XMAG = 0x40;
const ushort HIGHRES_IMU_UPDATED_ABS_PRESSURE = 0x200;
const ushort HIGHRES_IMU_UPDATED_PRESSURE_ALT = 0x800;
const ushort HIGHRES_IMU_UPDATED_TEMPERATURE = 0x1000;

var imu = mavLinkMessage.ToStructure<MAVLink.mavlink_highres_imu_t>();
if (imu.id == 0)
Expand Down Expand Up @@ -4643,8 +4650,8 @@ public void UpdateCurrentSettings(Action<CurrentState> bs, bool updatenow,
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, MAV.cs.raterc,
MAV.sysid,
MAV.compid); // request rc info
MAV.Camera?.RequestMessageIntervals(MAV.cs.ratestatus); // use ratestatus until we create a new setting for this
MAV.GimbalManager?.Discover();
// Use ratestatus until camera-specific configuration is available.
MAV.Camera?.UpdateRateIfChanged(MAV.cs.ratestatus);
}
catch
{
Expand Down
Loading
Loading