Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Math: Cleaned macro definitions 

Moved Definitions into a separate header. Replaced PI with M_PI and
removed the M_PI_*_F macros.
This commit is contained in:
dgrat 2016-02-25 18:13:02 +01:00 committed by Lucas De Marchi
parent 7c4c8ea579
commit 5148e41c1a
28 changed files with 168 additions and 158 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Attitude.cpp
View File

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

2
ArduCopter/commands_logic.cpp
View File

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

4
ArduCopter/control_poshold.cpp
View File

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

16
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

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

6
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

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

2
libraries/AC_PID/AC_PID.cpp
View File

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

2
libraries/AC_PID/AC_PI_2D.cpp
View File

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

6
libraries/AC_WPNav/AC_Circle.cpp
View File

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

2
libraries/AP_AccelCal/AccelCalibrator.cpp
View File

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

8
libraries/AP_Compass/Compass.cpp
View File

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

2
libraries/AP_Compass/CompassCalibrator.cpp
View File

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

2
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -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 &&

4
libraries/AP_Math/AP_Math.cpp
View File

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

74
libraries/AP_Math/AP_Math.h
View File

@ -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 <limits>
#include <type_traits>
#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

74
libraries/AP_Math/definitions.h Normal file
View File

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

46
libraries/AP_Math/examples/eulers/eulers.cpp
View File

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

6
libraries/AP_Math/examples/location/location.cpp
View File

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

16
libraries/AP_Math/location.cpp
View File

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

10
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

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

2
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

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

16
libraries/AP_Mount/SoloGimbalEKF.cpp
View File

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

8
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

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

2
libraries/Filter/LowPassFilter.cpp
View File

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

6
libraries/Filter/LowPassFilter2p.cpp
View File

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

2
libraries/Filter/examples/LowPassFilter/LowPassFilter.cpp
View File

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

2
libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp
View File

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

2
libraries/PID/PID.cpp
View File

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

2
libraries/SITL/SIM_Rover.cpp
View File

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