mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-19 22:35:23 +03:00
Pid Controller Cleanup
Correction to dump
This commit is contained in:
borisbstyle
parent
d076a60eb5
commit
d685b4d6d8
5 changed files with 62 additions and 507 deletions
|
|
@ -65,13 +65,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)
|
||||
{
|
||||
|
|
@ -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,
|
||||
rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
|
||||
{
|
||||
|
|
@ -806,24 +340,12 @@ 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:
|
||||
pid_controller = pidMultiWii23;
|
||||
break;
|
||||
case PID_CONTROLLER_MULTI_WII_HYBRID:
|
||||
pid_controller = pidMultiWiiHybrid;
|
||||
break;
|
||||
case PID_CONTROLLER_HARAKIRI:
|
||||
pid_controller = pidHarakiri;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue