From 23840888557e62259e691ceeba0265e3dca28fe6 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Sat, 26 May 2018 16:16:35 -0400 Subject: [PATCH] Requested changes, cleanup and control logic updates Also added a new parameter acro_trainer_gain to allow adjustments to the limiting strength. --- src/main/flight/pid.c | 81 +++++++++++++++++++---------------- src/main/flight/pid.h | 1 + src/main/interface/msp_box.c | 2 +- src/main/interface/settings.c | 1 + 4 files changed, 46 insertions(+), 39 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e2ab4b10c7..8b78dfba24 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -89,8 +89,8 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig, #endif #ifdef USE_ACRO_TRAINER -#define ACRO_TRAINER_GAIN 5.0f // matches default angle mode strength of 50 -#define ACRO_TRAINER_LOOKAHEAD_MIN 10000 // 10ms +#define ACRO_TRAINER_LOOKAHEAD_MIN_TIME 0.01f +#define ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT 500.0f // Max gyro rate for lookahead time scaling #endif // USE_ACRO_TRAINER PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 3); @@ -150,7 +150,8 @@ void resetPidProfile(pidProfile_t *pidProfile) .iterm_relax_cutoff_high = 15, .acro_trainer_angle_limit = 20, .acro_trainer_lookahead_ms = 50, - .acro_trainer_debug_axis = FD_ROLL + .acro_trainer_debug_axis = FD_ROLL, + .acro_trainer_gain = 75 ); } @@ -321,11 +322,12 @@ static FAST_RAM_ZERO_INIT bool itermRotation; static FAST_RAM_ZERO_INIT bool smartFeedforward; #ifdef USE_ACRO_TRAINER -static FAST_RAM_ZERO_INIT float acro_trainer_angle_limit; -static FAST_RAM_ZERO_INIT timeUs_t acro_trainer_lookahead_time; -static FAST_RAM_ZERO_INIT uint8_t acro_trainer_debug_axis; +static FAST_RAM_ZERO_INIT float acroTrainerAngleLimit; +static FAST_RAM_ZERO_INIT float acroTrainerLookaheadTime; +static FAST_RAM_ZERO_INIT uint8_t acroTrainerDebugAxis; static FAST_RAM_ZERO_INIT bool acroTrainerActive; -static FAST_RAM_ZERO_INIT int8_t acroTrainerAxisState[2]; // only need roll and pitch +static FAST_RAM_ZERO_INIT int acroTrainerAxisState[2]; // only need roll and pitch +static FAST_RAM_ZERO_INIT float acroTrainerGain; #endif // USE_ACRO_TRAINER void pidInitConfig(const pidProfile_t *pidProfile) @@ -369,9 +371,10 @@ void pidInitConfig(const pidProfile_t *pidProfile) itermRelaxCutoffHigh = pidProfile->iterm_relax_cutoff_high; #ifdef USE_ACRO_TRAINER - acro_trainer_angle_limit = pidProfile->acro_trainer_angle_limit; - acro_trainer_lookahead_time = pidProfile->acro_trainer_lookahead_ms * 1000; - acro_trainer_debug_axis = pidProfile->acro_trainer_debug_axis; + acroTrainerAngleLimit = pidProfile->acro_trainer_angle_limit; + acroTrainerLookaheadTime = (float)pidProfile->acro_trainer_lookahead_ms / 1000.0f; + acroTrainerDebugAxis = pidProfile->acro_trainer_debug_axis; + acroTrainerGain = (float)pidProfile->acro_trainer_gain / 10.0f; #endif // USE_ACRO_TRAINER } @@ -576,9 +579,9 @@ static void handleItermRotation() #ifdef USE_ACRO_TRAINER -int8_t acroTrainerSign(float x) +int acroTrainerSign(float x) { - return (x > 0.0f) - (x < 0.0f); + return x > 0 ? 1 : -1; } // Acro Trainer - Manipulate the setPoint to limit axis angle while in acro mode @@ -589,59 +592,61 @@ int8_t acroTrainerSign(float x) // Manage the setPoint to control the gyro rate as the actual angle approaches the limit (try to prevent overshoot) // 3. If no potential overflow is detected, then return the original setPoint -static float applyAcroTrainer(int axis, const rollAndPitchTrims_t *angleTrim, float setPoint) +// Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM. We accept the +// performance decrease when Acro Trainer mode is active under the assumption that user is unlikely to be +// expecting ultimate flight performance at very high loop rates when in this mode. +static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTrims_t *angleTrim, float setPoint) { float ret = setPoint; -#ifndef SIMULATOR_BUILD if (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) { bool resetIterm = false; - float projectedAngle; - const int8_t setpointSign = acroTrainerSign(setPoint); + float projectedAngle = 0; + const int setpointSign = acroTrainerSign(setPoint); const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f; - const int8_t angleSign = acroTrainerSign(currentAngle); + const int angleSign = acroTrainerSign(currentAngle); + + if ((acroTrainerAxisState[axis] != 0) && (acroTrainerAxisState[axis] != setpointSign)) { // stick has reversed - stop limiting + acroTrainerAxisState[axis] = 0; + } // Limit and correct the angle when it exceeds the limit - if ((fabsf(currentAngle) > acro_trainer_angle_limit) || (acroTrainerAxisState[axis] != 0)) { - if (angleSign == -setpointSign) { // stick has reversed - stop limiting - acroTrainerAxisState[axis] = 0; - } else { - if (acroTrainerAxisState[axis] != angleSign) { - acroTrainerAxisState[axis] = angleSign; - resetIterm = true; - } - ret = ((acro_trainer_angle_limit * angleSign) - currentAngle) * ACRO_TRAINER_GAIN; + if ((fabsf(currentAngle) > acroTrainerAngleLimit) && (acroTrainerAxisState[axis] == 0)) { + if (angleSign == setpointSign) { + acroTrainerAxisState[axis] = angleSign; + resetIterm = true; } } + + if (acroTrainerAxisState[axis] != 0) { + ret = ((acroTrainerAngleLimit * angleSign) - currentAngle) * acroTrainerGain; + } else { // Not currently over the limit so project the angle based on current angle and // gyro angular rate using a sliding window based on gyro rate (faster rotation means larger window. // If the projected angle exceeds the limit then apply limiting to minimize overshoot. - if (acroTrainerAxisState[axis] == 0) { - const timeUs_t checkInterval = MAX(lrintf(constrainf(fabsf(gyro.gyroADCf[axis]) / 500.0f, 0.0f, 1.0f) * acro_trainer_lookahead_time), ACRO_TRAINER_LOOKAHEAD_MIN); - projectedAngle = (gyro.gyroADCf[axis] * checkInterval * 0.000001f) + currentAngle; - const int8_t projectedAngleSign = acroTrainerSign(projectedAngle); - if ((fabsf(projectedAngle) > acro_trainer_angle_limit) && (projectedAngleSign == setpointSign)) { - ret = ((acro_trainer_angle_limit * projectedAngleSign) - projectedAngle) * ACRO_TRAINER_GAIN; + // Calculate the lookahead window by scaling proportionally with gyro rate from 0-500dps + float checkInterval = constrainf(fabsf(gyro.gyroADCf[axis]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT, 0.0f, 1.0f) * acroTrainerLookaheadTime; + checkInterval = MAX(checkInterval, ACRO_TRAINER_LOOKAHEAD_MIN_TIME); + projectedAngle = (gyro.gyroADCf[axis] * checkInterval) + currentAngle; + const int projectedAngleSign = acroTrainerSign(projectedAngle); + if ((fabsf(projectedAngle) > acroTrainerAngleLimit) && (projectedAngleSign == setpointSign)) { + ret = ((acroTrainerAngleLimit * projectedAngleSign) - projectedAngle) * acroTrainerGain; resetIterm = true; } } if (resetIterm) { - pidData[axis].I = 0.0f; + pidData[axis].I = 0; } - if (axis == acro_trainer_debug_axis) { + if (axis == acroTrainerDebugAxis) { DEBUG_SET(DEBUG_ACRO_TRAINER, 0, lrintf(currentAngle * 10.0f)); DEBUG_SET(DEBUG_ACRO_TRAINER, 1, acroTrainerAxisState[axis]); DEBUG_SET(DEBUG_ACRO_TRAINER, 2, lrintf(ret)); DEBUG_SET(DEBUG_ACRO_TRAINER, 3, lrintf(projectedAngle * 10.0f)); } } -#else - UNUSED(axis); - UNUSED(angleTrim); -#endif // SIMULATOR_BUILD return ret; } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index c098ab814b..f33cd2de96 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -131,6 +131,7 @@ typedef struct pidProfile_s { uint8_t acro_trainer_angle_limit; // Acro trainer roll/pitch angle limit in degrees uint16_t acro_trainer_lookahead_ms; // The lookahead window in milliseconds used to reduce overshoot uint8_t acro_trainer_debug_axis; // The axis for which record debugging values are captured 0=roll, 1=pitch + uint8_t acro_trainer_gain; // The strength of the limiting. Raising may reduce overshoot but also lead to oscillation around the angle limit } pidProfile_t; #ifndef USE_OSD_SLAVE diff --git a/src/main/interface/msp_box.c b/src/main/interface/msp_box.c index 481abe0e89..54ef9cec1c 100644 --- a/src/main/interface/msp_box.c +++ b/src/main/interface/msp_box.c @@ -94,7 +94,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = { { BOXPIDAUDIO, "PID AUDIO", 44 }, { BOXPARALYZE, "PARALYZE", 45 }, { BOXGPSRESCUE, "GPS RESCUE", 46 }, - { BOXACROTRAINER, "ACRO TRAINER", 47}, + { BOXACROTRAINER, "ACRO TRAINER", 47 }, }; // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index abc8fa4287..3e134a6340 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -771,6 +771,7 @@ const clivalue_t valueTable[] = { { "acro_trainer_angle_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 80 }, PG_PID_PROFILE, offsetof(pidProfile_t, acro_trainer_angle_limit) }, { "acro_trainer_lookahead_ms", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, acro_trainer_lookahead_ms) }, { "acro_trainer_debug_axis", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ACRO_TRAINER_DEBUG }, PG_PID_PROFILE, offsetof(pidProfile_t, acro_trainer_debug_axis) }, + { "acro_trainer_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 25, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, acro_trainer_gain) }, #endif // USE_ACRO_TRAINER { "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].P) },