From b976442e64af94771037ccec2054d34ce3267f74 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 20 Feb 2019 11:18:12 +1100 Subject: [PATCH] Tracker: move various sensor status flag updates up --- AntennaTracker/GCS_Mavlink.h | 3 -- AntennaTracker/GCS_Tracker.cpp | 68 +++++++++------------------------- AntennaTracker/GCS_Tracker.h | 2 +- 3 files changed, 19 insertions(+), 54 deletions(-) diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h index 43ae73a720..7bd0f9c544 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/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_MOTOR_OUTPUTS) - class GCS_MAVLINK_Tracker : public GCS_MAVLINK { diff --git a/AntennaTracker/GCS_Tracker.cpp b/AntennaTracker/GCS_Tracker.cpp index 04a2c835c0..adad5f12ab 100644 --- a/AntennaTracker/GCS_Tracker.cpp +++ b/AntennaTracker/GCS_Tracker.cpp @@ -38,47 +38,37 @@ void GCS_Tracker::request_datastream_airpressure(const uint8_t sysid, const uint } // update sensors and subsystems present, enabled and healthy flags for reporting to GCS -void GCS_Tracker::update_sensor_status_flags() +void GCS_Tracker::update_vehicle_sensor_status_flags() { // 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 (tracker.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; - } - const AP_Logger &logger = AP::logger(); - if (logger.logging_present()) { // primary logging only (usually File) - control_sensors_present |= MAV_SYS_STATUS_LOGGING; - } - - // 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_LOGGING & - ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & - ~MAV_SYS_STATUS_SENSOR_BATTERY); - - 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_enabled |= MAV_SYS_STATUS_SENSOR_GPS; } AP_AHRS &ahrs = AP::ahrs(); - // 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); const Compass &compass = AP::compass(); if (tracker.g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; @@ -86,28 +76,6 @@ void GCS_Tracker::update_sensor_status_flags() if (gps.is_healthy()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; } - 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 (!battery.healthy() || battery.has_failsafed()) { - control_sensors_enabled &= ~MAV_SYS_STATUS_SENSOR_BATTERY; - } - if (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); - } } // avoid building/linking Devo: diff --git a/AntennaTracker/GCS_Tracker.h b/AntennaTracker/GCS_Tracker.h index c86c393377..bc21efc164 100644 --- a/AntennaTracker/GCS_Tracker.h +++ b/AntennaTracker/GCS_Tracker.h @@ -17,7 +17,7 @@ public: GCS_MAVLINK_Tracker &chan(const uint8_t ofs) override { return _chan[ofs]; }; const GCS_MAVLINK_Tracker &chan(const uint8_t ofs) const override { return _chan[ofs]; }; - void update_sensor_status_flags() override; + void update_vehicle_sensor_status_flags() override; uint32_t custom_mode() const override; MAV_TYPE frame_type() const override;