diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index beba79ff1d..030c92e2d6 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -299,7 +299,7 @@ void Rover::one_second_loop(void) // 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(); + gcs().update_sensor_status_flags(); // need to set "likely flying" when armed to allow for compass // learning to run diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 8abf1e8a29..1ab4928a92 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -75,17 +75,6 @@ MAV_STATE GCS_MAVLINK_Rover::system_status() const return MAV_STATE_ACTIVE; } -void GCS_Rover::get_sensor_status_flags(uint32_t &present, - uint32_t &enabled, - uint32_t &health) -{ - rover.update_sensor_status_flags(); - - present = rover.control_sensors_present; - enabled = rover.control_sensors_enabled; - health = rover.control_sensors_health; -} - void GCS_MAVLINK_Rover::send_nav_controller_output() const { if (!rover.control_mode->is_autopilot_mode()) { diff --git a/APMrover2/GCS_Rover.h b/APMrover2/GCS_Rover.h index 0e4c826fe2..db130b7b49 100644 --- a/APMrover2/GCS_Rover.h +++ b/APMrover2/GCS_Rover.h @@ -17,7 +17,7 @@ public: // return GCS link at offset ofs const GCS_MAVLINK_Rover &chan(const uint8_t ofs) const override { return _chan[ofs]; }; - void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health); + void update_sensor_status_flags(void) override; private: diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index ff938b792c..d9a3a2ad96 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -302,10 +302,6 @@ private: AP_DEVO_Telem devo_telemetry; #endif - uint32_t control_sensors_present; - uint32_t control_sensors_enabled; - uint32_t control_sensors_health; - // 3D Location vectors // Location structure defined in AP_Common // The home location used for RTL. The location is set when we first get stable GPS lock @@ -493,7 +489,6 @@ private: void read_rangefinders(void); void init_proximity(); void read_airspeed(); - void update_sensor_status_flags(void); // Steering.cpp bool use_pivot_steering_at_next_WP(float yaw_error_cd); diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp index 2509558ff1..07d9453998 100644 --- a/APMrover2/sensors.cpp +++ b/APMrover2/sensors.cpp @@ -1,6 +1,7 @@ #include "Rover.h" #include +#include // initialise compass void Rover::init_compass() @@ -240,25 +241,29 @@ void Rover::rpm_update(void) // 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 Rover::update_sensor_status_flags(void) +void GCS_Rover::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) { + if (rover.g.compass_enabled) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present } + const AP_GPS &gps = AP::gps(); if (gps.status() > AP_GPS::NO_GPS) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; } - if (g2.visual_odom.enabled()) { + const AP_VisualOdom *visual_odom = AP::visualodom(); + if (visual_odom && visual_odom->enabled()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; } - if (rover.logger.logging_present()) { // primary logging only (usually File) + const AP_Logger &logger = AP::logger(); + if (logger.logging_present()) { // primary logging only (usually File) control_sensors_present |= MAV_SYS_STATUS_LOGGING; } - if (rover.g2.proximity.get_status() > AP_Proximity::Proximity_NotConnected) { + 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; } @@ -270,16 +275,16 @@ void Rover::update_sensor_status_flags(void) ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & ~MAV_SYS_STATUS_LOGGING & ~MAV_SYS_STATUS_SENSOR_BATTERY); - if (control_mode->attitude_stabilized()) { + if (rover.control_mode->attitude_stabilized()) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // 3D angular rate control } - if (control_mode->is_autopilot_mode()) { + if (rover.control_mode->is_autopilot_mode()) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control } - if (rover.logger.logging_enabled()) { + if (logger.logging_enabled()) { control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; } @@ -288,21 +293,26 @@ void Rover::update_sensor_status_flags(void) 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; } + 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 (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) { + 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 (g2.visual_odom.enabled() && !g2.visual_odom.healthy()) { + 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; } @@ -315,18 +325,19 @@ void Rover::update_sensor_status_flags(void) control_sensors_health &= ~MAV_SYS_STATUS_AHRS; } - if (rangefinder.num_sensors() > 0) { + const RangeFinder *rangefinder = RangeFinder::get_singleton(); + if (rangefinder && rangefinder->num_sensors() > 0) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - AP_RangeFinder_Backend *s = rangefinder.get_backend(0); + AP_RangeFinder_Backend *s = rangefinder->get_backend(0); if (s != nullptr && s->has_data()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } } - if (rover.g2.proximity.get_status() == AP_Proximity::Proximity_NoData) { + if (proximity && proximity->get_status() == AP_Proximity::Proximity_NoData) { control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_LASER_POSITION; } - if (rover.logger.logging_failed()) { + if (logger.logging_failed()) { control_sensors_health &= ~MAV_SYS_STATUS_LOGGING; } @@ -334,13 +345,13 @@ void Rover::update_sensor_status_flags(void) control_sensors_enabled &= ~MAV_SYS_STATUS_SENSOR_BATTERY; } - if (!initialised || ins.calibrating()) { + 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 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); + rover.frsky_telemetry.update_sensor_status_flags(~control_sensors_health & control_sensors_enabled & control_sensors_present); #endif }