Replace ABS(float) with fabsf(float)

Disregarding loads and stores, fabfs() compiles to a single vabs.f32
instruction, which completes in 1 cycle. ABS(float), however, compiles
to vcmpe.f32, a branch and vneg.f32, which needs 2 cycles + branching.
The compiler is not able to perform this transformation because
(f < 0 ? -f : f) doesn't always yield the absolute value for
floats (e.g. ABS(-0) will yield -0).

src/main/flight/pid.c

@@ -501,7 +501,7 @@ static void pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamic
     if (STATE(ANTI_WINDUP) || isFixedWingItermLimitActive(pidState->stickPosition)) {
         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
     } else {
-        pidState->errorGyroIfLimit = ABS(pidState->errorGyroIf);
+        pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
     }
 
     if (pidProfile()->fixedWingItermThrowLimit != 0) {
@@ -582,7 +582,7 @@ static void pidApplyMulticopterRateController(pidState_t *pidState, flight_dynam
     if (STATE(ANTI_WINDUP) || mixerIsOutputSaturated()) {
         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
     } else {
-        pidState->errorGyroIfLimit = ABS(pidState->errorGyroIf);
+        pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
     }
 
     axisPID[axis] = newOutputLimited;

src/main/io/osd.c

@@ -1692,7 +1692,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 }
             }
 
-            if (ABS(ky) < ABS(kx)) {
+            if (fabsf(ky) < fabsf(kx)) {
 
                 previous_orient = 0;
 

src/main/navigation/navigation.c

@@ -1106,8 +1106,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
     // If we have valid pos sensor OR we are configured to ignore GPS loss
     if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
         const float rthAltitudeMargin = STATE(FIXED_WING) ?
-                MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (FW_RTH_CLIMB_MARGIN_PERCENT/100.0) * ABS(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)) :  // Airplane
+                MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (FW_RTH_CLIMB_MARGIN_PERCENT/100.0) * fabsf(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)) :  // Airplane
-                MAX(MR_RTH_CLIMB_MARGIN_MIN_CM, (MR_RTH_CLIMB_MARGIN_PERCENT/100.0) * ABS(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z));   // Copters
+                MAX(MR_RTH_CLIMB_MARGIN_MIN_CM, (MR_RTH_CLIMB_MARGIN_PERCENT/100.0) * fabsf(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z));   // Copters
 
         if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.homeWaypointAbove.pos.z) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) {
             // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
@@ -1667,7 +1667,7 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
 
         if (pidFlags & PID_SHRINK_INTEGRATOR) {
             // Only allow integrator to shrink
-            if (ABS(newIntegrator) < ABS(pid->integrator)) {
+            if (fabsf(newIntegrator) < fabsf(pid->integrator)) {
                 pid->integrator = newIntegrator;
             }
         }
@@ -3311,7 +3311,7 @@ void onNewGPSData(void)
         GPS_home.alt = gpsSol.llh.alt;
         GPS_distanceToHome = 0;
         GPS_directionToHome = 0;
-        GPS_scaleLonDown = cos_approx((ABS((float)gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f);
+        GPS_scaleLonDown = cos_approx((fabsf((float)gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f);
     }
 }
 

src/main/navigation/navigation_fixedwing.c

@@ -410,7 +410,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
                 float velThrottleBoost = (NAV_FW_MIN_VEL_SPEED_BOOST - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate);
 
                 // If we are in the deadband of 50cm/s - don't update speed boost
-                if (ABS(posControl.actualState.velXY - NAV_FW_MIN_VEL_SPEED_BOOST) > 50) {
+                if (fabsf(posControl.actualState.velXY - NAV_FW_MIN_VEL_SPEED_BOOST) > 50) {
                     throttleSpeedAdjustment += velThrottleBoost;
                 }
 

src/main/navigation/navigation_fw_launch.c

@@ -76,7 +76,7 @@ static FixedWingLaunchState_t   launchState;
 #define SWING_LAUNCH_MIN_ROTATION_RATE      DEGREES_TO_RADIANS(100)     // expect minimum 100dps rotation rate
 static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs)
 {
-    const float swingVelocity = (ABS(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0;
+    const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0;
     const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.x > navConfig()->fw.launch_accel_thresh);
     const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle)));
 

src/main/navigation/navigation_multicopter.c

@@ -76,7 +76,7 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
 
     // limit max vertical acceleration to 1/5G (~200 cm/s/s) if we are increasing velocity.
     // if we are decelerating - don't limit (allow better recovery from falling)
-    if (ABS(targetVel) > ABS(posControl.desiredState.vel.z)) {
+    if (fabsf(targetVel) > fabsf(posControl.desiredState.vel.z)) {
         const float maxVelDifference = US2S(deltaMicros) * (GRAVITY_CMSS / 5.0f);
         posControl.desiredState.vel.z = constrainf(targetVel, posControl.desiredState.vel.z - maxVelDifference, posControl.desiredState.vel.z + maxVelDifference);
     }

src/main/navigation/navigation_pos_estimator.c

@@ -382,7 +382,7 @@ static void updateIMUTopic(void)
             const float gravityOffsetError = posEstimator.imu.accelNEU.z - calibratedGravityCMSS;
             calibratedGravityCMSS += gravityOffsetError * 0.0025f;
 
-            if (ABS(gravityOffsetError) < positionEstimationConfig()->gravity_calibration_tolerance) {  // Error should be within 0.5% of calibrated gravity
+            if (fabsf(gravityOffsetError) < positionEstimationConfig()->gravity_calibration_tolerance) {  // Error should be within 0.5% of calibrated gravity
                 if ((millis() - gravityCalibrationTimeout) > 250) {
                     posEstimator.imu.gravityCalibrationComplete = true;
                 }

src/main/sensors/acceleration.c

@@ -532,7 +532,7 @@ void accUpdate(void)
     }
 
     // Before filtering check for clipping and vibration levels
-    if (ABS(acc.accADCf[X]) > ACC_CLIPPING_THRESHOLD_G || ABS(acc.accADCf[Y]) > ACC_CLIPPING_THRESHOLD_G || ABS(acc.accADCf[Z]) > ACC_CLIPPING_THRESHOLD_G) {
+    if (fabsf(acc.accADCf[X]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Y]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Z]) > ACC_CLIPPING_THRESHOLD_G) {
         acc.accClipCount++;
     }
 

src/main/sensors/barometer.c

@@ -280,7 +280,7 @@ static void performBaroCalibrationCycle(void)
     const float baroGroundPressureError = baro.baroPressure - baroGroundPressure;
     baroGroundPressure += baroGroundPressureError * 0.15f;
 
-    if (ABS(baroGroundPressureError) < (baroGroundPressure * 0.00005f)) {    // 0.005% calibration error (should give c. 10cm calibration error)
+    if (fabsf(baroGroundPressureError) < (baroGroundPressure * 0.00005f)) {    // 0.005% calibration error (should give c. 10cm calibration error)
         if ((millis() - baroCalibrationTimeout) > 250) {
             baroGroundAltitude = pressureToAltitude(baroGroundPressure);
             baroCalibrationFinished = true;

src/main/sensors/pitotmeter.c

@@ -167,7 +167,7 @@ static void performPitotCalibrationCycle(void)
     const float pitotPressureZeroError = pitot.pressure - pitot.pressureZero;
     pitot.pressureZero += pitotPressureZeroError * 0.25f;
 
-    if (ABS(pitotPressureZeroError) < (P0 * 0.000005f)) {
+    if (fabsf(pitotPressureZeroError) < (P0 * 0.000005f)) {
         if ((millis() - pitot.calibrationTimeoutMs) > 500) {
             pitot.calibrationFinished = true;
         }