ardupilot/libraries/AP_Math/AP_Math.h

310 lines
8.3 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"
// 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");
2016-12-19 08:59:44 -04:00
return (fabsf(static_cast<float>(fVal1)) < FLT_EPSILON);
}
/*
* @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));
}
/*
* 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)
*/
template <typename T>
float wrap_PI(const T radian);
/*
* wrap an angle in radians to 0..2PI
*/
template <typename T>
float wrap_2PI(const T 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__))
inline int16_t constrain_int16(const int16_t amt, const int16_t low, const int16_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 int64_t constrain_int64(const int64_t amt, const int64_t low, const int64_t high)
{
return constrain_value(amt, low, high);
}
// degrees -> radians
static inline constexpr float radians(float deg)
{
return deg * DEG_TO_RAD;
}
// radians -> degrees
static inline constexpr float degrees(float rad)
{
return rad * RAD_TO_DEG;
}
template<typename T>
float sq(const T val)
{
float v = static_cast<float>(val);
return v*v;
}
/*
* Variadic template for calculating the square norm of a vector of any
* dimension.
*/
template<typename T, typename... Params>
float 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>
float norm(const T first, const U second, const Params... parameters)
{
return sqrtf(sq(first, second, parameters...));
}
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;
}
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
*/
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]
*/
constexpr 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
*/
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);
#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);