mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 16:25:26 +03:00
PID Controller Cleanup
Documentation Draft add cli names Cli name adaption cli set numeric and string correct names change Only PID1 and PID2 Add PID3
This commit is contained in:
parent
83109dbf17
commit
e347e7d4bf
6 changed files with 76 additions and 429 deletions
|
@ -219,7 +219,7 @@ Re-apply any new defaults as desired.
|
|||
| `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_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 |
|
||||
| `i_pitch` | | 0 | 200 | 30 | Profile | UINT8 |
|
||||
| `d_pitch` | | 0 | 200 | 23 | Profile | UINT8 |
|
||||
|
|
|
@ -61,22 +61,21 @@ If you are getting oscillations starting at say 3/4 throttle, set tpa breakpoint
|
|||
|
||||
## 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
|
||||
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
|
||||
Configurator, where `n` is the number of the controller you want to use. Please read these notes first before trying one
|
||||
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
|
||||
middle-of-the-road settings for this controller. It originates from the old MultiWii PID controller from MultiWii 2.2
|
||||
and earlier.
|
||||
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.
|
||||
|
||||
One of the quirks with this controller is that if you increase the P value for an axis, the maximum rotation rates for
|
||||
that axis are lowered. Hence you need to crank up the pitch or roll rates if you have higher and higher P values.
|
||||
The algorithm is handling roll and pitch differently to yaw. Users with problems on yaw authority should try this one.
|
||||
|
||||
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
|
||||
|
@ -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%
|
||||
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 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 3, "Harakiri"
|
||||
|
||||
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.
|
||||
|
||||
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
|
||||
|
||||
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.
|
||||
|
||||
### 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.
|
||||
|
|
|
@ -143,7 +143,7 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
|
|||
|
||||
static void resetPidProfile(pidProfile_t *pidProfile)
|
||||
{
|
||||
pidProfile->pidController = 0;
|
||||
pidProfile->pidController = 1;
|
||||
|
||||
pidProfile->P8[ROLL] = 40;
|
||||
pidProfile->I8[ROLL] = 30;
|
||||
|
@ -174,7 +174,6 @@ static void resetPidProfile(pidProfile_t *pidProfile)
|
|||
pidProfile->I8[PIDVEL] = 45;
|
||||
pidProfile->D8[PIDVEL] = 1;
|
||||
|
||||
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
|
||||
pidProfile->dterm_cut_hz = 0;
|
||||
pidProfile->pterm_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_sensitivity = 75;
|
||||
|
||||
pidProfile->pid5_oldyw = 0;
|
||||
|
||||
#ifdef GTUNE
|
||||
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.
|
||||
|
|
|
@ -64,13 +64,13 @@ static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
|
|||
static int32_t errorAngleI[2] = { 0, 0 };
|
||||
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);
|
||||
|
||||
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||
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)
|
||||
{
|
||||
|
@ -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,
|
||||
rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
|
||||
{
|
||||
|
@ -443,278 +348,6 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
|
|||
#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,
|
||||
rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
|
||||
{
|
||||
|
@ -845,24 +478,15 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
|
|||
void pidSetController(pidControllerType_e type)
|
||||
{
|
||||
switch (type) {
|
||||
case PID_CONTROLLER_MULTI_WII:
|
||||
default:
|
||||
pid_controller = pidMultiWii;
|
||||
break;
|
||||
case PID_CONTROLLER_REWRITE:
|
||||
pid_controller = pidRewrite;
|
||||
break;
|
||||
case PID_CONTROLLER_LUX_FLOAT:
|
||||
pid_controller = pidLuxFloat;
|
||||
break;
|
||||
case PID_CONTROLLER_MULTI_WII_23:
|
||||
case PID_CONTROLLER_MULTIWII23:
|
||||
pid_controller = pidMultiWii23;
|
||||
break;
|
||||
case PID_CONTROLLER_MULTI_WII_HYBRID:
|
||||
pid_controller = pidMultiWiiHybrid;
|
||||
break;
|
||||
case PID_CONTROLLER_HARAKIRI:
|
||||
pid_controller = pidHarakiri;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -18,8 +18,6 @@
|
|||
#pragma once
|
||||
|
||||
#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_MAX 500 // Maximum value for yaw P limiter
|
||||
|
||||
|
@ -38,18 +36,16 @@ typedef enum {
|
|||
} pidIndex_e;
|
||||
|
||||
typedef enum {
|
||||
PID_CONTROLLER_MULTI_WII,
|
||||
PID_CONTROLLER_REWRITE,
|
||||
PID_CONTROLLER_REWRITE = 1,
|
||||
PID_CONTROLLER_LUX_FLOAT,
|
||||
PID_CONTROLLER_MULTI_WII_23,
|
||||
PID_CONTROLLER_MULTI_WII_HYBRID,
|
||||
PID_CONTROLLER_HARAKIRI,
|
||||
PID_CONTROLLER_MULTIWII23,
|
||||
PID_COUNT
|
||||
} pidControllerType_e;
|
||||
|
||||
#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
|
||||
|
||||
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 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 gyro_cut_hz; // Used for soft gyro filtering
|
||||
|
||||
uint8_t pid5_oldyw; // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw
|
||||
|
||||
#ifdef GTUNE
|
||||
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.
|
||||
|
|
|
@ -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 }
|
||||
};
|
||||
|
||||
// sync this with pidControllerType_e
|
||||
static const char * const pidControllers[] = {
|
||||
"REWRITE", "LUXFLOAT", "MULTIWII23", NULL
|
||||
};
|
||||
|
||||
#ifndef CJMCU
|
||||
// sync this with sensors_e
|
||||
static const char * const sensorTypeNames[] = {
|
||||
|
@ -478,7 +483,7 @@ const clivalue_t valueTable[] = {
|
|||
{ "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 },
|
||||
|
||||
{ "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 },
|
||||
{ "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 },
|
||||
|
@ -516,13 +521,10 @@ const clivalue_t valueTable[] = {
|
|||
{ "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 },
|
||||
|
||||
{ "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 },
|
||||
{ "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 },
|
||||
|
||||
{ "pid5_oldyw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pid5_oldyw, 0, 1 },
|
||||
|
||||
#ifdef GTUNE
|
||||
{ "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 },
|
||||
|
@ -1410,8 +1412,14 @@ static void dumpValues(uint16_t mask)
|
|||
continue;
|
||||
}
|
||||
|
||||
printf("set %s = ", valueTable[i].name);
|
||||
cliPrintVar(value, 0);
|
||||
if (strstr(valueTable[i].name, "pid_controller")) {
|
||||
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");
|
||||
}
|
||||
}
|
||||
|
@ -2051,9 +2059,42 @@ static void cliSet(char *cmdline)
|
|||
valuef = fastA2F(eqptr);
|
||||
for (i = 0; i < VALUE_COUNT; 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 (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;
|
||||
if (valueTable[i].type & VAR_FLOAT)
|
||||
tmp.float_value = valuef;
|
||||
|
@ -2085,7 +2126,15 @@ static void cliGet(char *cmdline)
|
|||
if (strstr(valueTable[i].name, cmdline)) {
|
||||
val = &valueTable[i];
|
||||
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");
|
||||
|
||||
matchedCommands++;
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue