diff --git a/AntennaTracker/GCS_Tracker.cpp b/AntennaTracker/GCS_Tracker.cpp index 4447c88cdd..db713aed84 100644 --- a/AntennaTracker/GCS_Tracker.cpp +++ b/AntennaTracker/GCS_Tracker.cpp @@ -36,3 +36,76 @@ 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() +{ + // default sensors present + control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT; + + // first what sensors/controllers we have + if (tracker.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; + } + 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; + } + + 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; + } + 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); + } +} diff --git a/AntennaTracker/sensors.cpp b/AntennaTracker/sensors.cpp index d2cb395fdb..6d5d51b8f2 100644 --- a/AntennaTracker/sensors.cpp +++ b/AntennaTracker/sensors.cpp @@ -89,76 +89,3 @@ void Tracker::handle_battery_failsafe(const char* type_str, const int8_t action) // useful failsafes in the future would include actually recalling the vehicle // that is tracked before the tracker loses power to continue tracking it } - -// update sensors and subsystems present, enabled and healthy flags for reporting to GCS -void GCS_Tracker::update_sensor_status_flags() -{ - // default sensors present - control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT; - - // first what sensors/controllers we have - if (tracker.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; - } - 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; - } - - 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; - } - 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); - } -}