diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 6e5015b04e..13efc9727a 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -533,7 +533,7 @@ static void updateInflightCalibrationState(void) #if defined(USE_GPS) || defined(USE_MAG) void updateMagHold(void) { - if (ABS(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) { + if (fabsf(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) { int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHold; if (dif <= -180) dif += 360; @@ -989,9 +989,9 @@ static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs) if (((fabsf(pidData[FD_PITCH].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) || (fabsf(pidData[FD_ROLL].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) || (fabsf(pidData[FD_YAW].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)) - && ((ABS(gyroAbsRateDps(FD_PITCH)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP) - || (ABS(gyroAbsRateDps(FD_ROLL)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP) - || (ABS(gyroAbsRateDps(FD_YAW)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW))) { + && ((gyroAbsRateDps(FD_PITCH) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP) + || (gyroAbsRateDps(FD_ROLL) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP) + || (gyroAbsRateDps(FD_YAW) > RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW))) { if (runawayTakeoffTriggerUs == 0) { runawayTakeoffTriggerUs = currentTimeUs + RUNAWAY_TAKEOFF_ACTIVATE_DELAY; diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 34eb114944..8b046d9ca3 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -167,7 +167,7 @@ static void calculateSetpointRate(int axis) // scale rcCommandf to range [-1.0, 1.0] float rcCommandf = rcCommand[axis] / 500.0f; rcDeflection[axis] = rcCommandf; - const float rcCommandfAbs = ABS(rcCommandf); + const float rcCommandfAbs = fabsf(rcCommandf); rcDeflectionAbs[axis] = rcCommandfAbs; angleRate = applyRates(axis, rcCommandf, rcCommandfAbs); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 29d36dac8c..daaa41b24f 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -106,7 +106,7 @@ bool isUsingSticksForArming(void) bool areSticksInApModePosition(uint16_t ap_mode) { - return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode; + return fabsf(rcCommand[ROLL]) < ap_mode && fabsf(rcCommand[PITCH]) < ap_mode; } throttleStatus_e calculateThrottleStatus(void) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 4f661c529d..0025aa757f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -802,7 +802,7 @@ static float accelerationLimit(int axis, float currentPidSetpoint) static float previousSetpoint[XYZ_AXIS_COUNT]; const float currentVelocity = currentPidSetpoint - previousSetpoint[axis]; - if (ABS(currentVelocity) > maxVelocity[axis]) { + if (fabsf(currentVelocity) > maxVelocity[axis]) { currentPidSetpoint = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity[axis] : previousSetpoint[axis] - maxVelocity[axis]; } @@ -836,9 +836,9 @@ static void handleCrashRecovery( pidData[axis].I = 0.0f; if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs || (getMotorMixRange() < 1.0f - && ABS(gyro.gyroADCf[FD_ROLL]) < crashRecoveryRate - && ABS(gyro.gyroADCf[FD_PITCH]) < crashRecoveryRate - && ABS(gyro.gyroADCf[FD_YAW]) < crashRecoveryRate)) { + && fabsf(gyro.gyroADCf[FD_ROLL]) < crashRecoveryRate + && fabsf(gyro.gyroADCf[FD_PITCH]) < crashRecoveryRate + && fabsf(gyro.gyroADCf[FD_YAW]) < crashRecoveryRate)) { if (sensors(SENSOR_ACC)) { // check aircraft nearly level if (ABS(attitude.raw[FD_ROLL] - angleTrim->raw[FD_ROLL]) < crashRecoveryAngleDeciDegrees @@ -863,14 +863,14 @@ static void detectAndSetCrashRecovery( if ((crash_recovery || FLIGHT_MODE(GPS_RESCUE_MODE)) && !gyroOverflowDetected()) { if (ARMING_FLAG(ARMED)) { if (getMotorMixRange() >= 1.0f && !inCrashRecoveryMode - && ABS(delta) > crashDtermThreshold - && ABS(errorRate) > crashGyroThreshold - && ABS(getSetpointRate(axis)) < crashSetpointThreshold) { + && fabsf(delta) > crashDtermThreshold + && fabsf(errorRate) > crashGyroThreshold + && fabsf(getSetpointRate(axis)) < crashSetpointThreshold) { inCrashRecoveryMode = true; crashDetectedAtUs = currentTimeUs; } - if (inCrashRecoveryMode && cmpTimeUs(currentTimeUs, crashDetectedAtUs) < crashTimeDelayUs && (ABS(errorRate) < crashGyroThreshold - || ABS(getSetpointRate(axis)) > crashSetpointThreshold)) { + if (inCrashRecoveryMode && cmpTimeUs(currentTimeUs, crashDetectedAtUs) < crashTimeDelayUs && (fabsf(errorRate) < crashGyroThreshold + || fabsf(getSetpointRate(axis)) > crashSetpointThreshold)) { inCrashRecoveryMode = false; BEEP_OFF; } @@ -1027,7 +1027,7 @@ void FAST_CODE applySmartFeedforward(int axis) { if (smartFeedforward) { if (pidData[axis].P * pidData[axis].F > 0) { - if (ABS(pidData[axis].F) > ABS(pidData[axis].P)) { + if (fabsf(pidData[axis].F) > fabsf(pidData[axis].P)) { pidData[axis].P = 0; } else { pidData[axis].F = 0; diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 85b699581e..af4d20267e 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -1268,7 +1268,7 @@ float GPS_scaleLonDown = 1.0f; // this is used to offset the shrinking longitud void GPS_calc_longitude_scaling(int32_t lat) { - float rads = (ABS((float)lat) / 10000000.0f) * 0.0174532925f; + float rads = (fabsf((float)lat) / 10000000.0f) * 0.0174532925f; GPS_scaleLonDown = cos_approx(rads); } diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 84c5ee0638..36c754578c 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -23,6 +23,7 @@ #include #include #include +#include #include "platform.h" @@ -791,7 +792,7 @@ static void applyLedIndicatorLayer(bool updateNow, timeUs_t *timer) if (updateNow) { if (rxIsReceivingSignal()) { // calculate update frequency - int scale = MAX(ABS(rcCommand[ROLL]), ABS(rcCommand[PITCH])); // 0 - 500 + int scale = MAX(fabsf(rcCommand[ROLL]), fabsf(rcCommand[PITCH])); // 0 - 500 scale = scale - INDICATOR_DEADBAND; // start increasing frequency right after deadband *timer += HZ_TO_US(5 + (45 * scale) / (500 - INDICATOR_DEADBAND)); // 5 - 50Hz update, 2.5 - 25Hz blink diff --git a/src/main/io/pidaudio.c b/src/main/io/pidaudio.c index 0f244f2247..26be198290 100644 --- a/src/main/io/pidaudio.c +++ b/src/main/io/pidaudio.c @@ -86,19 +86,19 @@ void FAST_CODE_NOINLINE pidAudioUpdate(void) switch (pidAudioMode) { case PID_AUDIO_PIDSUM_X: { - const uint32_t pidSumX = MIN(ABS(pidData[FD_ROLL].Sum), PIDSUM_LIMIT); + const uint32_t pidSumX = MIN(fabsf(pidData[FD_ROLL].Sum), PIDSUM_LIMIT); tone = scaleRange(pidSumX, 0, PIDSUM_LIMIT, TONE_MAX, TONE_MIN); break; } case PID_AUDIO_PIDSUM_Y: { - const uint32_t pidSumY = MIN(ABS(pidData[FD_PITCH].Sum), PIDSUM_LIMIT); + const uint32_t pidSumY = MIN(fabsf(pidData[FD_PITCH].Sum), PIDSUM_LIMIT); tone = scaleRange(pidSumY, 0, PIDSUM_LIMIT, TONE_MAX, TONE_MIN); break; } case PID_AUDIO_PIDSUM_XY: { - const uint32_t pidSumXY = MIN((ABS(pidData[FD_ROLL].Sum) + ABS(pidData[FD_PITCH].Sum)) / 2, PIDSUM_LIMIT); + const uint32_t pidSumXY = MIN((fabsf(pidData[FD_ROLL].Sum) + fabsf(pidData[FD_PITCH].Sum)) / 2, PIDSUM_LIMIT); tone = scaleRange(pidSumXY, 0, PIDSUM_LIMIT, TONE_MAX, TONE_MIN); break; } diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 6c0dfccf78..53120dfd34 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -71,7 +71,7 @@ extern "C" { float getMotorMixRange(void) { return simulatedMotorMixRange; } float getSetpointRate(int axis) { return simulatedSetpointRate[axis]; } bool isAirmodeActivated() { return simulatedAirmodeEnabled; } - float getRcDeflectionAbs(int axis) { return ABS(simulatedRcDeflection[axis]); } + float getRcDeflectionAbs(int axis) { return fabsf(simulatedRcDeflection[axis]); } void systemBeep(bool) { } bool gyroOverflowDetected(void) { return false; } float getRcDeflection(int axis) { return simulatedRcDeflection[axis]; }