Rover: move sending of sys_status message up

APMrover2/GCS_Mavlink.cpp

@@ -75,30 +75,15 @@ MAV_STATE GCS_MAVLINK_Rover::system_status() const
     return MAV_STATE_ACTIVE;
 }
 
-void Rover::send_sys_status(mavlink_channel_t chan)
+void GCS_MAVLINK_Rover::get_sensor_status_flags(uint32_t &present,
+                                                uint32_t &enabled,
+                                                uint32_t &health)
 {
-    int16_t battery_current = -1;
-    int8_t battery_remaining = -1;
+    rover.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 = rover.control_sensors_present;
+    enabled = rover.control_sensors_enabled;
+    health = rover.control_sensors_health;
 }
 
 void Rover::send_nav_controller_output(mavlink_channel_t chan)
@@ -328,16 +313,6 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
 
     switch (id) {
 
-    case MSG_SYS_STATUS:
-        // send extended status only once vehicle has been initialised
-        // to avoid unnecessary errors being reported to user
-        if (!vehicle_initialised()) {
-            return true;
-        }
-        CHECK_PAYLOAD_SIZE(SYS_STATUS);
-        rover.send_sys_status(chan);
-        break;
-
     case MSG_NAV_CONTROLLER_OUTPUT:
         CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
         rover.send_nav_controller_output(chan);

APMrover2/GCS_Mavlink.h

@@ -31,6 +31,8 @@ protected:
 
     bool vehicle_initialised() const override;
 
+    void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health);
+
 private:
 
     void handleMessage(mavlink_message_t * msg) override;

APMrover2/Rover.h

@@ -438,7 +438,6 @@ private:
     void fence_check();
 
     // GCS_Mavlink.cpp
-    void send_sys_status(mavlink_channel_t chan);
     void send_nav_controller_output(mavlink_channel_t chan);
     void send_servo_out(mavlink_channel_t chan);
     void send_pid_tuning(mavlink_channel_t chan);