diff --git a/flight/Libraries/CoordinateConversions.c b/flight/Libraries/CoordinateConversions.c index e3f3850f3..98b2aef80 100644 --- a/flight/Libraries/CoordinateConversions.c +++ b/flight/Libraries/CoordinateConversions.c @@ -31,7 +31,7 @@ #include #include "CoordinateConversions.h" -#define F_PI 3.14159265358979323846f +#define F_PI ((float)M_PI) #define RAD2DEG (180.0f/ F_PI) #define DEG2RAD (F_PI /180.0f) diff --git a/flight/Libraries/math/sin_lookup.c b/flight/Libraries/math/sin_lookup.c index 4b7fa7a8d..2d67ca162 100644 --- a/flight/Libraries/math/sin_lookup.c +++ b/flight/Libraries/math/sin_lookup.c @@ -33,6 +33,7 @@ #include "stdbool.h" #include "stdint.h" +#define F_PI ((float)M_PI) #define FLASH_TABLE #ifdef FLASH_TABLE /** Version of the code which precomputes the lookup table in flash **/ @@ -81,7 +82,7 @@ int sin_lookup_initalize() return -1; for(uint32_t i = 0; i < 180; i++) - sin_table[i] = sinf((float)i * 2 * ((float)M_PI) / 360.0f); + sin_table[i] = sinf((float)i * 2 * F_PI / 360.0f); return 0; } @@ -126,7 +127,7 @@ float cos_lookup_deg(float angle) */ float sin_lookup_rad(float angle) { - int degrees = angle * 180.0f / ((float)M_PI); + int degrees = angle * 180.0f / F_PI; return sin_lookup_deg(degrees); } @@ -137,6 +138,6 @@ float sin_lookup_rad(float angle) */ float cos_lookup_rad(float angle) { - int degrees = angle * 180.0f / ((float)M_PI); + int degrees = angle * 180.0f / F_PI; return cos_lookup_deg(degrees); } diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index 81b351737..7a669dd2c 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -67,6 +67,8 @@ #define UPDATE_RATE 25.0f #define GYRO_NEUTRAL 1665 +#define F_PI ((float)M_PI) + #define PI_MOD(x) (fmod(x + M_PI, M_PI * 2) - M_PI) // Private types @@ -513,10 +515,10 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData) // Work out time derivative from INSAlgo writeup // Also accounts for the fact that gyros are in deg/s float qdot[4]; - qdot[0] = (-q[1] * gyros[0] - q[2] * gyros[1] - q[3] * gyros[2]) * dT * (((float)M_PI) / 180 / 2); - qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * (((float)M_PI) / 180 / 2); - qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * (((float)M_PI) / 180 / 2); - qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * gyros[2]) * dT * (((float)M_PI) / 180 / 2); + qdot[0] = (-q[1] * gyros[0] - q[2] * gyros[1] - q[3] * gyros[2]) * dT * (F_PI / 180.0f / 2.0f); + qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * (F_PI / 180.0f / 2.0f); + qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * (F_PI / 180.0f / 2.0f); + qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * gyros[2]) * dT * (F_PI / 180.0f / 2.0f); // Take a time step q[0] = q[0] + qdot[0];