rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Pid Controller Cleanup 

Correction to dump
This commit is contained in:
borisbstyle 2015-10-23 00:39:15 +02:00
parent d076a60eb5
commit d685b4d6d8
5 changed files with 62 additions and 507 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/main/config/config.c
View File

@ -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 = 40; pidProfile->dterm_cut_hz = 40;
pidProfile->P_f[ROLL] = 1.5f; // new PID with preliminary defaults test carefully pidProfile->P_f[ROLL] = 1.5f; // new PID with preliminary defaults test carefully
@ -190,8 +189,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.

482
src/main/flight/pid.c
View File

@ -65,13 +65,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)
{ {
@ -216,472 +216,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 };
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) - attitude.raw[axis] + angleTrim->raw[axis];
#else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.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
if (axis == YAW) {
PTerm = filterApplyPt1(PTerm, &yawPTermState, YAW_PTERM_FILTER, dT);
}
delta = (gyroADC[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroADC[axis];
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
DTerm = (delta * 3 * dynD8[axis]) / 32; // Multiplied by 3 to match old scaling
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)
{
UNUSED(rxConfig);
int axis, prop = 0;
int32_t rc, error, errorAngle;
int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm;
static int16_t lastGyro[2] = { 0, 0 };
int32_t delta;
if (FLIGHT_MODE(HORIZON_MODE)) {
prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512);
}
// PITCH & ROLL
for (axis = 0; axis < 2; axis++) {
rc = rcCommand[axis] << 1;
error = rc - (gyroADC[axis] / 4);
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp 16 bits is ok here
if (ABS(gyroADC[axis]) > (640 * 4)) {
errorGyroI[axis] = 0;
}
ITerm = (errorGyroI[axis] >> 7) * pidProfile->I8[axis] >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
PTerm = (int32_t)rc * pidProfile->P8[axis] >> 6;
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { // axis relying on ACC
// 50 degrees max inclination
#ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#endif
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp //16 bits is ok here
PTermACC = ((int32_t)errorAngle * pidProfile->P8[PIDLEVEL]) >> 7; // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768 16 bits is ok for result
int16_t limit = pidProfile->D8[PIDLEVEL] * 5;
PTermACC = constrain(PTermACC, -limit, +limit);
ITermACC = ((int32_t)errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
ITerm = ITermACC + ((ITerm - ITermACC) * prop >> 9);
PTerm = PTermACC + ((PTerm - PTermACC) * prop >> 9);
}
PTerm -= ((int32_t)(gyroADC[axis] / 4) * dynP8[axis]) >> 6; // 32 bits is needed for calculation
if (axis == YAW) {
PTerm = filterApplyPt1(PTerm, &yawPTermState, YAW_PTERM_FILTER, dT);
}
delta = (gyroADC[axis] - lastGyro[axis]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroADC[axis];
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
DTerm = ((int32_t)delta * 3 * dynD8[axis]) >> 5; // 32 bits is needed for calculation. Multiplied by 3 to match old scaling
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; // TODO: Bitwise shift on a signed integer is not recommended
// 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 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 };
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) - attitude.raw[axis] + angleTrim->raw[axis];
#else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.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
if (axis == YAW) {
PTerm = filterApplyPt1(PTerm, &yawPTermState, YAW_PTERM_FILTER, dT);
}
delta = (gyroADC[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroADC[axis];
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
DTerm = (delta * 3 * dynD8[axis]) / 32; // Multiplied by 3 to match old scaling
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) - attitude.raw[axis] + angleTrim->raw[axis];
#else
error = constrain(2.0f * rcCommandAxis, -((int) max_angle_inclination), +max_angle_inclination) - attitude.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;
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);
}
}
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)
{ {
@ -806,24 +340,12 @@ 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;
case PID_CONTROLLER_MULTI_WII_23:
pid_controller = pidMultiWii23;
break;
case PID_CONTROLLER_MULTI_WII_HYBRID:
pid_controller = pidMultiWiiHybrid;
break;
case PID_CONTROLLER_HARAKIRI:
pid_controller = pidHarakiri;
} }
} }

18
src/main/flight/pid.h
View File

@ -18,10 +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_MAX 500 // Maximum value for yaw P limiter
typedef enum { typedef enum {
PIDROLL, PIDROLL,
@ -38,18 +34,15 @@ 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_COUNT
PID_CONTROLLER_MULTI_WII_HYBRID,
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, 2 = luxfloat
uint8_t P8[PID_ITEM_COUNT]; uint8_t P8[PID_ITEM_COUNT];
uint8_t I8[PID_ITEM_COUNT]; uint8_t I8[PID_ITEM_COUNT];
@ -62,12 +55,7 @@ typedef struct pidProfile_s {
float H_level; float H_level;
uint8_t H_sensitivity; uint8_t H_sensitivity;
uint16_t yaw_p_limit; // set P term limit (fixed value was 300)
uint8_t dterm_cut_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5 uint8_t dterm_cut_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
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 #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

64
src/main/io/serial_cli.c
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", 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[] = {
@ -476,7 +481,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, 2 },
{ "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 },
@ -514,11 +519,8 @@ 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 },
{ "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 },
@ -1409,7 +1411,13 @@ static void dumpValues(uint16_t mask)
} }
printf("set %s = ", valueTable[i].name); printf("set %s = ", valueTable[i].name);
cliPrintVar(value, 0);
if (strstr(valueTable[i].name, "pid_controller")) {
cliPrint(pidControllers[*(uint8_t *)valueTable[i].ptr -1]);
} else {
cliPrintVar(value, 0);
}
cliPrint("\r\n"); cliPrint("\r\n");
} }
} }
@ -2049,9 +2057,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;
@ -2073,6 +2114,7 @@ static void cliSet(char *cmdline)
} }
} }
static void cliGet(char *cmdline) static void cliGet(char *cmdline)
{ {
uint32_t i; uint32_t i;
@ -2083,7 +2125,13 @@ 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")) {
cliPrint(pidControllers[*(uint8_t *)valueTable[i].ptr - 1]);
} else {
cliPrintVar(val, 0);
}
cliPrint("\r\n"); cliPrint("\r\n");
matchedCommands++; matchedCommands++;

2
src/main/io/serial_msp.c
View File

@ -1359,7 +1359,7 @@ static bool processInCommand(void)
case MSP_SET_LOOP_TIME: case MSP_SET_LOOP_TIME:
break; break;
case MSP_SET_PID_CONTROLLER: case MSP_SET_PID_CONTROLLER:
currentProfile->pidProfile.pidController = read8(); currentProfile->pidProfile.pidController = constrain(read8(), 1, 2); // Temporary configurator compatibility
pidSetController(currentProfile->pidProfile.pidController); pidSetController(currentProfile->pidProfile.pidController);
break; break;
case MSP_SET_PID: case MSP_SET_PID: