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

ekf2: properly reset IMU biases on calibration change (non-multi-EKF)

Browse Source 
- this was working in the multi-EKF case using vehicle_imu, but missing
in sensor_combined
This commit is contained in:
Daniel Agar
2022-04-11 12:23:55 -04:00
parent 8f891332f1
commit bb2ea574aa
3 changed files with 29 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/voted_sensors_update.cpp
+4 -1
View File
@@ -172,7 +172,8 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
_last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1);
_last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2);
_last_sensor_data[uorb_index].gyro_integral_dt = imu_report.delta_angle_dt;
_last_sensor_data[uorb_index].accel_calibration_count = imu_report.accel_calibration_count;
_last_sensor_data[uorb_index].gyro_calibration_count = imu_report.gyro_calibration_count;
_last_accel_timestamp[uorb_index] = imu_report.timestamp_sample;
@@ -231,6 +232,8 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
raw.accelerometer_integral_dt = _last_sensor_data[accel_best_index].accelerometer_integral_dt;
raw.gyro_integral_dt = _last_sensor_data[gyro_best_index].gyro_integral_dt;
raw.accelerometer_clipping = _last_sensor_data[accel_best_index].accelerometer_clipping;
raw.accel_calibration_count = _last_sensor_data[accel_best_index].accel_calibration_count;
raw.gyro_calibration_count = _last_sensor_data[gyro_best_index].gyro_calibration_count;
if ((accel_best_index != _accel.last_best_vote) || (_selection.accel_device_id != _accel_device_id[accel_best_index])) {
_accel.last_best_vote = (uint8_t)accel_best_index;