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

GPS rescue IMU, velocity iTerm and other fixes (#12900)

Browse source 
* GPS rescue update after merge of IMU fix

imuYawCog boost on error
initial turn radius adjustment
attitude debug changes
rescue velocity iterm fixes
earth referencing of yaw forced to on
sanity check failure time 30s

* vary IMU gain according to groundspeed

* Review suggestions implemented
This commit is contained in:
ctzsnooze 2023-07-06 01:51:28 +10:00 • committed by GitHub
parent 1333771140
commit 9186d05468
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
9 changed files with 80 additions and 53 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -386,7 +386,8 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_
// earthRef code here takes about 76 cycles, if conditional on angleEarthRef it takes about 100. sin_approx costs most of those cycles.
float sinAngle = sin_approx(DEGREES_TO_RADIANS(pidRuntime.angleTarget[axis == FD_ROLL ? FD_PITCH : FD_ROLL]));
sinAngle *= (axis == FD_ROLL) ? -1.0f : 1.0f; // must be negative for Roll
angleRate += pidRuntime.angleYawSetpoint * sinAngle * pidRuntime.angleEarthRef;
const float earthRefGain = FLIGHT_MODE(GPS_RESCUE_MODE) ? 1.0f : pidRuntime.angleEarthRef;
angleRate += pidRuntime.angleYawSetpoint * sinAngle * earthRefGain;
pidRuntime.angleTarget[axis] = angleTarget; // set target for alternate axis to current axis, for use in preceding calculation
// smooth final angle rate output to clean up attitude signal steps (500hz), GPS steps (10 or 100hz), RC steps etc
@ -889,9 +890,10 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// if earth referencing is requested, attenuate yaw axis setpoint when pitched or rolled
// and send yawSetpoint to Angle code to modulate pitch and roll
// code cost is 107 cycles when earthRef enabled, 20 otherwise, nearly all in cos_approx
if (pidRuntime.angleEarthRef) {
const float earthRefGain = FLIGHT_MODE(GPS_RESCUE_MODE) ? 1.0f : pidRuntime.angleEarthRef;
if (earthRefGain) {
pidRuntime.angleYawSetpoint = currentPidSetpoint;
float maxAngleTargetAbs = pidRuntime.angleEarthRef * fmaxf( fabsf(pidRuntime.angleTarget[FD_ROLL]), fabsf(pidRuntime.angleTarget[FD_PITCH]) );
float maxAngleTargetAbs = earthRefGain * fmaxf( fabsf(pidRuntime.angleTarget[FD_ROLL]), fabsf(pidRuntime.angleTarget[FD_PITCH]) );
maxAngleTargetAbs *= (FLIGHT_MODE(HORIZON_MODE)) ? horizonLevelStrength : 1.0f;
// reduce compensation whenever Horizon uses less levelling
currentPidSetpoint *= cos_approx(DEGREES_TO_RADIANS(maxAngleTargetAbs));