diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 311c9ed115..a12bbf2ffa 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -748,12 +748,10 @@ void Plane::update_flight_mode(void) break; case MANUAL: - SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->pwm_to_angle()); - SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->pwm_to_angle()); - steering_control.steering = steering_control.rudder = channel_rudder->pwm_to_angle(); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in_zero_dz()); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in_zero_dz()); + steering_control.steering = steering_control.rudder = channel_rudder->get_control_in_zero_dz(); break; - //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 - case QSTABILIZE: case QHOVER: diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 996b931961..17665fd95d 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -124,7 +124,7 @@ void Plane::stick_mix_channel(RC_Channel *channel, int16_t &servo_out) ch_inf = MIN(ch_inf, 400.0f); ch_inf = ((400.0f - ch_inf) / 400.0f); servo_out *= ch_inf; - servo_out += channel->pwm_to_angle(); + servo_out += channel->get_control_in(); } /*