diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h index 4bf250cade..7e56f23378 100644 --- a/APMrover2/GCS_Mavlink.h +++ b/APMrover2/GCS_Mavlink.h @@ -2,9 +2,6 @@ #include -// default sensors are present and healthy: gyro, accelerometer, 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_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | 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_Rover : public GCS_MAVLINK { public: diff --git a/APMrover2/GCS_Rover.cpp b/APMrover2/GCS_Rover.cpp index f8a5451b7c..5ea848ff33 100644 --- a/APMrover2/GCS_Rover.cpp +++ b/APMrover2/GCS_Rover.cpp @@ -20,94 +20,60 @@ bool GCS_Rover::supersimple_input_active() const return (rover.g2.simple_type == ModeSimple::Simple_CardinalDirections); } -// 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_Rover::update_sensor_status_flags(void) +void GCS_Rover::update_vehicle_sensor_status_flags(void) { - // default sensors present - control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT; - // first what sensors/controllers we have if (rover.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; } const AP_VisualOdom *visual_odom = AP::visualodom(); if (visual_odom && visual_odom->enabled()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; - } - 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_VISION_POSITION; } const AP_Proximity *proximity = AP_Proximity::get_singleton(); if (proximity && proximity->get_status() > AP_Proximity::Proximity_NotConnected) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } - // all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control and motor output which we will set individually - control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL & - ~MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION & - ~MAV_SYS_STATUS_SENSOR_YAW_POSITION & - ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & - ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & - ~MAV_SYS_STATUS_LOGGING & - ~MAV_SYS_STATUS_SENSOR_BATTERY); + control_sensors_present |= + MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | + MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | + MAV_SYS_STATUS_SENSOR_YAW_POSITION | + MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; + if (rover.control_mode->attitude_stabilized()) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control + control_sensors_health |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // 3D angular rate control + control_sensors_health |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // 3D angular rate control } if (rover.control_mode->is_autopilot_mode()) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position + control_sensors_health |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control - } - - if (logger.logging_enabled()) { - control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; - } - - // 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; - } - - const AP_BattMonitor &battery = AP::battery(); - if (battery.num_instances() > 0) { - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY; + control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control } AP_AHRS &ahrs = AP::ahrs(); const Compass &compass = AP::compass(); - // 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); if (rover.g.compass_enabled && compass.healthy(0) && 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 (visual_odom && visual_odom->enabled() && !visual_odom->healthy()) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION; + if (visual_odom && visual_odom->enabled() && visual_odom->healthy()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; } - 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; - } - const RangeFinder *rangefinder = RangeFinder::get_singleton(); if (rangefinder && rangefinder->num_sensors() > 0) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; @@ -117,20 +83,7 @@ void GCS_Rover::update_sensor_status_flags(void) control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } } - if (proximity && proximity->get_status() == AP_Proximity::Proximity_NoData) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } - if (logger.logging_failed()) { - control_sensors_health &= ~MAV_SYS_STATUS_LOGGING; - } - - if (!battery.healthy() || battery.has_failsafed()) { - control_sensors_enabled &= ~MAV_SYS_STATUS_SENSOR_BATTERY; - } - - if (!rover.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 (proximity && proximity->get_status() != AP_Proximity::Proximity_NoData) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } } diff --git a/APMrover2/GCS_Rover.h b/APMrover2/GCS_Rover.h index 4e5dbc01fe..fe0151c445 100644 --- a/APMrover2/GCS_Rover.h +++ b/APMrover2/GCS_Rover.h @@ -22,7 +22,7 @@ public: bool vehicle_initialised() const override; - void update_sensor_status_flags(void) override; + void update_vehicle_sensor_status_flags(void) override; bool simple_input_active() const override; bool supersimple_input_active() const override;