ardupilot/libraries/AP_Math/AP_Math.h

378 lines
10 KiB
C
Raw Normal View History

#pragma once
#include <cmath>
#include <limits>
#include <stdint.h>
#include <type_traits>
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include "definitions.h"
#include "crc.h"
#include "matrix3.h"
#include "polygon.h"
#include "quaternion.h"
#include "rotations.h"
#include "vector2.h"
#include "vector3.h"
#include "spline5.h"
2019-03-11 14:15:09 -03:00
#include "location.h"
2020-09-07 05:47:51 -03:00
#include "control.h"
2021-05-04 08:12:23 -03:00
#if HAL_WITH_EKF_DOUBLE
typedef Vector2<double> Vector2F;
typedef Vector3<double> Vector3F;
typedef Matrix3<double> Matrix3F;
typedef QuaternionD QuaternionF;
#else
typedef Vector2<float> Vector2F;
typedef Vector3<float> Vector3F;
typedef Matrix3<float> Matrix3F;
typedef Quaternion QuaternionF;
#endif
// define AP_Param types AP_Vector3f and Ap_Matrix3f
AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
/*
* Check whether two floats are equal
*/
template <typename Arithmetic1, typename Arithmetic2>
typename std::enable_if<std::is_integral<typename std::common_type<Arithmetic1, Arithmetic2>::type>::value ,bool>::type
is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2);
template <typename Arithmetic1, typename Arithmetic2>
typename std::enable_if<std::is_floating_point<typename std::common_type<Arithmetic1, Arithmetic2>::type>::value, bool>::type
is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2);
2015-04-28 03:06:13 -03:00
/*
* @brief: Check whether a float is zero
*/
template <typename T>
inline bool is_zero(const T fVal1) {
static_assert(std::is_floating_point<T>::value || std::is_base_of<T,AP_Float>::value,
"Template parameter not of type float");
return is_zero(static_cast<float>(fVal1));
}
/*
* @brief: Check whether a float is greater than zero
*/
template <typename T>
inline bool is_positive(const T fVal1) {
static_assert(std::is_floating_point<T>::value || std::is_base_of<T,AP_Float>::value,
"Template parameter not of type float");
return (static_cast<float>(fVal1) >= FLT_EPSILON);
}
/*
* @brief: Check whether a float is less than zero
*/
template <typename T>
inline bool is_negative(const T fVal1) {
static_assert(std::is_floating_point<T>::value || std::is_base_of<T,AP_Float>::value,
"Template parameter not of type float");
return (static_cast<float>(fVal1) <= (-1.0 * FLT_EPSILON));
}
/*
* @brief: Check whether a double is greater than zero
*/
inline bool is_positive(const double fVal1) {
return (fVal1 >= static_cast<double>(FLT_EPSILON));
}
/*
* @brief: Check whether a double is less than zero
*/
inline bool is_negative(const double fVal1) {
return (fVal1 <= static_cast<double>((-1.0 * FLT_EPSILON)));
}
/*
* A variant of asin() that checks the input ranges and ensures a valid angle
* as output. If nan is given as input then zero is returned.
*/
template <typename T>
float safe_asin(const T v);
/*
* A variant of sqrt() that checks the input ranges and ensures a valid value
* as output. If a negative number is given then 0 is returned. The reasoning
* is that a negative number for sqrt() in our code is usually caused by small
* numerical rounding errors, so the real input should have been zero
*/
template <typename T>
float safe_sqrt(const T v);
// matrix multiplication of two NxN matrices
2021-01-09 14:09:13 -04:00
template <typename T>
void mat_mul(const T *A, const T *B, T *C, uint16_t n);
// matrix inverse
2021-01-09 14:09:13 -04:00
template <typename T>
bool mat_inverse(const T *x, T *y, uint16_t dim) WARN_IF_UNUSED;
// matrix identity
2021-01-09 14:09:13 -04:00
template <typename T>
void mat_identity(T *x, uint16_t dim);
/*
* Constrain an angle to be within the range: -180 to 180 degrees. The second
* parameter changes the units. Default: 1 == degrees, 10 == dezi,
* 100 == centi.
*/
template <typename T>
T wrap_180(const T angle);
2013-06-01 05:18:50 -03:00
/*
* Wrap an angle in centi-degrees. See wrap_180().
2013-06-01 05:18:50 -03:00
*/
template <typename T>
T wrap_180_cd(const T angle);
2013-06-01 05:18:50 -03:00
2015-11-24 17:11:34 -04:00
/*
* Constrain an euler angle to be within the range: 0 to 360 degrees. The
* second parameter changes the units. Default: 1 == degrees, 10 == dezi,
* 100 == centi.
2015-11-24 17:11:34 -04:00
*/
float wrap_360(const float angle);
#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS
double wrap_360(const double angle);
#endif
int wrap_360(const int angle);
int wrap_360_cd(const int angle);
long wrap_360_cd(const long angle);
float wrap_360_cd(const float angle);
#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS
double wrap_360_cd(const double angle);
#endif
/*
wrap an angle in radians to -PI ~ PI (equivalent to +- 180 degrees)
*/
ftype wrap_PI(const ftype radian);
/*
* wrap an angle in radians to 0..2PI
*/
ftype wrap_2PI(const ftype radian);
2015-11-24 17:11:34 -04:00
/*
* Constrain a value to be within the range: low and high
*/
template <typename T>
T constrain_value(const T amt, const T low, const T high);
template <typename T>
T constrain_value_line(const T amt, const T low, const T high, uint32_t line);
#define constrain_float(amt, low, high) constrain_value_line(float(amt), float(low), float(high), uint32_t(__LINE__))
2021-05-04 08:12:23 -03:00
#define constrain_ftype(amt, low, high) constrain_value_line(ftype(amt), ftype(low), ftype(high), uint32_t(__LINE__))
inline int16_t constrain_int16(const int16_t amt, const int16_t low, const int16_t high)
{
return constrain_value(amt, low, high);
}
inline uint16_t constrain_uint16(const uint16_t amt, const uint16_t low, const uint16_t high)
{
return constrain_value(amt, low, high);
}
inline int32_t constrain_int32(const int32_t amt, const int32_t low, const int32_t high)
{
return constrain_value(amt, low, high);
}
inline uint32_t constrain_uint32(const uint32_t amt, const uint32_t low, const uint32_t high)
{
return constrain_value(amt, low, high);
}
inline int64_t constrain_int64(const int64_t amt, const int64_t low, const int64_t high)
{
return constrain_value(amt, low, high);
}
inline uint64_t constrain_uint64(const uint64_t amt, const uint64_t low, const uint64_t high)
{
return constrain_value(amt, low, high);
}
inline double constrain_double(const double amt, const double low, const double high)
{
return constrain_value(amt, low, high);
}
// degrees -> radians
static inline constexpr ftype radians(ftype deg)
{
return deg * DEG_TO_RAD;
}
// radians -> degrees
static inline constexpr float degrees(float rad)
{
return rad * RAD_TO_DEG;
}
template<typename T>
2021-05-04 08:12:23 -03:00
ftype sq(const T val)
{
2021-05-04 08:12:23 -03:00
ftype v = static_cast<ftype>(val);
return v*v;
}
static inline constexpr float sq(const float val)
{
return val*val;
}
/*
* Variadic template for calculating the square norm of a vector of any
* dimension.
*/
template<typename T, typename... Params>
2021-05-04 08:12:23 -03:00
ftype sq(const T first, const Params... parameters)
{
return sq(first) + sq(parameters...);
}
/*
* Variadic template for calculating the norm (pythagoras) of a vector of any
* dimension.
*/
template<typename T, typename U, typename... Params>
2021-05-04 08:12:23 -03:00
ftype norm(const T first, const U second, const Params... parameters)
{
2021-05-04 08:12:23 -03:00
return sqrtF(sq(first, second, parameters...));
}
#undef MIN
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)
{
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
return one < two ? one : two;
}
#undef MAX
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 MAX(const A &one, const B &two) -> decltype(one > two ? one : two)
{
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
return one > two ? one : two;
}
inline constexpr uint32_t hz_to_nsec(uint32_t freq)
{
return AP_NSEC_PER_SEC / freq;
}
inline constexpr uint32_t nsec_to_hz(uint32_t nsec)
{
return AP_NSEC_PER_SEC / nsec;
}
inline constexpr uint32_t usec_to_nsec(uint32_t usec)
{
return usec * AP_NSEC_PER_USEC;
}
inline constexpr uint32_t nsec_to_usec(uint32_t nsec)
{
return nsec / AP_NSEC_PER_USEC;
}
inline constexpr uint32_t hz_to_usec(uint32_t freq)
{
return AP_USEC_PER_SEC / freq;
}
inline constexpr uint32_t usec_to_hz(uint32_t usec)
{
return AP_USEC_PER_SEC / usec;
}
/*
linear interpolation based on a variable in a range
return value will be in the range [var_low,var_high]
Either polarity is supported, so var_low can be higher than var_high
*/
float linear_interpolate(float low_output, float high_output,
float var_value,
float var_low, float var_high);
2016-11-17 01:23:11 -04:00
/* cubic "expo" curve generator
* alpha range: [0,1] min to max expo
* input range: [-1,1]
*/
float expo_curve(float alpha, float input);
/* throttle curve generator
* thr_mid: output at mid stick
* alpha: expo coefficient
* thr_in: [0-1]
*/
float throttle_curve(float thr_mid, float alpha, float thr_in);
2016-11-17 01:23:11 -04:00
/* simple 16 bit random number generator */
uint16_t get_random16(void);
2017-04-12 08:16:07 -03:00
// generate a random float between -1 and 1, for use in SITL
float rand_float(void);
// generate a random Vector3f of size 1
Vector3f rand_vec3f(void);
2018-10-01 00:04:11 -03:00
// return true if two rotations are equal
bool rotation_equal(enum Rotation r1, enum Rotation r2) WARN_IF_UNUSED;
/*
* return a velocity correction (in m/s in NED) for a sensor's position given it's position offsets
* this correction should be added to the sensor NED measurement
* sensor_offset_bf is in meters in body frame (Foward, Right, Down)
* rot_ef_to_bf is a rotation matrix to rotate from earth-frame (NED) to body frame
* angular_rate is rad/sec
*/
2021-05-04 08:12:23 -03:00
Vector3F get_vel_correction_for_sensor_offset(const Vector3F &sensor_offset_bf, const Matrix3F &rot_ef_to_bf, const Vector3F &angular_rate);
/*
calculate a low pass filter alpha value
*/
float calc_lowpass_alpha_dt(float dt, float cutoff_freq);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// fill an array of float with NaN, used to invalidate memory in SITL
void fill_nanf(float *f, uint16_t count);
2021-05-04 08:12:23 -03:00
void fill_nanf(double *f, uint16_t count);
#endif
// from https://embeddedartistry.com/blog/2018/07/12/simple-fixed-point-conversion-in-c/
// Convert to/from 16-bit fixed-point and float
float fixed2float(const uint16_t input, const uint8_t fractional_bits = 8);
uint16_t float2fixed(const float input, const uint8_t fractional_bits = 8);
/*
calculate turn rate in deg/sec given a bank angle and airspeed for a
fixed wing aircraft
*/
float fixedwing_turn_rate(float bank_angle_deg, float airspeed);
2021-12-22 17:26:26 -04:00
// convert degrees farenheight to Kelvin
float degF_to_Kelvin(float temp_f);
/*
conversion functions to prevent undefined behaviour
*/
int16_t float_to_int16(const float v);
uint16_t float_to_uint16(const float v);
int32_t float_to_int32(const float v);
uint32_t float_to_uint32(const float v);
uint32_t double_to_uint32(const double v);
int32_t double_to_int32(const double v);