Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 22:05:17 +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))
#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))

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

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

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

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

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