Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +03:00
Code Issues Wiki Activity

Fix angle mode rate profile change bug (#13862)

Browse source 
fix angle mode rate profile change bug
This commit is contained in:
ctzsnooze 2024-08-30 23:10:01 +10:00 • committed by GitHub
parent ac384cf34c
commit cfe41ac022
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
3 changed files with 4 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -436,15 +436,17 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_
// We now use Acro Rates, transformed into the range +/- 1, to provide setpoints // We now use Acro Rates, transformed into the range +/- 1, to provide setpoints
const float angleLimit = pidProfile->angle_limit; const float angleLimit = pidProfile->angle_limit;
float angleFeedforward = 0.0f; float angleFeedforward = 0.0f;
// if user changes rates profile, update the max setpoint for angle mode
const float maxSetpointRateInv = 1.0f / getMaxRcRate(axis);
#ifdef USE_FEEDFORWARD #ifdef USE_FEEDFORWARD
angleFeedforward = angleLimit * getFeedforward(axis) * pidRuntime.angleFeedforwardGain * pidRuntime.maxRcRateInv[axis]; angleFeedforward = angleLimit * getFeedforward(axis) * pidRuntime.angleFeedforwardGain * maxSetpointRateInv;
// angle feedforward must be heavily filtered, at the PID loop rate, with limited user control over time constant // angle feedforward must be heavily filtered, at the PID loop rate, with limited user control over time constant
// it MUST be very delayed to avoid early overshoot and being too aggressive // it MUST be very delayed to avoid early overshoot and being too aggressive
angleFeedforward = pt3FilterApply(&pidRuntime.angleFeedforwardPt3[axis], angleFeedforward); angleFeedforward = pt3FilterApply(&pidRuntime.angleFeedforwardPt3[axis], angleFeedforward);
#endif #endif
float angleTarget = angleLimit * currentPidSetpoint * pidRuntime.maxRcRateInv[axis]; float angleTarget = angleLimit * currentPidSetpoint * maxSetpointRateInv;
// use acro rates for the angle target in both horizon and angle modes, converted to -1 to +1 range using maxRate // use acro rates for the angle target in both horizon and angle modes, converted to -1 to +1 range using maxRate
#ifdef USE_GPS_RESCUE #ifdef USE_GPS_RESCUE

1
src/main/flight/pid.h
View file

@ -473,7 +473,6 @@ typedef struct pidRuntime_s {
float angleEarthRef; float angleEarthRef;
float angleTarget[2]; float angleTarget[2];
bool axisInAngleMode[3]; bool axisInAngleMode[3];
float maxRcRateInv[2];
#endif #endif
#ifdef USE_WING #ifdef USE_WING

1
src/main/flight/pid_init.c
View file

@ -251,7 +251,6 @@ void pidInitFilters(const pidProfile_t *pidProfile)
for (int axis = 0; axis < 2; axis++) { // ROLL and PITCH only for (int axis = 0; axis < 2; axis++) { // ROLL and PITCH only
pt3FilterInit(&pidRuntime.attitudeFilter[axis], k); pt3FilterInit(&pidRuntime.attitudeFilter[axis], k);
pt3FilterInit(&pidRuntime.angleFeedforwardPt3[axis], k2); pt3FilterInit(&pidRuntime.angleFeedforwardPt3[axis], k2);
pidRuntime.maxRcRateInv[axis] = 1.0f / getMaxRcRate(axis);
} }
pidRuntime.angleYawSetpoint = 0.0f; pidRuntime.angleYawSetpoint = 0.0f;
#endif #endif