|
|
|
@ -156,6 +156,9 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
@@ -156,6 +156,9 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
|
|
|
|
|
control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; |
|
|
|
|
} |
|
|
|
|
#endif |
|
|
|
|
if (ap.rc_receiver_present) { |
|
|
|
|
control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// all present sensors enabled by default except altitude and position control which we will set individually |
|
|
|
|
control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL & ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL); |
|
|
|
@ -180,14 +183,17 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
@@ -180,14 +183,17 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
|
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// default to all healthy except compass and gps which we set individually |
|
|
|
|
control_sensors_health = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_3D_MAG & ~MAV_SYS_STATUS_SENSOR_GPS); |
|
|
|
|
// default to all healthy except compass, gps and receiver which we set individually |
|
|
|
|
control_sensors_health = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_3D_MAG & ~MAV_SYS_STATUS_SENSOR_GPS & ~MAV_SYS_STATUS_SENSOR_RC_RECEIVER); |
|
|
|
|
if (g.compass_enabled && compass.healthy && ahrs.use_compass()) { |
|
|
|
|
control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; |
|
|
|
|
} |
|
|
|
|
if (g_gps != NULL && g_gps->status() > GPS::NO_GPS && !gps_glitch.glitching()) { |
|
|
|
|
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; |
|
|
|
|
} |
|
|
|
|
if (ap.rc_receiver_present && !failsafe.radio) { |
|
|
|
|
control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
int16_t battery_current = -1; |
|
|
|
|
int8_t battery_remaining = -1; |
|
|
|
|