Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 04:45:24 +03:00
Code Issues Wiki Activity

Terminate start individually on each axis

Browse source 
added comments
This commit is contained in:
ctzsnooze 2024-10-27 18:03:49 +11:00
parent 298ae8d6b9
commit f20bee86d5
1 changed files with 196 additions and 191 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -53,40 +53,49 @@ static float throttleOut = 0.0f;
typedef struct { typedef struct {
float gpsDataIntervalS; float gpsDataIntervalS;
float gpsDataFreqHz; float gpsDataFreqHz;
float previousDistanceCm;
float sanityCheckDistance; float sanityCheckDistance;
bool isStarting; bool isStartingNS;
bool isStartingEW;
float peakInitialGroundspeed; float peakInitialGroundspeed;
float lpfCutoff; float lpfCutoff;
float pt1Gain;
bool sticksActive; bool sticksActive;
float previousVelocityEW; float previousDistanceNS;
float previousDistanceEW;
float previousVelocityNS; float previousVelocityNS;
float integralEW; float previousVelocityEW;
float previousPidSumNS;
float previousPidSumEW;
float integralNS; float integralNS;
float integralEW;
} posHoldState; } posHoldState;
static posHoldState posHold = { static posHoldState posHold = {
.gpsDataIntervalS = 0.1f, .gpsDataIntervalS = 0.1f,
.gpsDataFreqHz = 10.0f, .gpsDataFreqHz = 10.0f,
.previousDistanceCm = 0.0f,
.sanityCheckDistance = 1000.0f, .sanityCheckDistance = 1000.0f,
.isStarting = false, .isStartingNS = false,
.isStartingEW = false,
.peakInitialGroundspeed = 0.0f, .peakInitialGroundspeed = 0.0f,
.lpfCutoff = 1.0f, .lpfCutoff = 1.0f,
.pt1Gain = 1.0f,
.sticksActive = false, .sticksActive = false,
.previousVelocityEW = 0.0f, .previousDistanceNS = 0.0f,
.previousDistanceEW = 0.0f,
.previousVelocityNS = 0.0f, .previousVelocityNS = 0.0f,
.integralEW = 0.0f, .previousVelocityEW = 0.0f,
.previousPidSumNS = 0.0f,
.previousPidSumEW = 0.0f,
.integralNS = 0.0f, .integralNS = 0.0f,
.integralEW = 0.0f,
}; };
static gpsLocation_t currentTargetLocation = {0, 0, 0}; static gpsLocation_t currentTargetLocation = {0, 0, 0};
static gpsLocation_t previousLocation = {0, 0, 0};
float autopilotAngle[ANGLE_INDEX_COUNT]; float autopilotAngle[ANGLE_INDEX_COUNT];
static pt1Filter_t velocityNSLpf; static pt1Filter_t velocityNSLpf;
static pt1Filter_t velocityEWLpf; static pt1Filter_t velocityEWLpf;
static pt2Filter_t accelerationRollLpf; static pt2Filter_t accelerationNSLpf;
static pt2Filter_t accelerationPitchLpf; static pt2Filter_t accelerationEWLpf;
void autopilotInit(const autopilotConfig_t *config) void autopilotInit(const autopilotConfig_t *config)
{ {
@ -99,11 +108,11 @@ void autopilotInit(const autopilotConfig_t *config)
positionPidCoeffs.Kd = config->position_D * POSITION_D_SCALE; positionPidCoeffs.Kd = config->position_D * POSITION_D_SCALE;
positionPidCoeffs.Kf = config->position_A * POSITION_A_SCALE; // Kf used for acceleration positionPidCoeffs.Kf = config->position_A * POSITION_A_SCALE; // Kf used for acceleration
posHold.lpfCutoff = config->position_cutoff * 0.01f; posHold.lpfCutoff = config->position_cutoff * 0.01f;
const float pt1Gain = pt1FilterGain(posHold.lpfCutoff, 0.1f); // assume 10Hz GPS connection at start posHold.pt1Gain = pt1FilterGain(posHold.lpfCutoff, 0.1f); // assume 10Hz GPS connection at start
pt1FilterInit(&velocityNSLpf, pt1Gain); pt1FilterInit(&velocityNSLpf, posHold.pt1Gain);
pt1FilterInit(&velocityEWLpf, pt1Gain); pt1FilterInit(&velocityEWLpf, posHold.pt1Gain);
pt2FilterInit(&accelerationRollLpf, pt1Gain); pt2FilterInit(&accelerationNSLpf, posHold.pt1Gain);
pt2FilterInit(&accelerationPitchLpf, pt1Gain); pt2FilterInit(&accelerationEWLpf, posHold.pt1Gain);
} }
void resetAltitudeControl (void) { void resetAltitudeControl (void) {
@ -154,33 +163,35 @@ void setSticksActiveStatus(bool areSticksActive)
posHold.sticksActive = areSticksActive; posHold.sticksActive = areSticksActive;
} }
void resetPositionControlParams(void) { // at the start, and while sticks are moving void resetPositionControlParamsNS(void) { // at the start, and while sticks are moving
posHold.previousDistanceCm = 0.0f; posHold.previousDistanceNS = 0.0f;
posHold.sanityCheckDistance = 1000.0f;
posHold.previousVelocityEW = 0.0f;
posHold.previousVelocityNS = 0.0f; posHold.previousVelocityNS = 0.0f;
posHold.lpfCutoff = autopilotConfig()->position_cutoff * 0.003f; // slow rise at start posHold.previousPidSumNS = 0.0f;
const float pt1Gain = pt1FilterGain(posHold.lpfCutoff, 0.1f); pt1FilterInit(&velocityNSLpf, posHold.pt1Gain);
// reset all lowpass filter accumulators to zero pt2FilterInit(&accelerationNSLpf, posHold.pt1Gain);
pt1FilterInit(&velocityNSLpf, pt1Gain); posHold.isStartingNS = true;
pt1FilterInit(&velocityEWLpf, pt1Gain);
pt2FilterInit(&accelerationRollLpf, pt1Gain);
pt2FilterInit(&accelerationPitchLpf, pt1Gain);
posHold.isStarting = true;
} }
void resetPositionControl(gpsLocation_t initialTargetLocation) { // set only at the start void resetPositionControlParamsEW(void) { // at the start, and while sticks are moving
posHold.previousDistanceEW = 0.0f;
posHold.previousVelocityEW = 0.0f;
posHold.previousPidSumEW = 0.0f;
pt1FilterInit(&velocityEWLpf, posHold.pt1Gain);
pt2FilterInit(&accelerationEWLpf, posHold.pt1Gain);
posHold.isStartingEW = true;
}
void resetPositionControl(gpsLocation_t initialTargetLocation) { // set only at the start frmo pos_hold.c
currentTargetLocation = initialTargetLocation; currentTargetLocation = initialTargetLocation;
resetPositionControlParams(); resetPositionControlParamsNS();
resetPositionControlParamsEW();
posHold.peakInitialGroundspeed = 0.0f; posHold.peakInitialGroundspeed = 0.0f;
posHold.integralEW = 0.0f; posHold.integralEW = 0.0f;
posHold.integralNS = 0.0f; posHold.integralNS = 0.0f;
previousLocation = gpsSol.llh;
} }
void updateTargetLocation(gpsLocation_t newTargetLocation) { void updateTargetLocation(gpsLocation_t newTargetLocation) {
currentTargetLocation = newTargetLocation; currentTargetLocation = newTargetLocation;
resetPositionControlParams(); // don't reset accumulated iTerm
} }
bool positionControl(void) { bool positionControl(void) {
@ -201,50 +212,27 @@ bool positionControl(void) {
posHold.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.01s to 1.0s posHold.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.01s to 1.0s
posHold.gpsDataFreqHz = 1.0f / posHold.gpsDataIntervalS; posHold.gpsDataFreqHz = 1.0f / posHold.gpsDataIntervalS;
// get xy distances from current location (gpsSol.llh) to target location if (posHold.sticksActive) {
// if a Position Hold deadband is set, and sticks are outside deadband, allow pilot control in angle mode
resetPositionControlParamsNS();
resetPositionControlParamsEW();
autopilotAngle[AI_ROLL] = 0.0f;
autopilotAngle[AI_PITCH] = 0.0f;
} else {
// control position
// first get xy distances from current location (gpsSol.llh) to target location
float nsDistance; // cm, steps of 11.1cm, North of target is positive float nsDistance; // cm, steps of 11.1cm, North of target is positive
float ewDistance; // cm, steps of 11.1cm, East of target is positive float ewDistance; // cm, steps of 11.1cm, East of target is positive
GPS_distances(&gpsSol.llh, &currentTargetLocation, &nsDistance, &ewDistance); GPS_distances(&gpsSol.llh, &currentTargetLocation, &nsDistance, &ewDistance);
float distanceCm = sqrtf(sq(nsDistance) + sq(ewDistance)); float distanceCm = sqrtf(sq(nsDistance) + sq(ewDistance));
// ** handle startup ** posHold.pt1Gain = pt1FilterGain(posHold.lpfCutoff, posHold.gpsDataIntervalS);
// at the start, if the quad was moving, or randomly, distance from the start point will increase
// check for the distance increase to 'stop', then reset the target point
// this looks a lot better than a huge rebound
// D and A are primarily responsible for the immediate 'stopping' behaviour
// so that the stop is not too harsh, these are smoothed more than normal right at the start
if (posHold.isStarting) {
// very first time, or after the sticks were centered and the pilot flew around a bit
posHold.peakInitialGroundspeed = fmaxf(posHold.peakInitialGroundspeed, gpsSol.groundSpeed);
// at first, give the filter a long time constant to soften the harshness of the stop
if (gpsSol.groundSpeed > 0.5f * posHold.peakInitialGroundspeed) {
posHold.lpfCutoff = autopilotConfig()->position_cutoff * 0.003f; // 3x slower than normal
} else {
// when close to stopping, make the time constant shorter than normal
// so that D and A will quickly fade away as it comes to a stop
posHold.lpfCutoff = autopilotConfig()->position_cutoff * 0.03f;
}
// allow a larder sanity check distance if at higher speed when coming to a stop
// because the risk of overshoot is greater at higher speed
posHold.sanityCheckDistance = gpsSol.groundSpeed > 1000 ? gpsSol.groundSpeed : 1000.0f;
// when the quad stops, the start period is over
if (distanceCm < posHold.previousDistanceCm) {
// reset the target location to the stop point
currentTargetLocation = gpsSol.llh;
posHold.lpfCutoff = autopilotConfig()->position_cutoff * 0.01f; // use normal filter values
resetPositionControlParams();
posHold.peakInitialGroundspeed = 0.0f;
posHold.isStarting = false;
}
}
posHold.previousDistanceCm = distanceCm;
// intentionally using PT1 gain in PT2 filters to provide a lower effective cutoff
float pt1Gain = pt1FilterGain(posHold.lpfCutoff, posHold.gpsDataIntervalS);
// ** Sanity check ** // ** Sanity check **
// larger threshold if faster at start
if (posHold.isStartingNS || posHold.isStartingEW) {
posHold.sanityCheckDistance = gpsSol.groundSpeed > 1000 ? gpsSol.groundSpeed : 1000.0f;
}
// primarily to detect flyaway from no Mag or badly oriented Mag // primarily to detect flyaway from no Mag or badly oriented Mag
// but must accept some overshoot at the start, especially if entering at high speed // but must accept some overshoot at the start, especially if entering at high speed
if (distanceCm > posHold.sanityCheckDistance) { if (distanceCm > posHold.sanityCheckDistance) {
@ -264,15 +252,18 @@ bool positionControl(void) {
// ** I ** // ** I **
if (!posHold.isStarting){ const float leak = 1.0f - 0.4f * posHold.gpsDataIntervalS; // gpsDataIntervalS is not more than 1.0s
if (!posHold.isStartingNS){
// only accumulate iTerm after completing the start phase // only accumulate iTerm after completing the start phase
// perhaps need a timeout on the start phase ? // perhaps need a timeout on the start phase ?
posHold.integralNS += nsDistance * posHold.gpsDataIntervalS; posHold.integralNS += nsDistance * posHold.gpsDataIntervalS;
posHold.integralEW += ewDistance * posHold.gpsDataIntervalS;
} else { } else {
// while moving sticks, slowly leak iTerm away, approx 2s time constant // while moving sticks, slowly leak iTerm away, approx 2s time constant
const float leak = 1.0f - 0.4f * posHold.gpsDataIntervalS; // gpsDataIntervalS is not more than 1.0s
posHold.integralNS *= leak; posHold.integralNS *= leak;
}
if (!posHold.isStartingEW){
posHold.integralEW += ewDistance * posHold.gpsDataIntervalS;
} else {
posHold.integralEW *= leak; posHold.integralEW *= leak;
} }
@ -280,16 +271,13 @@ bool positionControl(void) {
const float ewI = posHold.integralEW * positionPidCoeffs.Ki; const float ewI = posHold.integralEW * positionPidCoeffs.Ki;
// ** D ** // // ** D ** //
// get change in distance in NS and EW directions from gps.c using the `GPS_distances` function // get change in distance in NS and EW directions from gps.c using the `GPS_distances` function
// this gives cleaner velocity data than the module supplied GPS Speed and Heading information // this gives cleaner velocity data than the module supplied GPS Speed and Heading information
float deltaDistanceNS; // step size is 1.11cm
float deltaDistanceEW;
GPS_distances(&gpsSol.llh, &previousLocation, &deltaDistanceNS, &deltaDistanceEW);
previousLocation = gpsSol.llh;
const float velocityNS = deltaDistanceNS * posHold.gpsDataFreqHz; // cm/s, minimum step 11.1 cm/s const float velocityNS = (nsDistance - posHold.previousDistanceNS) * posHold.gpsDataFreqHz; // cm/s, minimum step 11.1 cm/s
const float velocityEW = deltaDistanceEW * posHold.gpsDataFreqHz; const float velocityEW = (ewDistance - posHold.previousDistanceEW) * posHold.gpsDataFreqHz;
posHold.previousDistanceNS = nsDistance;
posHold.previousDistanceEW = ewDistance;
float accelerationNS = (velocityNS - posHold.previousVelocityNS) * posHold.gpsDataFreqHz; float accelerationNS = (velocityNS - posHold.previousVelocityNS) * posHold.gpsDataFreqHz;
posHold.previousVelocityNS = velocityNS; posHold.previousVelocityNS = velocityNS;
@ -298,20 +286,20 @@ bool positionControl(void) {
// scale and filter - filter cutoffs vary during the startup phase // scale and filter - filter cutoffs vary during the startup phase
float nsD = velocityNS * positionPidCoeffs.Kd; float nsD = velocityNS * positionPidCoeffs.Kd;
pt1FilterUpdateCutoff(&velocityNSLpf, pt1Gain); pt1FilterUpdateCutoff(&velocityNSLpf, posHold.pt1Gain);
nsD = pt1FilterApply(&velocityNSLpf, nsD); nsD = pt1FilterApply(&velocityNSLpf, nsD);
float ewD = velocityEW * positionPidCoeffs.Kd; float ewD = velocityEW * positionPidCoeffs.Kd;
pt1FilterUpdateCutoff(&velocityEWLpf, pt1Gain); pt1FilterUpdateCutoff(&velocityEWLpf, posHold.pt1Gain);
ewD = pt1FilterApply(&velocityEWLpf, ewD); ewD = pt1FilterApply(&velocityEWLpf, ewD);
float nsA = accelerationNS * positionPidCoeffs.Kf; float nsA = accelerationNS * positionPidCoeffs.Kf;
pt2FilterUpdateCutoff(&accelerationRollLpf, pt1Gain); // using PT1 gain for stronger cutoff pt2FilterUpdateCutoff(&accelerationNSLpf, posHold.pt1Gain); // using PT1 gain for stronger cutoff
nsA = pt2FilterApply(&accelerationRollLpf, nsA); nsA = pt2FilterApply(&accelerationNSLpf, nsA);
float ewA = accelerationEW * positionPidCoeffs.Kf; float ewA = accelerationEW * positionPidCoeffs.Kf;
pt2FilterUpdateCutoff(&accelerationPitchLpf, pt1Gain); // using PT1 gain for stronger cutoff pt2FilterUpdateCutoff(&accelerationEWLpf, posHold.pt1Gain); // using PT1 gain for stronger cutoff
ewA = pt2FilterApply(&accelerationPitchLpf, ewA); ewA = pt2FilterApply(&accelerationEWLpf, ewA);
// limit sum of D and A because otherwise can be too aggressive when starting at speed // limit sum of D and A because otherwise can be too aggressive when starting at speed
float nsDA = nsD + nsA; float nsDA = nsD + nsA;
@ -327,6 +315,23 @@ bool positionControl(void) {
float nsPidSum = nsP + nsI + nsDA; float nsPidSum = nsP + nsI + nsDA;
float ewPidSum = ewP + ewI + ewDA; float ewPidSum = ewP + ewI + ewDA;
// terminate initial startup behaviour as pidSum crosses zero
// this is best for smoothness of the transition from the stop to the new target location
if (posHold.isStartingNS && posHold.previousPidSumNS * nsPidSum < 0.0f) { // pidsum ns has reversed sign
resetPositionControlParamsNS();
currentTargetLocation.lat = gpsSol.llh.lat;
nsDistance = 0.0f;
posHold.isStartingNS = false;
}
if (posHold.isStartingEW && posHold.previousPidSumEW * ewPidSum < 0.0f) { // pidsum ns has reversed sign
resetPositionControlParamsEW();
ewDistance = 0.0f;
currentTargetLocation.lon = gpsSol.llh.lon;
posHold.isStartingEW = false;
}
posHold.previousPidSumNS = nsPidSum;
posHold.previousPidSumEW = ewPidSum;
// ** Rotate pid Sum to quad frame of reference, into pitch and roll ** // ** Rotate pid Sum to quad frame of reference, into pitch and roll **
float headingRads = (attitude.values.yaw / 10.0f) * RAD; // will be constrained to +/-pi in sin_approx() float headingRads = (attitude.values.yaw / 10.0f) * RAD; // will be constrained to +/-pi in sin_approx()
const float sinHeading = sin_approx(headingRads); const float sinHeading = sin_approx(headingRads);
@ -338,13 +343,10 @@ bool positionControl(void) {
// pidSum steps at GPS rate, and needs to change the pid.c upsampling filter for smoothness. // pidSum steps at GPS rate, and needs to change the pid.c upsampling filter for smoothness.
// ** Final output to pid.c Angle Mode ** // ** Final output to pid.c Angle Mode **
autopilotAngle[AI_ROLL] = pidSumRoll;
// if a Position Hold deadband is set, and sticks are outside deadband, allow pilot control in angle mode autopilotAngle[AI_PITCH] = pidSumPitch;
autopilotAngle[AI_ROLL] = posHold.sticksActive ? 0.0f : pidSumRoll;
autopilotAngle[AI_PITCH] = posHold.sticksActive ? 0.0f : pidSumPitch;
// Debugs... distances in cm, angles in degrees * 10, velocities cm/2 // Debugs... distances in cm, angles in degrees * 10, velocities cm/2
if (gyroConfig()->gyro_filter_debug_axis == FD_ROLL) { if (gyroConfig()->gyro_filter_debug_axis == FD_ROLL) {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(distanceCm)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(distanceCm));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(ewDistance)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(ewDistance));
@ -364,6 +366,9 @@ bool positionControl(void) {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(nsDA * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(nsDA * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(posHold.integralNS)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(posHold.integralNS));
} }
}
return true; return true;
} }