diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 0ab47e7ac0f6f2..d6ef31bc71be3e 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -71,30 +71,15 @@ MAV_STATE GCS_MAVLINK_Tracker::system_status() const return MAV_STATE_ACTIVE; } -void Tracker::send_sys_status(mavlink_channel_t chan) +void GCS_MAVLINK_Tracker::get_sensor_status_flags(uint32_t &present, + uint32_t &enabled, + uint32_t &health) { - int16_t battery_current = -1; - int8_t battery_remaining = -1; + tracker.update_sensor_status_flags(); - if (battery.has_current() && battery.healthy()) { - battery_remaining = battery.capacity_remaining_pct(); - battery_current = battery.current_amps() * 100; - } - - update_sensor_status_flags(); - - mavlink_msg_sys_status_send( - chan, - control_sensors_present, - control_sensors_enabled, - control_sensors_health, - static_cast(scheduler.load_average() * 1000), - battery.voltage() * 1000, // mV - battery_current, // in 10mA units - battery_remaining, // in % - 0, // comm drops %, - 0, // comm drops in pkts, - 0, 0, 0, 0); + present = tracker.control_sensors_present; + enabled = tracker.control_sensors_enabled; + health = tracker.control_sensors_health; } void Tracker::send_nav_controller_output(mavlink_channel_t chan) @@ -135,11 +120,6 @@ bool GCS_MAVLINK_Tracker::try_send_message(enum ap_message id) tracker.send_nav_controller_output(chan); break; - case MSG_SYS_STATUS: - CHECK_PAYLOAD_SIZE(SYS_STATUS); - tracker.send_sys_status(chan); - break; - default: return GCS_MAVLINK::try_send_message(id); } diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h index 245230e332ee51..1bd9c333471afd 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -41,4 +41,6 @@ class GCS_MAVLINK_Tracker : public GCS_MAVLINK MAV_MODE base_mode() const override; uint32_t custom_mode() const override; MAV_STATE system_status() const override; + void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health); + }; diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index 6353ad8904c338..c698cd78031e2f 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -222,7 +222,6 @@ class Tracker : public AP_HAL::HAL::Callbacks { bool servo_test_set_servo(uint8_t servo_num, uint16_t pwm); // GCS_Mavlink.cpp - void send_sys_status(mavlink_channel_t chan); void send_nav_controller_output(mavlink_channel_t chan); // Log.cpp