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

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;

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

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

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

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

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

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

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

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

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;

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

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

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

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

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

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

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)

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

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;

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;

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;

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;

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;

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

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

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

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

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;