Plane: factor vehicle's mavlink send_heartbeat

ArduPlane/GCS_Mavlink.cpp

@@ -2,21 +2,14 @@
 
 #include "Plane.h"
 
-void Plane::send_heartbeat(mavlink_channel_t chan)
+MAV_TYPE GCS_MAVLINK_Plane::frame_type() const
+{
+    return plane.quadplane.get_mav_type();
+}
+
+MAV_MODE GCS_MAVLINK_Plane::base_mode() const
 {
     uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
-    uint8_t system_status;
-    uint32_t custom_mode = control_mode;
-    
-    if (failsafe.state != FAILSAFE_NONE || battery.has_failsafed() || failsafe.adsb) {
-        system_status = MAV_STATE_CRITICAL;
-    } else if (plane.crash_state.is_crashed) {
-        system_status = MAV_STATE_EMERGENCY;
-    } else if (is_flying()) {
-        system_status = MAV_STATE_ACTIVE;
-    } else {
-        system_status = MAV_STATE_STANDBY;
-    }
 
     // work out the base_mode. This value is not very useful
     // for APM, but we calculate it as best we can so a generic
@@ -26,7 +19,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan)
     // only get useful information from the custom_mode, which maps to
     // the APM flight mode and has a well defined meaning in the
     // ArduPlane documentation
-    switch (control_mode) {
+    switch (plane.control_mode) {
     case MANUAL:
     case TRAINING:
     case ACRO:
@@ -57,45 +50,65 @@ void Plane::send_heartbeat(mavlink_channel_t chan)
         // positions", which APM does not currently do
         break;
     case INITIALISING:
-        system_status = MAV_STATE_CALIBRATING;
         break;
     }
 
-    if (!training_manual_pitch || !training_manual_roll) {
+    if (!plane.training_manual_pitch || !plane.training_manual_roll) {
         base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;        
     }
 
-    if (control_mode != MANUAL && control_mode != INITIALISING) {
+    if (plane.control_mode != MANUAL && plane.control_mode != INITIALISING) {
         // stabiliser of some form is enabled
         base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
     }
 
-    if (g.stick_mixing != STICK_MIXING_DISABLED && control_mode != INITIALISING) {
+    if (plane.g.stick_mixing != STICK_MIXING_DISABLED && plane.control_mode != INITIALISING) {
         // all modes except INITIALISING have some form of manual
         // override if stick mixing is enabled
         base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
     }
 
 #if HIL_SUPPORT
-    if (g.hil_mode == 1) {
+    if (plane.g.hil_mode == 1) {
         base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
     }
 #endif
 
     // we are armed if we are not initialising
-    if (control_mode != INITIALISING && arming.is_armed()) {
+    if (plane.control_mode != INITIALISING && plane.arming.is_armed()) {
         base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
     }
 
     // indicate we have set a custom mode
     base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
 
-    gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(quadplane.get_mav_type(),
-                                                   base_mode,
-                                                   custom_mode,
-                                                   system_status);
+    return (MAV_MODE)base_mode;
 }
 
+uint32_t GCS_MAVLINK_Plane::custom_mode() const
+{
+    return plane.control_mode;
+}
+
+MAV_STATE GCS_MAVLINK_Plane::system_status() const
+{
+    if (plane.control_mode == INITIALISING) {
+        return MAV_STATE_CALIBRATING;
+    }
+    if (plane.any_failsafe_triggered()) {
+        return MAV_STATE_CRITICAL;
+    }
+    if (plane.crash_state.is_crashed) {
+        return MAV_STATE_EMERGENCY;
+    }
+    if (plane.is_flying()) {
+        return MAV_STATE_ACTIVE;
+    }
+
+    return MAV_STATE_STANDBY;
+}
+
+
 void Plane::send_attitude(mavlink_channel_t chan)
 {
     float r = ahrs.roll;
@@ -404,7 +417,7 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
     case MSG_HEARTBEAT:
         CHECK_PAYLOAD_SIZE(HEARTBEAT);
         last_heartbeat_time = AP_HAL::millis();
-        plane.send_heartbeat(chan);
+        send_heartbeat();
         return true;
 
     case MSG_EXTENDED_STATUS1:

ArduPlane/GCS_Mavlink.h

@@ -44,4 +44,8 @@ private:
     bool try_send_message(enum ap_message id) override;
     void packetReceived(const mavlink_status_t &status, mavlink_message_t &msg) override;
 
+    MAV_TYPE frame_type() const override;
+    MAV_MODE base_mode() const override;
+    uint32_t custom_mode() const override;
+    MAV_STATE system_status() const override;
 };

ArduPlane/Plane.h

@@ -355,6 +355,10 @@ private:
         uint32_t AFS_last_valid_rc_ms;
     } failsafe;
 
+    bool any_failsafe_triggered() {
+        return failsafe.state != FAILSAFE_NONE || battery.has_failsafed() || failsafe.adsb;
+    }
+
     // A counter used to count down valid gps fixes to allow the gps estimate to settle
     // before recording our home position (and executing a ground start if we booted with an air start)
     uint8_t ground_start_count = 5;
@@ -776,7 +780,6 @@ private:
     
     void adjust_nav_pitch_throttle(void);
     void update_load_factor(void);
-    void send_heartbeat(mavlink_channel_t chan);
     void send_attitude(mavlink_channel_t chan);
     void send_fence_status(mavlink_channel_t chan);
     void update_sensor_status_flags(void);

ArduPlane/quadplane.cpp

@@ -2483,12 +2483,12 @@ bool QuadPlane::do_user_takeoff(float takeoff_altitude)
 /*
   return mav_type for heartbeat
  */
-uint8_t QuadPlane::get_mav_type(void) const
+MAV_TYPE QuadPlane::get_mav_type(void) const
 {
     if (mav_type.get() == 0) {
         return MAV_TYPE_FIXED_WING;
     }
-    return uint8_t(mav_type.get());
+    return MAV_TYPE(mav_type.get());
 }
 
 /*

ArduPlane/quadplane.h

@@ -268,7 +268,7 @@ private:
 
     // HEARTBEAT mav_type override
     AP_Int8 mav_type;
-    uint8_t get_mav_type(void) const;
+    MAV_TYPE get_mav_type(void) const;
     
     // time we last got an EKF yaw reset
     uint32_t ekfYawReset_ms;