refactoring, thanks PL

Co-Authored-By: Petr Ledvina <2318015+ledvinap@users.noreply.github.com>

src/main/flight/alt_hold.c

@@ -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;
     }
 }

src/main/flight/autopilot.c

@@ -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
+    // initialise filters with approximate filter gains; location isn't used at this point.
-    ap.upsampleLpfGain = pt3FilterGain(UPSAMPLING_CUTOFF_HZ, 0.01f); // 5Hz, assuming 100Hz task rate at init
+    resetPositionControl(&gpsSol.llh, 100);
-    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);
-    }
 }
 
 void resetAltitudeControl (void) {

src/main/flight/gps_rescue.c

@@ -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
+    // at or below 10m: descend at 0.6x set value; above 10m, descend faster, to max 3.0x at 50m
-    const float descentRateMultiplier = constrainf(rescueState.intent.targetAltitudeCm / 5000.0f, 0.0f, 1.0f);
+    altitudeStepCm *= scaleRangef(constrainf(rescueState.intent.targetAltitudeCm, 1000, 5000), 1000, 5000, 0.6f, 3.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
 
     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