Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: sync for 4.1.0beta releases

Browse Source
This commit is contained in:
Andrew Tridgell 2021-07-22 13:40:11 +10:00
parent 1d46e77942
commit dd2995af5d
13 changed files with 59 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_Math/AP_Math.cpp
View File

@ -244,36 +244,25 @@ long wrap_360_cd(const long angle)
}
return res;
}
template <typename T>
float wrap_PI(const T radian)
ftype wrap_PI(const ftype radian)
{
auto res = wrap_2PI(radian);
ftype res = wrap_2PI(radian);
if (res > M_PI) {
res -= M_2PI;
}
return res;
}
template float wrap_PI<int>(const int radian);
template float wrap_PI<short>(const short radian);
template float wrap_PI<float>(const float radian);
template float wrap_PI<double>(const double radian);
template <typename T>
float wrap_2PI(const T radian)
ftype wrap_2PI(const ftype radian)
{
float res = fmodf(static_cast<float>(radian), M_2PI);
ftype res = fmodF(radian, M_2PI);
if (res < 0) {
res += M_2PI;
}
return res;
}
template float wrap_2PI<int>(const int radian);
template float wrap_2PI<short>(const short radian);
template float wrap_2PI<float>(const float radian);
template float wrap_2PI<double>(const double radian);
template <typename T>
T constrain_value_line(const T amt, const T low, const T high, uint32_t line)
{
@ -423,7 +412,7 @@ void fill_nanf(float *f, uint16_t count)
void fill_nanf(double *f, uint16_t count)
{
while (count--) {
*f++ = 0;
*f++ = std::numeric_limits<double>::signaling_NaN();
}
}
#endif

8
libraries/AP_Math/AP_Math.h
View File

@ -142,14 +142,12 @@ double wrap_360_cd(const double angle);
/*
wrap an angle in radians to -PI ~ PI (equivalent to +- 180 degrees)
*/
template <typename T>
float wrap_PI(const T radian);
ftype wrap_PI(const ftype radian);
/*
* wrap an angle in radians to 0..2PI
*/
template <typename T>
float wrap_2PI(const T radian);
ftype wrap_2PI(const ftype radian);
/*
* Constrain a value to be within the range: low and high
@ -179,7 +177,7 @@ inline int64_t constrain_int64(const int64_t amt, const int64_t low, const int64
}
// degrees -> radians
static inline constexpr float radians(float deg)
static inline constexpr ftype radians(ftype deg)
{
return deg * DEG_TO_RAD;
}

4
libraries/AP_Math/SCurve.cpp
View File

@ -738,7 +738,7 @@ void SCurve::add_segments(float L)
// Vm - maximum constant velocity
// L - Length of the path
// t2_out, t4_out, t6_out are the segment durations needed to achieve the kinimatic path specified by the input variables
void SCurve::calculate_path(float tj, float Jm, float V0, float Am, float Vm, float L, float &Jm_out, float &t2_out, float &t4_out, float &t6_out) const
void SCurve::calculate_path(float tj, float Jm, float V0, float Am, float Vm, float L, float &Jm_out, float &t2_out, float &t4_out, float &t6_out)
{
// init outputs
Jm_out = 0.0f;
@ -772,6 +772,7 @@ void SCurve::calculate_path(float tj, float Jm, float V0, float Am, float Vm, fl
// solution = 2 - t6 t4 t2 = 0 1 0
t2_out = 0.0f;
t4_out = MIN(-(V0 - Vm + Am * tj + (Am * Am) / Jm) / Am, MAX(((Am * Am) * (-3.0f / 2.0f) + safe_sqrt((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm), ((Am * Am) * (-3.0f / 2.0f) - safe_sqrt((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm)));
t4_out = MAX(t4_out, 0.0);
t6_out = 0.0f;
}
} else {
@ -785,6 +786,7 @@ void SCurve::calculate_path(float tj, float Jm, float V0, float Am, float Vm, fl
// solution = 7 - t6 t4 t2 = 1 1 1
t2_out = Am / Jm - tj;
t4_out = MIN(-(V0 - Vm + Am * tj + (Am * Am) / Jm) / Am, MAX(((Am * Am) * (-3.0f / 2.0f) + safe_sqrt((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm), ((Am * Am) * (-3.0f / 2.0f) - safe_sqrt((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm)));
t4_out = MAX(t4_out, 0.0);
t6_out = t2_out;
}
}

19
libraries/AP_Math/SCurve.h
View File

@ -52,6 +52,16 @@ public:
// initialise and clear the path
void init();
// calculate the segment times for the trigonometric S-Curve path defined by:
// tj - duration of the raised cosine jerk profile (aka jerk time)
// Jm - maximum value of the raised cosine jerk profile (aka jerk max)
// V0 - initial velocity magnitude
// Am - maximum constant acceleration
// Vm - maximum constant velocity
// L - Length of the path
// this is an internal function, static for test suite
static void calculate_path(float tj, float Jm, float V0, float Am, float Vm, float L, float &Jm_out, float &t2_out, float &t4_out, float &t6_out);
// generate a trigonometric track in 3D space that moves over a straight line
// between two points defined by the origin and destination
void calculate_track(const Vector3f &origin, const Vector3f &destination,
@ -135,15 +145,6 @@ private:
// generate time segments for straight segment
void add_segments(float L);
// calculate the segment times for the trigonometric S-Curve path defined by:
// tj - duration of the raised cosine jerk profile (aka jerk time)
// Jm - maximum value of the raised cosine jerk profile (aka jerk max)
// V0 - initial velocity magnitude
// Am - maximum constant acceleration
// Vm - maximum constant velocity
// L - Length of the path
void calculate_path(float tj, float Jm, float V0, float Am, float Vm, float L, float &Jm_out, float &t2_out, float &t4_out, float &t6_out) const;
// generate three time segments forming the jerk profile
void add_segments_jerk(uint8_t &seg_pnt, float tj, float Jm, float Tcj);

11
libraries/AP_Math/control.cpp
View File

@ -155,6 +155,17 @@ void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
}
}
void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel,
float accel_max, float tc, float dt)
{
const Vector2f accel_input_2f {accel_input.x, accel_input.y};
Vector2f accel_2f {accel.x, accel.y};
shape_accel_xy(accel_input_2f, accel_2f, accel_max, tc, dt);
accel.x = accel_2f.x;
accel.y = accel_2f.y;
}
/* shape_vel_accel and shape_vel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input velocity.
The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.

5
libraries/AP_Math/control.h
View File

@ -2,6 +2,8 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/AP_HAL_Boards.h>
#include "vector2.h"
#include "vector3.h"
#ifndef HAL_WITH_POSTYPE_DOUBLE
#define HAL_WITH_POSTYPE_DOUBLE BOARD_FLASH_SIZE > 1024
@ -53,6 +55,9 @@ void shape_accel(const float accel_input, float& accel,
void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
const float accel_max, const float tc, const float dt);
void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel,
float accel_max, float tc, float dt);
/* shape_vel calculates a jerk limited path from the current velocity and acceleration to an input velocity.
The function takes the current velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
The kinematic path is constrained by :

2
libraries/AP_Math/definitions.h
View File

@ -7,7 +7,7 @@
#ifdef M_PI
# undef M_PI
#endif
#define M_PI (3.141592653589793f)
#define M_PI (3.141592653589793)
#ifdef M_PI_2
# undef M_PI_2

2
libraries/AP_Math/ftype.h
View File

@ -26,6 +26,7 @@ typedef double ftype;
#define fabsF(x) fabs(x)
#define ceilF(x) ceil(x)
#define fminF(x,y) fmin(x,y)
#define fmodF(x,y) fmod(x,y)
#define toftype todouble
#else
typedef float ftype;
@ -43,6 +44,7 @@ typedef float ftype;
#define fabsF(x) fabsf(x)
#define ceilF(x) ceilf(x)
#define fminF(x,y) fminf(x,y)
#define fmodF(x,y) fmodf(x,y)
#define toftype tofloat
#endif

47
libraries/AP_Math/quaternion.cpp
View File

@ -71,33 +71,6 @@ void QuaternionT<T>::rotation_matrix(Matrix3f &m) const
m.c.z = 1.0f-2.0f*(q2q2 + q3q3);
}
// return the rotation matrix equivalent for this quaternion after normalization
template <typename T>
void QuaternionT<T>::rotation_matrix_norm(Matrix3<T> &m) const
{
const T q1q1 = q1 * q1;
const T q1q2 = q1 * q2;
const T q1q3 = q1 * q3;
const T q1q4 = q1 * q4;
const T q2q2 = q2 * q2;
const T q2q3 = q2 * q3;
const T q2q4 = q2 * q4;
const T q3q3 = q3 * q3;
const T q3q4 = q3 * q4;
const T q4q4 = q4 * q4;
const T invs = 1.0f / (q1q1 + q2q2 + q3q3 + q4q4);
m.a.x = ( q2q2 - q3q3 - q4q4 + q1q1)*invs;
m.a.y = 2.0f*(q2q3 - q1q4)*invs;
m.a.z = 2.0f*(q2q4 + q1q3)*invs;
m.b.x = 2.0f*(q2q3 + q1q4)*invs;
m.b.y = (-q2q2 + q3q3 - q4q4 + q1q1)*invs;
m.b.z = 2.0f*(q3q4 - q1q2)*invs;
m.c.x = 2.0f*(q2q4 - q1q3)*invs;
m.c.y = 2.0f*(q3q4 + q1q2)*invs;
m.c.z = (-q2q2 - q3q3 + q4q4 + q1q1)*invs;
}
// return the rotation matrix equivalent for this quaternion
// Thanks to Martin John Baker
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
@ -481,9 +454,9 @@ void QuaternionT<T>::from_axis_angle(const Vector3<T> &axis, T theta)
q2=q3=q4=0.0f;
return;
}
const T st2 = sinF(theta/2.0f);
const T st2 = sinF(0.5*theta);
q1 = cosF(theta/2.0f);
q1 = cosF(0.5*theta);
q2 = axis.x * st2;
q3 = axis.y * st2;
q4 = axis.z * st2;
@ -507,7 +480,7 @@ void QuaternionT<T>::to_axis_angle(Vector3<T> &v) const
v = Vector3<T>(q2,q3,q4);
if (!is_zero(l)) {
v /= l;
v *= wrap_PI(2.0f * atan2f(l,q1));
v *= wrap_PI(2.0f * atan2F(l,q1));
}
}
@ -531,11 +504,11 @@ void QuaternionT<T>::from_axis_angle_fast(Vector3<T> v)
template <typename T>
void QuaternionT<T>::from_axis_angle_fast(const Vector3<T> &axis, T theta)
{
const T t2 = theta/2.0f;
const T t2 = 0.5*theta;
const T sqt2 = sq(t2);
const T st2 = t2-sqt2*t2/6.0f;
q1 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f;
q1 = 1.0f-(0.5*sqt2)+sq(sqt2)/24.0f;
q2 = axis.x * st2;
q3 = axis.y * st2;
q4 = axis.z * st2;
@ -550,13 +523,13 @@ void QuaternionT<T>::rotate_fast(const Vector3<T> &v)
if (is_zero(theta)) {
return;
}
const T t2 = theta/2.0f;
const T t2 = 0.5*theta;
const T sqt2 = sq(t2);
T st2 = t2-sqt2*t2/6.0f;
st2 /= theta;
//"rotation quaternion"
const T w2 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f;
const T w2 = 1.0f-(0.5*sqt2)+sq(sqt2)/24.0f;
const T x2 = v.x * st2;
const T y2 = v.y * st2;
const T z2 = v.z * st2;
@ -578,7 +551,7 @@ void QuaternionT<T>::rotate_fast(const Vector3<T> &v)
template <typename T>
T QuaternionT<T>::get_euler_roll() const
{
return (atan2f(2.0f*(q1*q2 + q3*q4), 1.0f - 2.0f*(q2*q2 + q3*q3)));
return (atan2F(2.0f*(q1*q2 + q3*q4), 1.0f - 2.0f*(q2*q2 + q3*q3)));
}
// get euler pitch angle
@ -592,7 +565,7 @@ T QuaternionT<T>::get_euler_pitch() const
template <typename T>
T QuaternionT<T>::get_euler_yaw() const
{
return atan2f(2.0f*(q1*q4 + q2*q3), 1.0f - 2.0f*(q3*q3 + q4*q4));
return atan2F(2.0f*(q1*q4 + q2*q3), 1.0f - 2.0f*(q3*q3 + q4*q4));
}
// create eulers from a quaternion
@ -772,7 +745,7 @@ T QuaternionT<T>::roll_pitch_difference(const QuaternionT<T> &v) const
const T vec_len_div2 = constrain_float(vec_diff.length() * 0.5, 0.0, 1.0);
// calculate and return angular difference
return (2.0 * asinf(vec_len_div2));
return (2.0 * asinF(vec_len_div2));
}
// define for float and double

3
libraries/AP_Math/quaternion.h
View File

@ -67,9 +67,6 @@ public:
void rotation_matrix(Matrix3f &m) const;
void rotation_matrix(Matrix3d &m) const;
// return the rotation matrix equivalent for this quaternion after normalization
void rotation_matrix_norm(Matrix3<T> &m) const;
// return the rotation matrix equivalent for this quaternion
void from_rotation_matrix(const Matrix3<T> &m);

2
libraries/AP_Math/tests/test_vector2.cpp
View File

@ -109,7 +109,7 @@ TEST(Vector2Test, angle)
{
EXPECT_FLOAT_EQ(M_PI/2, Vector2f(0, 1).angle());
EXPECT_FLOAT_EQ(M_PI/4, Vector2f(1, 1).angle());
EXPECT_FLOAT_EQ(0.0f, Vector2f(1, 0).angle());
EXPECT_FLOAT_EQ(0.0f, Vector2d(1, 0).angle());
EXPECT_FLOAT_EQ(M_PI*5/4, Vector2f(-1, -1).angle());
EXPECT_FLOAT_EQ(M_PI*5/4, Vector2f(-5, -5).angle());

1
libraries/AP_Math/vector2.h
View File

@ -34,6 +34,7 @@
#endif
#include <cmath>
#include <float.h>
#include <AP_Common/AP_Common.h>
#include "ftype.h"

6
libraries/AP_Math/vector3.h
View File

@ -87,6 +87,12 @@ public:
, y(y0)
, z(z0) {}
//Create a Vector3 from a Vector2 with z
constexpr Vector3<T>(const Vector2<T> &v0, const T z0)
: x(v0.x)
, y(v0.y)
, z(z0) {}
// test for equality
bool operator ==(const Vector3<T> &v) const;