diff --git a/src/modules/sensors/airspeed_selector/AirspeedSelector.cpp b/src/modules/sensors/airspeed_selector/AirspeedSelector.cpp index e47d530fa8..c49c1e72e2 100644 --- a/src/modules/sensors/airspeed_selector/AirspeedSelector.cpp +++ b/src/modules/sensors/airspeed_selector/AirspeedSelector.cpp @@ -58,30 +58,20 @@ AirspeedSelector::AirspeedSelector(): AirspeedSelector::~AirspeedSelector() { ScheduleClear(); - - perf_free(_perf_elapsed); } -int -AirspeedSelector::task_spawn(int argc, char *argv[]) +void AirspeedSelector::Start() { - AirspeedSelector *dev = new AirspeedSelector(); - - // check if the trampoline is called for the first time - if (!dev) { - PX4_ERR("alloc failed"); - return PX4_ERROR; - } - - _object.store(dev); - - dev->ScheduleOnInterval(SCHEDULE_INTERVAL, 10000); - _task_id = task_id_is_work_queue; - return PX4_OK; - + ScheduleOnInterval(SCHEDULE_INTERVAL, 10000); } -void -AirspeedSelector::init() + +void AirspeedSelector::Stop() +{ + ScheduleClear(); + Deinit(); +} + +void AirspeedSelector::init() { check_for_connected_airspeed_sensors(); @@ -111,8 +101,7 @@ AirspeedSelector::init() _prev_airspeed_src = _valid_airspeed_src; } -void -AirspeedSelector::check_for_connected_airspeed_sensors() +void AirspeedSelector::check_for_connected_airspeed_sensors() { // check for new connected airspeed sensor int detected_airspeed_sensors = 0; @@ -135,9 +124,7 @@ AirspeedSelector::check_for_connected_airspeed_sensors() _number_of_airspeed_sensors = detected_airspeed_sensors; } - -void -AirspeedSelector::Run() +void AirspeedSelector::Run() { _time_now_usec = hrt_absolute_time(); // hrt time of the current cycle @@ -288,10 +275,6 @@ AirspeedSelector::Run() _armed_prev = armed; perf_end(_perf_elapsed); - - if (should_exit()) { - exit_and_cleanup(); - } } void AirspeedSelector::update_params() @@ -676,47 +659,3 @@ void AirspeedSelector::update_throttle_filter(hrt_abstime now) } } } - -int AirspeedSelector::custom_command(int argc, char *argv[]) -{ - if (!is_running()) { - int ret = AirspeedSelector::task_spawn(argc, argv); - - if (ret) { - return ret; - } - } - - return print_usage("unknown command"); -} - -int AirspeedSelector::print_usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description -This module provides a single airspeed_validated topic, containing indicated (IAS), -calibrated (CAS), true airspeed (TAS) and the information if the estimation currently -is invalid and if based sensor readings or on groundspeed minus windspeed. -Supporting the input of multiple "raw" airspeed inputs, this module automatically switches -to a valid sensor in case of failure detection. For failure detection as well as for -the estimation of a scale factor from IAS to CAS, it runs several wind estimators -and also publishes those. - -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("airspeed_estimator", "estimator"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - - return 0; -} - -extern "C" __EXPORT int airspeed_selector_main(int argc, char *argv[]) -{ - return AirspeedSelector::main(argc, argv); -} diff --git a/src/modules/sensors/airspeed_selector/AirspeedSelector.hpp b/src/modules/sensors/airspeed_selector/AirspeedSelector.hpp index 2842497b26..87269b02a4 100644 --- a/src/modules/sensors/airspeed_selector/AirspeedSelector.hpp +++ b/src/modules/sensors/airspeed_selector/AirspeedSelector.hpp @@ -31,6 +31,20 @@ * ****************************************************************************/ +/** + * @file AirspeedSelector.hpp + * + * This class provides a single airspeed_validated topic, containing indicated (IAS), + * calibrated (CAS), true airspeed (TAS) and the information if the estimation currently + * is invalid and if based sensor readings or on groundspeed minus windspeed. + * Supporting the input of multiple "raw" airspeed inputs, this class automatically switches + * to a valid sensor in case of failure detection. For failure detection as well as for + * the estimation of a scale factor from IAS to CAS, it runs several wind estimators + * and also publishes those. + * + * @author Silvan Fuhrer + */ + #pragma once #include "AirspeedValidator.hpp" @@ -41,7 +55,6 @@ #include #include #include -#include #include #include #include @@ -72,23 +85,14 @@ #include #include -class AirspeedSelector : public ModuleBase, public ModuleParams, - public px4::ScheduledWorkItem +class AirspeedSelector : public ModuleParams, public px4::ScheduledWorkItem { public: - AirspeedSelector(); - ~AirspeedSelector() 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); + void Start(); + void Stop(); private: static constexpr uint32_t SCHEDULE_INTERVAL{100_ms}; /**< The schedule interval in usec (10 Hz) */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 97ceb6db21..82c27e539c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -96,6 +96,15 @@ Sensors::~Sensors() sub.unregisterCallback(); } +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) + + if (_airspeed_selector) { + _airspeed_selector->Stop(); + delete _airspeed_selector; + } + +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED + #if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION) _vehicle_acceleration.Stop(); #endif // CONFIG_SENSORS_VEHICLE_ACCELERATION @@ -397,6 +406,21 @@ void Sensors::adc_poll() } #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED) +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) +void Sensors::InitializeAirspeedSelector() +{ + if (_param_sys_has_num_aspd.get()) { + if (_airspeed_selector == nullptr) { + _airspeed_selector = new AirspeedSelector(); + + if (_airspeed_selector) { + _airspeed_selector->Start(); + } + } + } +} +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED + #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) void Sensors::InitializeVehicleAirData() { diff --git a/src/modules/sensors/sensors.hpp b/src/modules/sensors/sensors.hpp index 3a8bcd50bc..f0c76ea4cd 100644 --- a/src/modules/sensors/sensors.hpp +++ b/src/modules/sensors/sensors.hpp @@ -63,6 +63,7 @@ # include # include # include +# include "airspeed_selector/AirspeedSelector.hpp" #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) @@ -115,17 +116,13 @@ public: bool init(); private: - int parameters_update(); + void InitializeAirspeedSelector(); void InitializeVehicleAirData(); - void InitializeVehicleGPSPosition(); - void InitializeVehicleIMU(); - void InitializeVehicleMagnetometer(); - void InitializeVehicleOpticalFlow(); const bool _hil_enabled; /**< if true, HIL is active */ @@ -159,6 +156,8 @@ private: uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) + AirspeedSelector *_airspeed_selector {nullptr}; + /** * Poll the differential pressure sensor for updated data. * @@ -248,6 +247,9 @@ private: #endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW DEFINE_PARAMETERS( +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) + (ParamBool) _param_sys_has_num_aspd, +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) (ParamBool) _param_sys_has_baro, #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA