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

refactoring, thanks PL

Browse source 
Co-Authored-By: Petr Ledvina <2318015+ledvinap@users.noreply.github.com>
This commit is contained in:
ctzsnooze 2024-11-29 20:30:49 +11:00
parent 0bc110abb0
commit 1c8fc156f0
3 changed files with 7 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/flight/alt_hold.c
View file

@ -124,7 +124,8 @@ void altHoldUpdateTargetAltitude(void)
// otherwise it can be difficult to bring target altitude close to current altitude in a reasonable time
// using maxVelocity means the stick can bring altitude target to current within 1s
// this constrains the P and I response to user target changes, but not D of F responses
if (fabsf(getAltitudeCm() - altHold.targetAltitudeCm) < altHold.maxVelocity) {
// Range is compared to distance that might be traveled in one second
if (fabsf(getAltitudeCm() - altHold.targetAltitudeCm) < altHold.maxVelocity * 1.0 /* s */) {
altHold.targetAltitudeCm += altHold.targetVelocity * taskIntervalSeconds;
}
}

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

@ -147,15 +147,8 @@ void autopilotInit(void)
positionPidCoeffs.Ki = cfg->position_I * POSITION_I_SCALE;
positionPidCoeffs.Kd = cfg->position_D * POSITION_D_SCALE;
positionPidCoeffs.Kf = cfg->position_A * POSITION_A_SCALE; // Kf used for acceleration
// initialise filters with approximate filter gain
ap.upsampleLpfGain = pt3FilterGain(UPSAMPLING_CUTOFF_HZ, 0.01f); // 5Hz, assuming 100Hz task rate at init
resetUpsampleFilters();
// Initialise PT1 filters for deivative and acceleration in earth frame axes
ap.vaLpfCutoff = cfg->position_cutoff * 0.01f;
const float vaGain = pt1FilterGain(ap.vaLpfCutoff, 0.1f); // assume 10Hz GPS connection at start; value is overwritten before first filter use
for (unsigned i = 0; i < ARRAYLEN(ap.efAxis); i++) {
resetEFAxisFilters(&ap.efAxis[i], vaGain);
}
// initialise filters with approximate filter gains; location isn't used at this point.
resetPositionControl(&gpsSol.llh, 100);
}
void resetAltitudeControl (void) {

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

@ -664,10 +664,8 @@ void descend(bool newGpsData)
// set the altitude step, considering the interval between altitude readings and the descent rate
float altitudeStepCm = taskIntervalSeconds * gpsRescueConfig()->descendRate;
// descend faster while the quad is above 10m, to max 2.6x set value at 50m, slower below 10m to min 0.6x set value
const float descentRateMultiplier = constrainf(rescueState.intent.targetAltitudeCm / 5000.0f, 0.0f, 1.0f);
altitudeStepCm *= 0.6f + (2.0f * descentRateMultiplier);
// maximum descent rate increase is 2.6x default above 50m, 1.6x above 25m, 1.0x at 10m, 0.6x at ground level
// at or below 10m: descend at 0.6x set value; above 10m, descend faster, to max 3.0x at 50m
altitudeStepCm *= scaleRangef(constrainf(rescueState.intent.targetAltitudeCm, 1000, 5000), 1000, 5000, 0.6f, 3.0f);
rescueState.intent.targetAltitudeStepCm = -altitudeStepCm;
rescueState.intent.targetAltitudeCm -= altitudeStepCm;
@ -696,7 +694,7 @@ void gpsRescueUpdate(void)
rescueStop(); // sets phase to RESCUE_IDLE; does nothing else. RESCUE_IDLE tasks still run.
} else if (FLIGHT_MODE(GPS_RESCUE_MODE) && rescueState.phase == RESCUE_IDLE) {
rescueStart(); // sets phase to rescue_initialise if we enter GPS Rescue mode while idle
rescueAttainPosition(newGpsData); // Initialise basic parameters when a Rescue starts (can't initialise sensor data reliably)
rescueAttainPosition(false); // Initialise basic parameters when a Rescue starts (can't initialise sensor data reliably)
performSanityChecks(); // Initialises sanity check values when a Rescue starts
}
// Will now be in RESCUE_INITIALIZE mode, if just entered Rescue while IDLE, otherwise stays IDLE