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

Converge IMU faster at higher groundspeed during GPS Rescue (#12738)

Browse source 
* dynamic dcm_kp depending on groundspeed vs velocity to home

* reduce max pitch angle default to 40

* Only 10% of set velocity to home is enough, angle to 45

to reduce chance of sanity check against headwind

* fixes, thanks @KarateBrot

* Feedback from Ledvinap, ensure full pitch and yaw authority in descend

* pitch limit max of 1/3 and dcm_kp gain to 6x max

* apply IMU kP factor to yaw only via ez_ef

* More from @Ledvinap, constrain pitch in descent also

* Fix the velocity error gain, refactoring thanks to Ledvinap

* thanks, Mark!

* fixes, must be fabsf

* make IMU gain twice as sensitive to groundspeed error

* useCOG as float

* Fix unit test

* ensure cogYawGain is zero, except during gps rescue

* update comment after suggestion from ledvinap

* keep roll angle limit consistently at 50% of pitch limit when active

* changes to comments, thanks petr
This commit is contained in:
ctzsnooze 2023-05-11 06:24:34 +10:00 • committed by GitHub
parent 9ff4723c19
commit 8e3389f452
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
5 changed files with 59 additions and 27 deletions
Download patch file Download diff file Expand all files Collapse all files

43
src/main/flight/gps_rescue.c
View file

@ -112,6 +112,8 @@ typedef struct {
float alitutudeStepCm;
float maxPitchStep;
float absErrorAngle;
float groundspeedPitchAttenuator;
float imuYawCogGain;
} rescueSensorData_s;
typedef struct {
@ -233,7 +235,8 @@ static void rescueAttainPosition(void)
throttleI = 0.0f;
previousAltitudeError = 0.0f;
throttleAdjustment = 0;
rescueState.intent.disarmThreshold = gpsRescueConfig()->disarmThreshold / 10.0f;
rescueState.intent.disarmThreshold = gpsRescueConfig()->disarmThreshold * 0.1f;
rescueState.sensor.imuYawCogGain = 1.0f;
return;
case RESCUE_DO_NOTHING:
// 20s of slow descent for switch induced sanity failures to allow time to recover
@ -453,7 +456,7 @@ static void performSanityChecks(void)
if (rescueState.phase == RESCUE_FLY_HOME) {
const float velocityToHomeCmS = previousDistanceToHomeCm - rescueState.sensor.distanceToHomeCm; // cm/s
previousDistanceToHomeCm = rescueState.sensor.distanceToHomeCm;
rescueState.intent.secondsFailing += (velocityToHomeCmS < 0.5f * rescueState.intent.targetVelocityCmS) ? 1 : -1;
rescueState.intent.secondsFailing += (velocityToHomeCmS < 0.1f * rescueState.intent.targetVelocityCmS) ? 1 : -1;
rescueState.intent.secondsFailing = constrain(rescueState.intent.secondsFailing, 0, 15);
if (rescueState.intent.secondsFailing == 15) {
#ifdef USE_MAG
@ -477,7 +480,6 @@ static void performSanityChecks(void)
rescueState.failure = RESCUE_LOWSATS;
}
// These conditions ignore sanity mode settings, and apply in all rescues, to handle getting stuck in a climb or descend
const float actualAltitudeChange = rescueState.sensor.currentAltitudeCm - prevAltitudeCm;
@ -567,6 +569,25 @@ static void sensorUpdate(void)
rescueState.sensor.distanceToHomeM = rescueState.sensor.distanceToHomeCm / 100.0f;
rescueState.sensor.groundSpeedCmS = gpsSol.groundSpeed; // cm/s
// when there is significant velocity error, increase IMU COG Gain for yaw to a higher value, and reduce max pitch angle
if (gpsRescueConfig()->rescueGroundspeed) {
const float rescueGroundspeed = 1000.0f; // in cm/s, fixed 10 m/s groundspeed
// sets the slope of the increase in IMU COG Gain as velocity error increases; at 10, we get max CoG gain around 25m/s drift, approx the limit of what we can fix
const float groundspeedErrorRatio = fabsf(rescueState.sensor.groundSpeedCmS - rescueState.sensor.velocityToHomeCmS) / rescueGroundspeed;
// 0 if groundspeed = velocity to home,
// 1 if moving sideways at 10m/s but zero home velocity,
// 2 if moving backwards (away from home) at 10 m/s
// 4 if moving backwards (away from home) at 20 m/s
if ((rescueState.phase == RESCUE_ATTAIN_ALT) || (rescueState.phase == RESCUE_ROTATE)) {
// zero IMU CoG yaw during climb or rotate, to stop IMU error accumulation arising from drift during long climbs
rescueState.sensor.imuYawCogGain = 0;
} else {
// up to 6x increase in CoG IMU Yaw gain
rescueState.sensor.imuYawCogGain = constrainf(1.0f + (2.0f * groundspeedErrorRatio), 1.0f, 6.0f);
}
rescueState.sensor.groundspeedPitchAttenuator = 1.0f / constrainf(1.0f + groundspeedErrorRatio, 1.0f, 3.0f); // cut pitch angle by up to one third
}
rescueState.sensor.gpsDataIntervalSeconds = getGpsDataIntervalSeconds();
// Range from 10ms (100hz) to 1000ms (1Hz). Intended to cover common GPS data rates and exclude unusual values.
@ -648,10 +669,13 @@ void descend(void)
rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * rescueState.intent.proximityToLandingArea;
// reduce target velocity as we get closer to home. Zero within 2m of home, reducing risk of overshooting.
// if quad drifts further than 2m away from home, should by then have rotated towards home, so pitch is allowed
rescueState.intent.rollAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.intent.proximityToLandingArea;
rescueState.intent.rollAngleLimitDeg = 0.5f * gpsRescueConfig()->maxRescueAngle * rescueState.intent.proximityToLandingArea;
// reduce roll capability when closer to home, none within final 2m
rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.sensor.groundspeedPitchAttenuator;
}
rescueState.intent.yawAttenuator = 1.0f; // just in case entered descend phase before yaw authority was complete
// configure altitude step for descent, considering interval between altitude readings
rescueState.intent.altitudeStep = -1.0f * rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
@ -758,7 +782,8 @@ void gpsRescueUpdate(void)
rescueState.intent.yawAttenuator += rescueState.sensor.gpsRescueTaskIntervalSeconds;
}
if (rescueState.sensor.absErrorAngle < 30.0f) {
rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle; // allow pitch
// allow pitch, limiting allowed angle if we are drifting away from home
rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.sensor.groundspeedPitchAttenuator;
rescueState.phase = RESCUE_FLY_HOME; // enter fly home phase
rescueState.intent.secondsFailing = 0; // reset sanity timer for flight home
rescueState.intent.proximityToLandingArea = 1.0f; // velocity iTerm activated, initialise proximity for descent phase at 1.0
@ -771,7 +796,6 @@ void gpsRescueUpdate(void)
if (rescueState.intent.yawAttenuator < 1.0f) { // be sure to accumulate full yaw authority
rescueState.intent.yawAttenuator += rescueState.sensor.gpsRescueTaskIntervalSeconds;
}
// velocity PIDs are now active
// update target velocity gradually, aiming for rescueGroundspeed with a time constant of 1.0s
const float targetVelocityError = gpsRescueConfig()->rescueGroundspeed - rescueState.intent.targetVelocityCmS;
@ -794,6 +818,8 @@ void gpsRescueUpdate(void)
// gradually gain roll capability to max of half of max pitch angle
if (newGPSData) {
// cut back on allowed angle if there is a high groundspeed error
rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.sensor.groundspeedPitchAttenuator;
if (rescueState.sensor.distanceToHomeM <= rescueState.intent.descentDistanceM) {
rescueState.phase = RESCUE_DESCENT;
rescueState.intent.secondsFailing = 0; // reset sanity timer for descent
@ -853,6 +879,11 @@ float gpsRescueGetYawRate(void)
return rescueYaw;
}
float gpsRescueGetImuYawCogGain(void)
{
return rescueState.sensor.imuYawCogGain;
}
float gpsRescueGetThrottle(void)
{
// Calculated a desired commanded throttle scaled from 0.0 to 1.0 for use in the mixer.

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

@ -57,3 +57,4 @@ bool gpsRescueIsConfigured(void);
bool gpsRescueIsAvailable(void);
bool gpsRescueIsDisabled(void);
bool gpsRescueDisableMag(void);
float gpsRescueGetImuYawCogGain(void);

33
src/main/flight/imu.c
View file

@ -201,7 +201,7 @@ static float invSqrt(float x)
static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
bool useAcc, float ax, float ay, float az,
bool useMag,
bool useCOG, float courseOverGround, const float dcmKpGain)
float cogYawGain, float courseOverGround, const float dcmKpGain)
{
static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
@ -210,17 +210,15 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
// Use raw heading error (from GPS or whatever else)
float ex = 0, ey = 0, ez = 0;
if (useCOG) {
if (cogYawGain != 0.0f) {
// Used in a GPS Rescue to boost IMU yaw gain when course over ground and velocity to home differ significantly
while (courseOverGround > M_PIf) {
courseOverGround -= (2.0f * M_PIf);
}
while (courseOverGround < -M_PIf) {
courseOverGround += (2.0f * M_PIf);
}
const float ez_ef = (- sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0]);
const float ez_ef = cogYawGain * (- sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0]);
ex = rMat[2][0] * ez_ef;
ey = rMat[2][1] * ez_ef;
ez = rMat[2][2] * ez_ef;
@ -263,7 +261,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
// Use measured acceleration vector
float recipAccNorm = sq(ax) + sq(ay) + sq(az);
if (useAcc && recipAccNorm > 0.01f) {
// Normalise accelerometer measurement
// Normalise accelerometer measurement; useAcc is true when all smoothed acc axes are within 20% of 1G
recipAccNorm = invSqrt(recipAccNorm);
ax *= recipAccNorm;
ay *= recipAccNorm;
@ -417,7 +415,7 @@ static float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAvera
} else {
ret = imuRuntimeConfig.dcm_kp;
if (!armState) {
ret = ret * 10.0f; // Scale the kP to generally converge faster when disarmed.
ret *= 10.0f; // Scale the kP to generally converge faster when disarmed.
}
}
@ -476,8 +474,8 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
static timeUs_t previousIMUUpdateTime;
bool useAcc = false;
bool useMag = false;
bool useCOG = false; // Whether or not correct yaw via imuMahonyAHRSupdate from our ground course
float courseOverGround = 0; // To be used when useCOG is true. Stored in Radians
float cogYawGain = 0.0f; // IMU yaw gain to be applied in imuMahonyAHRSupdate from ground course, default to no correction from CoG
float courseOverGround = 0; // To be used when cogYawGain is non-zero, in radians
const timeDelta_t deltaT = currentTimeUs - previousIMUUpdateTime;
previousIMUUpdateTime = currentTimeUs;
@ -495,14 +493,15 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat > GPS_MIN_SAT_COUNT && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) {
// Use GPS course over ground to correct attitude.values.yaw
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
useCOG = true;
if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
cogYawGain = gpsRescueGetImuYawCogGain(); // do not modify IMU yaw gain unless in a rescue
}
if (shouldInitializeGPSHeading()) {
// Reset our reference and reinitialize quaternion. This will likely ideally happen more than once per flight, but for now,
// Reset our reference and reinitialize quaternion.
// shouldInitializeGPSHeading() returns true only once.
imuComputeQuaternionFromRPY(&qP, attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
useCOG = false; // Don't use the COG when we first reinitialize. Next time around though, yes.
cogYawGain = 0.0f; // Don't use the COG when we first initialize
}
}
#endif
@ -512,7 +511,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
UNUSED(imuIsAccelerometerHealthy);
UNUSED(useAcc);
UNUSED(useMag);
UNUSED(useCOG);
UNUSED(cogYawGain);
UNUSED(canUseGPSHeading);
UNUSED(courseOverGround);
UNUSED(deltaT);
@ -528,13 +527,13 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
gyroAverage[axis] = gyroGetFilteredDownsampled(axis);
}
useAcc = imuIsAccelerometerHealthy(acc.accADC);
useAcc = imuIsAccelerometerHealthy(acc.accADC); // all smoothed accADC values are within 20% of 1G
imuMahonyAHRSupdate(deltaT * 1e-6f,
DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),
useAcc, acc.accADC[X], acc.accADC[Y], acc.accADC[Z],
useMag,
useCOG, courseOverGround, imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
cogYawGain, courseOverGround, imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
imuUpdateEulerAngles();
#endif

2
src/main/pg/gps_rescue.c
View file

@ -40,7 +40,7 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
.initialAltitudeM = 30,
.rescueGroundspeed = 750,
.maxRescueAngle = 70,
.maxRescueAngle = 45,
.rollMix = 150,
.pitchCutoffHz = 75,

1
src/test/unit/flight_imu_unittest.cc
View file

@ -256,6 +256,7 @@ extern "C" {
float baroUpsampleAltitude() { return 0.0f; }
float pt2FilterGain(float, float) { return 0.0f; }
float getBaroAltitude(void) { return 3000.0f; }
float gpsRescueGetImuYawCogGain(void) { return 1.0f; }
void pt2FilterInit(pt2Filter_t *baroDerivativeLpf, float) {
UNUSED(baroDerivativeLpf);