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

renaming variables

Browse source
This commit is contained in:
ctzsnooze 2024-11-14 08:48:05 +11:00
parent e581bfd0a0
commit 39daf2f8e8
1 changed files with 11 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -53,7 +53,7 @@ static float altitudeI = 0.0f;
static float throttleOut = 0.0f; static float throttleOut = 0.0f;
typedef struct { typedef struct {
bool isStarting; bool isStopping;
float distance; float distance;
float previousDistance; float previousDistance;
float previousVelocity; float previousVelocity;
@ -72,7 +72,7 @@ typedef struct {
float gpsDataIntervalS; float gpsDataIntervalS;
float gpsDataFreqHz; float gpsDataFreqHz;
float sanityCheckDistance; float sanityCheckDistance;
float lpfCutoff; float pt1Cutoff;
float pt1Gain; float pt1Gain;
bool sticksActive; bool sticksActive;
float maxAngle; float maxAngle;
@ -85,7 +85,7 @@ static posHoldState posHold = {
.gpsDataIntervalS = 0.1f, .gpsDataIntervalS = 0.1f,
.gpsDataFreqHz = 10.0f, .gpsDataFreqHz = 10.0f,
.sanityCheckDistance = 1000.0f, .sanityCheckDistance = 1000.0f,
.lpfCutoff = 1.0f, .pt1Cutoff = 1.0f,
.pt1Gain = 1.0f, .pt1Gain = 1.0f,
.sticksActive = false, .sticksActive = false,
.pidSumCraft = { 0.0f, 0.0f }, .pidSumCraft = { 0.0f, 0.0f },
@ -101,7 +101,7 @@ void resetPositionControlEFParams(earthFrame_t *efAxis)
// at start only // at start only
pt1FilterInit(&efAxis->velocityLpf, posHold.pt1Gain); // Clear and initialise the filters pt1FilterInit(&efAxis->velocityLpf, posHold.pt1Gain); // Clear and initialise the filters
pt1FilterInit(&efAxis->accelerationLpf, posHold.pt1Gain); pt1FilterInit(&efAxis->accelerationLpf, posHold.pt1Gain);
efAxis->isStarting = true; // Enter starting 'phase' efAxis->isStopping = true; // Enter starting 'phase'
efAxis->integral = 0.0f; efAxis->integral = 0.0f;
} }
@ -138,8 +138,8 @@ void autopilotInit(const autopilotConfig_t *config)
pt3FilterInit(&posHold.upsample[AI_ROLL], upsampleCutoff); pt3FilterInit(&posHold.upsample[AI_ROLL], upsampleCutoff);
pt3FilterInit(&posHold.upsample[AI_PITCH], upsampleCutoff); pt3FilterInit(&posHold.upsample[AI_PITCH], upsampleCutoff);
// Initialise PT1 filters for earth frame axes NS and EW // Initialise PT1 filters for earth frame axes NS and EW
posHold.lpfCutoff = config->position_cutoff * 0.01f; posHold.pt1Cutoff = config->position_cutoff * 0.01f;
posHold.pt1Gain = pt1FilterGain(posHold.lpfCutoff, 0.1f); // assume 10Hz GPS connection at start posHold.pt1Gain = pt1FilterGain(posHold.pt1Cutoff, 0.1f); // assume 10Hz GPS connection at start
initializeEfAxisFilters(&posHold.efAxis[EW], posHold.pt1Gain); initializeEfAxisFilters(&posHold.efAxis[EW], posHold.pt1Gain);
initializeEfAxisFilters(&posHold.efAxis[NS], posHold.pt1Gain); initializeEfAxisFilters(&posHold.efAxis[NS], posHold.pt1Gain);
} }
@ -231,7 +231,7 @@ bool positionControl(void)
const float leak = 1.0f - 0.4f * posHold.gpsDataIntervalS; const float leak = 1.0f - 0.4f * posHold.gpsDataIntervalS;
// leak iTerm while sticks are centered, 2s time constant approximately // leak iTerm while sticks are centered, 2s time constant approximately
const float lpfGain = pt1FilterGain(posHold.lpfCutoff, posHold.gpsDataIntervalS); const float lpfGain = pt1FilterGain(posHold.pt1Cutoff, posHold.gpsDataIntervalS);
// ** Sanity check ** // ** Sanity check **
// primarily to detect flyaway from no Mag or badly oriented Mag // primarily to detect flyaway from no Mag or badly oriented Mag
@ -251,7 +251,7 @@ bool positionControl(void)
const float pidP = efAxis->distance * positionPidCoeffs.Kp; const float pidP = efAxis->distance * positionPidCoeffs.Kp;
// ** I ** // ** I **
efAxis->integral += efAxis->isStarting ? 0.0f : efAxis->distance * posHold.gpsDataIntervalS; efAxis->integral += efAxis->isStopping ? 0.0f : efAxis->distance * posHold.gpsDataIntervalS;
// only add to iTerm while in hold phase // only add to iTerm while in hold phase
const float pidI = efAxis->integral * positionPidCoeffs.Ki; const float pidI = efAxis->integral * positionPidCoeffs.Ki;
@ -272,19 +272,19 @@ bool positionControl(void)
if (posHold.sticksActive) { if (posHold.sticksActive) {
// sticks active 'phase' // sticks active 'phase'
efAxis->isStarting = true; efAxis->isStopping = true;
resetLocation(efAxis, loopAxis); resetLocation(efAxis, loopAxis);
// while sticks are moving, reset the location on each axis, to maintain a usable D value // while sticks are moving, reset the location on each axis, to maintain a usable D value
// slowly leak iTerm away, approx 2s time constant // slowly leak iTerm away, approx 2s time constant
efAxis->integral *= leak; efAxis->integral *= leak;
// increase sanity check distance depending on speed, typically maximal when sticks stop // increase sanity check distance depending on speed, typically maximal when sticks stop
} else if (efAxis->isStarting) { } else if (efAxis->isStopping) {
// 'phase' after sticks stop, but before craft has stopped // 'phase' after sticks stop, but before craft has stopped
pidD *= 1.6f; // aribitrary D boost to stop more quickly when sticks are centered pidD *= 1.6f; // aribitrary D boost to stop more quickly when sticks are centered
if (velocity * velocityFiltered < 0.0f) { if (velocity * velocityFiltered < 0.0f) {
// when an axis has nearly stopped moving, reset it and end it's start phase // when an axis has nearly stopped moving, reset it and end it's start phase
resetLocation(efAxis, loopAxis); resetLocation(efAxis, loopAxis);
efAxis->isStarting = false; efAxis->isStopping = false;
} }
} }