aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Math: Support changing update period

Browse Source
This commit is contained in:
Leonard Hall 2022-12-04 21:05:37 +10:30 committed by Randy Mackay
parent 4104da3864
commit 67024a9516
3 changed files with 22 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_Math/AP_Math.cpp
View File

@ -400,11 +400,18 @@ Vector3F get_vel_correction_for_sensor_offset(const Vector3F &sensor_offset_bf,
*/
float calc_lowpass_alpha_dt(float dt, float cutoff_freq)
{
if (dt <= 0.0f || cutoff_freq <= 0.0f) {
if (is_negative(dt) || is_negative(cutoff_freq)) {
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
return 1.0;
}
float rc = 1.0f/(M_2PI*cutoff_freq);
return dt/(dt+rc);
if (is_zero(cutoff_freq)) {
return 1.0;
}
if (is_zero(dt)) {
return 0.0;
}
float rc = 1.0f / (M_2PI * cutoff_freq);
return dt / (dt + rc);
}
#ifndef AP_MATH_FILL_NANF_USE_MEMCPY

18
libraries/AP_Math/control.cpp
View File

@ -112,9 +112,11 @@ void shape_accel(float accel_input, float& accel,
}
// jerk limit acceleration change
float accel_delta = accel_input - accel;
accel_delta = constrain_float(accel_delta, -jerk_max * dt, jerk_max * dt);
accel += accel_delta;
if (is_positive(dt)) {
float accel_delta = accel_input - accel;
accel_delta = constrain_float(accel_delta, -jerk_max * dt, jerk_max * dt);
accel += accel_delta;
}
}
// 2D version
@ -128,9 +130,11 @@ void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
}
// jerk limit acceleration change
Vector2f accel_delta = accel_input - accel;
accel_delta.limit_length(jerk_max * dt);
accel = accel + accel_delta;
if (is_positive(dt)) {
Vector2f accel_delta = accel_input - accel;
accel_delta.limit_length(jerk_max * dt);
accel = accel + accel_delta;
}
}
void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel,
@ -410,7 +414,7 @@ float sqrt_controller(float error, float p, float second_ord_lim, float dt)
correction_rate = error * p;
}
}
if (!is_zero(dt)) {
if (is_positive(dt)) {
// this ensures we do not get small oscillations by over shooting the error correction in the last time step.
return constrain_float(correction_rate, -fabsf(error) / dt, fabsf(error) / dt);
} else {

4
libraries/AP_Math/tests/test_math.cpp
View File

@ -665,10 +665,8 @@ TEST(MathTest, VELCORRECTION)
TEST(MathTest, LOWPASSALPHA)
{
const float accuracy = 1.0e-5f;
EXPECT_EQ(1.0f, calc_lowpass_alpha_dt(0.0f, 2.0f));
EXPECT_EQ(1.0f, calc_lowpass_alpha_dt(-1.0f, 2.0f));
EXPECT_EQ(0.0f, calc_lowpass_alpha_dt(0.0f, 2.0f));
EXPECT_EQ(1.0f, calc_lowpass_alpha_dt(1.0f, 0.0f));
EXPECT_EQ(1.0f, calc_lowpass_alpha_dt(1.0f, -2.0f));
EXPECT_NEAR(0.926288f, calc_lowpass_alpha_dt(1.0f, 2.0f), accuracy);
}