From 1f827d848bd42aec6e4eefce32caaf3d85722754 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 23 Sep 2012 09:33:17 +1000
Subject: [PATCH] APM: fixed stick mixing in CIRCLE mode on throttle failsafe
Many thanks to Andke for finding this bug!
---
ArduPlane/Attitude.pde | 98 ++++++++++++++++++++++++------------------
1 file changed, 56 insertions(+), 42 deletions(-)
diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde
index af0303506..e2860ea8b 100644
--- a/ArduPlane/Attitude.pde
+++ b/ArduPlane/Attitude.pde
@@ -33,6 +33,29 @@ static float get_speed_scaler(void)
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()
{
@@ -76,49 +99,40 @@ static void stabilize()
// Mix Stick input to allow users to override control surfaces
// -----------------------------------------------------------
- if ((control_mode < FLY_BY_WIRE_A) ||
- (g.stick_mixing &&
- geofence_stickmixing() &&
- control_mode > FLY_BY_WIRE_B &&
- failsafe == FAILSAFE_NONE)) {
-
- // TODO: use RC_Channel control_mix function?
- ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim;
- ch1_inf = fabs(ch1_inf);
- ch1_inf = min(ch1_inf, 400.0);
- ch1_inf = ((400.0 - ch1_inf) /400.0);
-
- ch2_inf = (float)g.channel_pitch.radio_in - g.channel_pitch.radio_trim;
- ch2_inf = fabs(ch2_inf);
- ch2_inf = min(ch2_inf, 400.0);
- ch2_inf = ((400.0 - ch2_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;
-
- // Mix in stick inputs
- // -------------------
- g.channel_roll.servo_out += g.channel_roll.pwm_to_angle();
- 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);
- }
+ if (stick_mixing_enabled()) {
+ if (control_mode > FLY_BY_WIRE_B) {
+ // do stick mixing in auto modes
+ ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim;
+ ch1_inf = fabs(ch1_inf);
+ ch1_inf = min(ch1_inf, 400.0);
+ ch1_inf = ((400.0 - ch1_inf) /400.0);
+
+ ch2_inf = (float)g.channel_pitch.radio_in - g.channel_pitch.radio_trim;
+ ch2_inf = fabs(ch2_inf);
+ ch2_inf = min(ch2_inf, 400.0);
+ ch2_inf = ((400.0 - ch2_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;
+
+ // Mix in stick inputs
+ // -------------------
+ g.channel_roll.servo_out += g.channel_roll.pwm_to_angle();
+ g.channel_pitch.servo_out += g.channel_pitch.pwm_to_angle();
+ }
- // 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);
- }
+ if (control_mode >= FLY_BY_WIRE_A) {
+ // stick mixing performed for rudder for all cases including FBW unless disabled for higher modes
+ // important for steering on the ground during landing
+ // -----------------------------------------------
+ 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
// ----------------------
--
GitLab