AP_Math: Cleaned macro definitions

Moved Definitions into a separate header. Replaced PI with M_PI and
removed the M_PI_*_F macros.

ArduCopter/Attitude.cpp

@@ -33,7 +33,7 @@ void Copter::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float
     }
 
     // do lateral tilt to euler roll conversion
-    roll_in = (18000/M_PI_F) * atanf(cosf(pitch_in*(M_PI_F/18000))*tanf(roll_in*(M_PI_F/18000)));
+    roll_in = (18000/M_PI) * atanf(cosf(pitch_in*(M_PI/18000))*tanf(roll_in*(M_PI/18000)));
 
     // return
     roll_out = roll_in;

ArduCopter/commands_logic.cpp

@@ -666,7 +666,7 @@ bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd)
     }
 
     // check if we have completed circling
-    return fabsf(circle_nav.get_angle_total()/M_2PI_F) >= LOWBYTE(cmd.p1);
+    return fabsf(circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
 }
 
 // verify_RTL - handles any state changes required to implement RTL

ArduCopter/control_poshold.cpp

@@ -625,8 +625,8 @@ void Copter::poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pit
     poshold.wind_comp_timer = 0;
 
     // convert earth frame desired accelerations to body frame roll and pitch lean angles
-    roll_angle = atanf((-poshold.wind_comp_ef.x*ahrs.sin_yaw() + poshold.wind_comp_ef.y*ahrs.cos_yaw())/981)*(18000/M_PI_F);
+    roll_angle = atanf((-poshold.wind_comp_ef.x*ahrs.sin_yaw() + poshold.wind_comp_ef.y*ahrs.cos_yaw())/981)*(18000/M_PI);
-    pitch_angle = atanf(-(poshold.wind_comp_ef.x*ahrs.cos_yaw() + poshold.wind_comp_ef.y*ahrs.sin_yaw())/981)*(18000/M_PI_F);
+    pitch_angle = atanf(-(poshold.wind_comp_ef.x*ahrs.cos_yaw() + poshold.wind_comp_ef.y*ahrs.sin_yaw())/981)*(18000/M_PI);
 }
 
 // poshold_roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

@@ -71,15 +71,15 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass
     }
 
     // handle flipping over pitch axis
-    if (_att_target_euler_rad.y > M_PI_F/2.0f) {
+    if (_att_target_euler_rad.y > M_PI/2.0f) {
-        _att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x + M_PI_F);
+        _att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x + M_PI);
-        _att_target_euler_rad.y = wrap_PI(M_PI_F - _att_target_euler_rad.x);
+        _att_target_euler_rad.y = wrap_PI(M_PI - _att_target_euler_rad.x);
-        _att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z + M_PI_F);
+        _att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z + M_PI);
     }
-    if (_att_target_euler_rad.y < -M_PI_F/2.0f) {
+    if (_att_target_euler_rad.y < -M_PI/2.0f) {
-        _att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x + M_PI_F);
+        _att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x + M_PI);
-        _att_target_euler_rad.y = wrap_PI(-M_PI_F - _att_target_euler_rad.x);
+        _att_target_euler_rad.y = wrap_PI(-M_PI - _att_target_euler_rad.x);
-        _att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z + M_PI_F);
+        _att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z + M_PI);
     }
 
     // convert body-frame angle errors to body-frame rate targets

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -958,9 +958,9 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler, bo
     accel_right = -accel_target_filtered.x*_ahrs.sin_yaw() + accel_target_filtered.y*_ahrs.cos_yaw();
 
     // update angle targets that will be passed to stabilize controller
-    _pitch_target = constrain_float(atanf(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI_F),-lean_angle_max, lean_angle_max);
+    _pitch_target = constrain_float(atanf(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI),-lean_angle_max, lean_angle_max);
-    float cos_pitch_target = cosf(_pitch_target*M_PI_F/18000);
+    float cos_pitch_target = cosf(_pitch_target*M_PI/18000);
-    _roll_target = constrain_float(atanf(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/M_PI_F), -lean_angle_max, lean_angle_max);
+    _roll_target = constrain_float(atanf(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/M_PI), -lean_angle_max, lean_angle_max);
 }
 
 // get_lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s

libraries/AC_PID/AC_PID.cpp

@@ -209,6 +209,6 @@ float AC_PID::get_filt_alpha() const
     }
 
     // calculate alpha
-    float rc = 1/(M_2PI_F*_filt_hz);
+    float rc = 1/(M_2PI*_filt_hz);
     return _dt / (_dt + rc);
 }

libraries/AC_PID/AC_PI_2D.cpp

@@ -173,6 +173,6 @@ void AC_PI_2D::calc_filt_alpha()
     }
   
     // calculate alpha
-    float rc = 1/(M_2PI_F*_filt_hz);
+    float rc = 1/(M_2PI*_filt_hz);
     _filt_alpha = _dt / (_dt + rc);
 }

libraries/AC_WPNav/AC_Circle.cpp

@@ -146,7 +146,7 @@ void AC_Circle::update()
             _pos_control.set_xy_target(target.x, target.y);
 
             // heading is 180 deg from vehicles target position around circle
-            _yaw = wrap_PI(_angle-PI) * AC_CIRCLE_DEGX100;
+            _yaw = wrap_PI(_angle-M_PI) * AC_CIRCLE_DEGX100;
         }else{
             // set target position to center
             Vector3f target;
@@ -245,12 +245,12 @@ void AC_Circle::init_start_angle(bool use_heading)
 
     // if use_heading is true
     if (use_heading) {
-        _angle = wrap_PI(_ahrs.yaw-PI);
+        _angle = wrap_PI(_ahrs.yaw-M_PI);
     } else {
         // if we are exactly at the center of the circle, init angle to directly behind vehicle (so vehicle will backup but not change heading)
         const Vector3f &curr_pos = _inav.get_position();
         if (is_equal(curr_pos.x,_center.x) && is_equal(curr_pos.y,_center.y)) {
-            _angle = wrap_PI(_ahrs.yaw-PI);
+            _angle = wrap_PI(_ahrs.yaw-M_PI);
         } else {
             // get bearing from circle center to vehicle in radians
             float bearing_rad = atan2f(curr_pos.y-_center.y,curr_pos.x-_center.x);

libraries/AP_AccelCal/AccelCalibrator.cpp

@@ -69,7 +69,7 @@ void AccelCalibrator::start(enum accel_cal_fit_type_t fit_type, uint8_t num_samp
     _conf_fit_type = fit_type;
 
     const uint16_t faces = 2*_conf_num_samples-4;
-    const float a = (4.0f * M_PI_F / (3.0f * faces)) + M_PI_F / 3.0f;
+    const float a = (4.0f * M_PI / (3.0f * faces)) + M_PI / 3.0f;
     const float theta = 0.5f * acosf(cosf(a) / (1.0f - cosf(a)));
     _min_sample_dist = GRAVITY_MSS * 2*sinf(theta/2);
 

libraries/AP_Compass/Compass.cpp

@@ -643,10 +643,10 @@ Compass::calculate_heading(const Matrix3f &dcm_matrix) const
     if( fabsf(_declination) > 0.0f )
     {
         heading = heading + _declination;
-        if (heading > PI)    // Angle normalization (-180 deg, 180 deg)
+        if (heading > M_PI)    // Angle normalization (-180 deg, 180 deg)
-            heading -= (2.0f * PI);
+            heading -= (2.0f * M_PI);
-        else if (heading < -PI)
+        else if (heading < -M_PI)
-            heading += (2.0f * PI);
+            heading += (2.0f * M_PI);
     }
 
     return heading;

libraries/AP_Compass/CompassCalibrator.cpp

@@ -365,7 +365,7 @@ void CompassCalibrator::thin_samples() {
 bool CompassCalibrator::accept_sample(const Vector3f& sample)
 {
     static const uint16_t faces = (2 * COMPASS_CAL_NUM_SAMPLES - 4);
-    static const float a = (4.0f * M_PI_F / (3.0f * faces)) + M_PI_F / 3.0f;
+    static const float a = (4.0f * M_PI / (3.0f * faces)) + M_PI / 3.0f;
     static const float theta = 0.5f * acosf(cosf(a) / (1.0f - cosf(a)));
 
     if(_sample_buffer == NULL) {

libraries/AP_L1_Control/AP_L1_Control.cpp

@@ -121,7 +121,7 @@ float AP_L1_Control::crosstrack_error(void) const
  */
 void AP_L1_Control::_prevent_indecision(float &Nu)
 {
-    const float Nu_limit = 0.9f*M_PI_F;
+    const float Nu_limit = 0.9f*M_PI;
     if (fabsf(Nu) > Nu_limit &&
         fabsf(_last_Nu) > Nu_limit &&
         fabsf(wrap_180_cd(_target_bearing_cd - _ahrs.yaw_sensor)) > 12000 &&

libraries/AP_Math/AP_Math.cpp

@@ -10,10 +10,10 @@ float safe_asin(float v)
         return 0.0f;
     }
     if (v >= 1.0f) {
-        return PI/2;
+        return M_PI/2;
     }
     if (v <= -1.0f) {
-        return -PI/2;
+        return -M_PI/2;
     }
     return asinf(v);
 }

libraries/AP_Math/AP_Math.h

@@ -1,23 +1,15 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#pragma once
 
-#ifndef AP_MATH_H
+#include "definitions.h"
-#define AP_MATH_H
 
-// MATH_CHECK_INDEXES modifies some objects (e.g. SoloGimbalEKF) to
+#include <limits>
-// include more debug information.  It is also used by some functions
+#include <type_traits>
-// to add extra code for debugging purposes.  If you wish to activate
-// this, do it here or as part of the top-level Makefile -
-// e.g. Tools/Replay/Makefile
-#ifndef MATH_CHECK_INDEXES
-#define MATH_CHECK_INDEXES 0
-#endif
-
-// Assorted useful math operations for ArduPilot(Mega)
 
 #include <AP_Common/AP_Common.h>
 #include <AP_Param/AP_Param.h>
 #include <math.h>
 #include <stdint.h>
+
 #include "rotations.h"
 #include "vector2.h"
 #include "vector3.h"
@@ -27,53 +19,6 @@
 #include "edc.h"
 #include <AP_Param/AP_Param.h>
 
-#ifndef M_PI_F
- #define M_PI_F 3.141592653589793f
-#endif
-#ifndef M_2PI_F
- #define M_2PI_F (2*M_PI_F)
-#endif
-#ifndef PI
- # define PI M_PI_F
-#endif
-#ifndef M_PI_2
- # define M_PI_2 1.570796326794897f
-#endif
-//Single precision conversions
-#define DEG_TO_RAD 0.017453292519943295769236907684886f
-#define RAD_TO_DEG 57.295779513082320876798154814105f
-
-//GPS Specific double precision conversions
-//The precision here does matter when using the wsg* functions for converting
-//between LLH and ECEF coordinates. Test code in examlpes/location/location.cpp
-#define DEG_TO_RAD_DOUBLE 0.0174532925199432954743716805978692718781530857086181640625  // equals to (M_PI / 180.0)
-#define RAD_TO_DEG_DOUBLE 57.29577951308232286464772187173366546630859375               // equals to (180.0 / M_PI)
-
-#define RadiansToCentiDegrees(x) ((x) * 5729.5779513082320876798154814105f)
-
-// acceleration due to gravity in m/s/s
-#define GRAVITY_MSS 9.80665f
-
-// radius of earth in meters
-#define RADIUS_OF_EARTH 6378100
-
-#define ROTATION_COMBINATION_SUPPORT 0
-
-// convert a longitude or latitude point to meters or centimeteres.
-// Note: this does not include the longitude scaling which is dependent upon location
-#define LATLON_TO_M  0.01113195f
-#define LATLON_TO_CM 1.113195f
-
-// Semi-major axis of the Earth, in meters.
-#define WGS84_A 6378137.0
-//Inverse flattening of the Earth
-#define WGS84_IF 298.257223563
-// The flattening of the Earth
-#define WGS84_F (1/WGS84_IF)
-// Semi-minor axis of the Earth in meters
-#define WGS84_B (WGS84_A*(1-WGS84_F))
-// Eccentricity of the Earth
-#define WGS84_E (sqrt(2*WGS84_F - WGS84_F*WGS84_F))
 
 // define AP_Param types AP_Vector3f and Ap_Matrix3f
 AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
@@ -236,10 +181,6 @@ static inline float pythagorous3(float a, float b, float c) {
 	return sqrtf(sq(a)+sq(b)+sq(c));
 }
 
-#ifdef radians
-#error "Build is including Arduino base headers"
-#endif
-
 template<typename A, typename B>
 static inline auto MIN(const A &one, const B &two) -> decltype(one < two ? one : two) {
     return one < two ? one : two;
@@ -250,10 +191,6 @@ static inline auto MAX(const A &one, const B &two) -> decltype(one > two ? one :
     return one > two ? one : two;
 }
 
-#define NSEC_PER_SEC 1000000000ULL
-#define NSEC_PER_USEC 1000ULL
-#define USEC_PER_SEC 1000000ULL
-
 inline uint32_t hz_to_nsec(uint32_t freq)
 {
     return NSEC_PER_SEC / freq;
@@ -284,4 +221,3 @@ inline uint32_t usec_to_hz(uint32_t usec)
     return USEC_PER_SEC / usec;
 }
 
-#endif // AP_MATH_H

libraries/AP_Math/definitions.h

@@ -0,0 +1,74 @@
+#pragma once
+
+#include <math.h>
+#include <AP_HAL/AP_HAL.h>
+
+
+// Double precision math must be activated
+#if defined(DBL_MATH) && CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+  #ifndef M_PI
+    #define M_PI      (3.14159265358979323846)
+  #endif
+
+  #ifndef M_PI_2
+    #define M_PI_2    (M_PI / 2)
+  #endif
+#else  		// Standard single precision math
+  #ifdef M_PI
+    #undef M_PI
+  #endif
+  #define M_PI      (3.141592653589793f)
+
+  #ifdef M_PI_2
+    #undef M_PI_2
+  #endif
+  #define M_PI_2    (M_PI / 2)
+#endif
+
+#define M_2PI         (M_PI * 2)
+
+// MATH_CHECK_INDEXES modifies some objects (e.g. SoloGimbalEKF) to
+// include more debug information.  It is also used by some functions
+// to add extra code for debugging purposes. If you wish to activate
+// this, do it here or as part of the top-level Makefile -
+// e.g. Tools/Replay/Makefile
+#ifndef MATH_CHECK_INDEXES
+  #define MATH_CHECK_INDEXES 0
+#endif
+ 
+#define DEG_TO_RAD      (M_PI / 180.0f)
+#define RAD_TO_DEG      (180.0f / M_PI)
+ 
+// GPS Specific double precision conversions
+// The precision here does matter when using the wsg* functions for converting
+// between LLH and ECEF coordinates.
+#define DEG_TO_RAD_DOUBLE 0.0174532925199432954743716805978692718781530857086181640625
+#define RAD_TO_DEG_DOUBLE 57.29577951308232286464772187173366546630859375
+ 
+#define RadiansToCentiDegrees(x) (static_cast<float>(x) * RAD_TO_DEG * static_cast<float>(100))
+ 
+// acceleration due to gravity in m/s/s
+#define GRAVITY_MSS     9.80665f
+ 
+// radius of earth in meters
+#define RADIUS_OF_EARTH 6378100
+ 
+// convert a longitude or latitude point to meters or centimeteres.
+// Note: this does not include the longitude scaling which is dependent upon location
+#define LATLON_TO_M     0.01113195f
+#define LATLON_TO_CM    1.113195f
+ 
+// Semi-major axis of the Earth, in meters.
+#define WGS84_A         6378137.0
+//Inverse flattening of the Earth
+#define WGS84_IF        298.257223563
+// The flattening of the Earth
+#define WGS84_F         (1.0 / WGS84_IF)
+// Semi-minor axis of the Earth in meters
+#define WGS84_B         (WGS84_A * (1 - WGS84_F))
+// Eccentricity of the Earth
+#define WGS84_E         (sqrt(2 * WGS84_F - WGS84_F * WGS84_F))
+ 
+#define NSEC_PER_SEC    1000000000ULL
+#define NSEC_PER_USEC   1000ULL
+#define USEC_PER_SEC    1000000ULL

libraries/AP_Math/examples/eulers/eulers.cpp

@@ -13,11 +13,11 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 static float rad_diff(float rad1, float rad2)
 {
     float diff = rad1 - rad2;
-    if (diff > PI) {
+    if (diff > M_PI) {
-        diff -= 2*PI;
+        diff -= 2*M_PI;
     }
-    if (diff < -PI) {
+    if (diff < -M_PI) {
-        diff += 2*PI;
+        diff += 2*M_PI;
     }
     return fabsf(diff);
 }
@@ -35,18 +35,18 @@ static void check_result(const char *msg,
 
     if (rad_diff(roll2,roll) > ToRad(179)) {
         // reverse all 3
-        roll2 += fmod(roll2+PI, 2*PI);
+        roll2 += fmod(roll2+M_PI, 2*M_PI);
-        pitch2 += fmod(pitch2+PI, 2*PI);
+        pitch2 += fmod(pitch2+M_PI, 2*M_PI);
-        yaw2 += fmod(yaw2+PI, 2*PI);
+        yaw2 += fmod(yaw2+M_PI, 2*M_PI);
     }
 
     if (rad_diff(roll2,roll) > 0.01f ||
         rad_diff(pitch2, pitch) > 0.01f ||
         rad_diff(yaw2, yaw) > 0.01f) {
-        if (pitch >= PI/2 ||
+        if (pitch >= M_PI/2 ||
-            pitch <= -PI/2 ||
+            pitch <= -M_PI/2 ||
-            ToDeg(rad_diff(pitch, PI/2)) < 1 ||
+            ToDeg(rad_diff(pitch, M_PI/2)) < 1 ||
-            ToDeg(rad_diff(pitch, -PI/2)) < 1) {
+            ToDeg(rad_diff(pitch, -M_PI/2)) < 1) {
             // we expect breakdown at these poles
 #if SHOW_POLES_BREAKDOWN
             hal.console->printf(
@@ -77,8 +77,8 @@ static void test_euler(float roll, float pitch, float yaw)
     check_result("test_euler", roll, pitch, yaw, roll2, pitch2, yaw2);
 }
 
-static const float angles[] = { 0, PI/8, PI/4, PI/2, PI,
+static const float angles[] = { 0, M_PI/8, M_PI/4, M_PI/2, M_PI,
-                                -PI/8, -PI/4, -PI/2, -PI};
+                                -M_PI/8, -M_PI/4, -M_PI/2, -M_PI};
 
 void test_matrix_eulers(void)
 {
@@ -126,15 +126,15 @@ void test_quaternion_eulers(void)
 
     hal.console->println("quaternion unit tests\n");
 
-    test_quaternion(PI/4, 0, 0);
+    test_quaternion(M_PI/4, 0, 0);
-    test_quaternion(0, PI/4, 0);
+    test_quaternion(0, M_PI/4, 0);
-    test_quaternion(0, 0, PI/4);
+    test_quaternion(0, 0, M_PI/4);
-    test_quaternion(-PI/4, 0, 0);
+    test_quaternion(-M_PI/4, 0, 0);
-    test_quaternion(0, -PI/4, 0);
+    test_quaternion(0, -M_PI/4, 0);
-    test_quaternion(0, 0, -PI/4);
+    test_quaternion(0, 0, -M_PI/4);
-    test_quaternion(-PI/4, 1, 1);
+    test_quaternion(-M_PI/4, 1, 1);
-    test_quaternion(1, -PI/4, 1);
+    test_quaternion(1, -M_PI/4, 1);
-    test_quaternion(1, 1, -PI/4);
+    test_quaternion(1, 1, -M_PI/4);
 
     test_quaternion(ToRad(89), 0, 0.1f);
     test_quaternion(0, ToRad(89), 0.1f);
@@ -284,7 +284,7 @@ void setup(void)
 {
     hal.console->println("euler unit tests\n");
 
-    test_conversion(0, PI, 0);
+    test_conversion(0, M_PI, 0);
 
     test_frame_transforms();
     test_conversions();

libraries/AP_Math/examples/location/location.cpp

@@ -194,9 +194,9 @@ static const struct {
 static const struct {
     float v, wv;
 } wrap_PI_tests[] = {
-    { 0.2f*PI,            0.2f*PI },
+    { 0.2f*M_PI,            0.2f*M_PI },
-    { 0.2f*PI + 100*PI,  0.2f*PI },
+    { 0.2f*M_PI + 100*M_PI,  0.2f*M_PI },
-    { -0.2f*PI - 100*PI,  -0.2f*PI },
+    { -0.2f*M_PI - 100*M_PI,  -0.2f*M_PI },
 };
 
 static void test_wrap_cd(void)

libraries/AP_Math/location.cpp

@@ -209,12 +209,12 @@ float wrap_180_cd_float(float angle)
  */
 float wrap_PI(float angle_in_radians)
 {
-    if (angle_in_radians > 10*PI || angle_in_radians < -10*PI) {
+    if (angle_in_radians > 10*M_PI || angle_in_radians < -10*M_PI) {
         // for very large numbers use modulus
-        angle_in_radians = fmodf(angle_in_radians, 2*PI);
+        angle_in_radians = fmodf(angle_in_radians, 2*M_PI);
     }
-    while (angle_in_radians > PI) angle_in_radians -= 2*PI;
+    while (angle_in_radians > M_PI) angle_in_radians -= 2*M_PI;
-    while (angle_in_radians < -PI) angle_in_radians += 2*PI;
+    while (angle_in_radians < -M_PI) angle_in_radians += 2*M_PI;
     return angle_in_radians;
 }
 
@@ -223,12 +223,12 @@ float wrap_PI(float angle_in_radians)
  */
 float wrap_2PI(float angle)
 {
-    if (angle > 10*PI || angle < -10*PI) {
+    if (angle > 10*M_PI || angle < -10*M_PI) {
         // for very large numbers use modulus
-        angle = fmodf(angle, 2*PI);
+        angle = fmodf(angle, 2*M_PI);
     }
-    while (angle > 2*PI) angle -= 2*PI;
+    while (angle > 2*M_PI) angle -= 2*M_PI;
-    while (angle < 0) angle += 2*PI;
+    while (angle < 0) angle += 2*M_PI;
     return angle;
 }
 

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -491,26 +491,26 @@ void AP_MotorsHeli_Single::servo_test()
     if ((_servo_test_cycle_time >= 0.0f && _servo_test_cycle_time < 0.5f)||                                   // Tilt swash back
         (_servo_test_cycle_time >= 6.0f && _servo_test_cycle_time < 6.5f)){
         _pitch_test += (4500 / (_loop_rate/2));
-        _oscillate_angle += 8 * M_PI_F / _loop_rate;
+        _oscillate_angle += 8 * M_PI / _loop_rate;
         _yaw_test = 2250 * sinf(_oscillate_angle);
     } else if ((_servo_test_cycle_time >= 0.5f && _servo_test_cycle_time < 4.5f)||                            // Roll swash around
                (_servo_test_cycle_time >= 6.5f && _servo_test_cycle_time < 10.5f)){
-        _oscillate_angle += M_PI_F / (2 * _loop_rate);
+        _oscillate_angle += M_PI / (2 * _loop_rate);
         _roll_test = 4500 * sinf(_oscillate_angle);
         _pitch_test = 4500 * cosf(_oscillate_angle);
         _yaw_test = 4500 * sinf(_oscillate_angle);
     } else if ((_servo_test_cycle_time >= 4.5f && _servo_test_cycle_time < 5.0f)||                            // Return swash to level
                (_servo_test_cycle_time >= 10.5f && _servo_test_cycle_time < 11.0f)){
         _pitch_test -= (4500 / (_loop_rate/2));
-        _oscillate_angle += 8 * M_PI_F / _loop_rate;
+        _oscillate_angle += 8 * M_PI / _loop_rate;
         _yaw_test = 2250 * sinf(_oscillate_angle);
     } else if (_servo_test_cycle_time >= 5.0f && _servo_test_cycle_time < 6.0f){                              // Raise swash to top
         _collective_test += (1000 / _loop_rate);
-        _oscillate_angle += 2 * M_PI_F / _loop_rate;
+        _oscillate_angle += 2 * M_PI / _loop_rate;
         _yaw_test = 4500 * sinf(_oscillate_angle);
     } else if (_servo_test_cycle_time >= 11.0f && _servo_test_cycle_time < 12.0f){                            // Lower swash to bottom
         _collective_test -= (1000 / _loop_rate);
-        _oscillate_angle += 2 * M_PI_F / _loop_rate;
+        _oscillate_angle += 2 * M_PI / _loop_rate;
         _yaw_test = 4500 * sinf(_oscillate_angle);
     } else {                                                                                                  // reset cycle
         _servo_test_cycle_time = 0.0f;

libraries/AP_Mount/AP_Mount_Servo.cpp

@@ -154,7 +154,7 @@ void AP_Mount_Servo::stabilize()
         // lead filter
         const Vector3f &gyro = _frontend._ahrs.get_gyro();
 
-        if (_state._stab_roll && !is_zero(_state._roll_stb_lead) && fabsf(_frontend._ahrs.pitch) < M_PI_F/3.0f) {
+        if (_state._stab_roll && !is_zero(_state._roll_stb_lead) && fabsf(_frontend._ahrs.pitch) < M_PI/3.0f) {
             // Compute rate of change of euler roll angle
             float roll_rate = gyro.x + (_frontend._ahrs.sin_pitch() / _frontend._ahrs.cos_pitch()) * (gyro.y * _frontend._ahrs.sin_roll() + gyro.z * _frontend._ahrs.cos_roll());
             _angle_bf_output_deg.x -= degrees(roll_rate) * _state._roll_stb_lead;

libraries/AP_Mount/SoloGimbalEKF.cpp

@@ -887,19 +887,19 @@ float SoloGimbalEKF::calcMagHeadingInnov()
     float innovation = atan2f(magMeasNED.y,magMeasNED.x) - declination;
 
     // wrap the innovation so it sits on the range from +-pi
-    if (innovation > M_PI_F) {
+    if (innovation > M_PI) {
-        innovation = innovation - 2*M_PI_F;
+        innovation = innovation - 2*M_PI;
-    } else if (innovation < -M_PI_F) {
+    } else if (innovation < -M_PI) {
-        innovation = innovation + 2*M_PI_F;
+        innovation = innovation + 2*M_PI;
     }
 
     // Unwrap so that a large yaw gyro bias offset that causes the heading to wrap does not lead to continual uncontrolled heading drift
-    if (innovation - lastInnovation > M_PI_F) {
+    if (innovation - lastInnovation > M_PI) {
         // Angle has wrapped in the positive direction to subtract an additional 2*Pi
-        innovationIncrement -= 2*M_PI_F;
+        innovationIncrement -= 2*M_PI;
-    } else if (innovation -innovationIncrement < -M_PI_F) {
+    } else if (innovation -innovationIncrement < -M_PI) {
         // Angle has wrapped in the negative direction so add an additional 2*Pi
-        innovationIncrement += 2*M_PI_F;
+        innovationIncrement += 2*M_PI;
     }
     lastInnovation = innovation;
 

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -881,12 +881,12 @@ float NavEKF2_core::calcMagHeadingInnov()
     innovation = wrap_PI(innovation);
 
     // Unwrap so that a large yaw gyro bias offset that causes the heading to wrap does not lead to continual uncontrolled heading drift
-    if (innovation - lastInnovation > M_PI_F) {
+    if (innovation - lastInnovation > M_PI) {
         // Angle has wrapped in the positive direction to subtract an additional 2*Pi
-        innovationIncrement -= 2*M_PI_F;
+        innovationIncrement -= M_2PI;
-    } else if (innovation -innovationIncrement < -M_PI_F) {
+    } else if (innovation -innovationIncrement < -M_PI) {
         // Angle has wrapped in the negative direction so add an additional 2*Pi
-        innovationIncrement += 2*M_PI_F;
+        innovationIncrement += M_2PI;
     }
     lastInnovation = innovation;
 

libraries/Filter/LowPassFilter.cpp

@@ -25,7 +25,7 @@ T DigitalLPF<T>::apply(const T &sample, float cutoff_freq, float dt) {
         return _output;
     }
 
-    float rc = 1.0f/(M_2PI_F*cutoff_freq);
+    float rc = 1.0f/(M_2PI*cutoff_freq);
     float alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
     _output += (sample - _output) * alpha;
     return _output;

libraries/Filter/LowPassFilter2p.cpp

@@ -37,14 +37,14 @@ void DigitalBiquadFilter<T>::compute_params(float sample_freq, float cutoff_freq
     ret.sample_freq = sample_freq;
 
     float fr = sample_freq/cutoff_freq;
-    float ohm = tanf(PI/fr);
+    float ohm = tanf(M_PI/fr);
-    float c = 1.0f+2.0f*cosf(PI/4.0f)*ohm + ohm*ohm;
+    float c = 1.0f+2.0f*cosf(M_PI/4.0f)*ohm + ohm*ohm;
 
     ret.b0 = ohm*ohm/c;
     ret.b1 = 2.0f*ret.b0;
     ret.b2 = ret.b0;
     ret.a1 = 2.0f*(ohm*ohm-1.0f)/c;
-    ret.a2 = (1.0f-2.0f*cosf(PI/4.0f)*ohm+ohm*ohm)/c;
+    ret.a2 = (1.0f-2.0f*cosf(M_PI/4.0f)*ohm+ohm*ohm)/c;
 }
 
 

libraries/Filter/examples/LowPassFilter/LowPassFilter.cpp

@@ -38,7 +38,7 @@ void loop()
     for( i=0; i<300; i++ ) {
 
         // new data value
-        new_value = sinf((float)i*2*PI*5/50.0f);  // 5hz
+        new_value = sinf((float)i*2*M_PI*5/50.0f);  // 5hz
 
         // output to user
         hal.console->printf("applying: %6.4f", new_value);

libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp

@@ -28,7 +28,7 @@ void loop()
     for( i=0; i<300; i++ ) {
 
         // new data value
-        new_value = sinf((float)i*2*PI*5/50.0f);  // 5hz
+        new_value = sinf((float)i*2*M_PI*5/50.0f);  // 5hz
 
         // output to user
         hal.console->printf("applying: %6.4f", new_value);

libraries/PID/PID.cpp

@@ -75,7 +75,7 @@ float PID::get_pid(float error, float scaler)
 
         // discrete low pass filter, cuts out the
         // high frequency noise that can drive the controller crazy
-        float RC = 1/(2*PI*_fCut);
+        float RC = 1/(2*M_PI*_fCut);
         derivative = _last_derivative +
                      ((delta_time / (RC + delta_time)) *
                       (derivative - _last_derivative));

libraries/SITL/SIM_Rover.cpp

@@ -69,7 +69,7 @@ float SimRover::calc_yaw_rate(float steering, float speed)
         return 0;
     }
     float d = turn_circle(steering);
-    float c = M_PI_F * d;
+    float c = M_PI * d;
     float t = c / speed;
     float rate = 360.0f / t;
     return rate;