Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 14:25:16 +03:00
Code Issues Wiki Activity

Replace ABS(float) with fabsf(float)

Browse source 
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).
This commit is contained in:
Alberto García Hierro 2019-01-19 17:10:14 +00:00
parent b0d285d66d
commit 0f4e74cd04
10 changed files with 14 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/flight/pid.c
View file

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

2
src/main/io/osd.c
View file

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

8
src/main/navigation/navigation.c
View file

@ -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(MR_RTH_CLIMB_MARGIN_MIN_CM, (MR_RTH_CLIMB_MARGIN_PERCENT/100.0) * ABS(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)); // Copters
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) * 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);
}
}

2
src/main/navigation/navigation_fixedwing.c
View file

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

2
src/main/navigation/navigation_fw_launch.c
View file

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

2
src/main/navigation/navigation_multicopter.c
View file

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

2
src/main/navigation/navigation_pos_estimator.c
View file

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

2
src/main/sensors/acceleration.c
View file

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

2
src/main/sensors/barometer.c
View file

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

2
src/main/sensors/pitotmeter.c
View file

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