diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 3bf8642db8..dbfbe7614c 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -420,7 +420,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan) (float)airspeed / 100.0, (float)g_gps->ground_speed / 100.0, (dcm.yaw_sensor / 100) % 360, - (uint16_t)(100 * g.channel_throttle.norm_output()), + (uint16_t)(100 * (g.channel_throttle.norm_output() / 2.0 + 0.5)), // scale -1,1 to 0-100 current_loc.alt / 100.0, 0); } diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 72577a7444..37f90e0cdc 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -248,10 +248,12 @@ RC_Channel::norm_input() float RC_Channel::norm_output() { + uint16_t mid = (radio_max + radio_min) / 2; + if(radio_out < radio_trim) - return (float)(radio_out - radio_trim) / (float)(radio_trim - radio_min); + return (float)(radio_out - mid) / (float)(mid - radio_min); else - return (float)(radio_out - radio_trim) / (float)(radio_max - radio_trim); + return (float)(radio_out - mid) / (float)(radio_max - mid); } void RC_Channel::set_apm_rc( APM_RC_Class * apm_rc )