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

ekf2: move AirspeedSample update to UpdateAirspeedSample()

Browse Source
This commit is contained in:
Daniel Agar
2020-10-31 15:14:51 -04:00
parent f4f3ba47a2
commit 651e292d42
2 changed files with 25 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+23 -18
View File
@@ -451,24 +451,7 @@ void EKF2::Run()
}
}
if (_airspeed_sub.updated()) {
airspeed_s airspeed;
if (_airspeed_sub.copy(&airspeed)) {
// only set airspeed data if condition for airspeed fusion are met
if ((_param_ekf2_arsp_thr.get() > FLT_EPSILON) && (airspeed.true_airspeed_m_s > _param_ekf2_arsp_thr.get())) {
airspeedSample airspeed_sample {};
airspeed_sample.time_us = airspeed.timestamp;
airspeed_sample.eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
airspeed_sample.true_airspeed = airspeed.true_airspeed_m_s;
_ekf.setAirspeedData(airspeed_sample);
}
ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}
}
UpdateAirspeedSample(ekf2_timestamps);
bool new_optical_flow_data_received = false;
optical_flow_s optical_flow;
@@ -1362,6 +1345,28 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
return amsl_hgt + _wgs84_hgt_offset;
}
void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
{
// EKF airspeed sample
airspeed_s airspeed;
if (_airspeed_sub.update(&airspeed)) {
// only set airspeed data if condition for airspeed fusion are met
if ((_param_ekf2_arsp_thr.get() > FLT_EPSILON) && (airspeed.true_airspeed_m_s > _param_ekf2_arsp_thr.get())) {
airspeedSample airspeed_sample {
.true_airspeed = airspeed.true_airspeed_m_s,
.eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s,
.time_us = airspeed.timestamp,
};
_ekf.setAirspeedData(airspeed_sample);
}
ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.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
+2
View File
@@ -146,6 +146,8 @@ private:
void PublishWindEstimate(const hrt_abstime &timestamp);
void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateMagCalibration(const hrt_abstime &timestamp);
/*