From e6f322d88a4a6192d4c27b9dd403326b9d45685f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 20 Nov 2012 13:50:11 +1100 Subject: [PATCH] APM: fixed throttle display to always be between 0 and 100 when rc3 is below RC3_MIN, don't give an invalid value --- ArduPlane/GCS_Mavlink.pde | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 5e57e2ca0c..1f8712744a 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -358,12 +358,15 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan) } else if (!ahrs.airspeed_estimate(&aspeed)) { aspeed = 0; } + float throttle_norm = g.channel_throttle.norm_output() * 100; + throttle_norm = constrain(throttle_norm, -100, 100); + uint16_t throttle = ((uint16_t)(throttle_norm + 100)) / 2; mavlink_msg_vfr_hud_send( chan, aspeed, (float)g_gps->ground_speed * 0.01, (ahrs.yaw_sensor / 100) % 360, - (uint16_t)(100 * (g.channel_throttle.norm_output() / 2.0 + 0.5)), // scale -1,1 to 0-100 + throttle, current_loc.alt / 100.0, barometer.get_climb_rate()); }