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

Optimize rescueAttainPosition()

Browse source 
- 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
1 changed files with 3 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

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

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