diff --git a/flight/Libraries/CoordinateConversions.c b/flight/Libraries/CoordinateConversions.c
index 98b2aef80..7fc0ed5c2 100644
--- a/flight/Libraries/CoordinateConversions.c
+++ b/flight/Libraries/CoordinateConversions.c
@@ -29,11 +29,10 @@
 
 #include <math.h>
 #include <stdint.h>
+#include <pios_math.h>
 #include "CoordinateConversions.h"
 
-#define F_PI ((float)M_PI)
-#define RAD2DEG (180.0f/ F_PI)
-#define DEG2RAD (F_PI /180.0f)
+#define MIN_ALLOWABLE_MAGNITUDE 1e-30f
 
 // ****** convert Lat,Lon,Alt to ECEF  ************
 void LLA2ECEF(float LLA[3], float ECEF[3])
@@ -43,10 +42,10 @@ void LLA2ECEF(float LLA[3], float ECEF[3])
 	float sinLat, sinLon, cosLat, cosLon;
 	float N;
 
-	sinLat = sinf(DEG2RAD * LLA[0]);
-	sinLon = sinf(DEG2RAD * LLA[1]);
-	cosLat = cosf(DEG2RAD * LLA[0]);
-	cosLon = cosf(DEG2RAD * LLA[1]);
+	sinLat = sinf(DEG2RAD(LLA[0]));
+	sinLon = sinf(DEG2RAD(LLA[1]));
+	cosLat = cosf(DEG2RAD(LLA[0]));
+	cosLon = cosf(DEG2RAD(LLA[1]));
 
 	N = a / sqrtf(1.0f - e * e * sinLat * sinLat);	//prime vertical radius of curvature
 
@@ -75,8 +74,8 @@ uint16_t ECEF2LLA(float ECEF[3], float LLA[3])
 #define MAX_ITER 10		// should not take more than 5 for valid coordinates
 #define ACCURACY 1.0e-11f	// used to be e-14, but we don't need sub micrometer exact calculations
 
-	LLA[1] = RAD2DEG * atan2f(y, x);
-	Lat = DEG2RAD * LLA[0];
+	LLA[1] = RAD2DEG(atan2f(y, x));
+	Lat = DEG2RAD(LLA[0]);
 	esLat = e * sinf(Lat);
 	N = a / sqrtf(1 - esLat * esLat);
 	NplusH = N + LLA[2];
@@ -93,7 +92,7 @@ uint16_t ECEF2LLA(float ECEF[3], float LLA[3])
 		iter += 1;
 	}
 
-	LLA[0] = RAD2DEG * Lat;
+	LLA[0] = RAD2DEG(Lat);
 	LLA[2] = NplusH - N;
 
 	return (iter < MAX_ITER);
@@ -104,10 +103,10 @@ void RneFromLLA(float LLA[3], float Rne[3][3])
 {
 	float sinLat, sinLon, cosLat, cosLon;
 
-	sinLat = (float)sinf(DEG2RAD * LLA[0]);
-	sinLon = (float)sinf(DEG2RAD * LLA[1]);
-	cosLat = (float)cosf(DEG2RAD * LLA[0]);
-	cosLon = (float)cosf(DEG2RAD * LLA[1]);
+	sinLat = (float)sinf(DEG2RAD(LLA[0]));
+	sinLon = (float)sinf(DEG2RAD(LLA[1]));
+	cosLat = (float)cosf(DEG2RAD(LLA[0]));
+	cosLon = (float)cosf(DEG2RAD(LLA[1]));
 
 	Rne[0][0] = -sinLat * cosLon;
 	Rne[0][1] = -sinLat * sinLon;
@@ -135,9 +134,9 @@ void Quaternion2RPY(const float q[4], float rpy[3])
 	R23 = 2.0f * (q[2] * q[3] + q[0] * q[1]);
 	R33 = q0s - q1s - q2s + q3s;
 
-	rpy[1] = RAD2DEG * asinf(-R13);	// pitch always between -pi/2 to pi/2
-	rpy[2] = RAD2DEG * atan2f(R12, R11);
-	rpy[0] = RAD2DEG * atan2f(R23, R33);
+	rpy[1] = RAD2DEG(asinf(-R13));	// pitch always between -pi/2 to pi/2
+	rpy[2] = RAD2DEG(atan2f(R12, R11));
+	rpy[0] = RAD2DEG(atan2f(R23, R33));
 
 	//TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2
 }
@@ -148,9 +147,9 @@ void RPY2Quaternion(const float rpy[3], float q[4])
 	float phi, theta, psi;
 	float cphi, sphi, ctheta, stheta, cpsi, spsi;
 
-	phi = DEG2RAD * rpy[0] / 2;
-	theta = DEG2RAD * rpy[1] / 2;
-	psi = DEG2RAD * rpy[2] / 2;
+	phi = DEG2RAD(rpy[0] / 2);
+	theta = DEG2RAD(rpy[1] / 2);
+	psi = DEG2RAD(rpy[2] / 2);
 	cphi = cosf(phi);
 	sphi = sinf(phi);
 	ctheta = cosf(theta);
@@ -294,13 +293,13 @@ uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[
 
 	// The first rows of rot matrices chosen in direction of v1
 	mag = VectorMagnitude(v1b);
-	if (fabsf(mag) < 1e-30f)
+	if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE)
 		return (-1);
 	for (i=0;i<3;i++)
 		Rib[0][i]=v1b[i]/mag;
 
 	mag = VectorMagnitude(v1e);
-	if (fabsf(mag) < 1e-30f)
+	if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE)
 		return (-1);
 	for (i=0;i<3;i++)
 		Rie[0][i]=v1e[i]/mag;
@@ -308,14 +307,14 @@ uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[
 	// The second rows of rot matrices chosen in direction of v1xv2
 	CrossProduct(v1b,v2b,&Rib[1][0]);
 	mag = VectorMagnitude(&Rib[1][0]);
-	if (fabsf(mag) < 1e-30f)
+	if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE)
 		return (-1);
 	for (i=0;i<3;i++)
 		Rib[1][i]=Rib[1][i]/mag;
 
 	CrossProduct(v1e,v2e,&Rie[1][0]);
 	mag = VectorMagnitude(&Rie[1][0]);
-	if (fabsf(mag) < 1e-30f)
+	if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE)
 		return (-1);
 	for (i=0;i<3;i++)
 		Rie[1][i]=Rie[1][i]/mag;
diff --git a/flight/Libraries/inc/WMMInternal.h b/flight/Libraries/inc/WMMInternal.h
index d09913fc9..5604a0773 100644
--- a/flight/Libraries/inc/WMMInternal.h
+++ b/flight/Libraries/inc/WMMInternal.h
@@ -26,6 +26,7 @@
 
 #ifndef WMMINTERNAL_H_
 #define WMMINTERNAL_H_
+#include <pios_math.h>
 
 	// internal constants
 #define TRUE            ((uint16_t)1)
@@ -35,9 +36,6 @@
 #define	NUMTERMS 91		// ((WMM_MAX_MODEL_DEGREES+1)*(WMM_MAX_MODEL_DEGREES+2)/2);
 #define NUMPCUP 92		// NUMTERMS +1
 #define NUMPCUPS 13		// WMM_MAX_MODEL_DEGREES +1
-#define M_PI_F ((float)M_PI)
-#define RAD2DEG(rad)    ((rad)*(180.0f/M_PI_F))
-#define DEG2RAD(deg)    ((deg)*(M_PI_F/180.0f))
 
 	// internal structure definitions
 typedef struct {
diff --git a/flight/Libraries/math/sin_lookup.c b/flight/Libraries/math/sin_lookup.c
index 2d67ca162..b49d500ea 100644
--- a/flight/Libraries/math/sin_lookup.c
+++ b/flight/Libraries/math/sin_lookup.c
@@ -32,8 +32,8 @@
 #include "math.h"
 #include "stdbool.h"
 #include "stdint.h"
+#include <pios_math.h>
 
-#define F_PI ((float)M_PI)
 #define FLASH_TABLE
 #ifdef FLASH_TABLE
  /** Version of the code which precomputes the lookup table in flash **/
@@ -82,7 +82,7 @@ int sin_lookup_initalize()
 		return -1;
 
 	for(uint32_t i = 0; i < 180; i++)
-		sin_table[i] = sinf((float)i * 2 * F_PI / 360.0f);
+		sin_table[i] = sinf((float)i * 2 * M_PI_F / 360.0f);
 
 	return 0;
 }
@@ -127,7 +127,7 @@ float cos_lookup_deg(float angle)
  */
 float sin_lookup_rad(float angle)
 {
-	int degrees = angle * 180.0f / F_PI;
+	int degrees = angle * 180.0f / M_PI_F;
 	return sin_lookup_deg(degrees);
 }
 
@@ -138,6 +138,6 @@ float sin_lookup_rad(float angle)
  */
 float cos_lookup_rad(float angle)
 {
-	int degrees = angle * 180.0f / F_PI;
+	int degrees = angle * 180.0f / M_PI_F;
 	return cos_lookup_deg(degrees);
 }
diff --git a/flight/Libraries/paths.c b/flight/Libraries/paths.c
index 353ae9b10..b8e9f804c 100644
--- a/flight/Libraries/paths.c
+++ b/flight/Libraries/paths.c
@@ -94,7 +94,7 @@ static void path_endpoint( float * start_point, float * end_point, float * cur_p
 	dist_diff = sqrtf( diff_north * diff_north + diff_east * diff_east );
 	dist_path = sqrtf( path_north * path_north + path_east * path_east );
 
-	if(dist_diff < 1e-6 ) {
+	if(dist_diff < 1e-6f ) {
 		status->fractional_progress = 1;
 		status->error = 0;
 		status->path_direction[0] = status->path_direction[1] = 0;
@@ -135,7 +135,7 @@ static void path_vector( float * start_point, float * end_point, float * cur_poi
 	dot = path_north * diff_north + path_east * diff_east;
 	dist_path = sqrtf( path_north * path_north + path_east * path_east );
 
-	if(dist_path < 1e-6) {
+	if(dist_path < 1e-6f) {
 		// if the path is too short, we cannot determine vector direction.
 		// Fly towards the endpoint to prevent flying away,
 		// but assume progress=1 either way.
@@ -188,7 +188,7 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin
 	radius = sqrtf( radius_north * radius_north + radius_east * radius_east );
 	cradius = sqrtf(  diff_north * diff_north   +   diff_east * diff_east );
 
-	if (cradius < 1e-6) {
+	if (cradius < 1e-6f) {
 		// cradius is zero, just fly somewhere and make sure correction is still a normal
 		status->fractional_progress = 1;
 		status->error = radius;
diff --git a/flight/Modules/Airspeed/revolution/gps_airspeed.c b/flight/Modules/Airspeed/revolution/gps_airspeed.c
index c91a6f8a6..ec85d10d5 100644
--- a/flight/Modules/Airspeed/revolution/gps_airspeed.c
+++ b/flight/Modules/Airspeed/revolution/gps_airspeed.c
@@ -35,7 +35,7 @@
 #include "gpsvelocity.h"
 #include "attitudeactual.h"
 #include "CoordinateConversions.h"
-
+#include <pios_math.h>
 
 
 // Private constants
@@ -44,9 +44,6 @@
 #define SAMPLING_DELAY_MS_GPS          100    //Needs to be settable in a UAVO
 #define GPS_AIRSPEED_TIME_CONSTANT_MS  500.0f //Needs to be settable in a UAVO
 
-#define F_PI 3.141526535897932f
-#define DEG2RAD (F_PI/180.0f)
-
 // Private types
 struct GPSGlobals {
 	float RbeCol1_old[3];
@@ -117,7 +114,7 @@ void gps_airspeedGet(float *v_air_GPS)
 	float cosDiff=(Rbe[0][0]*gps->RbeCol1_old[0]) + (Rbe[0][1]*gps->RbeCol1_old[1]) + (Rbe[0][2]*gps->RbeCol1_old[2]);
 	
 	//If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue.
-	if (fabs(cosDiff) < cos(5.0f*DEG2RAD)) {
+	if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) {
 		GPSVelocityData gpsVelData;
 		GPSVelocityGet(&gpsVelData);
 		
diff --git a/flight/Modules/Attitude/revolution/attitude.c b/flight/Modules/Attitude/revolution/attitude.c
index 91ccc688c..2a0b636f3 100644
--- a/flight/Modules/Attitude/revolution/attitude.c
+++ b/flight/Modules/Attitude/revolution/attitude.c
@@ -70,14 +70,13 @@
 #include "revosettings.h"
 #include "velocityactual.h"
 #include "CoordinateConversions.h"
-
+#include <pios_math.h>
 // Private constants
 #define STACK_SIZE_BYTES 2048
 #define TASK_PRIORITY (tskIDLE_PRIORITY+3)
 #define FAILSAFE_TIMEOUT_MS 10
 
-#define F_PI 3.14159265358979323846f
-#define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI)
+#define PI_MOD(x) (fmodf(x + M_PI_F, M_PI_F * 2) - M_PI_F)
 
 // low pass filter configuration to calculate offset
 // of barometric altitude sensor
@@ -303,9 +302,9 @@ static int32_t updateAttitudeComplementary(bool first_run)
 		AttitudeActualData attitudeActual;
 		AttitudeActualGet(&attitudeActual);
 		init = 0;
-		attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
-		attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
-		attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
+		attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / M_PI_F;
+		attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / M_PI_F;
+		attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / M_PI_F;
 
 		RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
 		AttitudeActualSet(&attitudeActual);
@@ -415,10 +414,10 @@ static int32_t updateAttitudeComplementary(bool first_run)
 	// 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] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * F_PI / 180 / 2;
-	qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * F_PI / 180 / 2;
-	qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * F_PI / 180 / 2;
-	qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * F_PI / 180 / 2;
+	qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * M_PI_F / 180 / 2;
+	qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * M_PI_F / 180 / 2;
+	qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * M_PI_F / 180 / 2;
+	qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * M_PI_F / 180 / 2;
 
 	// Take a time step
 	q[0] = q[0] + qdot[0];
@@ -641,7 +640,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 			INSSetBaroVar(revoCalibration.baro_var);
 
 			// Initialize the gyro bias from the settings
-			float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
+			float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f};
 			INSSetGyroBias(gyro_bias);
 
 			xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
@@ -649,9 +648,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 
 			// Set initial attitude
 			AttitudeActualData attitudeActual;
-			attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
-			attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
-			attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
+			attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / M_PI_F;
+			attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / M_PI_F;
+			attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / M_PI_F;
 			RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
 			AttitudeActualSet(&attitudeActual);
 
@@ -676,7 +675,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 			INSSetMagNorth(homeLocation.Be);
 
 			// Initialize the gyro bias from the settings
-			float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
+			float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f};
 			INSSetGyroBias(gyro_bias);
 
 			GPSPositionData gpsPosition;
@@ -693,9 +692,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 
 			// Set initial attitude
 			AttitudeActualData attitudeActual;
-			attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
-			attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
-			attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
+			attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / M_PI_F;
+			attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / M_PI_F;
+			attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / M_PI_F;
 			RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
 			AttitudeActualSet(&attitudeActual);
 
@@ -711,9 +710,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 			dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
 
 			GyrosBiasGet(&gyrosBias);
-			float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f, 
-				(gyrosData.y + gyrosBias.y) * F_PI / 180.0f, 
-				(gyrosData.z + gyrosBias.z) * F_PI / 180.0f};
+			float gyros[3] = {(gyrosData.x + gyrosBias.x) * M_PI_F / 180.0f,
+				(gyrosData.y + gyrosBias.y) * M_PI_F / 180.0f,
+				(gyrosData.z + gyrosBias.z) * M_PI_F / 180.0f};
 			INSStatePrediction(gyros, &accelsData.x, dT);
 			
 			AttitudeActualData attitude;
@@ -750,18 +749,18 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	// If the gyro bias setting was updated we should reset
 	// the state estimate of the EKF
 	if(gyroBiasSettingsUpdated) {
-		float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
+		float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f};
 		INSSetGyroBias(gyro_bias);
 		gyroBiasSettingsUpdated = false;
 	}
 
 	// Because the sensor module remove the bias we need to add it
 	// back in here so that the INS algorithm can track it correctly
-	float gyros[3] = {gyrosData.x * F_PI / 180.0f, gyrosData.y * F_PI / 180.0f, gyrosData.z * F_PI / 180.0f};
+	float gyros[3] = {gyrosData.x * M_PI_F / 180.0f, gyrosData.y * M_PI_F / 180.0f, gyrosData.z * M_PI_F / 180.0f};
 	if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
-		gyros[0] += gyrosBias.x * F_PI / 180.0f;
-		gyros[1] += gyrosBias.y * F_PI / 180.0f;
-		gyros[2] += gyrosBias.z * F_PI / 180.0f;
+		gyros[0] += gyrosBias.x * M_PI_F / 180.0f;
+		gyros[1] += gyrosBias.y * M_PI_F / 180.0f;
+		gyros[2] += gyrosBias.z * M_PI_F / 180.0f;
 	}
 
 	// Advance the state estimate
@@ -795,8 +794,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 
 		if (0) { // Old code to take horizontal velocity from GPS Position update
 			sensors |= HORIZ_SENSORS;
-			vel[0] = gpsData.Groundspeed * cosf(gpsData.Heading * F_PI / 180.0f);
-			vel[1] = gpsData.Groundspeed * sinf(gpsData.Heading * F_PI / 180.0f);
+			vel[0] = gpsData.Groundspeed * cosf(gpsData.Heading * M_PI_F / 180.0f);
+			vel[1] = gpsData.Groundspeed * sinf(gpsData.Heading * M_PI_F / 180.0f);
 			vel[2] = 0;
 		}
 		// Transform the GPS position into NED coordinates
@@ -870,9 +869,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 		// Copy the gyro bias into the UAVO except when it was updated
 		// from the settings during the calculation, then consume it
 		// next cycle
-		gyrosBias.x = Nav.gyro_bias[0] * 180.0f / F_PI;
-		gyrosBias.y = Nav.gyro_bias[1] * 180.0f / F_PI;
-		gyrosBias.z = Nav.gyro_bias[2] * 180.0f / F_PI;
+		gyrosBias.x = Nav.gyro_bias[0] * 180.0f / M_PI_F;
+		gyrosBias.y = Nav.gyro_bias[1] * 180.0f / M_PI_F;
+		gyrosBias.z = Nav.gyro_bias[2] * 180.0f / M_PI_F;
 		GyrosBiasSet(&gyrosBias);
 	}
 
diff --git a/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c
index d3071d676..ebf9e4cc6 100644
--- a/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c
+++ b/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c
@@ -66,14 +66,11 @@
 #include "velocitydesired.h"
 #include "velocityactual.h"
 #include "CoordinateConversions.h"
-
+#include <pios_math.h>
 // Private constants
 #define MAX_QUEUE_SIZE 4
 #define STACK_SIZE_BYTES 1548
 #define TASK_PRIORITY (tskIDLE_PRIORITY+2)
-#define F_PI 3.14159265358979323846f
-#define RAD2DEG (180.0f/F_PI)
-#define DEG2RAD (F_PI/180.0f)
 #define GEE 9.81f
 // Private types
 
@@ -293,7 +290,7 @@ static void updatePathVelocity()
 			break;
 	}
 	// make sure groundspeed is not zero
-	if (groundspeed<1e-2) groundspeed=1e-2;
+	if (groundspeed<1e-2f) groundspeed=1e-2f;
 	
 	// calculate velocity - can be zero if waypoints are too close
 	VelocityDesiredData velocityDesired;
@@ -313,8 +310,8 @@ static void updatePathVelocity()
 	// difference between correction_direction and velocityactual >90 degrees and
 	// difference between path_direction and velocityactual >90 degrees  ( 4th sector, facing away from eerything )
 	// fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
-	float angle1=RAD2DEG * ( atan2f(progress.path_direction[1],progress.path_direction[0]) - atan2f(velocityActual.East,velocityActual.North));
-	float angle2=RAD2DEG * ( atan2f(progress.correction_direction[1],progress.correction_direction[0]) - atan2f(velocityActual.East,velocityActual.North));
+	float angle1=RAD2DEG( atan2f(progress.path_direction[1],progress.path_direction[0]) - atan2f(velocityActual.East,velocityActual.North));
+	float angle2=RAD2DEG( atan2f(progress.correction_direction[1],progress.correction_direction[0]) - atan2f(velocityActual.East,velocityActual.North));
 	if (angle1<-180.0f) angle1+=360.0f;
 	if (angle1>180.0f) angle1-=360.0f;
 	if (angle2<-180.0f) angle2+=360.0f;
@@ -470,7 +467,7 @@ static uint8_t updateFixedDesiredAttitude()
 		result = 0;
 	}
 	
-	if (indicatedAirspeedActual<1e-6) {
+	if (indicatedAirspeedActual<1e-6f) {
 		// prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
 		// also we cannot handle planes flying backwards, lets just wait until the nose drops
 		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
@@ -583,8 +580,8 @@ static uint8_t updateFixedDesiredAttitude()
 	/**
 	 * Compute desired roll command
 	 */
-	if (groundspeedDesired> 1e-6) {
-		bearingError = RAD2DEG * (atan2f(velocityDesired.East,velocityDesired.North) - atan2f(velocityActual.East,velocityActual.North));
+	if (groundspeedDesired> 1e-6f) {
+		bearingError = RAD2DEG(atan2f(velocityDesired.East,velocityDesired.North) - atan2f(velocityActual.East,velocityActual.North));
 	} else {
 		// if we are not supposed to move, run in a circle
 		bearingError = -90.0f;
diff --git a/flight/Modules/PathPlanner/pathplanner.c b/flight/Modules/PathPlanner/pathplanner.c
index de2fe9184..65f540bee 100644
--- a/flight/Modules/PathPlanner/pathplanner.c
+++ b/flight/Modules/PathPlanner/pathplanner.c
@@ -329,7 +329,7 @@ static uint8_t conditionTimeOut() {
 		toWaypoint = waypointActive.Index;
 		toStarttime = PIOS_DELAY_GetRaw();
 	}
-	if (PIOS_DELAY_DiffuS(toStarttime) >= 1e6 * pathAction.ConditionParameters[0]) {
+	if (PIOS_DELAY_DiffuS(toStarttime) >= 1e6f * pathAction.ConditionParameters[0]) {
 		// make sure we reinitialize even if the same waypoint comes twice
 		toWaypoint = 0xFFFF;
 		return true;
diff --git a/flight/PiOS/Common/pios_mpxv.c b/flight/PiOS/Common/pios_mpxv.c
index 80e0b648d..b430286f3 100644
--- a/flight/PiOS/Common/pios_mpxv.c
+++ b/flight/PiOS/Common/pios_mpxv.c
@@ -77,7 +77,7 @@ float PIOS_MPXV_CalcAirspeed(PIOS_MPXV_descriptor *desc,uint16_t measurement)
 	}
 	
 	//Compute calibrated airspeed, as per http://en.wikipedia.org/wiki/Calibrated_airspeed
-	float calibratedAirspeed = A0*sqrt(5.0f*(pow(Qc/P0+1.0f,POWER)-1.0f));
+	float calibratedAirspeed = A0*sqrtf(5.0f*(powf(Qc/P0+1.0f,POWER)-1.0f));
 	
 	//Upper bound airspeed. No need to lower bound it, that comes from Qc
 	if (calibratedAirspeed > desc->maxSpeed) { //in [m/s]
diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c
index 20eecc112..9c09f3cd9 100644
--- a/flight/PiOS/Common/pios_rfm22b.c
+++ b/flight/PiOS/Common/pios_rfm22b.c
@@ -964,12 +964,12 @@ static void PIOS_RFM22B_Task(void *parameters)
 static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening)
 {
 	uint32_t datarate_bps = data_rate[datarate];
-	rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5);
+	rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5f);
 	if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED)
 	{
 		// Generate a pseudo-random number from 0-8 to add to the delay
 		uint8_t random = PIOS_CRC_updateByte(0, (uint8_t)(xTaskGetTickCount() & 0xff)) & 0x03;
-		rfm22b_dev->max_ack_delay = (uint16_t)((float)(sizeof(PHPacketHeader) * 8 * 1000) / (float)(datarate_bps) + 0.5) * 4 + random;
+		rfm22b_dev->max_ack_delay = (uint16_t)((float)(sizeof(PHPacketHeader) * 8 * 1000) / (float)(datarate_bps) + 0.5f) * 4 + random;
 	}
 	else
 		rfm22b_dev->max_ack_delay = CONNECT_ATTEMPT_PERIOD_MS;
diff --git a/flight/PiOS/inc/pios_helpers.h b/flight/PiOS/inc/pios_helpers.h
new file mode 100644
index 000000000..1377411a7
--- /dev/null
+++ b/flight/PiOS/inc/pios_helpers.h
@@ -0,0 +1,40 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_helpers.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Header for helper functions/macro definitions
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef PIOS_HELPERS_H_
+#define PIOS_HELPERS_H_
+
+
+/**
+ * @brief return the number of elements contained in the array x.
+ * @param[in] x the array
+ * @return  number of elements in x.
+ *
+ */
+#define NELEMENTS(x) (sizeof(x) / sizeof((x)[0]))
+
+
+#endif // PIOS_HELPERS_H_
diff --git a/flight/PiOS/inc/pios_math.h b/flight/PiOS/inc/pios_math.h
new file mode 100644
index 000000000..38702f363
--- /dev/null
+++ b/flight/PiOS/inc/pios_math.h
@@ -0,0 +1,35 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_math.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Header for math functions/macro definitions
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef PIOS_MATH_H_
+#define PIOS_MATH_H_
+
+#define M_PI_F ((float)M_PI)
+#define RAD2DEG(rad)    ((rad)*(180.0f/M_PI_F))
+#define DEG2RAD(deg)    ((deg)*(M_PI_F/180.0f))
+
+
+#endif // PIOS_MATH_H_
diff --git a/flight/PiOS/pios.h b/flight/PiOS/pios.h
index 96ca8304b..5e581b345 100644
--- a/flight/PiOS/pios.h
+++ b/flight/PiOS/pios.h
@@ -34,8 +34,7 @@
 #ifndef PIOS_H
 #define PIOS_H
 
-/* Used in pios_*.* files macro */
-#define NELEMENTS(x) (sizeof(x) / sizeof((x)[0]))
+#include <pios_helpers.h>
 
 #ifdef USE_SIM_POSIX
 /* SimPosix version of this file. This will probably be removed later */