aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APM: fixed stick mixing in CIRCLE mode on throttle failsafe

Browse Source 
Many thanks to Andke for finding this bug!
This commit is contained in:
Andrew Tridgell 2012-09-23 09:33:17 +10:00
parent bc2aa84394
commit 56c354eb4f
1 changed files with 53 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

92
ArduPlane/Attitude.pde
View File

@ -33,6 +33,29 @@ static float get_speed_scaler(void)
return speed_scaler; return speed_scaler;
} }
/*
return true if the current settings and mode should allow for stick mixing
*/
static bool stick_mixing_enabled(void)
{
if (control_mode == CIRCLE && failsafe != FAILSAFE_NONE) {
// we are in short failsafe
return false;
}
if (control_mode < FLY_BY_WIRE_A) {
// pilot has control, always mix in pilot controls
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;
}
static void stabilize() static void stabilize()
{ {
@ -76,50 +99,41 @@ static void stabilize()
// Mix Stick input to allow users to override control surfaces // Mix Stick input to allow users to override control surfaces
// ----------------------------------------------------------- // -----------------------------------------------------------
if ((control_mode < FLY_BY_WIRE_A) || if (stick_mixing_enabled()) {
(g.stick_mixing && if (control_mode > FLY_BY_WIRE_B) {
geofence_stickmixing() && // do stick mixing in auto modes
control_mode > FLY_BY_WIRE_B && ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim;
failsafe == FAILSAFE_NONE)) { ch1_inf = fabs(ch1_inf);
ch1_inf = min(ch1_inf, 400.0);
ch1_inf = ((400.0 - ch1_inf) /400.0);
// TODO: use RC_Channel control_mix function? ch2_inf = (float)g.channel_pitch.radio_in - g.channel_pitch.radio_trim;
ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim; ch2_inf = fabs(ch2_inf);
ch1_inf = fabs(ch1_inf); ch2_inf = min(ch2_inf, 400.0);
ch1_inf = min(ch1_inf, 400.0); ch2_inf = ((400.0 - ch2_inf) /400.0);
ch1_inf = ((400.0 - ch1_inf) /400.0);
// scale the sensor input based on the stick input
// -----------------------------------------------
g.channel_roll.servo_out *= ch1_inf;
g.channel_pitch.servo_out *= ch2_inf;
ch2_inf = (float)g.channel_pitch.radio_in - g.channel_pitch.radio_trim; // Mix in stick inputs
ch2_inf = fabs(ch2_inf); // -------------------
ch2_inf = min(ch2_inf, 400.0); g.channel_roll.servo_out += g.channel_roll.pwm_to_angle();
ch2_inf = ((400.0 - ch2_inf) /400.0); g.channel_pitch.servo_out += g.channel_pitch.pwm_to_angle();
}
// scale the sensor input based on the stick input if (control_mode >= FLY_BY_WIRE_A) {
// ----------------------------------------------- // stick mixing performed for rudder for all cases including FBW unless disabled for higher modes
g.channel_roll.servo_out *= ch1_inf; // important for steering on the ground during landing
g.channel_pitch.servo_out *= ch2_inf; // -----------------------------------------------
ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim;
// Mix in stick inputs ch4_inf = fabs(ch4_inf);
// ------------------- ch4_inf = min(ch4_inf, 400.0);
g.channel_roll.servo_out += g.channel_roll.pwm_to_angle(); ch4_inf = ((400.0 - ch4_inf) /400.0);
g.channel_pitch.servo_out += g.channel_pitch.pwm_to_angle(); }
//Serial.printf_P(PSTR(" servo_out[CH_ROLL] "));
//Serial.println(servo_out[CH_ROLL],DEC);
} }
// stick mixing performed for rudder for all cases including FBW unless disabled for higher modes
// important for steering on the ground during landing
// -----------------------------------------------
if (control_mode <= FLY_BY_WIRE_B ||
(g.stick_mixing &&
geofence_stickmixing() &&
failsafe == FAILSAFE_NONE)) {
ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim;
ch4_inf = fabs(ch4_inf);
ch4_inf = min(ch4_inf, 400.0);
ch4_inf = ((400.0 - ch4_inf) /400.0);
}
// Apply output to Rudder // Apply output to Rudder
// ---------------------- // ----------------------
calc_nav_yaw(speed_scaler, ch4_inf); calc_nav_yaw(speed_scaler, ch4_inf);