Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APM: fixed stick mixing for STABILISE mode 

this is embarrassing
This commit is contained in:
Andrew Tridgell 2012-09-24 07:13:57 +10:00
parent 8807758693
commit 061c676cc6
1 changed files with 23 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
ArduPlane/Attitude.pde
View File

@ -38,22 +38,19 @@ static float get_speed_scaler(void)
*/ */
static bool stick_mixing_enabled(void) static bool stick_mixing_enabled(void)
{ {
if (control_mode == CIRCLE && failsafe != FAILSAFE_NONE) { if (control_mode == CIRCLE || control_mode > FLY_BY_WIRE_B) {
// we are in short failsafe // we're in an auto mode. Check the stick mixing flag
return false; if (g.stick_mixing &&
geofence_stickmixing() &&
failsafe == FAILSAFE_NONE) {
// we're in an auto mode, and haven't triggered failsafe
return true;
} else {
return false;
}
} }
if (control_mode < FLY_BY_WIRE_A) { // non-auto mode. Always do stick mixing
// pilot has control, always mix in pilot controls return true;
return true;
}
if (g.stick_mixing &&
geofence_stickmixing() &&
failsafe == FAILSAFE_NONE) {
// we're in an auto mode, and haven't triggered failsafe
return true;
}
// we should not do stick mixing
return false;
} }
@ -100,8 +97,9 @@ static void stabilize()
// Mix Stick input to allow users to override control surfaces // Mix Stick input to allow users to override control surfaces
// ----------------------------------------------------------- // -----------------------------------------------------------
if (stick_mixing_enabled()) { if (stick_mixing_enabled()) {
if (control_mode > FLY_BY_WIRE_B) { if (control_mode < FLY_BY_WIRE_A || control_mode > FLY_BY_WIRE_C) {
// do stick mixing in auto modes // do stick mixing on aileron/elevator if not in a fly by
// wire mode
ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim; ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim;
ch1_inf = fabs(ch1_inf); ch1_inf = fabs(ch1_inf);
ch1_inf = min(ch1_inf, 400.0); ch1_inf = min(ch1_inf, 400.0);
@ -116,22 +114,20 @@ static void stabilize()
// ----------------------------------------------- // -----------------------------------------------
g.channel_roll.servo_out *= ch1_inf; g.channel_roll.servo_out *= ch1_inf;
g.channel_pitch.servo_out *= ch2_inf; g.channel_pitch.servo_out *= ch2_inf;
// Mix in stick inputs // Mix in stick inputs
// ------------------- // -------------------
g.channel_roll.servo_out += g.channel_roll.pwm_to_angle(); g.channel_roll.servo_out += g.channel_roll.pwm_to_angle();
g.channel_pitch.servo_out += g.channel_pitch.pwm_to_angle(); g.channel_pitch.servo_out += g.channel_pitch.pwm_to_angle();
} }
if (control_mode >= FLY_BY_WIRE_A) { // stick mixing performed for rudder for all cases including FBW
// stick mixing performed for rudder for all cases including FBW unless disabled for higher modes // important for steering on the ground during landing
// important for steering on the ground during landing // -----------------------------------------------
// ----------------------------------------------- ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim;
ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim; ch4_inf = fabs(ch4_inf);
ch4_inf = fabs(ch4_inf); ch4_inf = min(ch4_inf, 400.0);
ch4_inf = min(ch4_inf, 400.0); ch4_inf = ((400.0 - ch4_inf) /400.0);
ch4_inf = ((400.0 - ch4_inf) /400.0);
}
} }
// Apply output to Rudder // Apply output to Rudder