From 297b0d01cb2461d324d4fe2b6ad5089f0a4a1574 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Fri, 14 Mar 2014 01:47:37 -0400 Subject: [PATCH 1/7] OP-1260 remove anti-windup, 2 PIDs, mode transition --- flight/modules/Stabilization/stabilization.c | 74 ++++++++++++++++++- .../src/plugins/config/stabilization.ui | 16 ++-- .../stabilizationsettings.xml | 2 +- 3 files changed, 82 insertions(+), 10 deletions(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index e97d11681..78b6c6966 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -85,11 +85,17 @@ // number of flight mode switch positions #define NUM_FMS_POSITIONS 6 +#if 0 // The PID_RATE_ROLL set is used by Rate mode and the rate portion of Attitude mode // The PID_RATE set is used by the attitude portion of Attitude mode // The PID_RATEA_ROLL set is used by Rattitude mode because it needs to maintain // - two independant rate PIDs because it does rate and attitude simultaneously enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_RATEA_ROLL, PID_RATEA_PITCH, PID_RATEA_YAW, PID_MAX }; +#else +// The PID_RATE_ROLL set is used by Rate mode and the rate portion of Attitude mode +// The PID_RATE set is used by the attitude portion of Attitude mode +enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX }; +#endif enum { RATE_P, RATE_I, RATE_D, RATE_LIMIT, RATE_OFFSET }; enum { ATT_P, ATT_I, ATT_LIMIT, ATT_OFFSET }; @@ -109,7 +115,11 @@ struct pid pids[PID_MAX]; int cur_flight_mode = -1; +#if 0 static uint8_t rattitude_anti_windup; +#else +static float rattitude_mode_transition_stick_position; +#endif static float cruise_control_min_thrust; static float cruise_control_max_thrust; static uint8_t cruise_control_max_angle; @@ -127,8 +137,10 @@ static void SettingsUpdatedCb(UAVObjEvent *ev); static void BankUpdatedCb(UAVObjEvent *ev); static void SettingsBankUpdatedCb(UAVObjEvent *ev); +#if 0 static float stab_log2f(float x); static float stab_powf(float x, uint8_t p); +#endif /** * Module initialization @@ -388,7 +400,9 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) if (reinit) { pids[PID_ROLL + i].iAccumulator = 0; pids[PID_RATE_ROLL + i].iAccumulator = 0; +#if 0 pids[PID_RATEA_ROLL + i].iAccumulator = 0; +#endif } // Compute what Rate mode would give for this stick angle's rate @@ -411,8 +425,16 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) float rateDesiredAxisAttitude; rateDesiredAxisAttitude = pid_apply(&pids[PID_ROLL + i], attitude_error, dT); rateDesiredAxisAttitude = bound(rateDesiredAxisAttitude, +//it looks like P is degreespersecond per degree of angle error +//so it is NOT high enough to ramp up like I thought it would +//maybe I just need to measure the current stick angle for 179 bank angle +#if 0 cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]); +#else + cast_struct_to_array(stabBank.ManualRate, + stabBank.ManualRate.Roll)[i]); +#endif // Compute the weighted average rate desired // Using max() rather than sqrt() for cpu speed; @@ -422,9 +444,44 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // magnitude = sqrt(stabDesired.Roll*stabDesired.Roll + stabDesired.Pitch*stabDesired.Pitch); float magnitude; magnitude = fmaxf(fabsf(stabDesired.Roll), fabsf(stabDesired.Pitch)); + +#if 0 +#else + // modify magnitude to move the Att to Rate transition to the place + // specified by the user + // we are looking for where the stick angle == transition angle + // and the Att rate equals the Rate rate + // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion] + // == Rate x StickAngle [Rate pulling up according to stick angle] + // * StickAngle [X Ratt proportion] + // so 1-x == x*x or x*x+x-1=0 where xE(0,1) + // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2 + // and quadratic formula says that is 0.618033989f + // I tested 14.01 and came up with .61 without even remembering this number + // I thought that moving the P,I, and maxangle terms around would change this value + // and that I would have to take these into account, but varying + // all P's and I's by factors of 1/2 to 2 didn't change it noticeably + // and varying maxangle from 4 to 120 didn't either. + // so for now I'm not taking these into account + // while working with this, it occurred to me that Attitude mode, + // set up with maxangle=190 would be similar to Ratt, and it is. + #define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f + + // the following assumes the transition would otherwise be at 0.618033989f + // and that assumes that Att ramps up to max roll rate + // when a small number of degrees off of where it should be + + // if below the transition angle (still in attitude mode) + if (magnitude < rattitude_mode_transition_stick_position) { + magnitude *= STICK_VALUE_AT_MODE_TRANSITION / rattitude_mode_transition_stick_position; + } else { + magnitude = (magnitude - rattitude_mode_transition_stick_position) * (1.0f-STICK_VALUE_AT_MODE_TRANSITION) / (1.0f - rattitude_mode_transition_stick_position) + STICK_VALUE_AT_MODE_TRANSITION; + } +#endif rateDesiredAxis[i] = (1.0f - magnitude) * rateDesiredAxisAttitude + magnitude * rateDesiredAxisRate; +#if 0 // Compute the inner loop for both Rate mode and Attitude mode // actuatorDesiredAxis[i] is the weighted average of the two PIDs from the two rates actuatorDesiredAxis[i] = @@ -491,6 +548,12 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) pids[PID_RATE_ROLL + i].iAccumulator *= factor; } } +#else + // Compute the inner loop for the averaged rate + // actuatorDesiredAxis[i] is the weighted average + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); +#endif break; } @@ -715,6 +778,7 @@ static float bound(float val, float range) } +#if 0 // x small (0.0 < x < .01) so interpolation of fractional part is reasonable static float stab_log2f(float x) { @@ -748,6 +812,7 @@ static float stab_powf(float x, uint8_t p) return retval; } +#endif static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) @@ -859,6 +924,7 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) 0, bank.YawPI.ILimit); +#if 0 // Set the Rattitude roll rate PID constants pid_configure(&pids[PID_RATEA_ROLL], bank.RollRatePID.Kp, @@ -879,6 +945,7 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) bank.YawRatePID.Ki, bank.YawRatePID.Kd, bank.YawRatePID.ILimit); +#endif } @@ -919,7 +986,12 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) cur_flight_mode = -1; // Rattitude flight mode anti-windup factor - rattitude_anti_windup = settings.RattitudeAntiWindup; +#if 0 + rattitude_anti_windup = settings.RattitudeAntiWindup; +#else + rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransistion / 100.0f; + //rattitude_mode_transition_stick_position = ((float)settings.RattitudeAntiWindup) / 31.0f; +#endif cruise_control_min_thrust = (float)settings.CruiseControlMinThrust / 100.0f; cruise_control_max_thrust = (float)settings.CruiseControlMaxThrust / 100.0f; diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 0d640831f..e40f1f900 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -95,7 +95,7 @@ QTabWidget::Rounded - 0 + 3 false @@ -24004,7 +24004,7 @@ border-radius: 5; - + 0 @@ -24027,7 +24027,7 @@ border-radius: 5; Qt::StrongFocus - <html><head/><body><p>Higher values will keep larger Ki terms and limits from winding up at partial stick. Consider increasing this if you have high Ki values and limits and a sudden stick motion from one aircraft bank angle to another causes the aircraft to rotate and then slowly change rotation.</p></body></html> + <html><head/><body><p>Percentage of full stick where the transition from Attitude to Rate occurs. This transition always occurs when the aircraft is exactly inverted (bank angle 180 degrees). Small values are dangerous because they cause flips at small stick angles. Values significantly over 100 act like attitude mode and can never flip.</p></body></html> Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -24039,18 +24039,18 @@ border-radius: 5; 0 - 0.000000000000000 + 25.000000000000000 - 31.000000000000000 + 255.000000000000000 - 10.000000000000000 + 80.000000000000000 objname:StabilizationSettings - fieldname:RattitudeAntiWindup + fieldname:RattitudeModeTransistion haslimits:no scale:1 buttongroup:15 @@ -24090,7 +24090,7 @@ color: rgb(255, 255, 255); border-radius: 5; - Anti-Windup + ModeTransition Qt::AlignCenter diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 8aeaff3a5..76153fd9d 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -28,7 +28,7 @@ - + From 390533f2e35e9ff0c685829edb993a5ad4cb3063 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Fri, 14 Mar 2014 02:14:02 -0400 Subject: [PATCH 2/7] OP-1160 Oops, forgot to remove #if 0's after testing --- flight/modules/Stabilization/stabilization.c | 165 ------------------- 1 file changed, 165 deletions(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 78b6c6966..9ee03ceb1 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -85,17 +85,9 @@ // number of flight mode switch positions #define NUM_FMS_POSITIONS 6 -#if 0 -// The PID_RATE_ROLL set is used by Rate mode and the rate portion of Attitude mode -// The PID_RATE set is used by the attitude portion of Attitude mode -// The PID_RATEA_ROLL set is used by Rattitude mode because it needs to maintain -// - two independant rate PIDs because it does rate and attitude simultaneously -enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_RATEA_ROLL, PID_RATEA_PITCH, PID_RATEA_YAW, PID_MAX }; -#else // The PID_RATE_ROLL set is used by Rate mode and the rate portion of Attitude mode // The PID_RATE set is used by the attitude portion of Attitude mode enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX }; -#endif enum { RATE_P, RATE_I, RATE_D, RATE_LIMIT, RATE_OFFSET }; enum { ATT_P, ATT_I, ATT_LIMIT, ATT_OFFSET }; @@ -115,11 +107,7 @@ struct pid pids[PID_MAX]; int cur_flight_mode = -1; -#if 0 -static uint8_t rattitude_anti_windup; -#else static float rattitude_mode_transition_stick_position; -#endif static float cruise_control_min_thrust; static float cruise_control_max_thrust; static uint8_t cruise_control_max_angle; @@ -137,11 +125,6 @@ static void SettingsUpdatedCb(UAVObjEvent *ev); static void BankUpdatedCb(UAVObjEvent *ev); static void SettingsBankUpdatedCb(UAVObjEvent *ev); -#if 0 -static float stab_log2f(float x); -static float stab_powf(float x, uint8_t p); -#endif - /** * Module initialization */ @@ -400,9 +383,6 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) if (reinit) { pids[PID_ROLL + i].iAccumulator = 0; pids[PID_RATE_ROLL + i].iAccumulator = 0; -#if 0 - pids[PID_RATEA_ROLL + i].iAccumulator = 0; -#endif } // Compute what Rate mode would give for this stick angle's rate @@ -425,16 +405,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) float rateDesiredAxisAttitude; rateDesiredAxisAttitude = pid_apply(&pids[PID_ROLL + i], attitude_error, dT); rateDesiredAxisAttitude = bound(rateDesiredAxisAttitude, -//it looks like P is degreespersecond per degree of angle error -//so it is NOT high enough to ramp up like I thought it would -//maybe I just need to measure the current stick angle for 179 bank angle -#if 0 - cast_struct_to_array(stabBank.MaximumRate, - stabBank.MaximumRate.Roll)[i]); -#else cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]); -#endif // Compute the weighted average rate desired // Using max() rather than sqrt() for cpu speed; @@ -445,8 +417,6 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) float magnitude; magnitude = fmaxf(fabsf(stabDesired.Roll), fabsf(stabDesired.Pitch)); -#if 0 -#else // modify magnitude to move the Att to Rate transition to the place // specified by the user // we are looking for where the stick angle == transition angle @@ -477,83 +447,13 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) } else { magnitude = (magnitude - rattitude_mode_transition_stick_position) * (1.0f-STICK_VALUE_AT_MODE_TRANSITION) / (1.0f - rattitude_mode_transition_stick_position) + STICK_VALUE_AT_MODE_TRANSITION; } -#endif rateDesiredAxis[i] = (1.0f - magnitude) * rateDesiredAxisAttitude + magnitude * rateDesiredAxisRate; -#if 0 - // Compute the inner loop for both Rate mode and Attitude mode - // actuatorDesiredAxis[i] is the weighted average of the two PIDs from the two rates - actuatorDesiredAxis[i] = - (1.0f - magnitude) * pid_apply_setpoint(&pids[PID_RATEA_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT) - + magnitude * pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); - - // settings.RattitudeAntiWindup controls the iAccumulator zeroing - // - so both iAccs don't wind up too far; - // - nor do both iAccs get held too close to zero at mid stick - - // I suspect that there would be windup without it - // - since rate and attitude fight each other here - // - rate trying to pull it over the top and attitude trying to pull it back down - - // Wind-up increases linearly with cycles for a fixed error. - // We must never increase the iAcc or we risk oscillation. - - // Use the powf() function to make two anti-windup curves - // - one for zeroing rate close to center stick - // - the other for zeroing attitude close to max stick - - // the bigger the dT the more anti windup needed - // the bigger the PID[].i the more anti windup needed - // more anti windup is achieved with a lower powf() power - // a doubling of e.g. PID[].i should cause the runtime anti windup factor - // to get twice as far away from 1.0 (towards zero) - // e.g. from .90 to .80 - - // magic numbers - // these generate the inverted parabola like curves that go through [0,1] and [1,0] - // the higher the power, the closer the curve is to a straight line - // from [0,1] to [1,1] to [1,0] and the less anti windup is applied - - // the UAVO RattitudeAntiWindup varies from 0 to 31 - // 0 turns off anti windup - // 1 gives very little anti-windup because the power given to powf() is 31 - // 31 gives a lot of anti-windup because the power given to powf() is 1 - // the 32.1 is what does this - // the 7.966 and 17.668 cancel the default PID value and dT given to log2f() - // if these are non-default, tweaking is thus done so the user doesn't have to readjust - // the default value of 10 for UAVO RattitudeAntiWindup gives a power of 22 - // these calculations are for magnitude = 0.5, so 22 corresponds to the number of bits - // used in the mantissa of the float - // i.e. 1.0-(0.5^22) almost underflows - - // This may only be useful for aircraft with large Ki values and limits - if (dT > 0.0f && rattitude_anti_windup > 0.0f) { - float factor; - - // At magnitudes close to one, the Attitude accumulators gets zeroed - if (pids[PID_ROLL + i].i > 0.0f) { - factor = 1.0f - stab_powf(magnitude, ((uint8_t)(32.1f - 7.966f - stab_log2f(dT * pids[PID_ROLL + i].i))) - rattitude_anti_windup); - pids[PID_ROLL + i].iAccumulator *= factor; - } - if (pids[PID_RATEA_ROLL + i].i > 0.0f) { - factor = 1.0f - stab_powf(magnitude, ((uint8_t)(32.1f - 17.668f - stab_log2f(dT * pids[PID_RATEA_ROLL + i].i))) - rattitude_anti_windup); - pids[PID_RATEA_ROLL + i].iAccumulator *= factor; - } - - // At magnitudes close to zero, the Rate accumulator gets zeroed - if (pids[PID_RATE_ROLL + i].i > 0.0f) { - factor = 1.0f - stab_powf(1.0f - magnitude, ((uint8_t)(32.1f - 17.668f - stab_log2f(dT * pids[PID_RATE_ROLL + i].i))) - rattitude_anti_windup); - pids[PID_RATE_ROLL + i].iAccumulator *= factor; - } - } -#else // Compute the inner loop for the averaged rate // actuatorDesiredAxis[i] is the weighted average actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); -#endif break; } @@ -778,43 +678,6 @@ static float bound(float val, float range) } -#if 0 -// x small (0.0 < x < .01) so interpolation of fractional part is reasonable -static float stab_log2f(float x) -{ - union { - volatile float f; - volatile uint32_t i; - volatile unsigned char c[4]; - } __attribute__((packed)) u1, u2; - - u2.f = u1.f = x; - u1.i <<= 1; - u2.i &= 0xff800000; - - // get and unbias the exponent, add in a linear interpolation of the fractional part - return (float)(u1.c[3] - 127) + (x / u2.f) - 1.0f; -} - - -// 0<=x<=1, 0<=p<=31 -static float stab_powf(float x, uint8_t p) -{ - float retval = 1.0f; - - while (p) { - if (p & 1) { - retval *= x; - } - x *= x; - p >>= 1; - } - - return retval; -} -#endif - - static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { if (cur_flight_mode < 0 || cur_flight_mode >= NUM_FMS_POSITIONS) { @@ -923,29 +786,6 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) bank.YawPI.Ki, 0, bank.YawPI.ILimit); - -#if 0 - // Set the Rattitude roll rate PID constants - pid_configure(&pids[PID_RATEA_ROLL], - bank.RollRatePID.Kp, - bank.RollRatePID.Ki, - bank.RollRatePID.Kd, - bank.RollRatePID.ILimit); - - // Set the Rattitude pitch rate PID constants - pid_configure(&pids[PID_RATEA_PITCH], - bank.PitchRatePID.Kp, - bank.PitchRatePID.Ki, - bank.PitchRatePID.Kd, - bank.PitchRatePID.ILimit); - - // Set the Rattitude yaw rate PID constants - pid_configure(&pids[PID_RATEA_YAW], - bank.YawRatePID.Kp, - bank.YawRatePID.Ki, - bank.YawRatePID.Kd, - bank.YawRatePID.ILimit); -#endif } @@ -986,12 +826,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) cur_flight_mode = -1; // Rattitude flight mode anti-windup factor -#if 0 - rattitude_anti_windup = settings.RattitudeAntiWindup; -#else rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransistion / 100.0f; - //rattitude_mode_transition_stick_position = ((float)settings.RattitudeAntiWindup) / 31.0f; -#endif cruise_control_min_thrust = (float)settings.CruiseControlMinThrust / 100.0f; cruise_control_max_thrust = (float)settings.CruiseControlMaxThrust / 100.0f; From 1cb9d9548f2f7748cfbf943b571b6f1da8af2efc Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Mon, 17 Mar 2014 22:40:19 -0400 Subject: [PATCH 3/7] OP-1160 limit values in flight code (not just GCS) to avoid DZ. Set GCS tab index back to 0. --- flight/modules/Stabilization/stabilization.c | 11 ++++++++--- .../openpilotgcs/src/plugins/config/stabilization.ui | 7 ++++++- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 9ee03ceb1..ef2c72a53 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -442,7 +442,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // when a small number of degrees off of where it should be // if below the transition angle (still in attitude mode) - if (magnitude < rattitude_mode_transition_stick_position) { + // '<=' instead of ',' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ + if (magnitude <= rattitude_mode_transition_stick_position) { magnitude *= STICK_VALUE_AT_MODE_TRANSITION / rattitude_mode_transition_stick_position; } else { magnitude = (magnitude - rattitude_mode_transition_stick_position) * (1.0f-STICK_VALUE_AT_MODE_TRANSITION) / (1.0f - rattitude_mode_transition_stick_position) + STICK_VALUE_AT_MODE_TRANSITION; @@ -825,8 +826,12 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) // force flight mode update cur_flight_mode = -1; - // Rattitude flight mode anti-windup factor - rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransistion / 100.0f; + // Rattitude stick angle where the attitude to rate transition happens + if (settings.RattitudeModeTransistion < (uint8_t) 10) { + rattitude_mode_transition_stick_position = 10.0f / 100.0f; + } else { + rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransistion / 100.0f; + } cruise_control_min_thrust = (float)settings.CruiseControlMinThrust / 100.0f; cruise_control_max_thrust = (float)settings.CruiseControlMaxThrust / 100.0f; diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index e40f1f900..6414995eb 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -95,7 +95,7 @@ QTabWidget::Rounded - 3 + 0 false @@ -13377,6 +13377,7 @@ border-radius: 5; 5 + 0.000100000000000 @@ -19417,6 +19418,7 @@ border-radius: 5; false + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); @@ -19941,6 +19943,7 @@ border-radius: 5; 39 39 39 + @@ -21975,6 +21978,7 @@ border-radius: 5; + @@ -26158,6 +26162,7 @@ border-radius: 5; Qt::StrongFocus + From 945b60cf99d868dd6f91be2f405cf8bf7ee062f7 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Thu, 20 Mar 2014 12:41:48 -0400 Subject: [PATCH 4/7] OP-1260 Fixed typo in a fairly important comment --- flight/modules/Stabilization/stabilization.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index ef2c72a53..20fe1100b 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -442,7 +442,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // when a small number of degrees off of where it should be // if below the transition angle (still in attitude mode) - // '<=' instead of ',' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ + // '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ if (magnitude <= rattitude_mode_transition_stick_position) { magnitude *= STICK_VALUE_AT_MODE_TRANSITION / rattitude_mode_transition_stick_position; } else { From 820e8dfa3ed32bc9f9dd56639dfe78f724666850 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Fri, 21 Mar 2014 22:34:03 -0400 Subject: [PATCH 5/7] OP-1260 use predefined macro for size of Flight Mode Switch --- flight/modules/Stabilization/stabilization.c | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 20fe1100b..e5607272e 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -82,9 +82,6 @@ #define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // FLIGHT CONTROL priority #define FAILSAFE_TIMEOUT_MS 30 -// number of flight mode switch positions -#define NUM_FMS_POSITIONS 6 - // The PID_RATE_ROLL set is used by Rate mode and the rate portion of Attitude mode // The PID_RATE set is used by the attitude portion of Attitude mode enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX }; @@ -115,7 +112,7 @@ static float cruise_control_max_power_factor; static float cruise_control_power_trim; static int8_t cruise_control_inverted_power_switch; static float cruise_control_neutral_thrust; -static uint8_t cruise_control_flight_mode_switch_pos_enable[NUM_FMS_POSITIONS]; +static uint8_t cruise_control_flight_mode_switch_pos_enable[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM]; // Private functions static void stabilizationTask(void *parameters); @@ -162,9 +159,6 @@ int32_t StabilizationStart() */ int32_t StabilizationInitialize() { - // stop the compile if the number of switch positions changes, but has not been changed here - PIOS_STATIC_ASSERT(NUM_FMS_POSITIONS == sizeof(((FlightModeSettingsData *)0)->FlightModePosition) / sizeof((((FlightModeSettingsData *)0)->FlightModePosition)[0])); - // Initialize variables ManualControlCommandInitialize(); ManualControlSettingsInitialize(); @@ -568,7 +562,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // to maintain same altitdue with changing bank angle // but without manually adjusting thrust // do it here and all the various flight modes (e.g. Altitude Hold) can use it - if (flight_mode_switch_position < NUM_FMS_POSITIONS + if (flight_mode_switch_position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM && cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != (uint8_t)0 && cruise_control_max_power_factor > 0.0001f) { static uint8_t toggle; @@ -681,7 +675,7 @@ static float bound(float val, float range) static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - if (cur_flight_mode < 0 || cur_flight_mode >= NUM_FMS_POSITIONS) { + if (cur_flight_mode < 0 || cur_flight_mode >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) { return; } if ((ev) && ((settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) || From 5ecea4bf33589ca990ed0ff0b083e7090757172d Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sun, 23 Mar 2014 13:00:00 -0400 Subject: [PATCH 6/7] OP-1260 fix misspelling of RattitudeModeTransition --- flight/modules/Stabilization/stabilization.c | 2 +- ground/openpilotgcs/src/plugins/config/stabilization.ui | 8 ++++++-- shared/uavobjectdefinition/stabilizationsettings.xml | 2 +- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index e5607272e..be1b1616c 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -824,7 +824,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) if (settings.RattitudeModeTransistion < (uint8_t) 10) { rattitude_mode_transition_stick_position = 10.0f / 100.0f; } else { - rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransistion / 100.0f; + rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransition / 100.0f; } cruise_control_min_thrust = (float)settings.CruiseControlMinThrust / 100.0f; diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 6414995eb..468b4b527 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -13378,6 +13378,7 @@ border-radius: 5; 5 + 0.000100000000000 @@ -15942,6 +15943,7 @@ border-radius: 5; element:Kp haslimits:no scale:1 + buttongroup:5,20 @@ -17353,6 +17355,7 @@ border-radius: 5; 26 26 26 + @@ -24008,7 +24011,7 @@ border-radius: 5; - + 0 @@ -24054,7 +24057,7 @@ border-radius: 5; objname:StabilizationSettings - fieldname:RattitudeModeTransistion + fieldname:RattitudeModeTransition haslimits:no scale:1 buttongroup:15 @@ -24307,6 +24310,7 @@ border-radius: 5; buttongroup:16 + diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 76153fd9d..d9702c75e 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -28,7 +28,7 @@ - + From d36cfe28edbcc6494048b06192a36f08fc04ecdc Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sun, 23 Mar 2014 19:04:17 -0400 Subject: [PATCH 7/7] OP-1260 finish renaming RattitudeModeTransition I don't know how it built before. --- flight/modules/Stabilization/stabilization.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index be1b1616c..d6c5ded97 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -821,7 +821,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) cur_flight_mode = -1; // Rattitude stick angle where the attitude to rate transition happens - if (settings.RattitudeModeTransistion < (uint8_t) 10) { + if (settings.RattitudeModeTransition < (uint8_t) 10) { rattitude_mode_transition_stick_position = 10.0f / 100.0f; } else { rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransition / 100.0f;