Copter: split SYS_STATUS and POWER_STATUS onto separate ap_messages

ArduCopter/GCS_Mavlink.cpp

@@ -254,6 +254,10 @@ uint32_t GCS_MAVLINK_Copter::telem_delay() const
     return (uint32_t)(copter.g.telem_delay);
 }
 
+bool GCS_MAVLINK_Copter::vehicle_initialised() const {
+    return copter.ap.initialised;
+}
+
 // try to send a message, return false if it wasn't sent
 bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
 {
@@ -277,17 +281,14 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
 
     switch(id) {
 
-    case MSG_EXTENDED_STATUS1:
+    case MSG_SYS_STATUS:
         // send extended status only once vehicle has been initialised
         // to avoid unnecessary errors being reported to user
-        if (copter.ap.initialised) {
-            if (PAYLOAD_SIZE(chan, SYS_STATUS) +
-                PAYLOAD_SIZE(chan, POWER_STATUS) > comm_get_txspace(chan)) {
-                return false;
-            }
-            copter.send_sys_status(chan);
-            send_power_status();
+        if (!vehicle_initialised()) {
+            return true;
         }
+        CHECK_PAYLOAD_SIZE(SYS_STATUS);
+        copter.send_sys_status(chan);
         break;
 
     case MSG_NAV_CONTROLLER_OUTPUT:
@@ -445,7 +446,8 @@ static const ap_message STREAM_RAW_SENSORS_msgs[] = {
     MSG_SENSOR_OFFSETS
 };
 static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
-    MSG_EXTENDED_STATUS1, // SYS_STATUS, POWER_STATUS
+    MSG_SYS_STATUS,
+    MSG_POWER_STATUS,
     MSG_MEMINFO,
     MSG_CURRENT_WAYPOINT, // MISSION_CURRENT
     MSG_GPS_RAW,
@@ -1443,7 +1445,7 @@ void Copter::mavlink_delay_cb()
     if (tnow - last_1hz > 1000) {
         last_1hz = tnow;
         gcs_send_heartbeat();
-        gcs().send_message(MSG_EXTENDED_STATUS1);
+        gcs().send_message(MSG_SYS_STATUS);
     }
     if (tnow - last_50hz > 20) {
         last_50hz = tnow;

ArduCopter/GCS_Mavlink.h

@@ -47,6 +47,8 @@ private:
     void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override;
     bool try_send_message(enum ap_message id) override;
 
+    bool vehicle_initialised() const override;
+
     void packetReceived(const mavlink_status_t &status,
                         mavlink_message_t &msg) override;