diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 0e773cd474..a4014b730f 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -21,13 +21,10 @@ #define sq(x) ((x)*(x)) #endif -#ifdef M_PI -// M_PI should be float, but previous definition may be double -# undef M_PI -#endif -#define M_PI 3.14159265358979323846f +// Use floating point M_PI instead explicitly. +#define M_PIf 3.14159265358979323846f -#define RAD (M_PI / 180.0f) +#define RAD (M_PIf / 180.0f) #define min(a, b) ((a) < (b) ? (a) : (b)) #define max(a, b) ((a) > (b) ? (a) : (b)) diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index 590e6e922a..8795bc54da 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -319,7 +319,7 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf) gyro->read = mpu6000SpiGyroRead; // 16.4 dps/lsb scalefactor gyro->scale = 1.0f / 16.4f; - //gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f; + //gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f; delay(100); return true; } diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 123ac08c35..7b3893cc1d 100644 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -128,7 +128,7 @@ bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf) // 16.4 dps/lsb scalefactor gyro->scale = 1.0f / 16.4f; - //gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f; + //gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f; // default lpf is 42Hz if (lpf >= 188) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index abcd2a3410..e989308d29 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -90,17 +90,17 @@ void imuInit() { smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle))); accVelScale = 9.80665f / acc_1G / 10000.0f; - gyroScaleRad = gyro.scale * (M_PI / 180.0f) * 0.000001f; + gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f; } void calculateThrottleAngleScale(uint16_t throttle_correction_angle) { - throttleAngleScale = (1800.0f / M_PI) * (900.0f / throttle_correction_angle); + throttleAngleScale = (1800.0f / M_PIf) * (900.0f / throttle_correction_angle); } void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) { - fc_acc = 0.5f / (M_PI * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf + fc_acc = 0.5f / (M_PIf * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf } void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) @@ -259,7 +259,7 @@ static int16_t calculateHeading(t_fp_vector *vec) float sinePitch = sinf(anglerad[AI_PITCH]); float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll; float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll; - float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f; + float hd = (atan2f(Yh, Xh) * 1800.0f / M_PIf + magneticDeclination) / 10.0f; head = lrintf(hd); if (head < 0) head += 360; @@ -318,8 +318,8 @@ static void getEstimatedAttitude(void) // Attitude of the estimated vector anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z); anglerad[AI_PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)); - inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI)); - inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI)); + inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PIf)); + inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PIf)); if (sensors(SENSOR_MAG)) { rotateV(&EstM.V, &deltaGyroAngle); @@ -349,5 +349,5 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) int angle = lrintf(acosf(cosZ) * throttleAngleScale); if (angle > 900) angle = 900; - return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f))); + return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PIf / 2.0f))); } diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index 9eb5627e79..dd09430562 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -163,7 +163,7 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param) // Low pass filter cut frequency for derivative calculation // Set to "1 / ( 2 * PI * gps_lpf ) - float pidFilter = (1.0f / (2.0f * M_PI * (float)gpsProfile->gps_lpf)); + float pidFilter = (1.0f / (2.0f * M_PIf * (float)gpsProfile->gps_lpf)); // discrete low pass filter, cuts out the // high frequency noise that can drive the controller crazy pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative);