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

EKF2 use simplified ecl/EKF setIMUData

Browse Source
This commit is contained in:
Daniel Agar
2019-02-06 20:20:51 -05:00
parent ddc9522712
commit 07fbd2202a
1 changed files with 7 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+7 -12
View File
@@ -808,19 +808,14 @@ void Ekf2::run()
const hrt_abstime now = sensors.timestamp;
// push imu data into estimator
float gyro_integral[3];
float gyro_dt = sensors.gyro_integral_dt / 1.e6f;
gyro_integral[0] = sensors.gyro_rad[0] * gyro_dt;
gyro_integral[1] = sensors.gyro_rad[1] * gyro_dt;
gyro_integral[2] = sensors.gyro_rad[2] * gyro_dt;
imuSample imu_sample_new;
imu_sample_new.time_us = now;
imu_sample_new.delta_ang_dt = sensors.gyro_integral_dt * 1.e-6f;
imu_sample_new.delta_ang = Vector3f{sensors.gyro_rad} * imu_sample_new.delta_ang_dt;
imu_sample_new.delta_vel_dt = sensors.accelerometer_integral_dt * 1.e-6f;
imu_sample_new.delta_vel = Vector3f{sensors.accelerometer_m_s2} * imu_sample_new.delta_vel_dt;
float accel_integral[3];
float accel_dt = sensors.accelerometer_integral_dt / 1.e6f;
accel_integral[0] = sensors.accelerometer_m_s2[0] * accel_dt;
accel_integral[1] = sensors.accelerometer_m_s2[1] * accel_dt;
accel_integral[2] = sensors.accelerometer_m_s2[2] * accel_dt;
_ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt, gyro_integral, accel_integral);
_ekf.setIMUData(imu_sample_new);
// publish attitude immediately (uses quaternion from output predictor)
publish_attitude(sensors, now);