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

EKF: Ensure all data in buffers is initialised

Browse Source 
This is a defensive change to prevent introduction of NaN's into the filter if data is read from the incorrect place in the buffer.
This commit is contained in:
Paul Riseborough 2016-03-20 15:13:27 +11:00
parent 064a0e4dbc
commit 400a6e12ba
1 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
EKF/estimator_interface.cpp
View File

@ -299,6 +299,29 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
return false;
}
// zero the data in the observation buffers
for (int index=0; index < OBS_BUFFER_LENGTH; index++) {
gpsSample gps_sample_init = {};
_gps_buffer.push(gps_sample_init);
magSample mag_sample_init = {};
_mag_buffer.push(mag_sample_init);
baroSample baro_sample_init = {};
_baro_buffer.push(baro_sample_init);
rangeSample range_sample_init = {};
_range_buffer.push(range_sample_init);
airspeedSample airspeed_sample_init = {};
_airspeed_buffer.push(airspeed_sample_init);
flowSample flow_sample_init = {};
_flow_buffer.push(flow_sample_init);
}
// zero the data in the imu data and output observer state buffers
for (int index=0; index < IMU_BUFFER_LENGTH; index++) {
imuSample imu_sample_init = {};
_imu_buffer.push(imu_sample_init);
outputSample output_sample_init = {};
_output_buffer.push(output_sample_init);
}
_dt_imu_avg = 0.0f;