From 8b42bd36469839d955b810cda616f84e8e23db04 Mon Sep 17 00:00:00 2001 From: Kevin Plaizier <46289813+Quick-Flash@users.noreply.github.com> Date: Fri, 18 Mar 2022 18:08:01 -0600 Subject: [PATCH] remove absolute control, update iterm rotation --- src/main/blackbox/blackbox.c | 3 - src/main/cli/settings.c | 7 -- src/main/flight/pid.c | 127 ++++++---------------------------- src/main/flight/pid.h | 13 ---- src/main/flight/pid_init.c | 20 ------ src/main/msp/msp.c | 10 +-- src/main/target/common_post.h | 4 -- src/main/target/common_pre.h | 1 - src/test/Makefile | 3 +- 9 files changed, 23 insertions(+), 165 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 552760f141..ac90ddf303 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1379,9 +1379,6 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANTI_GRAVITY_MODE, "%d", currentPidProfile->antiGravityMode); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANTI_GRAVITY_THRESHOLD, "%d", currentPidProfile->itermThrottleThreshold); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANTI_GRAVITY_GAIN, "%d", currentPidProfile->itermAcceleratorGain); -#ifdef USE_ABSOLUTE_CONTROL - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ABS_CONTROL_GAIN, "%d", currentPidProfile->abs_control_gain); -#endif #ifdef USE_INTEGRATED_YAW_CONTROL BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_USE_INTEGRATED_YAW, "%d", currentPidProfile->use_integrated_yaw); #endif diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index f85ac5fd7b..6433e9f2b9 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1142,13 +1142,6 @@ const clivalue_t valueTable[] = { { "horizon_tilt_effect", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_effect) }, { "horizon_tilt_expert_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_expert_mode) }, -#if defined(USE_ABSOLUTE_CONTROL) - { PARAM_NAME_ABS_CONTROL_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_gain) }, - { "abs_control_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_limit) }, - { "abs_control_error_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 45 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_error_limit) }, - { "abs_control_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 45 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_cutoff) }, -#endif - #ifdef USE_INTEGRATED_YAW_CONTROL { PARAM_NAME_USE_INTEGRATED_YAW, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, use_integrated_yaw) }, { "integrated_yaw_relax", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, integrated_yaw_relax) }, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 6722dab7df..b64272d848 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -78,10 +78,6 @@ FAST_DATA_ZERO_INIT uint32_t targetPidLooptime; FAST_DATA_ZERO_INIT pidAxisData_t pidData[XYZ_AXIS_COUNT]; FAST_DATA_ZERO_INIT pidRuntime_t pidRuntime; -#if defined(USE_ABSOLUTE_CONTROL) -STATIC_UNIT_TESTED FAST_DATA_ZERO_INIT float axisError[XYZ_AXIS_COUNT]; -#endif - #if defined(USE_THROTTLE_BOOST) FAST_DATA_ZERO_INIT float throttleBoost; pt1Filter_t throttleLpf; @@ -160,7 +156,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .itermLimit = 400, .throttle_boost = 5, .throttle_boost_cutoff = 15, - .iterm_rotation = false, + .iterm_rotation = true, .iterm_relax = ITERM_RELAX_RP, .iterm_relax_cutoff = ITERM_RELAX_CUTOFF_DEFAULT, .iterm_relax_type = ITERM_RELAX_SETPOINT, @@ -168,10 +164,6 @@ void resetPidProfile(pidProfile_t *pidProfile) .acro_trainer_lookahead_ms = 50, .acro_trainer_debug_axis = FD_ROLL, .acro_trainer_gain = 75, - .abs_control_gain = 0, - .abs_control_limit = 90, - .abs_control_error_limit = 20, - .abs_control_cutoff = 11, .antiGravityMode = ANTI_GRAVITY_SMOOTH, .dterm_lpf1_static_hz = DTERM_LPF1_DYN_MIN_HZ_DEFAULT, // NOTE: dynamic lpf is enabled by default so this setting is actually @@ -285,9 +277,6 @@ void pidResetIterm(void) { for (int axis = 0; axis < 3; axis++) { pidData[axis].I = 0.0f; -#if defined(USE_ABSOLUTE_CONTROL) - axisError[axis] = 0.0f; -#endif } } @@ -614,47 +603,29 @@ static float accelerationLimit(int axis, float currentPidSetpoint) return currentPidSetpoint; } -static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT]) +#define SIN2(R) ((R)-(R)*(R)*(R)/6) +#define COS2(R) (1.0f-(R)*(R)/2) + +static inline void rotateAroundYaw(float *x, float *y, float r) { - // rotate v around rotation vector rotation - // rotation in radians, all elements must be small - for (int i = 0; i < XYZ_AXIS_COUNT; i++) { - int i_1 = (i + 1) % 3; - int i_2 = (i + 2) % 3; - float newV = v[i_1] + v[i_2] * rotation[i]; - v[i_2] -= v[i_1] * rotation[i]; - v[i_1] = newV; - } + float a,b,s,c; + + s = SIN2(r); + c = COS2(r); + + a = x[0]*c + y[0]*s; + b = y[0]*c - x[0]*s; + + x[0] = a; + y[0] = b; } -STATIC_UNIT_TESTED void rotateItermAndAxisError() +STATIC_UNIT_TESTED void rotateIterm() { - if (pidRuntime.itermRotation -#if defined(USE_ABSOLUTE_CONTROL) - || pidRuntime.acGain > 0 || debugMode == DEBUG_AC_ERROR -#endif - ) { - const float gyroToAngle = pidRuntime.dT * RAD; - float rotationRads[XYZ_AXIS_COUNT]; - for (int i = FD_ROLL; i <= FD_YAW; i++) { - rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle; - } -#if defined(USE_ABSOLUTE_CONTROL) - if (pidRuntime.acGain > 0 || debugMode == DEBUG_AC_ERROR) { - rotateVector(axisError, rotationRads); - } -#endif - if (pidRuntime.itermRotation) { - float v[XYZ_AXIS_COUNT]; - for (int i = 0; i < XYZ_AXIS_COUNT; i++) { - v[i] = pidData[i].I; - } - rotateVector(v, rotationRads ); - for (int i = 0; i < XYZ_AXIS_COUNT; i++) { - pidData[i].I = v[i]; - } - } + if (pidRuntime.itermRotation) { + rotateAroundYaw(&pidData[FD_ROLL].I, &pidData[FD_ROLL].I, gyro.gyroADCf[FD_YAW] * pidRuntime.dT * RAD); } + } #ifdef USE_RC_SMOOTHING_FILTER @@ -675,46 +646,6 @@ float FAST_CODE applyRcSmoothingFeedforwardFilter(int axis, float pidSetpointDel #endif // USE_RC_SMOOTHING_FILTER #if defined(USE_ITERM_RELAX) -#if defined(USE_ABSOLUTE_CONTROL) -STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate) -{ - if (pidRuntime.acGain > 0 || debugMode == DEBUG_AC_ERROR) { - const float setpointLpf = pt1FilterApply(&pidRuntime.acLpf[axis], *currentPidSetpoint); - const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf); - float acErrorRate = 0; - const float gmaxac = setpointLpf + 2 * setpointHpf; - const float gminac = setpointLpf - 2 * setpointHpf; - if (gyroRate >= gminac && gyroRate <= gmaxac) { - const float acErrorRate1 = gmaxac - gyroRate; - const float acErrorRate2 = gminac - gyroRate; - if (acErrorRate1 * axisError[axis] < 0) { - acErrorRate = acErrorRate1; - } else { - acErrorRate = acErrorRate2; - } - if (fabsf(acErrorRate * pidRuntime.dT) > fabsf(axisError[axis]) ) { - acErrorRate = -axisError[axis] * pidRuntime.pidFrequency; - } - } else { - acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate; - } - - if (isAirmodeActivated()) { - axisError[axis] = constrainf(axisError[axis] + acErrorRate * pidRuntime.dT, - -pidRuntime.acErrorLimit, pidRuntime.acErrorLimit); - const float acCorrection = constrainf(axisError[axis] * pidRuntime.acGain, -pidRuntime.acLimit, pidRuntime.acLimit); - *currentPidSetpoint += acCorrection; - *itermErrorRate += acCorrection; - DEBUG_SET(DEBUG_AC_CORRECTION, axis, lrintf(acCorrection * 10)); - if (axis == FD_ROLL) { - DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(acCorrection * 10)); - } - } - DEBUG_SET(DEBUG_AC_ERROR, axis, lrintf(axisError[axis] * 10)); - } -} -#endif - STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm, const float gyroRate, float *itermErrorRate, float *currentPidSetpoint) { @@ -742,10 +673,6 @@ STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm, DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(*itermErrorRate)); } } - -#if defined(USE_ABSOLUTE_CONTROL) - applyAbsoluteControl(axis, gyroRate, currentPidSetpoint, itermErrorRate); -#endif } } #endif @@ -926,7 +853,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim gyroRateDterm[axis] = pidRuntime.dtermLowpass2ApplyFn((filter_t *) &pidRuntime.dtermLowpass2[axis], gyroRateDterm[axis]); } - rotateItermAndAxisError(); + rotateIterm(); #ifdef USE_RPM_FILTER rpmFilterUpdate(); @@ -1002,9 +929,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim const float previousIterm = pidData[axis].I; float itermErrorRate = errorRate; -#ifdef USE_ABSOLUTE_CONTROL - float uncorrectedSetpoint = currentPidSetpoint; -#endif #if defined(USE_ITERM_RELAX) if (!launchControlActive && !pidRuntime.inCrashRecoveryMode) { @@ -1012,9 +936,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim errorRate = currentPidSetpoint - gyroRate; } #endif -#ifdef USE_ABSOLUTE_CONTROL - float setpointCorrection = currentPidSetpoint - uncorrectedSetpoint; -#endif // --------low-level gyro-based PID based on 2DOF PID controller. ---------- // 2-DOF PID controller with optional filter on derivative term. @@ -1114,18 +1035,12 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim previousGyroRateDterm[axis] = gyroRateDterm[axis]; // -----calculate feedforward component -#ifdef USE_ABSOLUTE_CONTROL - // include abs control correction in feedforward - pidSetpointDelta += setpointCorrection - pidRuntime.oldSetpointCorrection[axis]; - pidRuntime.oldSetpointCorrection[axis] = setpointCorrection; -#endif - // no feedforward in launch control float feedforwardGain = launchControlActive ? 0.0f : pidRuntime.pidCoefficient[axis].Kf; if (feedforwardGain > 0) { // halve feedforward in Level mode since stick sensitivity is weaker by about half feedforwardGain *= FLIGHT_MODE(ANGLE_MODE) ? 0.5f : 1.0f; - // transition now calculated in feedforward.c when new RC data arrives + // transition now calculated in feedforward.c when new RC data arrives float feedForward = feedforwardGain * pidSetpointDelta * pidRuntime.pidFrequency; #ifdef USE_FEEDFORWARD diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index a38e698a3a..12a4631a21 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -175,10 +175,6 @@ typedef struct pidProfile_s { 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 uint16_t acro_trainer_lookahead_ms; // The lookahead window in milliseconds used to reduce overshoot - uint8_t abs_control_gain; // How strongly should the absolute accumulated error be corrected for - uint8_t abs_control_limit; // Limit to the correction - uint8_t abs_control_error_limit; // Limit to the accumulated error - uint8_t abs_control_cutoff; // Cutoff frequency for path estimation in abs control uint8_t dterm_lpf2_type; // Filter type for 2nd dterm lowpass uint16_t dterm_lpf1_dyn_min_hz; // Dterm lowpass filter 1 min hz when in dynamic mode uint16_t dterm_lpf1_dyn_max_hz; // Dterm lowpass filter 1 max hz when in dynamic mode @@ -319,15 +315,6 @@ typedef struct pidRuntime_s { uint8_t itermRelaxCutoff; #endif -#ifdef USE_ABSOLUTE_CONTROL - float acCutoff; - float acGain; - float acLimit; - float acErrorLimit; - pt1Filter_t acLpf[XYZ_AXIS_COUNT]; - float oldSetpointCorrection[XYZ_AXIS_COUNT]; -#endif - #ifdef USE_D_MIN pt2Filter_t dMinRange[XYZ_AXIS_COUNT]; pt2Filter_t dMinLowpass[XYZ_AXIS_COUNT]; diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index f8d7de86ac..a7df831618 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -210,14 +210,6 @@ void pidInitFilters(const pidProfile_t *pidProfile) } #endif -#if defined(USE_ABSOLUTE_CONTROL) - if (pidRuntime.itermRelax) { - for (int i = 0; i < XYZ_AXIS_COUNT; i++) { - pt1FilterInit(&pidRuntime.acLpf[i], pt1FilterGain(pidRuntime.acCutoff, pidRuntime.dT)); - } - } -#endif - #if defined(USE_D_MIN) // Initialize the filters for all axis even if the d_min[axis] value is 0 // Otherwise if the pidProfile->d_min_xxx parameters are ever added to @@ -338,17 +330,6 @@ void pidInitConfig(const pidProfile_t *pidProfile) pidRuntime.acroTrainerGain = (float)pidProfile->acro_trainer_gain / 10.0f; #endif // USE_ACRO_TRAINER -#if defined(USE_ABSOLUTE_CONTROL) - pidRuntime.acGain = (float)pidProfile->abs_control_gain; - pidRuntime.acLimit = (float)pidProfile->abs_control_limit; - pidRuntime.acErrorLimit = (float)pidProfile->abs_control_error_limit; - pidRuntime.acCutoff = (float)pidProfile->abs_control_cutoff; - for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - float iCorrection = -pidRuntime.acGain * PTERM_SCALE / ITERM_SCALE * pidRuntime.pidCoefficient[axis].Kp; - pidRuntime.pidCoefficient[axis].Ki = MAX(0.0f, pidRuntime.pidCoefficient[axis].Ki + iCorrection); - } -#endif - #ifdef USE_DYN_LPF if (pidProfile->dterm_lpf1_dyn_min_hz > 0) { switch (pidProfile->dterm_lpf1_type) { @@ -440,4 +421,3 @@ void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex) memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t)); } } - diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 386004cf12..cb80b8256e 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1418,7 +1418,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) sbufWriteU16(dst, (uint16_t)constrain(gpsSol.llh.altCm / 100, 0, UINT16_MAX)); // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. To maintain backwards compatibility compensate to 1m per lsb in MSP again. sbufWriteU16(dst, gpsSol.groundSpeed); sbufWriteU16(dst, gpsSol.groundCourse); - // Added in API version 1.44 + // Added in API version 1.44 sbufWriteU16(dst, gpsSol.hdop); break; @@ -1848,11 +1848,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, 0); sbufWriteU8(dst, 0); #endif -#if defined(USE_ABSOLUTE_CONTROL) - sbufWriteU8(dst, currentPidProfile->abs_control_gain); -#else sbufWriteU8(dst, 0); -#endif #if defined(USE_THROTTLE_BOOST) sbufWriteU8(dst, currentPidProfile->throttle_boost); #else @@ -2939,11 +2935,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbufReadU8(src); sbufReadU8(src); #endif -#if defined(USE_ABSOLUTE_CONTROL) - currentPidProfile->abs_control_gain = sbufReadU8(src); -#else sbufReadU8(src); -#endif #if defined(USE_THROTTLE_BOOST) currentPidProfile->throttle_boost = sbufReadU8(src); #else diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 450aaca111..e2c74f8c57 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -397,10 +397,6 @@ extern uint8_t __config_end; #undef USE_DYN_IDLE #endif -#ifndef USE_ITERM_RELAX -#undef USE_ABSOLUTE_CONTROL -#endif - #if defined(USE_CUSTOM_DEFAULTS) #define USE_CUSTOM_DEFAULTS_ADDRESS #endif diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index aa1d8a8f4a..b92ad61c61 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -399,7 +399,6 @@ extern uint8_t _dmaram_end__; #define USE_TELEMETRY_MAVLINK #define USE_UNCOMMON_MIXERS #define USE_SIGNATURE -#define USE_ABSOLUTE_CONTROL #define USE_HOTT_TEXTMODE #define USE_LED_STRIP_STATUS_MODE #define USE_VARIO diff --git a/src/test/Makefile b/src/test/Makefile index e8e77ac86a..2ff98f952c 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -280,7 +280,7 @@ scheduler_unittest_SRC := \ $(USER_DIR)/common/streambuf.c scheduler_unittest_DEFINES := \ - USE_OSD= + USE_OSD= sensor_gyro_unittest_SRC := \ $(USER_DIR)/sensors/gyro.c \ @@ -390,7 +390,6 @@ pid_unittest_SRC := \ pid_unittest_DEFINES := \ USE_ITERM_RELAX= \ USE_RC_SMOOTHING_FILTER= \ - USE_ABSOLUTE_CONTROL= \ USE_LAUNCH_CONTROL= \ USE_FEEDFORWARD=