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

ekf2: prefer airspeed_validated over raw airspeed if available (#19159)

Browse Source 
* ekf2: prefer airspeed_validated over raw airspeed if available

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Daniel Agar
2022-02-14 09:37:29 -05:00
committed by GitHub
parent df0e402c44
commit b157afde6a
2 changed files with 59 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+54 -21
View File
@@ -1388,33 +1388,66 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
{
// EKF airspeed sample
const unsigned last_generation = _airspeed_sub.get_last_generation();
airspeed_s airspeed;
// prefer ORB_ID(airspeed_validated) if available, otherwise fallback to raw airspeed ORB_ID(airspeed)
if (_airspeed_validated_sub.updated()) {
const unsigned last_generation = _airspeed_validated_sub.get_last_generation();
airspeed_validated_s airspeed_validated;
if (_airspeed_sub.update(&airspeed)) {
if (_msg_missed_airspeed_perf == nullptr) {
_msg_missed_airspeed_perf = perf_alloc(PC_COUNT, MODULE_NAME": airspeed messages missed");
if (_airspeed_validated_sub.update(&airspeed_validated)) {
if (_msg_missed_airspeed_validated_perf == nullptr) {
_msg_missed_airspeed_validated_perf = perf_alloc(PC_COUNT, MODULE_NAME": airspeed validated messages missed");
} else if (_airspeed_sub.get_last_generation() != last_generation + 1) {
perf_count(_msg_missed_airspeed_perf);
} else if (_airspeed_validated_sub.get_last_generation() != last_generation + 1) {
perf_count(_msg_missed_airspeed_validated_perf);
}
if (PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
&& PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
&& (airspeed_validated.calibrated_airspeed_m_s > 0.f)
) {
airspeedSample airspeed_sample {
.time_us = airspeed_validated.timestamp,
.true_airspeed = airspeed_validated.true_airspeed_m_s,
.eas2tas = airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s,
};
_ekf.setAirspeedData(airspeed_sample);
}
_airspeed_validated_timestamp_last = airspeed_validated.timestamp;
}
// The airspeed measurement received via the airspeed.msg topic has not been corrected
// for scale favtor errors and requires the ASPD_SCALE correction to be applied.
// This could be avoided if true_airspeed_m_s from the airspeed-validated.msg topic
// was used instead, however this would introduce a potential circular dependency
// via the wind estimator that uses EKF velocity estimates.
const float true_airspeed_m_s = airspeed.true_airspeed_m_s * _airspeed_scale_factor;
} else if ((hrt_elapsed_time(&_airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) {
// use ORB_ID(airspeed) if ORB_ID(airspeed_validated) is unavailable
const unsigned last_generation = _airspeed_sub.get_last_generation();
airspeed_s airspeed;
airspeedSample airspeed_sample {
.time_us = airspeed.timestamp,
.true_airspeed = true_airspeed_m_s,
.eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s,
};
_ekf.setAirspeedData(airspeed_sample);
if (_airspeed_sub.update(&airspeed)) {
if (_msg_missed_airspeed_perf == nullptr) {
_msg_missed_airspeed_perf = perf_alloc(PC_COUNT, MODULE_NAME": airspeed messages missed");
ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
} else if (_airspeed_sub.get_last_generation() != last_generation + 1) {
perf_count(_msg_missed_airspeed_perf);
}
// The airspeed measurement received via ORB_ID(airspeed) topic has not been corrected
// for scale factor errors and requires the ASPD_SCALE correction to be applied.
const float true_airspeed_m_s = airspeed.true_airspeed_m_s * _airspeed_scale_factor;
if (PX4_ISFINITE(airspeed.true_airspeed_m_s)
&& PX4_ISFINITE(airspeed.indicated_airspeed_m_s)
&& (airspeed.indicated_airspeed_m_s > 0.f)
) {
airspeedSample airspeed_sample {
.time_us = airspeed.timestamp,
.true_airspeed = true_airspeed_m_s,
.eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_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);
}
}
}
src/modules/ekf2/EKF2.hpp
+5 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2015-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -64,6 +64,7 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/ekf_gps_drift.h>
@@ -190,6 +191,7 @@ private:
perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")};
perf_counter_t _msg_missed_air_data_perf{nullptr};
perf_counter_t _msg_missed_airspeed_perf{nullptr};
perf_counter_t _msg_missed_airspeed_validated_perf{nullptr};
perf_counter_t _msg_missed_distance_sensor_perf{nullptr};
perf_counter_t _msg_missed_gps_perf{nullptr};
perf_counter_t _msg_missed_landing_target_pose_perf{nullptr};
@@ -240,11 +242,13 @@ private:
float _last_baro_bias_published{};
float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
hrt_abstime _airspeed_validated_timestamp_last{0};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};
uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};
uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};