Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 12:25:20 +03:00
Code Issues Wiki Activity

lower PID gains, slowly leak iTerm while sticks move

Browse source
This commit is contained in:
ctzsnooze 2024-10-25 12:26:14 +11:00
parent 76adbc41de
commit c1f79e7d73
1 changed files with 23 additions and 17 deletions
Download patch file Download diff file Expand all files Collapse all files

40
src/main/flight/autopilot.c
View file

@ -39,9 +39,9 @@
#define ALTITUDE_I_SCALE 0.003f #define ALTITUDE_I_SCALE 0.003f
#define ALTITUDE_D_SCALE 0.01f #define ALTITUDE_D_SCALE 0.01f
#define ALTITUDE_F_SCALE 0.01f #define ALTITUDE_F_SCALE 0.01f
#define POSITION_P_SCALE 0.001f #define POSITION_P_SCALE 0.0008f
#define POSITION_I_SCALE 0.0004f #define POSITION_I_SCALE 0.0002f
#define POSITION_D_SCALE 0.003f #define POSITION_D_SCALE 0.0015f
#define POSITION_A_SCALE 0.0008f #define POSITION_A_SCALE 0.0008f
static pidCoefficient_t altitudePidCoeffs; static pidCoefficient_t altitudePidCoeffs;
@ -298,6 +298,7 @@ bool positionControl(void) {
// roll // roll
float velocityRoll = rollVelProp * velocity + velocityToTarget * rollProportion; float velocityRoll = rollVelProp * velocity + velocityToTarget * rollProportion;
float accelerationRoll = rollVelProp * acceleration; float accelerationRoll = rollVelProp * acceleration;
// lowpass filters // lowpass filters
pt1FilterUpdateCutoff(&velocityRollLpf, pt1Gain); pt1FilterUpdateCutoff(&velocityRollLpf, pt1Gain);
velocityRoll = pt1FilterApply(&velocityRollLpf, velocityRoll); velocityRoll = pt1FilterApply(&velocityRollLpf, velocityRoll);
@ -331,7 +332,7 @@ bool positionControl(void) {
// Based on the difference in angle between the nose of the quad and the current error bearing // Based on the difference in angle between the nose of the quad and the current error bearing
if (!posHold.isStarting){ if (!posHold.isStarting){
// only integrate while not actively stopping // only add to iTerm while not actively stopping
float bearingRadians = bearingDeg * RAD; // 0-360, so constrain to +/- π float bearingRadians = bearingDeg * RAD; // 0-360, so constrain to +/- π
if (bearingRadians > M_PIf) { if (bearingRadians > M_PIf) {
@ -367,11 +368,11 @@ bool positionControl(void) {
posHold.iTermRoll = -sin_approx(headingErrorRads) * fabsf(posHold.NSIntegral) + cos_approx(headingErrorRads) * fabsf(posHold.EWIntegral); // +1 when iTerm points left, -1 when iTerm points right posHold.iTermRoll = -sin_approx(headingErrorRads) * fabsf(posHold.NSIntegral) + cos_approx(headingErrorRads) * fabsf(posHold.EWIntegral); // +1 when iTerm points left, -1 when iTerm points right
posHold.iTermPitch = cos_approx(headingErrorRads) * fabsf(posHold.NSIntegral) + sin_approx(headingErrorRads) * fabsf(posHold.EWIntegral); // +1 when iTerm points nose forward, -1 when iTerm should pitch back posHold.iTermPitch = cos_approx(headingErrorRads) * fabsf(posHold.NSIntegral) + sin_approx(headingErrorRads) * fabsf(posHold.EWIntegral); // +1 when iTerm points nose forward, -1 when iTerm should pitch back
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(EWError * 1000)); // positive means North of target } else {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(posHold.EWIntegral * 10)); // while moving sticks, slowly leak iTerm away, approx 3s time constant
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(EFIntegralAngleRads * 100)); // smoothed integral bearing rads const float leak = 1.0f - 0.25f * gpsDataIntervalS; // assumes gpsDataIntervalS not more than 1.0s
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(headingErrorRads * 100)); // after wrapping posHold.NSIntegral *= leak;
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(headingRadians * 100)); posHold.EWIntegral *= leak;
} }
} }
} }
@ -405,20 +406,25 @@ bool positionControl(void) {
// autopilotAngle[] is added to angle setpoint in pid.c, in degrees // autopilotAngle[] is added to angle setpoint in pid.c, in degrees
// stick angle setpoint forced to zero within the same deadband via rc.c // stick angle setpoint forced to zero within the same deadband via rc.c
if (gyroConfig()->gyro_filter_debug_axis == FD_ROLL) { if (gyroConfig()->gyro_filter_debug_axis == FD_ROLL) {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(normalisedErrorAngle)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(bearingDeg));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(-posHold.distanceCm * rollProportion)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(-posHold.distanceCm * rollProportion));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(pidSumRoll * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(pidSumRoll * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(velocityToTarget * rollProportion));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(rollP * 10)); // degrees*10 DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(rollP * 10)); // degrees*10
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(posHold.iTermRoll * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(posHold.iTermRoll * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(rollD * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(rollDA * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(rollA * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(posHold.EWIntegral * 10));
} else { } else {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(rollP * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(bearingDeg));;
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(posHold.iTermRoll * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(-posHold.distanceCm * pitchProportion));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(pidSumPitch * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(velocityToTarget * pitchProportion));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(pitchP * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(posHold.iTermPitch * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(pitchD * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(pitchVelProp * velocity));
// DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(posHold.NSIntegral * 10));
} }
return true; return true;
} }