1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00

Optimize rescueAttainPosition()

- Fix previousSpeedError type to avoid unnecessary float conversion
- Fix unnecessary static scalingRate variable for better optimisation,
  and reorganise usage for clarity
This commit is contained in:
Mathias Rasmussen 2021-12-03 06:38:40 +01:00
parent 2a954c6203
commit fc024cdacc

View file

@ -282,12 +282,10 @@ static void setBearing(int16_t desiredHeading)
static void rescueAttainPosition()
{
// Speed and altitude controller internal variables
static float previousSpeedError = 0;
static int16_t previousSpeedError = 0;
static int16_t speedIntegral = 0;
int zVelocityError;
static int previousZVelocityError = 0;
static float zVelocityIntegral = 0;
static float scalingRate = 0;
static int16_t altitudeAdjustment = 0;
if (rescueState.phase == RESCUE_INITIALIZE) {
@ -332,12 +330,14 @@ static void rescueAttainPosition()
const int16_t altitudeError = rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm;
// P component
float scalingRate;
if (ABS(altitudeError) > 0 && ABS(altitudeError) < GPS_RESCUE_ZVELOCITY_THRESHOLD) {
scalingRate = (float)altitudeError / GPS_RESCUE_ZVELOCITY_THRESHOLD;
} else {
scalingRate = 1;
}
int zVelocityError;
if (altitudeError > 0) {
zVelocityError = gpsRescueConfig()->ascendRate * scalingRate - rescueState.sensor.zVelocity;
} else if (altitudeError < 0) {