Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 13:55:18 +03:00
Code Issues Wiki Activity

Switched to explicitly using M_PI.

Browse source
This commit is contained in:
Pierre Hugo 2015-01-21 19:20:19 -08:00
parent 3e6e8f44c2
commit 6711c56dc5
5 changed files with 13 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

9
src/main/common/maths.h
View file

@ -21,13 +21,10 @@
#define sq(x) ((x)*(x)) #define sq(x) ((x)*(x))
#endif #endif
#ifdef M_PI // Use floating point M_PI instead explicitly.
// M_PI should be float, but previous definition may be double #define M_PIf 3.14159265358979323846f
# undef M_PI
#endif
#define M_PI 3.14159265358979323846f
#define RAD (M_PI / 180.0f) #define RAD (M_PIf / 180.0f)
#define min(a, b) ((a) < (b) ? (a) : (b)) #define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) > (b) ? (a) : (b)) #define max(a, b) ((a) > (b) ? (a) : (b))

2
src/main/drivers/accgyro_spi_mpu6000.c
View file

@ -319,7 +319,7 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
gyro->read = mpu6000SpiGyroRead; gyro->read = mpu6000SpiGyroRead;
// 16.4 dps/lsb scalefactor // 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f; 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); delay(100);
return true; return true;
} }

2
src/main/drivers/accgyro_spi_mpu6500.c
View file

@ -128,7 +128,7 @@ bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
// 16.4 dps/lsb scalefactor // 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f; 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 // default lpf is 42Hz
if (lpf >= 188) if (lpf >= 188)

14
src/main/flight/imu.c
View file

@ -90,17 +90,17 @@ void imuInit()
{ {
smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle))); smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle)));
accVelScale = 9.80665f / acc_1G / 10000.0f; 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) 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) 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) 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 sinePitch = sinf(anglerad[AI_PITCH]);
float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll; 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 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); head = lrintf(hd);
if (head < 0) if (head < 0)
head += 360; head += 360;
@ -318,8 +318,8 @@ static void getEstimatedAttitude(void)
// Attitude of the estimated vector // Attitude of the estimated vector
anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z); 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)); 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.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PIf));
inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI)); inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PIf));
if (sensors(SENSOR_MAG)) { if (sensors(SENSOR_MAG)) {
rotateV(&EstM.V, &deltaGyroAngle); rotateV(&EstM.V, &deltaGyroAngle);
@ -349,5 +349,5 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
int angle = lrintf(acosf(cosZ) * throttleAngleScale); int angle = lrintf(acosf(cosZ) * throttleAngleScale);
if (angle > 900) if (angle > 900)
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)));
} }

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

@ -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 // Low pass filter cut frequency for derivative calculation
// Set to "1 / ( 2 * PI * gps_lpf ) // 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 // discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy // high frequency noise that can drive the controller crazy
pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative); pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative);