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

AP_DAL: check for zero time in accel/gyro filtering

Browse Source
This commit is contained in:
Andrew Tridgell 2020-11-07 21:43:38 +11:00
parent 367c993673
commit 70abf799c3
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_DAL/AP_DAL_InertialSensor.cpp
View File

@ -51,6 +51,10 @@ void AP_DAL_InertialSensor::start_frame()
// update filtered gyro and accel
void AP_DAL_InertialSensor::update_filtered(uint8_t i)
{
gyro_filtered[i] = gyro_filtered[i] * alpha + (_RISI[i].delta_angle/_RISI[i].delta_angle_dt) * (1-alpha);
accel_filtered[i] = accel_filtered[i] * alpha + (_RISI[i].delta_velocity/_RISI[i].delta_velocity_dt) * (1-alpha);
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);
}
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);
}
}