Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 23:35:34 +03:00
Code Issues Wiki Activity

Only calculate errorAngle if it is actually used.

Browse source
This commit is contained in:
Martin Budden 2015-12-29 00:07:49 +00:00 • committed by borisbstyle
parent 66cd5e86b6
commit b478d947f0
1 changed files with 30 additions and 29 deletions
Download patch file Download diff file Expand all files Collapse all files

59
src/main/flight/pid.c
View file

@ -100,7 +100,7 @@ static filterStatePt1_t yawPTermState;
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, static void pidLuxFloat(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)
{ {
float RateError, errorAngle, AngleRate, gyroRate; float RateError, AngleRate, gyroRate;
float ITerm,PTerm,DTerm; float ITerm,PTerm,DTerm;
int32_t stickPosAil, stickPosEle, mostDeflectedPos; int32_t stickPosAil, stickPosEle, mostDeflectedPos;
static float lastError[3]; static float lastError[3];
@ -143,22 +143,22 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
AngleRate = (float)((rate + 10) * rcCommand[YAW]) / 50.0f; AngleRate = (float)((rate + 10) * rcCommand[YAW]) / 50.0f;
} else { } else {
// calculate error and limit the angle to the max inclination // ACRO mode, control is GYRO based direct sticks control is applied to rate PID
AngleRate = (float)((rate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max roll/pitch rate
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
// calculate error and limit the angle to the max inclination
#ifdef GPS #ifdef GPS
errorAngle = (constrainf(((float)rcCommand[axis] * ((float)max_angle_inclination / 500.0f)) + GPS_angle[axis], -((int) max_angle_inclination), const float errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
#else #else
errorAngle = (constrainf((float)rcCommand[axis] * ((float)max_angle_inclination / 500.0f), -((int) max_angle_inclination), const float errorAngle = (constrain(rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
#endif #endif
if (FLIGHT_MODE(ANGLE_MODE)) {
if (FLIGHT_MODE(ANGLE_MODE)) { // ANGLE mode - control is angle based, so control loop is needed
// it's the ANGLE mode - control is angle based, so control loop is needed AngleRate = errorAngle * pidProfile->A_level;
AngleRate = errorAngle * pidProfile->A_level; } else {
} else { // HORIZON mode - direct sticks control is applied to rate PID
//control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRate = (float)((rate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max roll/pitch rate
if (FLIGHT_MODE(HORIZON_MODE)) {
// mix up angle error to desired AngleRate to add a little auto-level feel // mix up angle error to desired AngleRate to add a little auto-level feel
AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength; AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength;
} }
@ -240,7 +240,6 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
{ {
UNUSED(rxConfig); UNUSED(rxConfig);
int32_t errorAngle;
int axis, deltaCount; int axis, deltaCount;
int32_t delta, deltaSum; int32_t delta, deltaSum;
static int32_t previousDelta[3][8]; static int32_t previousDelta[3][8];
@ -280,26 +279,28 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
uint8_t rate = controlRateConfig->rates[axis]; uint8_t rate = controlRateConfig->rates[axis];
// -----Get the desired angle rate depending on flight mode // -----Get the desired angle rate depending on flight mode
if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) if (axis == FD_YAW) {
AngleRateTmp = (((int32_t)(rate + 27) * rcCommand[YAW]) >> 5); // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[YAW]) >> 5;
} else { } else {
AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
// calculate error and limit the angle to max configured inclination // calculate error and limit the angle to max configured inclination
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
#ifdef GPS #ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#else #else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#endif #endif
if (FLIGHT_MODE(ANGLE_MODE)) {
if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID // ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4; AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
if (FLIGHT_MODE(HORIZON_MODE)) { } else {
// mix up angle error to desired AngleRateTmp to add a little auto-level feel. horizonLevelStrength is scaled to the stick input // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel,
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4; // horizonLevelStrength is scaled to the stick input
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4;
} }
} else { // it's the ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
} }
} }