From 9a2ca025dd83a9d6cb3e83c045c19dd075ec7a29 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 23 Jan 2019 14:22:04 +1100
Subject: [PATCH] Tracker: move sending of sys_status message up

---
 AntennaTracker/GCS_Mavlink.cpp | 34 +++++++---------------------------
 AntennaTracker/GCS_Mavlink.h   |  2 ++
 AntennaTracker/Tracker.h       |  1 -
 3 files changed, 9 insertions(+), 28 deletions(-)

diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp
index 0ab47e7ac0..d6ef31bc71 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<uint16_t>(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 245230e332..1bd9c33347 100644
--- a/AntennaTracker/GCS_Mavlink.h
+++ b/AntennaTracker/GCS_Mavlink.h
@@ -41,4 +41,6 @@ private:
     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 6353ad8904..c698cd7803 100644
--- a/AntennaTracker/Tracker.h
+++ b/AntennaTracker/Tracker.h
@@ -222,7 +222,6 @@ private:
     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