Tracker: move sending of sys_status message up

amirrezasadeqi · Feb 5, 2019 · 9a2ca02 · 9a2ca02
1 parent b48cb1e
commit 9a2ca02
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 28 deletions.
diff --git 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
@@ -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
@@ -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