diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index b38abe9d61..be8f11b5cb 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -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; - - 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); + } 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; - ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 - - (int64_t)ekf2_timestamps.timestamp / 100); + if (_airspeed_sub.update(&airspeed)) { + if (_msg_missed_airspeed_perf == nullptr) { + _msg_missed_airspeed_perf = perf_alloc(PC_COUNT, MODULE_NAME": airspeed messages missed"); + + } 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); + } } } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index b062cd3392..d7c9dc8da6 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -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 #include #include +#include #include #include #include @@ -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)};