From 796db86121cab916970b82a03401a013ba1fb033 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 18 Feb 2026 14:34:00 +0100 Subject: [PATCH] sensors: split airspeed selector header into sperate file --- ...selector_main.cpp => AirspeedSelector.cpp} | 190 +-------------- .../airspeed_selector/AirspeedSelector.hpp | 223 ++++++++++++++++++ .../sensors/airspeed_selector/CMakeLists.txt | 3 +- 3 files changed, 226 insertions(+), 190 deletions(-) rename src/modules/sensors/airspeed_selector/{airspeed_selector_main.cpp => AirspeedSelector.cpp} (76%) create mode 100644 src/modules/sensors/airspeed_selector/AirspeedSelector.hpp diff --git a/src/modules/sensors/airspeed_selector/airspeed_selector_main.cpp b/src/modules/sensors/airspeed_selector/AirspeedSelector.cpp similarity index 76% rename from src/modules/sensors/airspeed_selector/airspeed_selector_main.cpp rename to src/modules/sensors/airspeed_selector/AirspeedSelector.cpp index 1fa832bee1..6d5480ff86 100644 --- a/src/modules/sensors/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/sensors/airspeed_selector/AirspeedSelector.cpp @@ -31,203 +31,15 @@ * ****************************************************************************/ -#include "AirspeedValidator.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "AirspeedSelector.hpp" using namespace time_literals; -static constexpr uint32_t SCHEDULE_INTERVAL{100_ms}; /**< The schedule interval in usec (10 Hz) */ -static constexpr float _kThrottleFilterTimeConstant{0.5f}; - using matrix::Dcmf; using matrix::Quatf; using matrix::Vector2f; using matrix::Vector3f; -class AirspeedModule : public ModuleBase, public ModuleParams, - public px4::ScheduledWorkItem -{ -public: - - AirspeedModule(); - - ~AirspeedModule() override; - - /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); - - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); - - /** @see ModuleBase */ - static int print_usage(const char *reason = nullptr); - -private: - - void Run() override; - - static constexpr int MAX_NUM_AIRSPEED_SENSORS = 3; /**< Support max 3 airspeed sensors */ - enum class AirspeedSource : int { - DISABLED = -1, - GROUND_MINUS_WIND, - SENSOR_1, - SENSOR_2, - SENSOR_3, - SYNTHETIC - }; - - uORB::Publication _airspeed_validated_pub {ORB_ID(airspeed_validated)}; /**< airspeed validated topic*/ - uORB::PublicationMulti _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {{ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */ - orb_advert_t _mavlink_log_pub {nullptr}; /**< mavlink log topic*/ - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; - uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; - uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; - uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; - uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; - uORB::Subscription _position_setpoint_sub{ORB_ID(position_setpoint)}; - uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)}; - uORB::SubscriptionMultiArray _airspeed_subs{ORB_ID::airspeed}; - uORB::SubscriptionData _flight_phase_estimation_sub{ORB_ID(flight_phase_estimation)}; - - - tecs_status_s _tecs_status {}; - estimator_status_s _estimator_status {}; - vehicle_acceleration_s _accel {}; - vehicle_air_data_s _vehicle_air_data {}; - vehicle_land_detected_s _vehicle_land_detected {}; - vehicle_local_position_s _vehicle_local_position {}; - vehicle_status_s _vehicle_status {}; - position_setpoint_s _position_setpoint {}; - - WindEstimator _wind_estimator_sideslip; /**< wind estimator instance only fusing sideslip */ - airspeed_wind_s _wind_estimate_sideslip {}; /**< wind estimate message for wind estimator instance only fusing sideslip */ - - int32_t _number_of_airspeed_sensors{0}; /**< number of airspeed sensors in use (detected during initialization)*/ - int32_t _prev_number_of_airspeed_sensors{0}; /**< number of airspeed sensors in previous loop (to detect a new added sensor)*/ - AirspeedValidator _airspeed_validator[MAX_NUM_AIRSPEED_SENSORS] {}; /**< airspeedValidator instances (one for each sensor) */ - - matrix::Quatf _q_att; - hrt_abstime _time_now_usec{0}; - AirspeedSource _valid_airspeed_src{AirspeedSource::DISABLED}; - AirspeedSource _prev_airspeed_src{AirspeedSource::DISABLED}; - bool _initialized{false}; /**< module initialized*/ - bool _gnss_lpos_valid{false}; /**< local position (from GNSS) valid */ - bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */ - float _ground_minus_wind_TAS{NAN}; /**< true airspeed from groundspeed minus windspeed */ - float _ground_minus_wind_CAS{NAN}; /**< calibrated airspeed from groundspeed minus windspeed */ - bool _armed_prev{false}; - - hrt_abstime _time_last_airspeed_update[MAX_NUM_AIRSPEED_SENSORS] {}; - - perf_counter_t _perf_elapsed{}; - - float _param_airspeed_scale[MAX_NUM_AIRSPEED_SENSORS] {}; /** array to save the airspeed scale params in */ - - enum CheckTypeBits { - CHECK_TYPE_ONLY_DATA_MISSING_BIT = (1 << 0), - CHECK_TYPE_DATA_STUCK_BIT = (1 << 1), - CHECK_TYPE_INNOVATION_BIT = (1 << 2), - CHECK_TYPE_LOAD_FACTOR_BIT = (1 << 3), - CHECK_TYPE_FIRST_PRINCIPLE_BIT = (1 << 4) - }; - - - param_t _param_handle_pitch_sp_offset{PARAM_INVALID}; - float _param_pitch_sp_offset{0.0f}; - param_t _param_handle_fw_thr_max{PARAM_INVALID}; - float _param_fw_thr_max{0.0f}; - - AlphaFilter _throttle_filtered{_kThrottleFilterTimeConstant}; - uint64_t _t_last_throttle_fw{0}; - - DEFINE_PARAMETERS( - (ParamFloat) _param_aspd_wind_nsd, - (ParamFloat) _param_aspd_scale_nsd, - (ParamFloat) _param_west_tas_noise, - (ParamFloat) _param_west_beta_noise, - (ParamInt) _param_west_tas_gate, - (ParamInt) _param_west_beta_gate, - (ParamInt) _param_aspd_scale_apply, - (ParamFloat) _param_airspeed_scale_1, - (ParamFloat) _param_airspeed_scale_2, - (ParamFloat) _param_airspeed_scale_3, - (ParamInt) _param_airspeed_primary_index, - (ParamInt) _param_airspeed_checks_on, - (ParamInt) _param_airspeed_fallback, - - (ParamFloat) _tas_innov_threshold, /**< innovation check threshold */ - (ParamFloat) _tas_innov_integ_threshold, /**< innovation check integrator threshold */ - (ParamFloat) _checks_fail_delay, /**< delay to declare airspeed invalid */ - (ParamFloat) _checks_clear_delay, /**< delay to declare airspeed valid again */ - - (ParamFloat) _param_wind_sigma_max_synth_tas, - (ParamFloat) _aspd_fp_t_window, - - // external parameters - (ParamFloat) _param_fw_airspd_stall, - (ParamFloat) _param_fw_airspd_min, - (ParamFloat) _param_fw_airspd_trim, - (ParamFloat) _param_fw_airspd_max, - (ParamFloat) _param_fw_thr_aspd_min, - (ParamFloat) _param_fw_thr_trim, - (ParamFloat) _param_fw_thr_aspd_max - ) - - void init(); /**< initialization of the airspeed validator instances */ - void check_for_connected_airspeed_sensors(); /**< check for airspeed sensors (airspeed topics) and get _number_of_airspeed_sensors */ - void update_params(); /**< update parameters */ - void poll_topics(); /**< poll all topics required beside airspeed (e.g. current temperature) */ - void update_wind_estimator_sideslip(); /**< update the wind estimator instance only fusing sideslip */ - void update_ground_minus_wind_airspeed(); /**< update airspeed estimate based on groundspeed minus windspeed */ - void select_airspeed_and_publish(); /**< select airspeed sensor (or groundspeed-windspeed) */ - float get_synthetic_airspeed(float throttle); - void update_throttle_filter(hrt_abstime t_now); -}; - AirspeedModule::AirspeedModule(): ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) diff --git a/src/modules/sensors/airspeed_selector/AirspeedSelector.hpp b/src/modules/sensors/airspeed_selector/AirspeedSelector.hpp new file mode 100644 index 0000000000..0b3c53024b --- /dev/null +++ b/src/modules/sensors/airspeed_selector/AirspeedSelector.hpp @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2026 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "AirspeedValidator.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class AirspeedModule : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + + AirspeedModule(); + + ~AirspeedModule() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + +private: + static constexpr uint32_t SCHEDULE_INTERVAL{100_ms}; /**< The schedule interval in usec (10 Hz) */ + static constexpr float _kThrottleFilterTimeConstant{0.5f}; + + void Run() override; + + static constexpr int MAX_NUM_AIRSPEED_SENSORS = 3; /**< Support max 3 airspeed sensors */ + enum class AirspeedSource : int { + DISABLED = -1, + GROUND_MINUS_WIND, + SENSOR_1, + SENSOR_2, + SENSOR_3, + SYNTHETIC + }; + + uORB::Publication _airspeed_validated_pub {ORB_ID(airspeed_validated)}; /**< airspeed validated topic*/ + uORB::PublicationMulti _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {{ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */ + orb_advert_t _mavlink_log_pub {nullptr}; /**< mavlink log topic*/ + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; + uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Subscription _position_setpoint_sub{ORB_ID(position_setpoint)}; + uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)}; + uORB::SubscriptionMultiArray _airspeed_subs{ORB_ID::airspeed}; + uORB::SubscriptionData _flight_phase_estimation_sub{ORB_ID(flight_phase_estimation)}; + + + tecs_status_s _tecs_status {}; + estimator_status_s _estimator_status {}; + vehicle_acceleration_s _accel {}; + vehicle_air_data_s _vehicle_air_data {}; + vehicle_land_detected_s _vehicle_land_detected {}; + vehicle_local_position_s _vehicle_local_position {}; + vehicle_status_s _vehicle_status {}; + position_setpoint_s _position_setpoint {}; + + WindEstimator _wind_estimator_sideslip; /**< wind estimator instance only fusing sideslip */ + airspeed_wind_s _wind_estimate_sideslip {}; /**< wind estimate message for wind estimator instance only fusing sideslip */ + + int32_t _number_of_airspeed_sensors{0}; /**< number of airspeed sensors in use (detected during initialization)*/ + int32_t _prev_number_of_airspeed_sensors{0}; /**< number of airspeed sensors in previous loop (to detect a new added sensor)*/ + AirspeedValidator _airspeed_validator[MAX_NUM_AIRSPEED_SENSORS] {}; /**< airspeedValidator instances (one for each sensor) */ + + matrix::Quatf _q_att; + hrt_abstime _time_now_usec{0}; + AirspeedSource _valid_airspeed_src{AirspeedSource::DISABLED}; + AirspeedSource _prev_airspeed_src{AirspeedSource::DISABLED}; + bool _initialized{false}; /**< module initialized*/ + bool _gnss_lpos_valid{false}; /**< local position (from GNSS) valid */ + bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */ + float _ground_minus_wind_TAS{NAN}; /**< true airspeed from groundspeed minus windspeed */ + float _ground_minus_wind_CAS{NAN}; /**< calibrated airspeed from groundspeed minus windspeed */ + bool _armed_prev{false}; + + hrt_abstime _time_last_airspeed_update[MAX_NUM_AIRSPEED_SENSORS] {}; + + perf_counter_t _perf_elapsed{}; + + float _param_airspeed_scale[MAX_NUM_AIRSPEED_SENSORS] {}; /** array to save the airspeed scale params in */ + + enum CheckTypeBits { + CHECK_TYPE_ONLY_DATA_MISSING_BIT = (1 << 0), + CHECK_TYPE_DATA_STUCK_BIT = (1 << 1), + CHECK_TYPE_INNOVATION_BIT = (1 << 2), + CHECK_TYPE_LOAD_FACTOR_BIT = (1 << 3), + CHECK_TYPE_FIRST_PRINCIPLE_BIT = (1 << 4) + }; + + + param_t _param_handle_pitch_sp_offset{PARAM_INVALID}; + float _param_pitch_sp_offset{0.0f}; + param_t _param_handle_fw_thr_max{PARAM_INVALID}; + float _param_fw_thr_max{0.0f}; + + AlphaFilter _throttle_filtered{_kThrottleFilterTimeConstant}; + uint64_t _t_last_throttle_fw{0}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_aspd_wind_nsd, + (ParamFloat) _param_aspd_scale_nsd, + (ParamFloat) _param_west_tas_noise, + (ParamFloat) _param_west_beta_noise, + (ParamInt) _param_west_tas_gate, + (ParamInt) _param_west_beta_gate, + (ParamInt) _param_aspd_scale_apply, + (ParamFloat) _param_airspeed_scale_1, + (ParamFloat) _param_airspeed_scale_2, + (ParamFloat) _param_airspeed_scale_3, + (ParamInt) _param_airspeed_primary_index, + (ParamInt) _param_airspeed_checks_on, + (ParamInt) _param_airspeed_fallback, + + (ParamFloat) _tas_innov_threshold, /**< innovation check threshold */ + (ParamFloat) _tas_innov_integ_threshold, /**< innovation check integrator threshold */ + (ParamFloat) _checks_fail_delay, /**< delay to declare airspeed invalid */ + (ParamFloat) _checks_clear_delay, /**< delay to declare airspeed valid again */ + + (ParamFloat) _param_wind_sigma_max_synth_tas, + (ParamFloat) _aspd_fp_t_window, + + // external parameters + (ParamFloat) _param_fw_airspd_stall, + (ParamFloat) _param_fw_airspd_min, + (ParamFloat) _param_fw_airspd_trim, + (ParamFloat) _param_fw_airspd_max, + (ParamFloat) _param_fw_thr_aspd_min, + (ParamFloat) _param_fw_thr_trim, + (ParamFloat) _param_fw_thr_aspd_max + ) + + void init(); /**< initialization of the airspeed validator instances */ + void check_for_connected_airspeed_sensors(); /**< check for airspeed sensors (airspeed topics) and get _number_of_airspeed_sensors */ + void update_params(); /**< update parameters */ + void poll_topics(); /**< poll all topics required beside airspeed (e.g. current temperature) */ + void update_wind_estimator_sideslip(); /**< update the wind estimator instance only fusing sideslip */ + void update_ground_minus_wind_airspeed(); /**< update airspeed estimate based on groundspeed minus windspeed */ + void select_airspeed_and_publish(); /**< select airspeed sensor (or groundspeed-windspeed) */ + float get_synthetic_airspeed(float throttle); + void update_throttle_filter(hrt_abstime t_now); +}; diff --git a/src/modules/sensors/airspeed_selector/CMakeLists.txt b/src/modules/sensors/airspeed_selector/CMakeLists.txt index 52641b765a..62115bb137 100644 --- a/src/modules/sensors/airspeed_selector/CMakeLists.txt +++ b/src/modules/sensors/airspeed_selector/CMakeLists.txt @@ -32,7 +32,8 @@ ############################################################################ px4_add_library(airspeed_selector - airspeed_selector_main.cpp + AirspeedSelector.cpp + AirspeedSelector.hpp AirspeedValidator.cpp AirspeedValidator.hpp )