mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 22:48:29 -04:00
d4daf19151
compile time optimization Functions like sq() are better moved to the header file as inline. Compiler can then optimize these out when used in code, this saves cpu cycles with stack push, pop during function calls.
247 lines
7.6 KiB
C
247 lines
7.6 KiB
C
// -*- 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>
|
|
#ifdef __AVR__
|
|
# include "AP_Math_AVR_Compat.h"
|
|
#endif
|
|
#include <stdint.h>
|
|
#include "rotations.h"
|
|
#include "vector2.h"
|
|
#include "vector3.h"
|
|
#include "matrix3.h"
|
|
#include "quaternion.h"
|
|
#include "polygon.h"
|
|
#include "edc.h"
|
|
#include "float.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.pde
|
|
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
|
#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)
|
|
#endif
|
|
|
|
#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(Matrix3f, Matrix3f, AP_PARAM_MATRIX3F);
|
|
AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
|
|
|
|
// are two floats equal
|
|
static inline bool is_equal(const float fVal1, const float fVal2) { return fabsf(fVal1 - fVal2) < FLT_EPSILON ? true : false; }
|
|
|
|
// 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.
|
|
float safe_asin(float v);
|
|
|
|
// a varient of sqrt() that always gives a valid answer.
|
|
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
|
|
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);
|
|
|
|
// 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);
|
|
|
|
/*
|
|
wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)
|
|
*/
|
|
float wrap_PI(float angle_in_radians);
|
|
|
|
/*
|
|
* 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);
|
|
|
|
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
|
// 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);
|
|
#endif
|
|
|
|
// 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
|
|
#error "Build is including Arduino base headers"
|
|
#endif
|
|
|
|
/* The following three functions used to be arduino core macros */
|
|
#define max(a,b) ((a)>(b)?(a):(b))
|
|
#define min(a,b) ((a)<(b)?(a):(b))
|
|
|
|
static inline float maxf(float a, float b)
|
|
{
|
|
return (a>b?a:b);
|
|
}
|
|
|
|
static inline float minf(float a, float b)
|
|
{
|
|
return (a<b?a:b);
|
|
}
|
|
|
|
#undef INLINE
|
|
#endif // AP_MATH_H
|
|
|