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

9
libraries/AP_Math/AP_Math.cpp
View File

@ -400,9 +400,16 @@ Vector3F get_vel_correction_for_sensor_offset(const Vector3F &sensor_offset_bf,
*/ */
float calc_lowpass_alpha_dt(float dt, float cutoff_freq) 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; return 1.0;
} }
if (is_zero(cutoff_freq)) {
return 1.0;
}
if (is_zero(dt)) {
return 0.0;
}
float rc = 1.0f / (M_2PI * cutoff_freq); float rc = 1.0f / (M_2PI * cutoff_freq);
return dt / (dt + rc); return dt / (dt + rc);
} }

6
libraries/AP_Math/control.cpp
View File

@ -112,10 +112,12 @@ void shape_accel(float accel_input, float& accel,
} }
// jerk limit acceleration change // jerk limit acceleration change
if (is_positive(dt)) {
float accel_delta = accel_input - accel; float accel_delta = accel_input - accel;
accel_delta = constrain_float(accel_delta, -jerk_max * dt, jerk_max * dt); accel_delta = constrain_float(accel_delta, -jerk_max * dt, jerk_max * dt);
accel += accel_delta; accel += accel_delta;
} }
}
// 2D version // 2D version
void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel, void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
@ -128,10 +130,12 @@ void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
} }
// jerk limit acceleration change // jerk limit acceleration change
if (is_positive(dt)) {
Vector2f accel_delta = accel_input - accel; Vector2f accel_delta = accel_input - accel;
accel_delta.limit_length(jerk_max * dt); accel_delta.limit_length(jerk_max * dt);
accel = accel + accel_delta; accel = accel + accel_delta;
} }
}
void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel, void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel,
float jerk_max, float dt) float jerk_max, float dt)
@ -410,7 +414,7 @@ float sqrt_controller(float error, float p, float second_ord_lim, float dt)
correction_rate = error * p; 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. // 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); return constrain_float(correction_rate, -fabsf(error) / dt, fabsf(error) / dt);
} else { } else {

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

@ -665,10 +665,8 @@ TEST(MathTest, VELCORRECTION)
TEST(MathTest, LOWPASSALPHA) TEST(MathTest, LOWPASSALPHA)
{ {
const float accuracy = 1.0e-5f; const float accuracy = 1.0e-5f;
EXPECT_EQ(1.0f, calc_lowpass_alpha_dt(0.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, 2.0f));
EXPECT_EQ(1.0f, calc_lowpass_alpha_dt(1.0f, 0.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); EXPECT_NEAR(0.926288f, calc_lowpass_alpha_dt(1.0f, 2.0f), accuracy);
} }