Unverified Commit a96cc2d2 authored by Don Gagne's avatar Don Gagne Committed by GitHub

Merge pull request #6098 from DonLakeFlyer/Latency2

Support HIGH_LATENCY2.failure_flags
parents 52816235 45cfae38
......@@ -814,17 +814,6 @@ void Vehicle::_handleHighLatency2(mavlink_message_t& message)
mavlink_high_latency2_t highLatency2;
mavlink_msg_high_latency2_decode(&message, &highLatency2);
#if 0
typedef struct __mavlink_high_latency2_t {
uint16_t wp_num; /*< Current waypoint number*/
uint16_t failure_flags; /*< Indicates failures as defined in MAV_FAILURE_FLAG ENUM. */
uint8_t flight_mode; /*< Flight Mode of the vehicle as defined in the FLIGHT_MODE ENUM*/
uint8_t failsafe; /*< Indicates if a failsafe mode is triggered, defined in MAV_FAILSAFE_FLAG ENUM*/
}) mavlink_high_latency2_t;
#endif
// FIXME: flight mode not yet supported
_coordinate.setLatitude(highLatency2.latitude / (double)1E7);
_coordinate.setLongitude(highLatency2.longitude / (double)1E7);
_coordinate.setAltitude(highLatency2.altitude);
......@@ -849,6 +838,40 @@ void Vehicle::_handleHighLatency2(mavlink_message_t& message)
_gpsFactGroup.count()->setRawValue(0);
_gpsFactGroup.hdop()->setRawValue(highLatency2.eph == UINT8_MAX ? std::numeric_limits<double>::quiet_NaN() : highLatency2.eph / 10.0);
_gpsFactGroup.vdop()->setRawValue(highLatency2.epv == UINT8_MAX ? std::numeric_limits<double>::quiet_NaN() : highLatency2.epv / 10.0);
struct failure2Sensor_s {
MAV_FAILURE_FLAG failureBit;
MAV_SYS_STATUS_SENSOR sensorBit;
};
static const failure2Sensor_s rgFailure2Sensor[] = {
{ MAV_FAILURE_FLAG_GPS, MAV_SYS_STATUS_SENSOR_GPS },
{ MAV_FAILURE_FLAG_AIRSPEED, MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE },
{ MAV_FAILURE_FLAG_BAROMETER, MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE },
{ MAV_FAILURE_FLAG_ACCELEROMETER, MAV_SYS_STATUS_SENSOR_3D_ACCEL },
{ MAV_FAILURE_FLAG_GYROSCOPE, MAV_SYS_STATUS_SENSOR_3D_GYRO },
{ MAV_FAILURE_FLAG_MAGNETOMETER, MAV_SYS_STATUS_SENSOR_3D_MAG },
// { MAV_FAILURE_FLAG_MISSION, ???? },
// { MAV_FAILURE_FLAG_ESTIMATOR, ???? },
// { MAV_FAILURE_FLAG_LIDAR, ???? },
// { MAV_FAILURE_FLAG_OFFBOARD_LINK, ???? },
};
// Map from MAV_FAILURE bits to standard SYS_STATUS message handling
uint32_t newOnboardControlSensorsEnabled = 0;
for (size_t i=0; i<sizeof(rgFailure2Sensor)/sizeof(failure2Sensor_s); i++) {
const failure2Sensor_s* pFailure2Sensor = &rgFailure2Sensor[i];
if (highLatency2.failure_flags & pFailure2Sensor->failureBit) {
// Assume if reporting as unhealthy that is it present and enabled
newOnboardControlSensorsEnabled |= pFailure2Sensor->sensorBit;
}
}
if (newOnboardControlSensorsEnabled != _onboardControlSensorsEnabled) {
_onboardControlSensorsEnabled = newOnboardControlSensorsEnabled;
_onboardControlSensorsPresent = newOnboardControlSensorsEnabled;
_onboardControlSensorsUnhealthy = 0;
emit unhealthySensorsChanged();
}
}
void Vehicle::_handleAltitude(mavlink_message_t& message)
......@@ -2726,6 +2749,7 @@ QStringList Vehicle::unhealthySensors(void) const
{ MAV_SYS_STATUS_TERRAIN, "Terrain" },
{ MAV_SYS_STATUS_REVERSE_MOTOR, "Motors reversed" },
{ MAV_SYS_STATUS_LOGGING, "Logging" },
{ MAV_SYS_STATUS_SENSOR_BATTERY, "Battery" },
};
for (size_t i=0; i<sizeof(rgSensorInfo)/sizeof(sensorInfo_s); i++) {
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment