mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-03 12:14:10 -04:00
Copter: update sensor status error flags independently of sending a sys_status message
Without this, there is no update to the sensor status flags in the Frsky lib unless there's an active Mavlink connection configured to send extended_status1
This commit is contained in:
floaledm
Andrew Tridgell
parent
6864864797
commit
34718b130a
@ -502,6 +502,12 @@ void Copter::one_hz_loop()
|
|||||||
terrain_logging();
|
terrain_logging();
|
||||||
|
|
||||||
adsb.set_is_flying(!ap.land_complete);
|
adsb.set_is_flying(!ap.land_complete);
|
||||||
|
|
||||||
|
// update error mask of sensors and subsystems. The mask uses the
|
||||||
|
// MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
|
||||||
|
// indicates that the sensor or subsystem is present but not
|
||||||
|
// functioning correctly
|
||||||
|
update_sensor_status_flags();
|
||||||
}
|
}
|
||||||
|
|
||||||
// called at 50hz
|
// called at 50hz
|
||||||
|
|||||||
@ -449,6 +449,11 @@ private:
|
|||||||
AP_Frsky_Telem frsky_telemetry;
|
AP_Frsky_Telem frsky_telemetry;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
// Variables for extended status MAVLink messages
|
||||||
|
uint32_t control_sensors_present;
|
||||||
|
uint32_t control_sensors_enabled;
|
||||||
|
uint32_t control_sensors_health;
|
||||||
|
|
||||||
// Altitude
|
// Altitude
|
||||||
// The cm/s we are moving up or down based on filtered data - Positive = UP
|
// The cm/s we are moving up or down based on filtered data - Positive = UP
|
||||||
int16_t climb_rate;
|
int16_t climb_rate;
|
||||||
@ -949,6 +954,7 @@ private:
|
|||||||
void failsafe_disable();
|
void failsafe_disable();
|
||||||
void fence_check();
|
void fence_check();
|
||||||
void fence_send_mavlink_status(mavlink_channel_t chan);
|
void fence_send_mavlink_status(mavlink_channel_t chan);
|
||||||
|
void update_sensor_status_flags(void);
|
||||||
bool set_mode(control_mode_t mode, mode_reason_t reason);
|
bool set_mode(control_mode_t mode, mode_reason_t reason);
|
||||||
bool gcs_set_mode(uint8_t mode) { return set_mode((control_mode_t)mode, MODE_REASON_GCS_COMMAND); }
|
bool gcs_set_mode(uint8_t mode) { return set_mode((control_mode_t)mode, MODE_REASON_GCS_COMMAND); }
|
||||||
void update_flight_mode();
|
void update_flight_mode();
|
||||||
|
|||||||
@ -3,9 +3,6 @@
|
|||||||
|
|
||||||
#include "GCS_Mavlink.h"
|
#include "GCS_Mavlink.h"
|
||||||
|
|
||||||
// default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
|
|
||||||
#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
|
|
||||||
|
|
||||||
void Copter::gcs_send_heartbeat(void)
|
void Copter::gcs_send_heartbeat(void)
|
||||||
{
|
{
|
||||||
gcs_send_message(MSG_HEARTBEAT);
|
gcs_send_message(MSG_HEARTBEAT);
|
||||||
@ -111,130 +108,6 @@ NOINLINE void Copter::send_limits_status(mavlink_channel_t chan)
|
|||||||
|
|
||||||
NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan)
|
NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
uint32_t control_sensors_present;
|
|
||||||
uint32_t control_sensors_enabled;
|
|
||||||
uint32_t control_sensors_health;
|
|
||||||
|
|
||||||
// default sensors present
|
|
||||||
control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT;
|
|
||||||
|
|
||||||
// first what sensors/controllers we have
|
|
||||||
if (g.compass_enabled) {
|
|
||||||
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present
|
|
||||||
}
|
|
||||||
if (gps.status() > AP_GPS::NO_GPS) {
|
|
||||||
control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
|
|
||||||
}
|
|
||||||
#if OPTFLOW == ENABLED
|
|
||||||
if (optflow.enabled()) {
|
|
||||||
control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
#if PRECISION_LANDING == ENABLED
|
|
||||||
if (precland.enabled()) {
|
|
||||||
control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
if (ap.rc_receiver_present) {
|
|
||||||
control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
|
|
||||||
}
|
|
||||||
if (copter.DataFlash.logging_present()) { // primary logging only (usually File)
|
|
||||||
control_sensors_present |= MAV_SYS_STATUS_LOGGING;
|
|
||||||
}
|
|
||||||
#if PROXIMITY_ENABLED == ENABLED
|
|
||||||
if (copter.g2.proximity.get_status() > AP_Proximity::Proximity_NotConnected) {
|
|
||||||
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// all present sensors enabled by default except altitude and position control and motors 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 &
|
|
||||||
~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS &
|
|
||||||
~MAV_SYS_STATUS_LOGGING);
|
|
||||||
|
|
||||||
switch (control_mode) {
|
|
||||||
case AUTO:
|
|
||||||
case AVOID_ADSB:
|
|
||||||
case GUIDED:
|
|
||||||
case LOITER:
|
|
||||||
case RTL:
|
|
||||||
case CIRCLE:
|
|
||||||
case LAND:
|
|
||||||
case POSHOLD:
|
|
||||||
case BRAKE:
|
|
||||||
case THROW:
|
|
||||||
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
|
|
||||||
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
|
|
||||||
break;
|
|
||||||
case ALT_HOLD:
|
|
||||||
case GUIDED_NOGPS:
|
|
||||||
case SPORT:
|
|
||||||
case AUTOTUNE:
|
|
||||||
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
// stabilize, acro, drift, and flip have no automatic x,y or z control (i.e. all manual)
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
// set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED)
|
|
||||||
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
|
|
||||||
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (copter.DataFlash.logging_enabled()) {
|
|
||||||
control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// default to all healthy
|
|
||||||
control_sensors_health = control_sensors_present;
|
|
||||||
|
|
||||||
if (!barometer.all_healthy()) {
|
|
||||||
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
|
|
||||||
}
|
|
||||||
if (!g.compass_enabled || !compass.healthy() || !ahrs.use_compass()) {
|
|
||||||
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_MAG;
|
|
||||||
}
|
|
||||||
if (gps.status() == AP_GPS::NO_GPS) {
|
|
||||||
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_GPS;
|
|
||||||
}
|
|
||||||
if (!ap.rc_receiver_present || failsafe.radio) {
|
|
||||||
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
|
|
||||||
}
|
|
||||||
#if OPTFLOW == ENABLED
|
|
||||||
if (!optflow.healthy()) {
|
|
||||||
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
#if PRECISION_LANDING == ENABLED
|
|
||||||
if (!precland.healthy()) {
|
|
||||||
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) {
|
|
||||||
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO;
|
|
||||||
}
|
|
||||||
if (!ins.get_accel_health_all()) {
|
|
||||||
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (ahrs.initialised() && !ahrs.healthy()) {
|
|
||||||
// AHRS subsystem is unhealthy
|
|
||||||
control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (copter.DataFlash.logging_failed()) {
|
|
||||||
control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
|
|
||||||
}
|
|
||||||
|
|
||||||
#if PROXIMITY_ENABLED == ENABLED
|
|
||||||
if (copter.g2.proximity.get_status() < AP_Proximity::Proximity_Good) {
|
|
||||||
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_LASER_POSITION;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
int16_t battery_current = -1;
|
int16_t battery_current = -1;
|
||||||
int8_t battery_remaining = -1;
|
int8_t battery_remaining = -1;
|
||||||
|
|
||||||
@ -243,39 +116,6 @@ NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan)
|
|||||||
battery_current = battery.current_amps() * 100;
|
battery_current = battery.current_amps() * 100;
|
||||||
}
|
}
|
||||||
|
|
||||||
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
|
|
||||||
switch (terrain.status()) {
|
|
||||||
case AP_Terrain::TerrainStatusDisabled:
|
|
||||||
break;
|
|
||||||
case AP_Terrain::TerrainStatusUnhealthy:
|
|
||||||
// To-Do: restore unhealthy terrain status reporting once terrain is used in copter
|
|
||||||
//control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
|
|
||||||
//control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
|
|
||||||
//break;
|
|
||||||
case AP_Terrain::TerrainStatusOK:
|
|
||||||
control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
|
|
||||||
control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
|
|
||||||
control_sensors_health |= MAV_SYS_STATUS_TERRAIN;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if RANGEFINDER_ENABLED == ENABLED
|
|
||||||
if (rangefinder.num_sensors() > 0) {
|
|
||||||
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
|
|
||||||
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
|
|
||||||
if (rangefinder.has_data()) {
|
|
||||||
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
if (!ap.initialised || ins.calibrating()) {
|
|
||||||
// while initialising the gyros and accels are not enabled
|
|
||||||
control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
|
|
||||||
control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
|
|
||||||
}
|
|
||||||
|
|
||||||
mavlink_msg_sys_status_send(
|
mavlink_msg_sys_status_send(
|
||||||
chan,
|
chan,
|
||||||
control_sensors_present,
|
control_sensors_present,
|
||||||
@ -288,13 +128,6 @@ NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan)
|
|||||||
0, // comm drops %,
|
0, // comm drops %,
|
||||||
0, // comm drops in pkts,
|
0, // comm drops in pkts,
|
||||||
0, 0, 0, 0);
|
0, 0, 0, 0);
|
||||||
|
|
||||||
#if FRSKY_TELEM_ENABLED == ENABLED
|
|
||||||
// give mask of error flags to Frsky_Telemetry
|
|
||||||
uint32_t sensors_error_flags = (~control_sensors_health) & control_sensors_enabled & control_sensors_present;
|
|
||||||
frsky_telemetry.update_sensor_status_flags(sensors_error_flags);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void NOINLINE Copter::send_location(mavlink_channel_t chan)
|
void NOINLINE Copter::send_location(mavlink_channel_t chan)
|
||||||
|
|||||||
@ -2,6 +2,9 @@
|
|||||||
|
|
||||||
#include <GCS_MAVLink/GCS.h>
|
#include <GCS_MAVLink/GCS.h>
|
||||||
|
|
||||||
|
// default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
|
||||||
|
#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
|
||||||
|
|
||||||
class GCS_MAVLINK_Copter : public GCS_MAVLINK
|
class GCS_MAVLINK_Copter : public GCS_MAVLINK
|
||||||
{
|
{
|
||||||
|
|
||||||
|
|||||||
@ -265,3 +265,167 @@ void Copter::update_proximity(void)
|
|||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// update error mask of sensors and subsystems. The mask
|
||||||
|
// uses the MAV_SYS_STATUS_* values from mavlink. If a bit is set
|
||||||
|
// then it indicates that the sensor or subsystem is present but
|
||||||
|
// not functioning correctly.
|
||||||
|
void Copter::update_sensor_status_flags(void)
|
||||||
|
{
|
||||||
|
// default sensors present
|
||||||
|
control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT;
|
||||||
|
|
||||||
|
// first what sensors/controllers we have
|
||||||
|
if (g.compass_enabled) {
|
||||||
|
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present
|
||||||
|
}
|
||||||
|
if (gps.status() > AP_GPS::NO_GPS) {
|
||||||
|
control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
|
||||||
|
}
|
||||||
|
#if OPTFLOW == ENABLED
|
||||||
|
if (optflow.enabled()) {
|
||||||
|
control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
#if PRECISION_LANDING == ENABLED
|
||||||
|
if (precland.enabled()) {
|
||||||
|
control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
if (ap.rc_receiver_present) {
|
||||||
|
control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
|
||||||
|
}
|
||||||
|
if (copter.DataFlash.logging_present()) { // primary logging only (usually File)
|
||||||
|
control_sensors_present |= MAV_SYS_STATUS_LOGGING;
|
||||||
|
}
|
||||||
|
#if PROXIMITY_ENABLED == ENABLED
|
||||||
|
if (copter.g2.proximity.get_status() > AP_Proximity::Proximity_NotConnected) {
|
||||||
|
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// all present sensors enabled by default except altitude and position control and motors 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 &
|
||||||
|
~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS &
|
||||||
|
~MAV_SYS_STATUS_LOGGING);
|
||||||
|
|
||||||
|
switch (control_mode) {
|
||||||
|
case AUTO:
|
||||||
|
case AVOID_ADSB:
|
||||||
|
case GUIDED:
|
||||||
|
case LOITER:
|
||||||
|
case RTL:
|
||||||
|
case CIRCLE:
|
||||||
|
case LAND:
|
||||||
|
case POSHOLD:
|
||||||
|
case BRAKE:
|
||||||
|
case THROW:
|
||||||
|
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
|
||||||
|
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
|
||||||
|
break;
|
||||||
|
case ALT_HOLD:
|
||||||
|
case GUIDED_NOGPS:
|
||||||
|
case SPORT:
|
||||||
|
case AUTOTUNE:
|
||||||
|
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
// stabilize, acro, drift, and flip have no automatic x,y or z control (i.e. all manual)
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED)
|
||||||
|
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
|
||||||
|
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (copter.DataFlash.logging_enabled()) {
|
||||||
|
control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// default to all healthy
|
||||||
|
control_sensors_health = control_sensors_present;
|
||||||
|
|
||||||
|
if (!barometer.all_healthy()) {
|
||||||
|
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
|
||||||
|
}
|
||||||
|
if (!g.compass_enabled || !compass.healthy() || !ahrs.use_compass()) {
|
||||||
|
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_MAG;
|
||||||
|
}
|
||||||
|
if (gps.status() == AP_GPS::NO_GPS) {
|
||||||
|
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_GPS;
|
||||||
|
}
|
||||||
|
if (!ap.rc_receiver_present || failsafe.radio) {
|
||||||
|
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
|
||||||
|
}
|
||||||
|
#if OPTFLOW == ENABLED
|
||||||
|
if (!optflow.healthy()) {
|
||||||
|
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
#if PRECISION_LANDING == ENABLED
|
||||||
|
if (!precland.healthy()) {
|
||||||
|
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) {
|
||||||
|
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO;
|
||||||
|
}
|
||||||
|
if (!ins.get_accel_health_all()) {
|
||||||
|
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (ahrs.initialised() && !ahrs.healthy()) {
|
||||||
|
// AHRS subsystem is unhealthy
|
||||||
|
control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (copter.DataFlash.logging_failed()) {
|
||||||
|
control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
|
||||||
|
}
|
||||||
|
|
||||||
|
#if PROXIMITY_ENABLED == ENABLED
|
||||||
|
if (copter.g2.proximity.get_status() < AP_Proximity::Proximity_Good) {
|
||||||
|
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_LASER_POSITION;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
|
||||||
|
switch (terrain.status()) {
|
||||||
|
case AP_Terrain::TerrainStatusDisabled:
|
||||||
|
break;
|
||||||
|
case AP_Terrain::TerrainStatusUnhealthy:
|
||||||
|
// To-Do: restore unhealthy terrain status reporting once terrain is used in copter
|
||||||
|
//control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
|
||||||
|
//control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
|
||||||
|
//break;
|
||||||
|
case AP_Terrain::TerrainStatusOK:
|
||||||
|
control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
|
||||||
|
control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
|
||||||
|
control_sensors_health |= MAV_SYS_STATUS_TERRAIN;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if RANGEFINDER_ENABLED == ENABLED
|
||||||
|
if (rangefinder.num_sensors() > 0) {
|
||||||
|
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
|
||||||
|
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
|
||||||
|
if (rangefinder.has_data()) {
|
||||||
|
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
if (!ap.initialised || ins.calibrating()) {
|
||||||
|
// while initialising the gyros and accels are not enabled
|
||||||
|
control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
|
||||||
|
control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
|
||||||
|
}
|
||||||
|
|
||||||
|
#if FRSKY_TELEM_ENABLED == ENABLED
|
||||||
|
// give mask of error flags to Frsky_Telemetry
|
||||||
|
frsky_telemetry.update_sensor_status_flags(!control_sensors_health & control_sensors_enabled & control_sensors_present);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user