// -*- 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 #include #include #include #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 #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); // 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); // 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); // 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); /* 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 #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; } template 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