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

AP_Math: added calc_lowpass_alpha_dt()

Browse Source 
used in several places
This commit is contained in:
Andrew Tridgell 2020-11-08 21:56:41 +11:00
parent 6ee1b23d28
commit cdcf32d22c
2 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Math/AP_Math.cpp
View File

@ -382,6 +382,18 @@ Vector3f get_vel_correction_for_sensor_offset(const Vector3f &sensor_offset_bf,
return rot_ef_to_bf.mul_transpose(vel_offset_body) * -1.0f;
}
/*
calculate a low pass filter alpha value
*/
float calc_lowpass_alpha_dt(float dt, float cutoff_freq)
{
if (dt <= 0.0f || cutoff_freq <= 0.0f) {
return 1.0;
}
float rc = 1.0f/(M_2PI*cutoff_freq);
return constrain_float(dt/(dt+rc), 0.0f, 1.0f);
}
#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)

5
libraries/AP_Math/AP_Math.h
View File

@ -290,6 +290,11 @@ bool rotation_equal(enum Rotation r1, enum Rotation r2) WARN_IF_UNUSED;
*/
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);