APM: Fix rudder in elevon mode.

elevon planes can have rudders too
This commit is contained in:
Phil 2011-12-27 00:42:50 -08:00 committed by Andrew Tridgell
parent a43ee36bee
commit 9242c157ee
2 changed files with 3 additions and 4 deletions

View File

@ -314,7 +314,6 @@ static void set_servos(void)
if (g.mix_mode == 0) {
g.channel_roll.calc_pwm();
g.channel_pitch.calc_pwm();
g.channel_rudder.calc_pwm();
if (g_rc_function[RC_Channel_aux::k_aileron]) {
g_rc_function[RC_Channel_aux::k_aileron]->servo_out = g.channel_roll.servo_out;
g_rc_function[RC_Channel_aux::k_aileron]->calc_pwm();
@ -329,6 +328,7 @@ static void set_servos(void)
g.channel_roll.radio_out = elevon1_trim + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0/ SERVO_MAX));
g.channel_pitch.radio_out = elevon2_trim + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX));
}
g.channel_rudder.calc_pwm();
#if THROTTLE_OUT == 0
g.channel_throttle.servo_out = 0;

View File

@ -157,7 +157,6 @@ static void trim_control_surfaces()
if(g.mix_mode == 0){
g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
}else{
@ -169,6 +168,7 @@ static void trim_control_surfaces()
g.channel_roll.radio_trim = center;
g.channel_pitch.radio_trim = center;
}
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
// save to eeprom
g.channel_roll.save_eeprom();
@ -190,7 +190,6 @@ static void trim_radio()
g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
//g.channel_throttle.radio_trim = g.channel_throttle.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
} else {
@ -199,8 +198,8 @@ static void trim_radio()
uint16_t center = 1500;
g.channel_roll.radio_trim = center;
g.channel_pitch.radio_trim = center;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
}
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
// save to eeprom
g.channel_roll.save_eeprom();