ardupilot/libraries/AP_Math/AP_Math.h

289 lines
8.8 KiB
C
Raw Normal View History

2013-05-29 20:51:51 -03:00
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef AP_MATH_H
#define AP_MATH_H
// 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"
#include "matrix3.h"
#include "quaternion.h"
#include "polygon.h"
#include "edc.h"
2015-04-28 03:06:13 -03:00
#include "float.h"
#include <AP_Param/AP_Param.h>
2013-12-10 19:22:47 -04:00
#ifndef M_PI_F
#define M_PI_F 3.141592653589793f
#endif
#ifndef M_2PI_F
2015-04-28 03:06:13 -03:00
#define M_2PI_F (2*M_PI_F)
#endif
#ifndef PI
2013-12-10 19:22:47 -04:00
# 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);
2015-05-04 23:29:05 -03:00
// are two floats equal
static inline bool is_equal(const float fVal1, const float fVal2) { return fabsf(fVal1 - fVal2) < FLT_EPSILON ? true : false; }
2015-04-28 03:06:13 -03:00
2015-05-04 23:29:05 -03:00
// is a float is zero
static inline bool is_zero(const float fVal1) { return fabsf(fVal1) < FLT_EPSILON ? true : false; }
// a varient of asin() that always gives a valid answer.
2012-08-17 03:20:14 -03:00
float safe_asin(float v);
// a varient of sqrt() that always gives a valid answer.
2012-08-17 03:20:14 -03:00
float safe_sqrt(float v);
#if ROTATION_COMBINATION_SUPPORT
// find a rotation that is the combination of two other
// rotations. This is used to allow us to add an overall board
// rotation to an existing rotation of a sensor such as the compass
2012-08-17 03:20:14 -03:00
enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *found = NULL);
#endif
// longitude_scale - returns the scaler to compensate for shrinking longitude as you move north or south from the equator
// Note: this does not include the scaling to convert longitude/latitude points to meters or centimeters
float longitude_scale(const struct Location &loc);
// return distance in meters between two locations
float get_distance(const struct Location &loc1, const struct Location &loc2);
// return distance in centimeters between two locations
uint32_t get_distance_cm(const struct Location &loc1, const struct Location &loc2);
// return bearing in centi-degrees between two locations
int32_t get_bearing_cd(const struct Location &loc1, const struct Location &loc2);
// return determinant of square matrix
float detnxn(const float C[], const uint8_t n);
// Output inverted nxn matrix when returns true, otherwise matrix is Singular
bool inversenxn(const float x[], float y[], const uint8_t n);
// invOut is an inverted 4x4 matrix when returns true, otherwise matrix is Singular
bool inverse3x3(float m[], float invOut[]);
// invOut is an inverted 3x3 matrix when returns true, otherwise matrix is Singular
bool inverse4x4(float m[],float invOut[]);
// matrix multiplication of two NxN matrices
float* mat_mul(float *A, float *B, uint8_t n);
2012-08-17 03:20:14 -03:00
// see if location is past a line perpendicular to
// the line between point1 and point2. If point1 is
// our previous waypoint and point2 is our target waypoint
// then this function returns true if we have flown past
// the target waypoint
bool location_passed_point(const struct Location & location,
const struct Location & point1,
const struct Location & point2);
/*
return the proportion we are along the path from point1 to
point2. This will be less than >1 if we have passed point2
*/
float location_path_proportion(const struct Location &location,
const struct Location &point1,
const struct Location &point2);
// extrapolate latitude/longitude given bearing and distance
void location_update(struct Location &loc, float bearing, float distance);
// extrapolate latitude/longitude given distances north and east
void location_offset(struct Location &loc, float ofs_north, float ofs_east);
/*
return the distance in meters in North/East plane as a N/E vector
from loc1 to loc2
*/
Vector2f location_diff(const struct Location &loc1, const struct Location &loc2);
/*
wrap an angle in centi-degrees
*/
int32_t wrap_360_cd(int32_t error);
int32_t wrap_180_cd(int32_t error);
float wrap_360_cd_float(float angle);
float wrap_180_cd_float(float angle);
2013-06-01 05:18:50 -03:00
/*
wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)
*/
float wrap_PI(float angle_in_radians);
2015-11-24 17:11:34 -04:00
/*
wrap an angle defined in radians to the interval [0,2*PI)
*/
float wrap_2PI(float angle);
/*
* check if lat and lng match. Ignore altitude and options
*/
bool locations_are_same(const struct Location &loc1, const struct Location &loc2);
/*
print a int32_t lat/long in decimal degrees
*/
void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon);
// Converts from WGS84 geodetic coordinates (lat, lon, height)
// into WGS84 Earth Centered, Earth Fixed (ECEF) coordinates
// (X, Y, Z)
void wgsllh2ecef(const Vector3d &llh, Vector3d &ecef);
// Converts from WGS84 Earth Centered, Earth Fixed (ECEF)
// coordinates (X, Y, Z), into WHS84 geodetic
// coordinates (lat, lon, height)
void wgsecef2llh(const Vector3d &ecef, Vector3d &llh);
// constrain a value
// constrain a value
static inline float constrain_float(float amt, float low, float high)
{
// the check for NaN as a float prevents propogation of
// floating point errors through any function that uses
// constrain_float(). The normal float semantics already handle -Inf
// and +Inf
if (isnan(amt)) {
return (low+high)*0.5f;
}
return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}
// constrain a int16_t value
static inline int16_t constrain_int16(int16_t amt, int16_t low, int16_t high) {
return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}
// constrain a int32_t value
static inline int32_t constrain_int32(int32_t amt, int32_t low, int32_t high) {
return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}
//matrix algebra
bool inverse(float x[], float y[], uint16_t dim);
// degrees -> radians
static inline float radians(float deg) {
return deg * DEG_TO_RAD;
}
// radians -> degrees
static inline float degrees(float rad) {
return rad * RAD_TO_DEG;
}
// square
static inline float sq(float v) {
return v*v;
}
// 2D vector length
static inline float pythagorous2(float a, float b) {
return sqrtf(sq(a)+sq(b));
}
// 3D vector length
static inline float pythagorous3(float a, float b, float c) {
return sqrtf(sq(a)+sq(b)+sq(c));
}
#ifdef radians
2012-12-21 21:52:42 -04:00
#error "Build is including Arduino base headers"
#endif
AP_Math: turn MIN/MAX macros into inline functions The problem with the current MIN/MAX macros is that they evaluate twice the arguments. For example, these cases provide unintended results: // a is incremented twice a = MIN(a++, b); // foo() with side-effects a = MIN(foo(), b); The alternative implementation here was provided by Daniel Frenzel using template function. It doesn't have type safety as std::min and std::max, but adding type safety would mean to check case by case what would be a reasonable type and add proper casts. Here the arguments for MIN and MAX can have different types and the return type is deduced from the expression in the function. Inspecting the current callers no place was found with the unintended results above, but some in which now we don't calculate twice the parameters will benefit from this new version. Examples: float velocity_max = MIN(_pos_control.get_speed_xy(), safe_sqrt(0.5f*_pos_control.get_accel_xy()*_radius)); float acro_level_mix = constrain_float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch() accel_x_cmss = (GRAVITY_MSS * 100) * (-(_ahrs.cos_yaw() * _ahrs.sin_pitch() / MAX(_ahrs.cos_pitch(),0.5f)) - _ahrs.sin_yaw() * _ahrs.sin_roll() / MAX(_ahrs.cos_roll(),0.5f)); track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy); RC_Channel_aux::move_servo(RC_Channel_aux::k_sprayer_pump, MIN(MAX(ground_speed * _pump_pct_1ms, 100 *_pump_min_pct),10000),0,10000);
2015-11-27 13:27:32 -04:00
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;
}
template<typename A, typename B>
static inline auto MAX(const A &one, const B &two) -> decltype(one > two ? one : two) {
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;
}
inline uint32_t nsec_to_hz(uint32_t nsec)
{
return NSEC_PER_SEC / nsec;
}
inline uint32_t usec_to_nsec(uint32_t usec)
{
return usec * NSEC_PER_USEC;
}
inline uint32_t nsec_to_usec(uint32_t nsec)
{
return nsec / NSEC_PER_USEC;
}
inline uint32_t hz_to_usec(uint32_t freq)
{
return USEC_PER_SEC / freq;
}
inline uint32_t usec_to_hz(uint32_t usec)
{
return USEC_PER_SEC / usec;
}
#undef INLINE
#endif // AP_MATH_H