Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 07:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_DAL: setup low pass IMU filter for 20Hz

Browse Source
This commit is contained in:
Andrew Tridgell 2020-11-08 21:57:50 +11:00
parent 9e73922fd9
commit 8cb3013217
2 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_DAL/AP_DAL_InertialSensor.cpp
View File

@ -55,10 +55,16 @@ void AP_DAL_InertialSensor::start_frame()
// update filtered gyro and accel
void AP_DAL_InertialSensor::update_filtered(uint8_t i)
{
if (!is_positive(alpha)) {
// we use a constant 20Hz for EKF filtered accel/gyro, making the EKF
// independent of the INS filter settings
const float cutoff_hz = 20.0;
alpha = calc_lowpass_alpha_dt(get_loop_delta_t(), cutoff_hz);
}
if (is_positive(_RISI[i].delta_angle_dt)) {
gyro_filtered[i] = gyro_filtered[i] * alpha + (_RISI[i].delta_angle/_RISI[i].delta_angle_dt) * (1-alpha);
gyro_filtered[i] += ((_RISI[i].delta_angle/_RISI[i].delta_angle_dt) - gyro_filtered[i]) * alpha;
}
if (is_positive(_RISI[i].delta_velocity_dt)) {
accel_filtered[i] = accel_filtered[i] * alpha + (_RISI[i].delta_velocity/_RISI[i].delta_velocity_dt) * (1-alpha);
accel_filtered[i] += ((_RISI[i].delta_velocity/_RISI[i].delta_velocity_dt) - accel_filtered[i]) * alpha;
}
}

4
libraries/AP_DAL/AP_DAL_InertialSensor.h
View File

@ -63,11 +63,9 @@ public:
}
private:
// filter constant for deltas to gyro/accel
const float alpha = 0.9;
struct log_RISH _RISH;
struct log_RISI _RISI[INS_MAX_INSTANCES];
float alpha;
// sensor positions
Vector3f pos[INS_MAX_INSTANCES];