fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 06:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2 handle accelerometer clipping

Browse Source 
- track clipping per IMU axis and pass through to ecl/EKF
 - update ecl/EKF to include delta velocity clipping changes (PX4/ecl#663)
This commit is contained in:
Daniel Agar
2020-04-07 20:11:08 -04:00
committed by GitHub
parent 74aa3201ce
commit cf37be8c44
12 changed files with 101 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+12
View File
@@ -777,6 +777,12 @@ void Ekf2::Run()
imu_sample_new.delta_vel_dt = imu.dt * 1.e-6f;
imu_sample_new.delta_vel = Vector3f{imu.delta_velocity};
if (imu.delta_velocity_clipping > 0) {
imu_sample_new.delta_vel_clipping[0] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_X;
imu_sample_new.delta_vel_clipping[1] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Y;
imu_sample_new.delta_vel_clipping[2] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Z;
}
imu_dt = imu.dt;
bias.accel_device_id = imu.accel_device_id;
@@ -792,6 +798,12 @@ void Ekf2::Run()
imu_sample_new.delta_vel_dt = sensor_combined.accelerometer_integral_dt * 1.e-6f;
imu_sample_new.delta_vel = Vector3f{sensor_combined.accelerometer_m_s2} * imu_sample_new.delta_vel_dt;
if (sensor_combined.accelerometer_clipping > 0) {
imu_sample_new.delta_vel_clipping[0] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_X;
imu_sample_new.delta_vel_clipping[1] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Y;
imu_sample_new.delta_vel_clipping[2] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Z;
}
imu_dt = sensor_combined.gyro_integral_dt;
}