1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00

Merge pull request #1012 from borisbstyle/pidCleanup

PID controllers cleanup (less pid controllers)
This commit is contained in:
Dominic Clifton 2015-11-13 23:30:40 +00:00
commit fb85e7f08a
6 changed files with 76 additions and 429 deletions

View file

@ -220,7 +220,7 @@ Re-apply any new defaults as desired.
| `baro_cf_alt` | | 0 | 1 | 0.965 | Profile | FLOAT | | `baro_cf_alt` | | 0 | 1 | 0.965 | Profile | FLOAT |
| `mag_hardware` | 0 = Default, use whatever mag hardware is defined for your board type ; 1 = None, disable mag ; 2 = HMC5883 ; 3 = AK8975 (for versions <= 1.7.1: 1 = HMC5883 ; 2 = AK8975 ; 3 = None, disable mag) | 0 | 3 | 0 | Master | UINT8 | | `mag_hardware` | 0 = Default, use whatever mag hardware is defined for your board type ; 1 = None, disable mag ; 2 = HMC5883 ; 3 = AK8975 (for versions <= 1.7.1: 1 = HMC5883 ; 2 = AK8975 ; 3 = None, disable mag) | 0 | 3 | 0 | Master | UINT8 |
| `mag_declination` | Current location magnetic declination in format. For example, -6deg 37min, = for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ | -18000 | 18000 | 0 | Profile | INT16 | | `mag_declination` | Current location magnetic declination in format. For example, -6deg 37min, = for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ | -18000 | 18000 | 0 | Profile | INT16 |
| `pid_controller` | | 0 | 5 | 0 | Profile | UINT8 | | `pid_controller` | | 1 | 3 | 1 | Profile | UINT8 |
| `p_pitch` | | 0 | 200 | 40 | Profile | UINT8 | | `p_pitch` | | 0 | 200 | 40 | Profile | UINT8 |
| `i_pitch` | | 0 | 200 | 30 | Profile | UINT8 | | `i_pitch` | | 0 | 200 | 30 | Profile | UINT8 |
| `d_pitch` | | 0 | 200 | 23 | Profile | UINT8 | | `d_pitch` | | 0 | 200 | 23 | Profile | UINT8 |

View file

@ -61,22 +61,21 @@ If you are getting oscillations starting at say 3/4 throttle, set tpa breakpoint
## PID controllers ## PID controllers
Cleanflight has 6 built-in PID controllers which each have different flight behavior. Each controller requires Cleanflight has 4 built-in PID controllers which each have different flight behavior. Each controller requires
different PID settings for best performance, so if you tune your craft using one PID controller, those settings will different PID settings for best performance, so if you tune your craft using one PID controller, those settings will
likely not work well on any of the other controllers. likely not work well on any of the other controllers.
You can change between PID controllers by running `set pid_controller=n` on the CLI tab of the Cleanflight You can change between PID controllers by running `set pid_controller=n` on the CLI tab of the Cleanflight
Configurator, where `n` is the number of the controller you want to use. Please read these notes first before trying one Configurator, where `n` is the number of the controller you want to use. Please read these notes first before trying one
out. out.
Note that older Cleanflight versions had 6 pid controllers and that with some versions you may have different pid controller indexes.
### PID controller 0, "MultiWii" (default) ### PID controller 0, "MultiWii23" (default)
PID Controller 0 is the default controller in Cleanflight, and Cleanflight's default PID settings are tuned to be PID Controller 0 is the default controller in Cleanflight, and Cleanflight's default PID settings are tuned to be
middle-of-the-road settings for this controller. It originates from the old MultiWii PID controller from MultiWii 2.2 middle-of-the-road settings for this controller. PID Controller 3 is an direct port of the PID controller from MultiWii 2.3 and later.
and earlier.
One of the quirks with this controller is that if you increase the P value for an axis, the maximum rotation rates for The algorithm is handling roll and pitch differently to yaw. Users with problems on yaw authority should try this one.
that axis are lowered. Hence you need to crank up the pitch or roll rates if you have higher and higher P values.
In Horizon and Angle modes, this controller uses both the LEVEL "P" and "I" settings in order to tune the In Horizon and Angle modes, this controller uses both the LEVEL "P" and "I" settings in order to tune the
auto-leveling corrections in a similar way to the way that P and I settings are applied to roll and yaw axes in the acro auto-leveling corrections in a similar way to the way that P and I settings are applied to roll and yaw axes in the acro
@ -130,23 +129,7 @@ stick, 50% self-leveling will be applied at 50% stick, and no self-leveling will
sensitivity is decreased to 75, 100% self-leveling will be applied at center stick, 50% will be applied at 63% sensitivity is decreased to 75, 100% self-leveling will be applied at center stick, 50% will be applied at 63%
stick, and no self-leveling will be applied at 75% stick and onwards. stick, and no self-leveling will be applied at 75% stick and onwards.
### PID controller 3, "MultiWii23" (default for the ALIENWIIF1 and ALIENWIIF3 targets) ### PID controller 3, "Harakiri"
PID Controller 3 is an direct port of the PID controller from MultiWii 2.3 and later.
The algorithm is handling roll and pitch differently to yaw. Users with problems on yaw authority should try this one.
For the ALIENWII32 targets the gyroscale is removed for even more yaw authority. This will provide best performance on very small multicopters with brushed motors.
### PID controller 4, "MultiWiiHybrid"
PID Controller 4 is an hybrid version of two MultiWii PID controllers. Roll and pitch is using the MultiWii 2.2 algorithm and yaw is using the 2.3 algorithm.
This PID controller was initialy implemented for testing purposes but is also performing quite well.
For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This will provide best performance on very small multicopters with brushed motors.
### PID controller 5, "Harakiri"
PID Controller 5 is an port of the PID controller from the Harakiri firmware. PID Controller 5 is an port of the PID controller from the Harakiri firmware.
@ -167,14 +150,14 @@ Yaw authority is also quite good.
An overall multiplier on the RC stick inputs for pitch, rol;, and yaw. An overall multiplier on the RC stick inputs for pitch, rol;, and yaw.
On PID Controllers 0, and 3-5 can be used to set the "feel" around center stick for small control movements. (RC Expo also affects this).For PID Controllers 1 and 2, this basically sets the baseline stick sensitivity On PID Controllers 0, and 3 can be used to set the "feel" around center stick for small control movements. (RC Expo also affects this).For PID Controllers 1 and 2, this basically sets the baseline stick sensitivity
### Pitch and Roll rates ### Pitch and Roll rates
In PID Controllers 0 and 3-5, the affect of the PID error terms for P and D are gradually lessened as the control sticks are moved away from center, ie 0.3 rate gives a 30% reduction of those terms at full throw, effectively making the stabilizing effect of the PID controller less at stick extremes. This results in faster rotation rates. So for these controllers, you can set center stick sensitivity to control movement with RC rate above, and yet have much faster rotation rates at stick extremes. In PID Controllers 0 and 3, the affect of the PID error terms for P and D are gradually lessened as the control sticks are moved away from center, ie 0.3 rate gives a 30% reduction of those terms at full throw, effectively making the stabilizing effect of the PID controller less at stick extremes. This results in faster rotation rates. So for these controllers, you can set center stick sensitivity to control movement with RC rate above, and yet have much faster rotation rates at stick extremes.
For PID Controllers 1 and 2, this is an multiplier on overall stick sensitivity, like RC rate, but for roll and pitch independently. Stablility (to outside factors like turbulence) is not reduced at stick extremes. A zero value is no increase in stick sensitivity over that set by RC rate above. Higher values increases stick sensitivity across the entire stick movement range. For PID Controllers 1 and 2, this is an multiplier on overall stick sensitivity, like RC rate, but for roll and pitch independently. Stablility (to outside factors like turbulence) is not reduced at stick extremes. A zero value is no increase in stick sensitivity over that set by RC rate above. Higher values increases stick sensitivity across the entire stick movement range.
### Yaw Rate ### Yaw Rate
In PID Controllers 0 and 5, it acts as a PID reduction as explained above. In PID Controllers 1-4, it acts as a stick sensitivity multiplier, as explained above. In PID Controller 5, it acts as a PID reduction as explained above. In PID Controllers 1-2, it acts as a stick sensitivity multiplier, as explained above.

View file

@ -143,7 +143,7 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
static void resetPidProfile(pidProfile_t *pidProfile) static void resetPidProfile(pidProfile_t *pidProfile)
{ {
pidProfile->pidController = 0; pidProfile->pidController = 1;
pidProfile->P8[ROLL] = 40; pidProfile->P8[ROLL] = 40;
pidProfile->I8[ROLL] = 30; pidProfile->I8[ROLL] = 30;
@ -174,7 +174,6 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->I8[PIDVEL] = 45; pidProfile->I8[PIDVEL] = 45;
pidProfile->D8[PIDVEL] = 1; pidProfile->D8[PIDVEL] = 1;
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->dterm_cut_hz = 0; pidProfile->dterm_cut_hz = 0;
pidProfile->pterm_cut_hz = 0; pidProfile->pterm_cut_hz = 0;
pidProfile->gyro_cut_hz = 0; pidProfile->gyro_cut_hz = 0;
@ -192,8 +191,6 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->H_level = 3.0f; pidProfile->H_level = 3.0f;
pidProfile->H_sensitivity = 75; pidProfile->H_sensitivity = 75;
pidProfile->pid5_oldyw = 0;
#ifdef GTUNE #ifdef GTUNE
pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune. pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune.
pidProfile->gtune_lolimP[PITCH] = 10; // [0..200] Lower limit of PITCH P during G tune. pidProfile->gtune_lolimP[PITCH] = 10; // [0..200] Lower limit of PITCH P during G tune.

View file

@ -64,13 +64,13 @@ static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
static int32_t errorAngleI[2] = { 0, 0 }; static int32_t errorAngleI[2] = { 0, 0 };
static float errorAngleIf[2] = { 0.0f, 0.0f }; static float errorAngleIf[2] = { 0.0f, 0.0f };
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype
pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii pidControllerFuncPtr pid_controller = pidRewrite; // which pid controller are we using, defaultMultiWii
void pidResetErrorAngle(void) void pidResetErrorAngle(void)
{ {
@ -223,101 +223,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
} }
} }
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
UNUSED(rxConfig);
int axis, prop;
int32_t error, errorAngle;
int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
static int16_t lastGyro[3] = { 0, 0, 0 };
static int32_t delta1[3], delta2[3];
int32_t deltaSum;
int32_t delta;
UNUSED(controlRateConfig);
// **** PITCH & ROLL & YAW PID ****
prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 500); // range [0;500]
for (axis = 0; axis < 3; axis++) {
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
// observe max inclination
#ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
#else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
#endif
PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
ITermACC = (errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12;
}
if (!FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || axis == FD_YAW) { // MODE relying on GYRO or YAW axis
error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
error -= gyroADC[axis] / 4;
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if ((ABS(gyroADC[axis]) > (640 * 4)) || (axis == FD_YAW && ABS(rcCommand[axis]) > 100))
errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64;
}
if (FLIGHT_MODE(HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) {
PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500;
} else {
if (FLIGHT_MODE(ANGLE_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) {
PTerm = PTermACC;
ITerm = ITermACC;
} else {
PTerm = PTermGYRO;
ITerm = ITermGYRO;
}
}
PTerm -= ((int32_t)gyroADC[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
delta = (gyroADC[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroADC[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
}
DTerm = (deltaSum * dynD8[axis]) / 32;
axisPID[axis] = PTerm + ITerm - DTerm;
#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
}
}
static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{ {
@ -443,278 +348,6 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
#endif #endif
} }
static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
UNUSED(rxConfig);
int axis, prop;
int32_t rc, error, errorAngle;
int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
static int16_t lastGyro[2] = { 0, 0 };
static int32_t delta1[2] = { 0, 0 }, delta2[2] = { 0, 0 };
int32_t deltaSum;
int32_t delta;
UNUSED(controlRateConfig);
// **** PITCH & ROLL ****
prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 500); // range [0;500]
for (axis = 0; axis < 2; axis++) {
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { // MODE relying on ACC
// observe max inclination
#ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
#else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
#endif
PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
ITermACC = (errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12;
}
if (!FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { // MODE relying on GYRO
error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
error -= gyroADC[axis] / 4;
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if (ABS(gyroADC[axis]) > (640 * 4))
errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64;
}
if (FLIGHT_MODE(HORIZON_MODE)) {
PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500;
} else {
if (FLIGHT_MODE(ANGLE_MODE)) {
PTerm = PTermACC;
ITerm = ITermACC;
} else {
PTerm = PTermGYRO;
ITerm = ITermGYRO;
}
}
PTerm -= ((int32_t)gyroADC[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
delta = (gyroADC[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroADC[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
}
DTerm = (deltaSum * dynD8[axis]) / 32;
axisPID[axis] = PTerm + ITerm - DTerm;
#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
}
//YAW
rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->rates[FD_YAW] + 30) >> 5;
#ifdef ALIENWII32
error = rc - gyroADC[FD_YAW];
#else
error = rc - (gyroADC[FD_YAW] / 4);
#endif
errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW];
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28));
if (ABS(rc) > 50) errorGyroI[FD_YAW] = 0;
PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6;
// Constrain YAW by D value if not servo driven in that case servolimits apply
if(motorCount >= 4 && pidProfile->yaw_p_limit < YAW_P_LIMIT_MAX) {
PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
}
ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX);
axisPID[FD_YAW] = PTerm + ITerm;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(FD_YAW);
}
#endif
#ifdef BLACKBOX
axisPID_P[FD_YAW] = PTerm;
axisPID_I[FD_YAW] = ITerm;
axisPID_D[FD_YAW] = 0;
#endif
}
static void pidHarakiri(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
UNUSED(rxConfig);
float delta, RCfactor, rcCommandAxis, MainDptCut, gyroADCQuant;
float PTerm, ITerm, DTerm, PTermACC = 0.0f, ITermACC = 0.0f, ITermGYRO, error, prop = 0.0f;
static float lastGyro[2] = { 0.0f, 0.0f }, lastDTerm[2] = { 0.0f, 0.0f };
uint8_t axis;
float ACCDeltaTimeINS, FLOATcycleTime, Mwii3msTimescale;
if (pidProfile->dterm_cut_hz) {
MainDptCut = RCconstPI / constrain(pidProfile->dterm_cut_hz, 1, 50); // dterm_cut_hz (default 0, Range 1-50Hz)
} else {
MainDptCut = RCconstPI / 12.0f; // default is 12Hz to maintain initial behavior of PID5
}
FLOATcycleTime = (float)constrain(cycleTime, 1, 100000); // 1us - 100ms
ACCDeltaTimeINS = FLOATcycleTime * 0.000001f; // ACCDeltaTimeINS is in seconds now
RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS); // used for pt1 element
if (FLIGHT_MODE(HORIZON_MODE)) {
prop = (float)MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 450) / 450.0f;
}
for (axis = 0; axis < 2; axis++) {
int32_t tmp = (int32_t)((float)gyroADC[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea
gyroADCQuant = (float)tmp * 3.2f; // but delivers more accuracy and also reduces jittery flight
rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
#ifdef GPS
error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
#else
error = constrain(2.0f * rcCommandAxis, -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
#endif
PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f;
float limitf = (float)pidProfile->D8[PIDLEVEL] * 5.0f;
PTermACC = constrain(PTermACC, -limitf, +limitf);
errorAngleIf[axis] = constrainf(errorAngleIf[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f);
ITermACC = errorAngleIf[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f;
}
if (!FLIGHT_MODE(ANGLE_MODE)) {
if (ABS((int16_t)gyroADC[axis]) > 2560) {
errorGyroIf[axis] = 0.0f;
} else {
error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroADCQuant;
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f);
}
ITermGYRO = errorGyroIf[axis] * (float)pidProfile->I8[axis] * 0.01f;
if (FLIGHT_MODE(HORIZON_MODE)) {
PTerm = PTermACC + prop * (rcCommandAxis - PTermACC);
ITerm = ITermACC + prop * (ITermGYRO - ITermACC);
} else {
PTerm = rcCommandAxis;
ITerm = ITermGYRO;
}
} else {
PTerm = PTermACC;
ITerm = ITermACC;
}
PTerm -= gyroADCQuant * dynP8[axis] * 0.003f;
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
delta = (gyroADCQuant - lastGyro[axis]) / ACCDeltaTimeINS;
lastGyro[axis] = gyroADCQuant;
lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]);
DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f;
axisPID[axis] = lrintf(PTerm + ITerm - DTerm); // Round up result.
#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
}
Mwii3msTimescale = (int32_t)FLOATcycleTime & (int32_t)~3; // Filter last 2 bit jitter
Mwii3msTimescale /= 3000.0f;
if (pidProfile->pid5_oldyw) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw
PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->rates[FD_YAW] * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100;
int32_t tmp = lrintf(gyroADC[FD_YAW] * 0.25f);
PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80;
if ((ABS(tmp) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) {
errorGyroI[FD_YAW] = 0;
} else {
error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp;
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * Mwii3msTimescale), -16000, +16000); // WindUp
ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6;
}
} else {
int32_t tmp = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->rates[FD_YAW] << 1) + 40)) >> 5;
error = tmp - lrintf(gyroADC[FD_YAW] * 0.25f); // Less Gyrojitter works actually better
if (ABS(tmp) > 50) {
errorGyroI[FD_YAW] = 0;
} else {
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * Mwii3msTimescale), -268435454, +268435454);
}
ITerm = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX);
PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6;
if(motorCount >= 4 && pidProfile->yaw_p_limit < YAW_P_LIMIT_MAX) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply
PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
}
}
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
axisPID[FD_YAW] = PTerm + ITerm;
axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]); // Round up result.
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(FD_YAW);
}
#endif
#ifdef BLACKBOX
axisPID_P[FD_YAW] = PTerm;
axisPID_I[FD_YAW] = ITerm;
axisPID_D[FD_YAW] = 0;
#endif
}
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{ {
@ -845,24 +478,15 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
void pidSetController(pidControllerType_e type) void pidSetController(pidControllerType_e type)
{ {
switch (type) { switch (type) {
case PID_CONTROLLER_MULTI_WII:
default: default:
pid_controller = pidMultiWii;
break;
case PID_CONTROLLER_REWRITE: case PID_CONTROLLER_REWRITE:
pid_controller = pidRewrite; pid_controller = pidRewrite;
break; break;
case PID_CONTROLLER_LUX_FLOAT: case PID_CONTROLLER_LUX_FLOAT:
pid_controller = pidLuxFloat; pid_controller = pidLuxFloat;
break; break;
case PID_CONTROLLER_MULTI_WII_23: case PID_CONTROLLER_MULTIWII23:
pid_controller = pidMultiWii23; pid_controller = pidMultiWii23;
break;
case PID_CONTROLLER_MULTI_WII_HYBRID:
pid_controller = pidMultiWiiHybrid;
break;
case PID_CONTROLLER_HARAKIRI:
pid_controller = pidHarakiri;
} }
} }

View file

@ -18,8 +18,6 @@
#pragma once #pragma once
#define GYRO_I_MAX 256 // Gyro I limiter #define GYRO_I_MAX 256 // Gyro I limiter
#define RCconstPI 0.159154943092f // 0.5f / M_PI;
#define OLD_YAW 0 // [0/1] 0 = MultiWii 2.3 yaw, 1 = older yaw.
#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter #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_P_LIMIT_MAX 500 // Maximum value for yaw P limiter
@ -38,18 +36,16 @@ typedef enum {
} pidIndex_e; } pidIndex_e;
typedef enum { typedef enum {
PID_CONTROLLER_MULTI_WII, PID_CONTROLLER_REWRITE = 1,
PID_CONTROLLER_REWRITE,
PID_CONTROLLER_LUX_FLOAT, PID_CONTROLLER_LUX_FLOAT,
PID_CONTROLLER_MULTI_WII_23, PID_CONTROLLER_MULTIWII23,
PID_CONTROLLER_MULTI_WII_HYBRID, PID_COUNT
PID_CONTROLLER_HARAKIRI,
} pidControllerType_e; } pidControllerType_e;
#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2) #define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
typedef struct pidProfile_s { typedef struct pidProfile_s {
uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 1, 2 = Luggi09s new baseflight pid uint8_t pidController; // 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Luggi09s new baseflight pid
uint8_t P8[PID_ITEM_COUNT]; uint8_t P8[PID_ITEM_COUNT];
uint8_t I8[PID_ITEM_COUNT]; uint8_t I8[PID_ITEM_COUNT];
@ -67,8 +63,6 @@ typedef struct pidProfile_s {
uint8_t pterm_cut_hz; // Used for fitlering Pterm noise on noisy frames uint8_t pterm_cut_hz; // Used for fitlering Pterm noise on noisy frames
uint8_t gyro_cut_hz; // Used for soft gyro filtering uint8_t gyro_cut_hz; // Used for soft gyro filtering
uint8_t pid5_oldyw; // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw
#ifdef GTUNE #ifdef GTUNE
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune
uint8_t gtune_hilimP[3]; // [0..200] Higher limit of P during G tune. 0 Disables tuning for that axis. uint8_t gtune_hilimP[3]; // [0..200] Higher limit of P during G tune. 0 Disables tuning for that axis.

View file

@ -186,6 +186,11 @@ static const rxFailsafeChannelMode_e rxFailsafeModesTable[RX_FAILSAFE_TYPE_COUNT
{ RX_FAILSAFE_MODE_INVALID, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_SET } { RX_FAILSAFE_MODE_INVALID, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_SET }
}; };
// sync this with pidControllerType_e
static const char * const pidControllers[] = {
"REWRITE", "LUXFLOAT", "MULTIWII23", NULL
};
#ifndef CJMCU #ifndef CJMCU
// sync this with sensors_e // sync this with sensors_e
static const char * const sensorTypeNames[] = { static const char * const sensorTypeNames[] = {
@ -479,7 +484,7 @@ const clivalue_t valueTable[] = {
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, 0, MAG_MAX }, { "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, 0, MAG_MAX },
{ "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 }, { "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 },
{ "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidController, 0, 5 }, { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidController, 1, 3 },
{ "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 },
{ "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 },
@ -517,13 +522,10 @@ const clivalue_t valueTable[] = {
{ "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], 0, 200 }, { "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], 0, 200 },
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 }, { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 },
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX },
{ "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, 0, 200 }, { "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, 0, 200 },
{ "pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pterm_cut_hz, 0, 200 }, { "pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pterm_cut_hz, 0, 200 },
{ "gyro_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_cut_hz, 0, 200 }, { "gyro_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_cut_hz, 0, 200 },
{ "pid5_oldyw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pid5_oldyw, 0, 1 },
#ifdef GTUNE #ifdef GTUNE
{ "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], 10, 200 }, { "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], 10, 200 },
{ "gtune_loP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_PITCH], 10, 200 }, { "gtune_loP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_PITCH], 10, 200 },
@ -1411,8 +1413,14 @@ static void dumpValues(uint16_t mask)
continue; continue;
} }
printf("set %s = ", valueTable[i].name); if (strstr(valueTable[i].name, "pid_controller")) {
cliPrintVar(value, 0); void *pidPtr= value->ptr;
pidPtr= ((uint8_t *)pidPtr) + (sizeof(profile_t) * masterConfig.current_profile_index);
cliPrint(pidControllers[*(uint8_t *)pidPtr-1]);
} else {
cliPrintVar(value, 0);
}
cliPrint("\r\n"); cliPrint("\r\n");
} }
} }
@ -2052,9 +2060,42 @@ static void cliSet(char *cmdline)
valuef = fastA2F(eqptr); valuef = fastA2F(eqptr);
for (i = 0; i < VALUE_COUNT; i++) { for (i = 0; i < VALUE_COUNT; i++) {
val = &valueTable[i]; val = &valueTable[i];
// ensure exact match when setting to prevent setting variables with shorter names
if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) { if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) {
if (valuef >= valueTable[i].min && valuef <= valueTable[i].max) { // here we compare the float value since... it should work, RIGHT? // ensure exact match when setting to prevent setting variables with shorter names
if (strstr(valueTable[i].name, "pid_controller")) {
int_float_value_t tmp;
tmp.int_value = 0;
bool pidControllerSet = false;
if (value) {
if (value >= valueTable[i].min && value <= valueTable[i].max) {
tmp.int_value = value;
cliSetVar(val, tmp);
printf("%s set to %s \r\n", valueTable[i].name, pidControllers[value - 1]);
pidControllerSet = true;
}
} else {
for (int pid = 0; pid < (PID_COUNT - 1); pid++) {
if (strstr(eqptr, pidControllers[pid])) {
tmp.int_value = pid + 1;
cliSetVar(val, tmp);
printf("%s set to %s \r\n", valueTable[i].name, pidControllers[pid]);
pidControllerSet = true;
}
}
}
if (!pidControllerSet) {
printf("Invalid Value! (Available PID Controllers: ");
for (int pid = 0; pid < (PID_COUNT - 1); pid++) {
printf(pidControllers[pid]);
printf(" ");
}
printf(")\r\n");
}
return;
} else if (valuef >= valueTable[i].min && valuef <= valueTable[i].max) { // here we compare the float value since... it should work, RIGHT?
int_float_value_t tmp; int_float_value_t tmp;
if (valueTable[i].type & VAR_FLOAT) if (valueTable[i].type & VAR_FLOAT)
tmp.float_value = valuef; tmp.float_value = valuef;
@ -2086,7 +2127,15 @@ static void cliGet(char *cmdline)
if (strstr(valueTable[i].name, cmdline)) { if (strstr(valueTable[i].name, cmdline)) {
val = &valueTable[i]; val = &valueTable[i];
printf("%s = ", valueTable[i].name); printf("%s = ", valueTable[i].name);
cliPrintVar(val, 0);
if (strstr(valueTable[i].name, "pid_controller")) {
void *pidPtr= val->ptr;
pidPtr= ((uint8_t *)pidPtr) + (sizeof(profile_t) * masterConfig.current_profile_index);
cliPrint(pidControllers[*(uint8_t *)pidPtr-1]);
} else {
cliPrintVar(val, 0);
}
cliPrint("\r\n"); cliPrint("\r\n");
matchedCommands++; matchedCommands++;