mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
Plane: removed use of pwm_to_angle()
This commit is contained in:
parent
c638be54a3
commit
081909bf6a
@ -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:
|
||||
|
@ -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();
|
||||
}
|
||||
|
||||
/*
|
||||
|
Loading…
Reference in New Issue
Block a user