diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index cdd03e55f8..22ab352c06 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -251,16 +251,9 @@ void NOINLINE Sub::send_nav_controller_output(mavlink_channel_t chan) 0); } -void NOINLINE Sub::send_vfr_hud(mavlink_channel_t chan) +int16_t GCS_MAVLINK_Sub::vfr_hud_throttle() const { - mavlink_msg_vfr_hud_send( - chan, - gps.ground_speed(), - gps.ground_speed(), - (ahrs.yaw_sensor / 100) % 360, - (int16_t)(motors.get_throttle() * 100), - current_loc.alt / 100.0f, - climb_rate / 100.0f); + return (int16_t)(sub.motors.get_throttle() * 100); } /* @@ -427,11 +420,6 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id) sub.send_nav_controller_output(chan); break; - case MSG_VFR_HUD: - CHECK_PAYLOAD_SIZE(VFR_HUD); - sub.send_vfr_hud(chan); - break; - case MSG_RPM: #if RPM_ENABLED == ENABLED CHECK_PAYLOAD_SIZE(RPM); diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index 9b1a2934c1..8c7d7e5c40 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -46,4 +46,8 @@ private: MAV_MODE base_mode() const override; uint32_t custom_mode() const override; MAV_STATE system_status() const override; + + int16_t vfr_hud_throttle() const override; + bool vfr_hud_make_alt_relative() const override { return true; } + }; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index a721db8cbe..0761e89415 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -476,7 +476,6 @@ private: void send_heartbeat(mavlink_channel_t chan); void send_extended_status1(mavlink_channel_t chan); void send_nav_controller_output(mavlink_channel_t chan); - void send_vfr_hud(mavlink_channel_t chan); #if RPM_ENABLED == ENABLED void send_rpm(mavlink_channel_t chan); void rpm_update();