From 9a2d5ac5a6605099f24e60859cd6feb5823d06ce Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 20 Feb 2019 10:01:55 +1100 Subject: [PATCH] Copter: move various sensor status flag updates up --- ArduCopter/GCS_Copter.cpp | 126 +++++++++++++------------------------- ArduCopter/GCS_Copter.h | 2 +- ArduCopter/GCS_Mavlink.h | 3 - 3 files changed, 42 insertions(+), 89 deletions(-) diff --git a/ArduCopter/GCS_Copter.cpp b/ArduCopter/GCS_Copter.cpp index f1b31aba5f..7cd26f81e7 100644 --- a/ArduCopter/GCS_Copter.cpp +++ b/ArduCopter/GCS_Copter.cpp @@ -17,48 +17,57 @@ bool GCS_Copter::supersimple_input_active() const return copter.ap.simple_mode == 2; } -// 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 GCS_Copter::update_sensor_status_flags(void) +void GCS_Copter::update_vehicle_sensor_status_flags(void) { - // default sensors present - control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT; + control_sensors_present |= + MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | + MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | + MAV_SYS_STATUS_SENSOR_YAW_POSITION; + + control_sensors_enabled |= + MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | + MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | + MAV_SYS_STATUS_SENSOR_YAW_POSITION; + + control_sensors_health |= + MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | + MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | + MAV_SYS_STATUS_SENSOR_YAW_POSITION; - // first what sensors/controllers we have if (copter.g.compass_enabled) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present + control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; } const AP_GPS &gps = AP::gps(); if (gps.status() > AP_GPS::NO_GPS) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; } #if OPTFLOW == ENABLED const OpticalFlow *optflow = AP::opticalflow(); if (optflow && optflow->enabled()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; } #endif #if PRECISION_LANDING == ENABLED if (copter.precland.enabled()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; } #endif #if VISUAL_ODOMETRY_ENABLED == ENABLED const AP_VisualOdom *visual_odom = AP::visualodom(); if (visual_odom && visual_odom->enabled()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; } #endif const Copter::ap_t &ap = copter.ap; if (ap.rc_receiver_present) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; - } - const AP_Logger &logger = AP::logger(); - if (logger.logging_present()) { // primary logging only (usually File) - control_sensors_present |= MAV_SYS_STATUS_LOGGING; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; } #if PROXIMITY_ENABLED == ENABLED if (copter.g2.proximity.sensor_present()) { @@ -77,15 +86,8 @@ void GCS_Copter::update_sensor_status_flags(void) } #endif - // all sensors are present except these, which may be set as enabled below: - 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 & - ~MAV_SYS_STATUS_SENSOR_BATTERY & - ~MAV_SYS_STATUS_GEOFENCE & - ~MAV_SYS_STATUS_SENSOR_LASER_POSITION & - ~MAV_SYS_STATUS_SENSOR_PROXIMITY); + control_sensors_present |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; + control_sensors_present |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; switch (copter.control_mode) { case AUTO: @@ -114,19 +116,6 @@ void GCS_Copter::update_sensor_status_flags(void) 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 (logger.logging_enabled()) { - control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; - } - - const AP_BattMonitor &battery = AP::battery(); - if (battery.num_instances() > 0) { - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY; - } #if AC_FENCE == ENABLED if (copter.fence.sys_status_enabled()) { control_sensors_enabled |= MAV_SYS_STATUS_GEOFENCE; @@ -138,60 +127,36 @@ void GCS_Copter::update_sensor_status_flags(void) } #endif - - // default to all healthy - control_sensors_health = control_sensors_present; - - const AP_Baro &barometer = AP::baro(); - if (!barometer.all_healthy()) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - } AP_AHRS &ahrs = AP::ahrs(); const Compass &compass = AP::compass(); - if (!copter.g.compass_enabled || !compass.healthy() || !ahrs.use_compass()) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_MAG; + if (copter.g.compass_enabled && compass.healthy() && ahrs.use_compass()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; } - if (!gps.is_healthy()) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_GPS; + if (gps.is_healthy()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; } - if (!ap.rc_receiver_present || copter.failsafe.radio) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_RC_RECEIVER; + if (ap.rc_receiver_present && !copter.failsafe.radio) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; } #if OPTFLOW == ENABLED - if (!optflow || !optflow->healthy()) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + if (optflow && optflow->healthy()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; } #endif #if PRECISION_LANDING == ENABLED - if (copter.precland.enabled() && !copter.precland.healthy()) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION; + if (!copter.precland.enabled() || copter.precland.healthy()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; } #endif #if VISUAL_ODOMETRY_ENABLED == ENABLED if (visual_odom && visual_odom->enabled() && visual_odom->healthy()) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION; + control_sensors_health |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; } #endif - const AP_InertialSensor &ins = AP::ins(); - 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 (logger.logging_failed()) { - control_sensors_health &= ~MAV_SYS_STATUS_LOGGING; - } #if PROXIMITY_ENABLED == ENABLED - if (copter.g2.proximity.sensor_failed()) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_PROXIMITY; + if (!copter.g2.proximity.sensor_failed()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_PROXIMITY; } #endif @@ -215,24 +180,15 @@ void GCS_Copter::update_sensor_status_flags(void) #if RANGEFINDER_ENABLED == ENABLED if (copter.rangefinder_state.enabled) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - if (!rangefinder || !rangefinder->has_data_orient(ROTATION_PITCH_270)) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_LASER_POSITION; + if (rangefinder && rangefinder->has_data_orient(ROTATION_PITCH_270)) { + 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 (!copter.battery.healthy() || copter.battery.has_failsafed()) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_BATTERY; - } #if AC_FENCE == ENABLED - if (copter.fence.sys_status_failed()) { - control_sensors_health &= ~MAV_SYS_STATUS_GEOFENCE; + if (!copter.fence.sys_status_failed()) { + control_sensors_health |= MAV_SYS_STATUS_GEOFENCE; } #endif } diff --git a/ArduCopter/GCS_Copter.h b/ArduCopter/GCS_Copter.h index 7fc09e1bf9..789975288f 100644 --- a/ArduCopter/GCS_Copter.h +++ b/ArduCopter/GCS_Copter.h @@ -20,7 +20,7 @@ public: return _chan[ofs]; }; - void update_sensor_status_flags(void) override; + void update_vehicle_sensor_status_flags(void) override; uint32_t custom_mode() const override; MAV_TYPE frame_type() const override; diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index 532dcc6d7e..a7772fe197 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -2,9 +2,6 @@ #include -// 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 | MAV_SYS_STATUS_SENSOR_BATTERY) - class GCS_MAVLINK_Copter : public GCS_MAVLINK {