diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index d79b26e21b..550e055d20 100644 --- a/ArduCopter/Attitude.cpp +++ b/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; diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index c2aa1fa229..1cdb6b4e53 100644 --- a/ArduCopter/commands_logic.cpp +++ b/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 diff --git a/ArduCopter/control_poshold.cpp b/ArduCopter/control_poshold.cpp index 73c621542e..1f0607697e 100644 --- a/ArduCopter/control_poshold.cpp +++ b/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); - pitch_angle = atanf(-(poshold.wind_comp_ef.x*ahrs.cos_yaw() + poshold.wind_comp_ef.y*ahrs.sin_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); } // poshold_roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 4453ea6909..68db7e7f1e 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/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) { - _att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x + M_PI_F); - _att_target_euler_rad.y = wrap_PI(M_PI_F - _att_target_euler_rad.x); - _att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z + M_PI_F); + if (_att_target_euler_rad.y > M_PI/2.0f) { + _att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x + M_PI); + _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); } - if (_att_target_euler_rad.y < -M_PI_F/2.0f) { - _att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x + M_PI_F); - _att_target_euler_rad.y = wrap_PI(-M_PI_F - _att_target_euler_rad.x); - _att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z + M_PI_F); + if (_att_target_euler_rad.y < -M_PI/2.0f) { + _att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x + M_PI); + _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); } // convert body-frame angle errors to body-frame rate targets diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 8e77e49d3a..5392893fea 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/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); - float cos_pitch_target = cosf(_pitch_target*M_PI_F/18000); - _roll_target = constrain_float(atanf(accel_right*cos_pitch_target/(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/18000); + _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 diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index 04ddd3ddcb..2483334dfb 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/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); } diff --git a/libraries/AC_PID/AC_PI_2D.cpp b/libraries/AC_PID/AC_PI_2D.cpp index 41702d2a47..cde6ace442 100644 --- a/libraries/AC_PID/AC_PI_2D.cpp +++ b/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); } diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index edfe36641a..fc4c7cca86 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/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); diff --git a/libraries/AP_AccelCal/AccelCalibrator.cpp b/libraries/AP_AccelCal/AccelCalibrator.cpp index a3ae4a3ffa..08d18efcf9 100644 --- a/libraries/AP_AccelCal/AccelCalibrator.cpp +++ b/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); diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 244e68f128..891b86b194 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/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) - heading -= (2.0f * PI); - else if (heading < -PI) - heading += (2.0f * PI); + if (heading > M_PI) // Angle normalization (-180 deg, 180 deg) + heading -= (2.0f * M_PI); + else if (heading < -M_PI) + heading += (2.0f * M_PI); } return heading; diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index eec263e05c..c5db2002ba 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/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) { diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index 2c9bd8effe..a5813bd882 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/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 && diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index 07dbe4c9b8..318b39b9ba 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/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); } diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 97cd532b17..3e8d056f3c 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/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 -#define AP_MATH_H +#include "definitions.h" -// 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 - -// Assorted useful math operations for ArduPilot(Mega) +#include +#include #include #include #include #include + #include "rotations.h" #include "vector2.h" #include "vector3.h" @@ -27,53 +19,6 @@ #include "edc.h" #include -#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 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 diff --git a/libraries/AP_Math/definitions.h b/libraries/AP_Math/definitions.h new file mode 100644 index 0000000000..159ba4b692 --- /dev/null +++ b/libraries/AP_Math/definitions.h @@ -0,0 +1,74 @@ +#pragma once + +#include +#include + + +// 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(x) * RAD_TO_DEG * static_cast(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 diff --git a/libraries/AP_Math/examples/eulers/eulers.cpp b/libraries/AP_Math/examples/eulers/eulers.cpp index b0fe97f663..72b5cf4833 100644 --- a/libraries/AP_Math/examples/eulers/eulers.cpp +++ b/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) { - diff -= 2*PI; + if (diff > M_PI) { + diff -= 2*M_PI; } - if (diff < -PI) { - diff += 2*PI; + if (diff < -M_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); - pitch2 += fmod(pitch2+PI, 2*PI); - yaw2 += fmod(yaw2+PI, 2*PI); + roll2 += fmod(roll2+M_PI, 2*M_PI); + pitch2 += fmod(pitch2+M_PI, 2*M_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 || - pitch <= -PI/2 || - ToDeg(rad_diff(pitch, PI/2)) < 1 || - ToDeg(rad_diff(pitch, -PI/2)) < 1) { + if (pitch >= M_PI/2 || + pitch <= -M_PI/2 || + ToDeg(rad_diff(pitch, M_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, - -PI/8, -PI/4, -PI/2, -PI}; +static const float angles[] = { 0, M_PI/8, M_PI/4, M_PI/2, M_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(0, PI/4, 0); - test_quaternion(0, 0, PI/4); - test_quaternion(-PI/4, 0, 0); - test_quaternion(0, -PI/4, 0); - test_quaternion(0, 0, -PI/4); - test_quaternion(-PI/4, 1, 1); - test_quaternion(1, -PI/4, 1); - test_quaternion(1, 1, -PI/4); + test_quaternion(M_PI/4, 0, 0); + test_quaternion(0, M_PI/4, 0); + test_quaternion(0, 0, M_PI/4); + test_quaternion(-M_PI/4, 0, 0); + test_quaternion(0, -M_PI/4, 0); + test_quaternion(0, 0, -M_PI/4); + test_quaternion(-M_PI/4, 1, 1); + test_quaternion(1, -M_PI/4, 1); + 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(); diff --git a/libraries/AP_Math/examples/location/location.cpp b/libraries/AP_Math/examples/location/location.cpp index 265b0d9975..02ee508588 100644 --- a/libraries/AP_Math/examples/location/location.cpp +++ b/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*PI + 100*PI, 0.2f*PI }, - { -0.2f*PI - 100*PI, -0.2f*PI }, + { 0.2f*M_PI, 0.2f*M_PI }, + { 0.2f*M_PI + 100*M_PI, 0.2f*M_PI }, + { -0.2f*M_PI - 100*M_PI, -0.2f*M_PI }, }; static void test_wrap_cd(void) diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp index fb82db2a41..2a18103999 100644 --- a/libraries/AP_Math/location.cpp +++ b/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 < -PI) angle_in_radians += 2*PI; + while (angle_in_radians > M_PI) angle_in_radians -= 2*M_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 < 0) angle += 2*PI; + while (angle > 2*M_PI) angle -= 2*M_PI; + while (angle < 0) angle += 2*M_PI; return angle; } diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 864e9194eb..1e30b99017 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/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; diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index dfdd98833e..9b6398842d 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/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; diff --git a/libraries/AP_Mount/SoloGimbalEKF.cpp b/libraries/AP_Mount/SoloGimbalEKF.cpp index c267a98b12..e437d092bb 100644 --- a/libraries/AP_Mount/SoloGimbalEKF.cpp +++ b/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) { - innovation = innovation - 2*M_PI_F; - } else if (innovation < -M_PI_F) { - innovation = innovation + 2*M_PI_F; + if (innovation > M_PI) { + innovation = innovation - 2*M_PI; + } else if (innovation < -M_PI) { + 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; - } else if (innovation -innovationIncrement < -M_PI_F) { + innovationIncrement -= 2*M_PI; + } 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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index ef28770864..15e341cd0e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/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; - } else if (innovation -innovationIncrement < -M_PI_F) { + innovationIncrement -= M_2PI; + } 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; diff --git a/libraries/Filter/LowPassFilter.cpp b/libraries/Filter/LowPassFilter.cpp index aec67714f4..eba28c3922 100644 --- a/libraries/Filter/LowPassFilter.cpp +++ b/libraries/Filter/LowPassFilter.cpp @@ -25,7 +25,7 @@ T DigitalLPF::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; diff --git a/libraries/Filter/LowPassFilter2p.cpp b/libraries/Filter/LowPassFilter2p.cpp index d897013fac..f823829a2e 100644 --- a/libraries/Filter/LowPassFilter2p.cpp +++ b/libraries/Filter/LowPassFilter2p.cpp @@ -37,14 +37,14 @@ void DigitalBiquadFilter::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 c = 1.0f+2.0f*cosf(PI/4.0f)*ohm + ohm*ohm; + float ohm = tanf(M_PI/fr); + 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; } diff --git a/libraries/Filter/examples/LowPassFilter/LowPassFilter.cpp b/libraries/Filter/examples/LowPassFilter/LowPassFilter.cpp index 53f3369c59..7a730b487c 100644 --- a/libraries/Filter/examples/LowPassFilter/LowPassFilter.cpp +++ b/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); diff --git a/libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp b/libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp index 3a2029ea72..05d5ae3498 100644 --- a/libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp +++ b/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); diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp index c42741b793..354f955670 100644 --- a/libraries/PID/PID.cpp +++ b/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)); diff --git a/libraries/SITL/SIM_Rover.cpp b/libraries/SITL/SIM_Rover.cpp index 35a436ded2..aa28a20bab 100644 --- a/libraries/SITL/SIM_Rover.cpp +++ b/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;