diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 58aa63c571..7cbe7205dd 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -768,7 +768,6 @@ private: void send_fence_status(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); void send_rpm(mavlink_channel_t chan); void send_pid_tuning(mavlink_channel_t chan); void gcs_data_stream_send(void); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 4c90f5fad8..6a0fa910ac 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -171,16 +171,9 @@ void NOINLINE Copter::send_nav_controller_output(mavlink_channel_t chan) 0); } -void NOINLINE Copter::send_vfr_hud(mavlink_channel_t chan) +int16_t GCS_MAVLINK_Copter::vfr_hud_throttle() const { - mavlink_msg_vfr_hud_send( - chan, - gps.ground_speed(), - ahrs.groundspeed(), - (ahrs.yaw_sensor / 100) % 360, - (int16_t)(motors->get_throttle() * 100), - current_loc.alt / 100.0f, - climb_rate / 100.0f); + return (int16_t)(copter.motors->get_throttle() * 100); } /* @@ -308,11 +301,6 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id) copter.send_nav_controller_output(chan); break; - case MSG_VFR_HUD: - CHECK_PAYLOAD_SIZE(VFR_HUD); - copter.send_vfr_hud(chan); - break; - case MSG_RPM: #if RPM_ENABLED == ENABLED CHECK_PAYLOAD_SIZE(RPM); diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index 46d823a551..2edaae7755 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -51,4 +51,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; } + };