GCS_MAVLink: move sending of sys_status message up

libraries/GCS_MAVLink/GCS.h

@@ -225,6 +225,7 @@ public:
     static void send_collision_all(const AP_Avoidance::Obstacle &threat, MAV_COLLISION_ACTION behaviour);
     void send_accelcal_vehicle_position(uint32_t position);
     void send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag));
+    void send_sys_status();
 
     // return a bitmap of active channels. Used by libraries to loop
     // over active channels to send to all active channels    
@@ -295,6 +296,7 @@ protected:
     virtual MAV_MODE base_mode() const = 0;
     virtual uint32_t custom_mode() const = 0;
     virtual MAV_STATE system_status() const = 0;
+    virtual void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health) = 0;
 
     bool            waypoint_receiving; // currently receiving
     // the following two variables are only here because of Tracker

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -3696,6 +3696,45 @@ void GCS_MAVLINK::send_hwstatus()
         0);
 }
 
+
+void GCS_MAVLINK::send_sys_status()
+{
+    // send extended status only once vehicle has been initialised
+    // to avoid unnecessary errors being reported to user
+    if (!vehicle_initialised()) {
+        return;
+    }
+
+    int16_t battery_current = -1;
+    int8_t battery_remaining = -1;
+
+    const AP_BattMonitor &battery = AP::battery();
+
+    if (battery.has_current() && battery.healthy()) {
+        battery_remaining = battery.capacity_remaining_pct();
+        battery_current = battery.current_amps() * 100;
+    }
+
+    uint32_t control_sensors_present;
+    uint32_t control_sensors_enabled;
+    uint32_t control_sensors_health;
+
+    get_sensor_status_flags(control_sensors_present, control_sensors_enabled, control_sensors_health);
+
+    mavlink_msg_sys_status_send(
+        chan,
+        control_sensors_present,
+        control_sensors_enabled,
+        control_sensors_health,
+        static_cast<uint16_t>(AP::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);
+}
+
 void GCS_MAVLINK::send_attitude() const
 {
     const AP_AHRS &ahrs = AP::ahrs();
@@ -3955,6 +3994,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
         send_simstate();
         break;
 
+    case MSG_SYS_STATUS:
+        CHECK_PAYLOAD_SIZE(SYS_STATUS);
+        send_sys_status();
+        break;
+
     case MSG_AHRS2:
         CHECK_PAYLOAD_SIZE(AHRS2);
         send_ahrs2();

libraries/GCS_MAVLink/GCS_Dummy.h

@@ -32,6 +32,7 @@ protected:
     MAV_MODE base_mode() const override { return (MAV_MODE)MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; }
     uint32_t custom_mode() const override { return 3; } // magic number
     MAV_STATE system_status() const override { return MAV_STATE_CALIBRATING; }
+    void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health) override { present = 0; enabled = 0; health = 0; }
 
 };
 

libraries/GCS_MAVLink/examples/routing/routing.cpp

@@ -38,6 +38,7 @@ protected:
     MAV_MODE base_mode() const override { return (MAV_MODE)MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; }
     uint32_t custom_mode() const override { return 3; } // magic number
     MAV_STATE system_status() const override { return MAV_STATE_CALIBRATING; }
+    void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health) override { present = 0; enabled = 0; health = 0; }
 
 private: