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

ekf2: Don't send un-usable mag and baro data to the EKF

Browse Source 
Fixes:

1) Invalid data with a zero time stamp could be the EKF ends up in the data buffers and result in loss of 'good' data from the buffers

2) Magnetometer data was arriving at a rate faster than the data buffers could handle resulting in loss of data.
This commit is contained in:
Paul Riseborough 2016-11-06 17:10:44 +11:00 committed by Beat Küng
parent 7d0d29f62d
commit ef7ed97cbd
1 changed files with 57 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
src/modules/ekf2/ekf2_main.cpp
View File

@ -135,6 +135,27 @@ private:
float _default_ev_pos_noise = 0.05f; // external vision position noise used when an invalid value is supplied
float _default_ev_ang_noise = 0.05f; // external vision angle noise used when an invalid value is supplied
// Initialise time stamps used to send sensor data to the EKF and for logging
uint64_t _timestamp_mag_us = 0;
uint64_t _timestamp_balt_us = 0;
// Used to down sample magnetometer data
float _mag_data_sum[3]; // summed magnetometer readings (Ga)
uint64_t _mag_time_sum_ms; // summed magnetoemter time stamps (msec)
uint8_t _mag_sample_count = 0; // number of magnetometer measurements summed
uint32_t _mag_time_ms_last_used = 0; // time stamp in msec of the last averaged magnetometer measurement used by the EKF
int _sensors_sub = -1;
int _gps_sub = -1;
int _airspeed_sub = -1;
int _params_sub = -1;
int _optical_flow_sub = -1;
int _range_finder_sub = -1;
int _ev_pos_sub = -1;
int _actuator_armed_sub = -1;
int _vehicle_land_detected_sub = -1;
int _status_sub = -1;
bool _prev_landed = true; // landed status from the previous frame
float _acc_hor_filt = 0.0f; // low-pass filtered horizontal acceleration
@ -513,18 +534,47 @@ void Ekf2::task_main()
// read mag data
if (sensors.magnetometer_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
_ekf.setMagData(0, sensors.magnetometer_ga);
// set a zero timestamp to let the ekf replay program know that this data is not valid
_timestamp_mag_us = 0;
} else {
_ekf.setMagData(sensors.timestamp + sensors.magnetometer_timestamp_relative, sensors.magnetometer_ga);
if ((sensors.timestamp + sensors.magnetometer_timestamp_relative) != _timestamp_mag_us) {
_timestamp_mag_us = sensors.timestamp + sensors.magnetometer_timestamp_relative;
// If the time last used by the EKF is less than 50msec, then accumulate the
// data and push the average when the 50msec is reached.
_mag_time_sum_ms += _timestamp_mag_us/1000;
_mag_sample_count++;
_mag_data_sum[0] += sensors.magnetometer_ga[0];
_mag_data_sum[1] += sensors.magnetometer_ga[1];
_mag_data_sum[2] += sensors.magnetometer_ga[2];
uint64_t mag_time_ms = _mag_time_sum_ms / _mag_sample_count;
if (mag_time_ms - _mag_time_ms_last_used > 50) {
float mag_sample_count_inv = 1.0f / (float)_mag_sample_count;
float mag_data_avg_ga[3] = {_mag_data_sum[0]*mag_sample_count_inv , _mag_data_sum[1]*mag_sample_count_inv , _mag_data_sum[2]*mag_sample_count_inv};
_ekf.setMagData(1000*(uint64_t)mag_time_ms, mag_data_avg_ga);
_mag_time_ms_last_used = mag_time_ms;
_mag_time_sum_ms = 0;
_mag_sample_count = 0;
_mag_data_sum[0] = 0.0f;
_mag_data_sum[1] = 0.0f;
_mag_data_sum[2] = 0.0f;
}
}
}
// read baro data
if (sensors.baro_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
_ekf.setBaroData(0, &sensors.baro_alt_meter);
// set a zero timestamp to let the ekf replay program know that this data is not valid
_timestamp_balt_us = 0;
} else {
_ekf.setBaroData(sensors.timestamp + sensors.baro_timestamp_relative, &sensors.baro_alt_meter);
if ((sensors.timestamp + sensors.baro_timestamp_relative) != _timestamp_balt_us) {
_timestamp_balt_us = sensors.timestamp + sensors.baro_timestamp_relative;
_ekf.setBaroData(_timestamp_balt_us, &sensors.baro_alt_meter);
}
}
// read gps data if available
@ -548,6 +598,7 @@ void Ekf2::task_main()
gps_msg.gdop = 0.0f;
_ekf.setGpsData(gps.timestamp, &gps_msg);
}
// only set airspeed data if condition for airspeed fusion are met
@ -932,8 +983,8 @@ void Ekf2::task_main()
replay.time_ref = now;
replay.gyro_integral_dt = sensors.gyro_integral_dt;
replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt;
replay.magnetometer_timestamp = sensors.timestamp + sensors.magnetometer_timestamp_relative;
replay.baro_timestamp = sensors.timestamp + sensors.baro_timestamp_relative;
replay.magnetometer_timestamp = _timestamp_mag_us;
replay.baro_timestamp = _timestamp_balt_us;
memcpy(replay.gyro_rad, sensors.gyro_rad, sizeof(replay.gyro_rad));
memcpy(replay.accelerometer_m_s2, sensors.accelerometer_m_s2, sizeof(replay.accelerometer_m_s2));
memcpy(replay.magnetometer_ga, sensors.magnetometer_ga, sizeof(replay.magnetometer_ga));