Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues 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-10 16:15:45 +11:00
parent 6ee1b23d28
commit cdcf32d22c
2 changed files with 17 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AP_Math/AP_Math.cpp
+12
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)