Plane: removed use of pwm_to_angle()

This commit is contained in:
Andrew Tridgell 2017-01-07 16:36:30 +11:00
parent c638be54a3
commit 081909bf6a
2 changed files with 4 additions and 6 deletions

View File

@ -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:

View File

@ -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();
}
/*