Plane: move sending of vfr_hud up

ArduPlane/GCS_Mavlink.cpp

@@ -237,22 +237,37 @@ void Plane::send_servo_out(mavlink_channel_t chan)
         rssi.read_receiver_rssi_uint8());
 }
 
-void Plane::send_vfr_hud(mavlink_channel_t chan)
+float GCS_MAVLINK_Plane::vfr_hud_airspeed() const
 {
-    float aspeed;
-    if (airspeed.enabled()) {
-        aspeed = airspeed.get_airspeed();
-    } else if (!ahrs.airspeed_estimate(&aspeed)) {
-        aspeed = 0;
+    // airspeed sensors are best.  While the AHRS airspeed_estimate
+    // will use an airspeed sensor, that value is constrained by the
+    // ground speed.  When reporting we should send the true airspeed
+    // value if possible:
+    if (plane.airspeed.enabled()) {
+        return plane.airspeed.get_airspeed();
     }
-    mavlink_msg_vfr_hud_send(
-        chan,
-        aspeed,
-        ahrs.groundspeed(),
-        (ahrs.yaw_sensor / 100) % 360,
-        abs(throttle_percentage()),
-        current_loc.alt / 100.0f,
-        (g2.soaring_controller.is_active() ? g2.soaring_controller.get_vario_reading() : barometer.get_climb_rate()));
+
+    // airspeed estimates are OK:
+    float aspeed;
+    if (AP::ahrs().airspeed_estimate(&aspeed)) {
+        return aspeed;
+    }
+
+    // lying is worst:
+    return 0;
+}
+
+int16_t GCS_MAVLINK_Plane::vfr_hud_throttle() const
+{
+    return abs(plane.throttle_percentage());
+}
+
+float GCS_MAVLINK_Plane::vfr_hud_climbrate() const
+{
+    if (plane.g2.soaring_controller.is_active()) {
+        return plane.g2.soaring_controller.get_vario_reading();
+    }
+    return AP::baro().get_climb_rate();
 }
 
 /*
@@ -418,11 +433,6 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
 #endif
         break;
 
-    case MSG_VFR_HUD:
-        CHECK_PAYLOAD_SIZE(VFR_HUD);
-        plane.send_vfr_hud(chan);
-        break;
-
     case MSG_FENCE_STATUS:
 #if GEOFENCE_ENABLED == ENABLED
         CHECK_PAYLOAD_SIZE(FENCE_STATUS);

ArduPlane/GCS_Mavlink.h

@@ -53,4 +53,9 @@ private:
     MAV_STATE system_status() const override;
 
     uint8_t radio_in_rssi() const;
+
+    float vfr_hud_airspeed() const override;
+    int16_t vfr_hud_throttle() const override;
+    float vfr_hud_climbrate() const override;
+
 };

ArduPlane/Plane.h

@@ -773,7 +773,6 @@ private:
     void send_extended_status1(mavlink_channel_t chan);
     void send_nav_controller_output(mavlink_channel_t chan);
     void send_servo_out(mavlink_channel_t chan);
-    void send_vfr_hud(mavlink_channel_t chan);
     void send_wind(mavlink_channel_t chan);
     void send_pid_info(const mavlink_channel_t chan, const DataFlash_Class::PID_Info *pid_info, const uint8_t axis, const float achieved);
     void send_pid_tuning(mavlink_channel_t chan);