From 87c259a26e92ace7ecad1c38d24dc259b4c395d6 Mon Sep 17 00:00:00 2001 From: Mathias Rasmussen Date: Fri, 3 Dec 2021 03:58:19 +0100 Subject: [PATCH] Fix use of floating point math functions --- lib/main/MAVLink/mavlink_conversions.h | 2 +- src/main/flight/gps_rescue.c | 2 +- src/main/flight/mixer.c | 4 ++-- src/main/flight/pid.c | 5 +++-- src/main/io/vtx_smartaudio.c | 2 +- src/main/sensors/gyro.c | 6 +++--- src/test/unit/flight_imu_unittest.cc | 2 +- src/test/unit/maths_unittest.cc | 14 +++++++------- src/test/unit/pid_unittest.cc | 2 +- 9 files changed, 20 insertions(+), 19 deletions(-) diff --git a/lib/main/MAVLink/mavlink_conversions.h b/lib/main/MAVLink/mavlink_conversions.h index 2fb1f11ad..a99c54142 100644 --- a/lib/main/MAVLink/mavlink_conversions.h +++ b/lib/main/MAVLink/mavlink_conversions.h @@ -68,7 +68,7 @@ MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float d MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw) { float phi, theta, psi; - theta = asin(-dcm[2][0]); + theta = asinf(-dcm[2][0]); if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) { phi = 0.0f; diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 30ce7e907..7e95379f0 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -324,7 +324,7 @@ static void rescueAttainPosition() gpsRescueAngle[AI_PITCH] = constrain(gpsRescueAngle[AI_PITCH] + MIN(angleAdjustment, 80), rescueState.intent.minAngleDeg * 100, rescueState.intent.maxAngleDeg * 100); - const float ct = cos(DECIDEGREES_TO_RADIANS(gpsRescueAngle[AI_PITCH] / 10)); + const float ct = cosf(DECIDEGREES_TO_RADIANS(gpsRescueAngle[AI_PITCH] / 10)); /** Altitude controller diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 4703919b0..b9e0ad992 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -366,9 +366,9 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t motorOutput = (motorOutput < motorRangeMin) ? mixerRuntime.disarmMotorOutput : motorOutput; // Prevent getting into special reserved range } #endif - motorOutput = constrain(motorOutput, mixerRuntime.disarmMotorOutput, motorRangeMax); + motorOutput = constrainf(motorOutput, mixerRuntime.disarmMotorOutput, motorRangeMax); } else { - motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax); + motorOutput = constrainf(motorOutput, motorRangeMin, motorRangeMax); } motor[i] = motorOutput; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9624a1ab5..9fbdef792 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1233,13 +1233,14 @@ bool pidAntiGravityEnabled(void) #ifdef USE_DYN_LPF void dynLpfDTermUpdate(float throttle) { - unsigned int cutoffFreq; if (pidRuntime.dynLpfFilter != DYN_LPF_NONE) { + float cutoffFreq; if (pidRuntime.dynLpfCurveExpo > 0) { cutoffFreq = dynLpfCutoffFreq(throttle, pidRuntime.dynLpfMin, pidRuntime.dynLpfMax, pidRuntime.dynLpfCurveExpo); } else { - cutoffFreq = fmax(dynThrottle(throttle) * pidRuntime.dynLpfMax, pidRuntime.dynLpfMin); + cutoffFreq = fmaxf(dynThrottle(throttle) * pidRuntime.dynLpfMax, pidRuntime.dynLpfMin); } + switch (pidRuntime.dynLpfFilter) { case DYN_LPF_PT1: for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index fbb0abc71..f65d91d07 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -1036,7 +1036,7 @@ static uint8_t vtxSAGetPowerLevels(const vtxDevice_t *vtxDevice, uint16_t *level for (uint8_t i = 0; i < saSupportedNumPowerLevels; i++) { levels[i] = saSupportedPowerValues[i]; - uint16_t power = (uint16_t)pow(10.0,levels[i]/10.0); + uint16_t power = (uint16_t)powf(10.0f, levels[i] / 10.0f); if (levels[i] > 14) { // For powers greater than 25mW round up to a multiple of 50 to match expectations diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 28af11f61..abccba6d9 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -619,13 +619,13 @@ float dynThrottle(float throttle) { void dynLpfGyroUpdate(float throttle) { if (gyro.dynLpfFilter != DYN_LPF_NONE) { - unsigned int cutoffFreq; + float cutoffFreq; if (gyro.dynLpfCurveExpo > 0) { cutoffFreq = dynLpfCutoffFreq(throttle, gyro.dynLpfMin, gyro.dynLpfMax, gyro.dynLpfCurveExpo); } else { - cutoffFreq = fmax(dynThrottle(throttle) * gyro.dynLpfMax, gyro.dynLpfMin); + cutoffFreq = fmaxf(dynThrottle(throttle) * gyro.dynLpfMax, gyro.dynLpfMin); } - DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq); + DEBUG_SET(DEBUG_DYN_LPF, 2, lrintf(cutoffFreq)); const float gyroDt = gyro.targetLooptime * 1e-6f; switch (gyro.dynLpfFilter) { case DYN_LPF_PT1: diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 5c04c02e8..59c59b504 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -72,7 +72,7 @@ extern "C" { #include "unittest_macros.h" #include "gtest/gtest.h" -const float sqrt2over2 = sqrt(2) / 2.0f; +const float sqrt2over2 = sqrtf(2) / 2.0f; TEST(FlightImuTest, TestCalculateRotationMatrix) { diff --git a/src/test/unit/maths_unittest.cc b/src/test/unit/maths_unittest.cc index 6ba779e20..a96c589d8 100644 --- a/src/test/unit/maths_unittest.cc +++ b/src/test/unit/maths_unittest.cc @@ -207,8 +207,8 @@ TEST(MathsUnittest, TestFastTrigonometrySinCos) { double sinError = 0; for (float x = -10 * M_PI; x < 10 * M_PI; x += M_PI / 300) { - double approxResult = sin_approx(x); - double libmResult = sinf(x); + float approxResult = sin_approx(x); + double libmResult = sin(x); sinError = MAX(sinError, fabs(approxResult - libmResult)); } printf("sin_approx maximum absolute error = %e\n", sinError); @@ -216,8 +216,8 @@ TEST(MathsUnittest, TestFastTrigonometrySinCos) double cosError = 0; for (float x = -10 * M_PI; x < 10 * M_PI; x += M_PI / 300) { - double approxResult = cos_approx(x); - double libmResult = cosf(x); + float approxResult = cos_approx(x); + double libmResult = cos(x); cosError = MAX(cosError, fabs(approxResult - libmResult)); } printf("cos_approx maximum absolute error = %e\n", cosError); @@ -229,8 +229,8 @@ TEST(MathsUnittest, TestFastTrigonometryATan2) double error = 0; for (float x = -1.0f; x < 1.0f; x += 0.01) { for (float y = -1.0f; x < 1.0f; x += 0.001) { - double approxResult = atan2_approx(y, x); - double libmResult = atan2f(y, x); + float approxResult = atan2_approx(y, x); + double libmResult = atan2(y, x); error = MAX(error, fabs(approxResult - libmResult)); } } @@ -242,7 +242,7 @@ TEST(MathsUnittest, TestFastTrigonometryACos) { double error = 0; for (float x = -1.0f; x < 1.0f; x += 0.001) { - double approxResult = acos_approx(x); + float approxResult = acos_approx(x); double libmResult = acos(x); error = MAX(error, fabs(approxResult - libmResult)); } diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index dd68b1781..7e3b6d663 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -224,7 +224,7 @@ void setStickPosition(int axis, float stickRatio) { // All calculations will have 10% tolerance float calculateTolerance(float input) { - return fabs(input * 0.1f); + return fabsf(input * 0.1f); } TEST(pidControllerTest, testInitialisation)