diff --git a/Makefile b/Makefile index 8ae93eb9c..a0540d557 100644 --- a/Makefile +++ b/Makefile @@ -437,8 +437,6 @@ COMMON_SRC = \ flight/imu.c \ flight/mixer.c \ flight/pid.c \ - flight/pid_legacy.c \ - flight/pid_betaflight.c \ io/beeper.c \ io/serial.c \ io/serial_4way.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index c0c4bd9ef..f039f7ec1 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1201,7 +1201,6 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("rates:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[ROLL], masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[PITCH], masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[YAW]); - BLACKBOX_PRINT_HEADER_LINE("pidController:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.pidController); BLACKBOX_PRINT_HEADER_LINE("rollPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[ROLL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[ROLL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[ROLL]); @@ -1235,7 +1234,6 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_notch_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_notch_cutoff); - BLACKBOX_PRINT_HEADER_LINE("deltaMethod:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.deltaMethod); BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate); BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate); BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index a7c3a7f7c..832ca622a 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -134,12 +134,6 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) static void resetPidProfile(pidProfile_t *pidProfile) { -#if defined(SKIP_PID_FLOAT) - pidProfile->pidController = PID_CONTROLLER_LEGACY; -#else - pidProfile->pidController = PID_CONTROLLER_BETAFLIGHT; -#endif - pidProfile->P8[ROLL] = 43; pidProfile->I8[ROLL] = 40; pidProfile->D8[ROLL] = 20; @@ -178,7 +172,6 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->dterm_notch_hz = 260; pidProfile->dterm_notch_cutoff = 160; - pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->vbatPidCompensation = 0; pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; @@ -761,7 +754,6 @@ void activateConfig(void) #ifdef TELEMETRY telemetryUseConfig(&masterConfig.telemetryConfig); #endif - pidSetController(currentProfile->pidProfile.pidController); #ifdef GPS gpsUseProfile(&masterConfig.gpsProfile); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e14841ed1..ce6e19fd6 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -848,7 +848,7 @@ static bool processOutCommand(uint8_t cmdMSP, mspPostProcessFnPtr *mspPostProces break; case MSP_PID_CONTROLLER: headSerialReply(1); - serialize8(currentProfile->pidProfile.pidController); + serialize8(PID_CONTROLLER_BETAFLIGHT); // Needs Cleanup in the future break; case MSP_MODE_RANGES: headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT); @@ -1286,7 +1286,7 @@ static bool processOutCommand(uint8_t cmdMSP, mspPostProcessFnPtr *mspPostProces serialize16(currentProfile->pidProfile.rollPitchItermIgnoreRate); serialize16(currentProfile->pidProfile.yawItermIgnoreRate); serialize16(currentProfile->pidProfile.yaw_p_limit); - serialize8(currentProfile->pidProfile.deltaMethod); + serialize8(0); // reserved serialize8(currentProfile->pidProfile.vbatPidCompensation); serialize8(currentProfile->pidProfile.setpointRelaxRatio); serialize8(currentProfile->pidProfile.dtermSetpointWeight); @@ -1395,10 +1395,6 @@ static bool processInCommand(uint8_t cmdMSP) read16(); break; case MSP_SET_PID_CONTROLLER: -#ifndef SKIP_PID_FLOAT - currentProfile->pidProfile.pidController = constrain(read8(), 0, 1); - pidSetController(currentProfile->pidProfile.pidController); -#endif break; case MSP_SET_PID: for (i = 0; i < PID_ITEM_COUNT; i++) { @@ -1892,7 +1888,7 @@ static bool processInCommand(uint8_t cmdMSP) currentProfile->pidProfile.rollPitchItermIgnoreRate = read16(); currentProfile->pidProfile.yawItermIgnoreRate = read16(); currentProfile->pidProfile.yaw_p_limit = read16(); - currentProfile->pidProfile.deltaMethod = read8(); + read8(); // reserved currentProfile->pidProfile.vbatPidCompensation = read8(); currentProfile->pidProfile.setpointRelaxRatio = read8(); currentProfile->pidProfile.dtermSetpointWeight = read8(); diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 9087592a2..583b74619 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -187,10 +187,7 @@ void calculateSetpointRate(int axis, int16_t rc) { debug[axis] = angleRate; } - if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY) - setpointRate[axis] = constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection - else - setpointRate[axis] = constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec) + setpointRate[axis] = constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec) } void scaleRcCommandToFpvCamAngle(void) { @@ -697,7 +694,7 @@ void subTaskPidController(void) { const uint32_t startTime = micros(); // PID - note this is function pointer set by setPIDController() - pid_controller( + pidController( ¤tProfile->pidProfile, masterConfig.max_angle_inclination, &masterConfig.accelerometerTrims, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 554218281..ecc164ca4 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -47,6 +47,9 @@ #include "fc/runtime_config.h" +extern float rcInput[3]; +extern float setpointRate[3]; + uint32_t targetPidLooptime; bool pidStabilisationEnabled; uint8_t PIDweight[3]; @@ -63,11 +66,7 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; int32_t errorGyroI[3]; float errorGyroIf[3]; -#ifdef SKIP_PID_FLOAT -pidControllerFuncPtr pid_controller = pidLegacy; // which pid controller are we using -#else -pidControllerFuncPtr pid_controller = pidBetaflight; // which pid controller are we using -#endif +pidControllerFuncPtr pid_controller; // which pid controller are we using void setTargetPidLooptime(uint32_t pidLooptime) { @@ -127,16 +126,192 @@ void initFilters(const pidProfile_t *pidProfile) { } } -void pidSetController(pidControllerType_e type) +// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. +// Based on 2DOF reference design (matlab) +void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, + const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) { - switch (type) { - default: - case PID_CONTROLLER_LEGACY: - pid_controller = pidLegacy; - break; -#ifndef SKIP_PID_FLOAT - case PID_CONTROLLER_BETAFLIGHT: - pid_controller = pidBetaflight; + float errorRate = 0, rD = 0, PVRate = 0, dynC; + float ITerm,PTerm,DTerm; + static float lastRateError[2]; + static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3], relaxFactor[3]; + float delta; + int axis; + float horizonLevelStrength = 1; + + float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float + + initFilters(pidProfile); + + if (FLIGHT_MODE(HORIZON_MODE)) { + // Figure out the raw stick positions + const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); + const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); + const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); + // Progressively turn off the horizon self level strength as the stick is banged over + horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection + if(pidProfile->D8[PIDLEVEL] == 0){ + horizonLevelStrength = 0; + } else { + horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); + } + } + + // Yet Highly experimental and under test and development + // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) + static float kiThrottleGain = 1.0f; + if (pidProfile->itermThrottleGain) { + const uint16_t maxLoopCount = 20000 / targetPidLooptime; + const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; + static int16_t previousThrottle; + static uint16_t loopIncrement; + + if (loopIncrement >= maxLoopCount) { + kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5 + previousThrottle = rcCommand[THROTTLE]; + loopIncrement = 0; + } else { + loopIncrement++; + } + } + + // ----------PID controller---------- + for (axis = 0; axis < 3; axis++) { + + static uint8_t configP[3], configI[3], configD[3]; + + // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now + // Prepare all parameters for PID controller + if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) { + Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; + Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; + Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; + c[axis] = pidProfile->dtermSetpointWeight / 100.0f; + relaxFactor[axis] = 1.0f - (pidProfile->setpointRelaxRatio / 100.0f); + yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT(); + rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT(); + + configP[axis] = pidProfile->P8[axis]; + configI[axis] = pidProfile->I8[axis]; + configD[axis] = pidProfile->D8[axis]; + } + + // Limit abrupt yaw inputs / stops + float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; + if (maxVelocity) { + float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; + if (ABS(currentVelocity) > maxVelocity) { + setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; + } + previousSetpoint[axis] = setpointRate[axis]; + } + + // Yaw control is GYRO based, direct sticks control is applied to rate PID + if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { + // calculate error angle and limit the angle to the max inclination +#ifdef GPS + const float errorAngle = (constrainf(pidProfile->levelSensitivity * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here +#else + const float errorAngle = (constrainf(pidProfile->levelSensitivity * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here +#endif + if (FLIGHT_MODE(ANGLE_MODE)) { + // ANGLE mode - control is angle based, so control loop is needed + setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; + } else { + // HORIZON mode - direct sticks control is applied to rate PID + // mix up angle error to desired AngleRate to add a little auto-level feel + setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); + } + } + + PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec + + // --------low-level gyro-based PID based on 2DOF PID controller. ---------- + // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- + // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes + // ----- calculate error / angle rates ---------- + errorRate = setpointRate[axis] - PVRate; // r - y + + // -----calculate P component and add Dynamic Part based on stick input + PTerm = Kp[axis] * errorRate * tpaFactor; + + // -----calculate I component. + // Reduce strong Iterm accumulation during higher stick inputs + float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f); + + // Handle All windup Scenarios + // limit maximum integrator value to prevent WindUp + float itermScaler = setpointRateScaler * kiThrottleGain; + + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); + + // I coefficient (I8) moved before integration to make limiting independent from PID settings + ITerm = errorGyroIf[axis]; + + //-----calculate D-term (Yaw D not yet supported) + if (axis != YAW) { + static float previousSetpoint[3]; + dynC = c[axis]; + if (pidProfile->setpointRelaxRatio < 100) { + dynC = c[axis]; + if (setpointRate[axis] > 0) { + if ((setpointRate[axis] - previousSetpoint[axis]) < previousSetpoint[axis]) + dynC = dynC * powerf(rcInput[axis], 2) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]); + } else if (setpointRate[axis] < 0) { + if ((setpointRate[axis] - previousSetpoint[axis]) > previousSetpoint[axis]) + dynC = dynC * powerf(rcInput[axis], 2) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]); + } + } + previousSetpoint[axis] = setpointRate[axis]; + rD = dynC * setpointRate[axis] - PVRate; // cr - y + delta = rD - lastRateError[axis]; + lastRateError[axis] = rD; + + // Divide delta by targetLooptime to get differential (ie dr/dt) + delta *= (1.0f / getdT()); + + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * tpaFactor; + + // Filter delta + if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); + + if (pidProfile->dterm_lpf_hz) { + if (pidProfile->dterm_filter_type == FILTER_BIQUAD) + delta = biquadFilterApply(&dtermFilterLpf[axis], delta); + else if (pidProfile->dterm_filter_type == FILTER_PT1) + delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); + else + delta = firFilterUpdate(&dtermDenoisingState[axis], delta); + } + + DTerm = Kd[axis] * delta * tpaFactor; + + // -----calculate total PID output + axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -pidProfile->pidSumLimit, pidProfile->pidSumLimit); + } else { + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); + + axisPID[axis] = lrintf(PTerm + ITerm); + + DTerm = 0.0f; // needed for blackbox + } + + // Disable PID control at zero throttle + if (!pidStabilisationEnabled) axisPID[axis] = 0; + +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; #endif } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index da2494e7d..49a8df956 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -19,16 +19,11 @@ #include -#define GYRO_I_MAX 256 // Gyro I limiter +#define PID_CONTROLLER_BETAFLIGHT 1 #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter -#define YAW_JUMP_PREVENTION_LIMIT_LOW 80 -#define YAW_JUMP_PREVENTION_LIMIT_HIGH 400 #define PIDSUM_LIMIT 700 -#define DYNAMIC_PTERM_STICK_THRESHOLD 400 - - // Scaling factors for Pids for better tunable range in configurator for betaflight pid controller. The scaling is based on legacy pid controller or previous float #define PTERM_SCALE 0.032029f #define ITERM_SCALE 0.244381f @@ -48,17 +43,6 @@ typedef enum { PID_ITEM_COUNT } pidIndex_e; -typedef enum { - PID_CONTROLLER_LEGACY = 0, // Legacy PID controller. Old INT / Rewrite with 2.9 status. Fastest performance....least math. Will stay same in the future - PID_CONTROLLER_BETAFLIGHT, // Betaflight PID controller. Old luxfloat -> float evolution. More math added and maintained in the future - PID_COUNT -} pidControllerType_e; - -typedef enum { - DELTA_FROM_ERROR = 0, - DELTA_FROM_MEASUREMENT -} pidDeltaType_e; - typedef enum { SUPEREXPO_YAW_OFF = 0, SUPEREXPO_YAW_ON, @@ -71,8 +55,6 @@ typedef enum { } pidStabilisationState_e; typedef struct pidProfile_s { - uint8_t pidController; // 1 = rewrite betaflight evolved from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Betaflight PIDc (Evolved Luxfloat) - uint8_t P8[PID_ITEM_COUNT]; uint8_t I8[PID_ITEM_COUNT]; uint8_t D8[PID_ITEM_COUNT]; @@ -82,7 +64,6 @@ typedef struct pidProfile_s { uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy uint16_t dterm_notch_hz; // Biquad dterm notch hz uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff - uint8_t deltaMethod; // Alternative delta Calculation uint16_t rollPitchItermIgnoreRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates uint16_t yaw_p_limit; @@ -114,9 +95,7 @@ struct rxConfig_s; typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); // pid controller function prototype -void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); -void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, +void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); extern int16_t axisPID[3]; @@ -127,7 +106,6 @@ extern uint32_t targetPidLooptime; // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction extern uint8_t PIDweight[3]; -void pidSetController(pidControllerType_e type); void pidResetErrorGyroState(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); void setTargetPidLooptime(uint32_t pidLooptime); diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c deleted file mode 100644 index 92b3fc3da..000000000 --- a/src/main/flight/pid_betaflight.c +++ /dev/null @@ -1,258 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include -#include - -#include - -#ifndef SKIP_PID_FLOAT - -#include "build/build_config.h" -#include "build/debug.h" - -#include "common/axis.h" -#include "common/maths.h" -#include "common/filter.h" - -#include "drivers/sensor.h" - -#include "drivers/accgyro.h" -#include "sensors/sensors.h" -#include "sensors/gyro.h" -#include "sensors/acceleration.h" - -#include "rx/rx.h" - -#include "io/gps.h" - -#include "fc/rc_controls.h" -#include "fc/runtime_config.h" - -#include "flight/pid.h" -#include "flight/imu.h" -#include "flight/navigation.h" -#include "flight/gtune.h" - -extern float rcInput[3]; -extern float setpointRate[3]; - -extern float errorGyroIf[3]; -extern bool pidStabilisationEnabled; - -extern pt1Filter_t deltaFilter[3]; -extern pt1Filter_t yawFilter; -extern biquadFilter_t dtermFilterLpf[3]; -extern biquadFilter_t dtermFilterNotch[3]; -extern bool dtermNotchInitialised; -extern firFilterState_t dtermDenoisingState[3]; - -void initFilters(const pidProfile_t *pidProfile); -float getdT(void); - -// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. -// Based on 2DOF reference design (matlab) -void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) -{ - float errorRate = 0, rD = 0, PVRate = 0, dynC; - float ITerm,PTerm,DTerm; - static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3], relaxFactor[3]; - float delta; - int axis; - float horizonLevelStrength = 1; - - float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float - - initFilters(pidProfile); - - if (FLIGHT_MODE(HORIZON_MODE)) { - // Figure out the raw stick positions - const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); - const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); - const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); - // Progressively turn off the horizon self level strength as the stick is banged over - horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection - if(pidProfile->D8[PIDLEVEL] == 0){ - horizonLevelStrength = 0; - } else { - horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); - } - } - - // Yet Highly experimental and under test and development - // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) - static float kiThrottleGain = 1.0f; - if (pidProfile->itermThrottleGain) { - const uint16_t maxLoopCount = 20000 / targetPidLooptime; - const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; - static int16_t previousThrottle; - static uint16_t loopIncrement; - - if (loopIncrement >= maxLoopCount) { - kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5 - previousThrottle = rcCommand[THROTTLE]; - loopIncrement = 0; - } else { - loopIncrement++; - } - } - - // ----------PID controller---------- - for (axis = 0; axis < 3; axis++) { - - static uint8_t configP[3], configI[3], configD[3]; - - // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now - // Prepare all parameters for PID controller - if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) { - Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; - Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; - Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; - c[axis] = pidProfile->dtermSetpointWeight / 100.0f; - relaxFactor[axis] = 1.0f - (pidProfile->setpointRelaxRatio / 100.0f); - yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT(); - rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT(); - - configP[axis] = pidProfile->P8[axis]; - configI[axis] = pidProfile->I8[axis]; - configD[axis] = pidProfile->D8[axis]; - } - - // Limit abrupt yaw inputs / stops - float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; - if (maxVelocity) { - float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; - if (ABS(currentVelocity) > maxVelocity) { - setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; - } - previousSetpoint[axis] = setpointRate[axis]; - } - - // Yaw control is GYRO based, direct sticks control is applied to rate PID - if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { - // calculate error angle and limit the angle to the max inclination -#ifdef GPS - const float errorAngle = (constrainf(pidProfile->levelSensitivity * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here -#else - const float errorAngle = (constrainf(pidProfile->levelSensitivity * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here -#endif - if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed - setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; - } else { - // HORIZON mode - direct sticks control is applied to rate PID - // mix up angle error to desired AngleRate to add a little auto-level feel - setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); - } - } - - PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec - - // --------low-level gyro-based PID based on 2DOF PID controller. ---------- - // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- - // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - // ----- calculate error / angle rates ---------- - errorRate = setpointRate[axis] - PVRate; // r - y - - // -----calculate P component and add Dynamic Part based on stick input - PTerm = Kp[axis] * errorRate * tpaFactor; - - // -----calculate I component. - // Reduce strong Iterm accumulation during higher stick inputs - float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f); - - // Handle All windup Scenarios - // limit maximum integrator value to prevent WindUp - float itermScaler = setpointRateScaler * kiThrottleGain; - - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); - - // I coefficient (I8) moved before integration to make limiting independent from PID settings - ITerm = errorGyroIf[axis]; - - //-----calculate D-term (Yaw D not yet supported) - if (axis != YAW) { - static float previousSetpoint[3]; - dynC = c[axis]; - if (pidProfile->setpointRelaxRatio < 100) { - dynC = c[axis]; - if (setpointRate[axis] > 0) { - if ((setpointRate[axis] - previousSetpoint[axis]) < previousSetpoint[axis]) - dynC = dynC * powerf(rcInput[axis], 2) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]); - } else if (setpointRate[axis] < 0) { - if ((setpointRate[axis] - previousSetpoint[axis]) > previousSetpoint[axis]) - dynC = dynC * powerf(rcInput[axis], 2) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]); - } - } - previousSetpoint[axis] = setpointRate[axis]; - rD = dynC * setpointRate[axis] - PVRate; // cr - y - delta = rD - lastRateError[axis]; - lastRateError[axis] = rD; - - // Divide delta by targetLooptime to get differential (ie dr/dt) - delta *= (1.0f / getdT()); - - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * tpaFactor; - - // Filter delta - if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); - - if (pidProfile->dterm_lpf_hz) { - if (pidProfile->dterm_filter_type == FILTER_BIQUAD) - delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - else if (pidProfile->dterm_filter_type == FILTER_PT1) - delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - else - delta = firFilterUpdate(&dtermDenoisingState[axis], delta); - } - - DTerm = Kd[axis] * delta * tpaFactor; - - // -----calculate total PID output - axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -pidProfile->pidSumLimit, pidProfile->pidSumLimit); - } else { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = lrintf(PTerm + ITerm); - - DTerm = 0.0f; // needed for blackbox - } - - // Disable PID control at zero throttle - if (!pidStabilisationEnabled) axisPID[axis] = 0; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; -#endif - } -} -#endif - diff --git a/src/main/flight/pid_legacy.c b/src/main/flight/pid_legacy.c deleted file mode 100644 index 338249ae9..000000000 --- a/src/main/flight/pid_legacy.c +++ /dev/null @@ -1,212 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include -#include - -#include - -#include "build/build_config.h" -#include "build/debug.h" - -#include "common/axis.h" -#include "common/maths.h" -#include "common/filter.h" - -#include "drivers/sensor.h" - -#include "drivers/accgyro.h" - -#include "sensors/sensors.h" -#include "sensors/gyro.h" -#include "sensors/acceleration.h" - -#include "rx/rx.h" - -#include "io/gps.h" - -#include "fc/runtime_config.h" -#include "fc/rc_controls.h" - -#include "flight/pid.h" -#include "flight/imu.h" -#include "flight/navigation.h" -#include "flight/gtune.h" - - -extern uint8_t motorCount; -extern uint8_t PIDweight[3]; -extern bool pidStabilisationEnabled; -extern float setpointRate[3]; -extern int32_t errorGyroI[3]; -extern pt1Filter_t deltaFilter[3]; -extern pt1Filter_t yawFilter; -extern biquadFilter_t dtermFilterLpf[3]; -extern firFilterState_t dtermDenoisingState[3]; - -void initFilters(const pidProfile_t *pidProfile); -float getdT(void); - - -// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. -// Don't expect much development in the future -void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) -{ - int axis; - int32_t PTerm, ITerm, DTerm, delta; - static int32_t lastRateError[3]; - int32_t AngleRateTmp = 0, RateError = 0, gyroRate = 0; - - int8_t horizonLevelStrength = 100; - - initFilters(pidProfile); - - if (FLIGHT_MODE(HORIZON_MODE)) { - // Figure out the raw stick positions - const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); - const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); - const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); - // Progressively turn off the horizon self level strength as the stick is banged over - horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection - // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine. - // For more rate mode increase D and slower flips and rolls will be possible - horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100); - } - - // ----------PID controller---------- - for (axis = 0; axis < 3; axis++) { - - // -----Get the desired angle rate depending on flight mode - AngleRateTmp = (int32_t)setpointRate[axis]; - - if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { - // calculate error angle and limit the angle to max configured inclination -#ifdef GPS - const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#else - const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#endif - if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed - AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4; - } else { - // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, - // horizonLevelStrength is scaled to the stick input - AngleRateTmp = AngleRateTmp + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4); - } - } - - // --------low-level gyro-based PID. ---------- - // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - // -----calculate scaled error.AngleRates - // multiplication of rcCommand corresponds to changing the sticks scaling here - gyroRate = gyroADC[axis] / 4; - - RateError = AngleRateTmp - gyroRate; - - // -----calculate P component - PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; - - // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply - if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { - PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); - } - - // -----calculate I component - // there should be no division before accumulating the error to integrator, because the precision would be reduced. - // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. - // Time correction (to avoid different I scaling for different builds based on average cycle time) - // is normalized to cycle time = 2048. - // Prevent Accumulation - uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); - uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; - - errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * dynamicKi; - - // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. - // I coefficient (I8) moved before integration to make limiting independent from PID settings - errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); - - ITerm = errorGyroI[axis] >> 13; - - //-----calculate D-term - if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = PTerm + ITerm; - - if (motorCount >= 4) { - int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); - - // prevent "yaw jump" during yaw correction - axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); - } - - DTerm = 0; // needed for blackbox - } else { - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = RateError - lastRateError[axis]; - lastRateError[axis] = RateError; - } else { - delta = -(gyroRate - lastRateError[axis]); - lastRateError[axis] = gyroRate; - } - - // Divide delta by targetLooptime to get differential (ie dr/dt) - delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; - - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; - - // Filter delta - if (pidProfile->dterm_lpf_hz) { - float deltaf = delta; // single conversion - if (pidProfile->dterm_filter_type == FILTER_BIQUAD) - delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - else if (pidProfile->dterm_filter_type == FILTER_PT1) - delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - else - delta = firFilterUpdate(&dtermDenoisingState[axis], delta); - - delta = lrintf(deltaf); - } - - DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; - - // -----calculate total PID output - axisPID[axis] = PTerm + ITerm + DTerm; - } - - if (!pidStabilisationEnabled) axisPID[axis] = 0; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; -#endif - } -} - diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 6689fcaf6..a1c205e5a 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -413,10 +413,6 @@ static const char * const lookupTableBlackboxDevice[] = { }; #endif -static const char * const lookupTablePidController[] = { - "LEGACY", "BETAFLIGHT" -}; - #ifdef SERIAL_RX static const char * const lookupTableSerialRX[] = { "SPEK1024", @@ -523,10 +519,6 @@ static const char * const lookupTablePwmProtocol[] = { "OFF", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED" }; -static const char * const lookupTableDeltaMethod[] = { - "ERROR", "MEASUREMENT" -}; - static const char * const lookupTableRcInterpolation[] = { "OFF", "PRESET", "AUTO", "MANUAL" }; @@ -559,7 +551,6 @@ typedef enum { #ifdef USE_SERVOS TABLE_GIMBAL_MODE, #endif - TABLE_PID_CONTROLLER, #ifdef SERIAL_RX TABLE_SERIAL_RX, #endif @@ -577,7 +568,6 @@ typedef enum { TABLE_DEBUG, TABLE_SUPEREXPO_YAW, TABLE_MOTOR_PWM_PROTOCOL, - TABLE_DELTA_METHOD, TABLE_RC_INTERPOLATION, TABLE_LOWPASS_TYPE, TABLE_FAILSAFE, @@ -601,7 +591,6 @@ static const lookupTableEntry_t lookupTables[] = { #ifdef USE_SERVOS { lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) }, #endif - { lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) }, #ifdef SERIAL_RX { lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) }, #endif @@ -619,7 +608,6 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) }, { lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) }, { lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) }, - { lookupTableDeltaMethod, sizeof(lookupTableDeltaMethod) / sizeof(char *) }, { lookupTableRcInterpolation, sizeof(lookupTableRcInterpolation) / sizeof(char *) }, { lookupTableLowpassType, sizeof(lookupTableLowpassType) / sizeof(char *) }, { lookupTableFailsafe, sizeof(lookupTableFailsafe) / sizeof(char *) }, @@ -795,7 +783,6 @@ const clivalue_t valueTable[] = { { "align_board_yaw", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.yawDegrees, .config.minmax = { -180, 360 } }, { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } }, - { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, @@ -890,10 +877,6 @@ const clivalue_t valueTable[] = { { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, -#ifndef SKIP_PID_FLOAT - { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } }, -#endif - { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], .config.minmax = { 0, 200 } }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], .config.minmax = { 0, 200 } }, { "d_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PITCH], .config.minmax = { 0, 200 } }, diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index f19b3e499..b6ef71a96 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -118,7 +118,6 @@ #ifdef CC3D_OPBL #define SKIP_CLI_COMMAND_HELP -#define SKIP_PID_FLOAT #undef BARO #undef SONAR #undef LED_STRIP diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index a9cd9ff46..07ba2dd9a 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -1042,8 +1042,6 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) cycleTime = bstRead16(); break; case BST_SET_PID_CONTROLLER: - currentProfile->pidProfile.pidController = bstRead8(); - pidSetController(currentProfile->pidProfile.pidController); break; case BST_SET_PID: for (i = 0; i < PID_ITEM_COUNT; i++) {