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

ekf2: move BaroSample update to UpdateBaroSample()

Browse Source
This commit is contained in:
Daniel Agar
2020-11-11 20:17:52 -05:00
parent 6b30f01daf
commit 0f56793116
2 changed files with 20 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+19 -14
View File
@@ -421,20 +421,7 @@ void EKF2::Run()
}
}
// read baro data
if (_airdata_sub.updated()) {
vehicle_air_data_s airdata;
if (_airdata_sub.copy(&airdata)) {
_ekf.set_air_density(airdata.rho);
const baroSample baro_sample {airdata.baro_alt_meter, airdata.timestamp_sample};
_ekf.setBaroData(baro_sample);
ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
_device_id_baro = airdata.baro_device_id;
}
}
UpdateBaroSample(ekf2_timestamps);
if (_vehicle_gps_position_sub.updated()) {
vehicle_gps_position_s gps;
@@ -1372,6 +1359,24 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
}
}
void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
{
// EKF baro sample
vehicle_air_data_s airdata;
if (_airdata_sub.update(&airdata)) {
_ekf.set_air_density(airdata.rho);
const baroSample baro_sample {airdata.baro_alt_meter, airdata.timestamp_sample};
_ekf.setBaroData(baro_sample);
_device_id_baro = airdata.baro_device_id;
ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}
}
void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
{
/* Check and save learned magnetometer bias estimates */
src/modules/ekf2/EKF2.hpp
+1
View File
@@ -148,6 +148,7 @@ private:
void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateMagCalibration(const hrt_abstime &timestamp);