From 8e4992d3bc4b57cd8b4c0a241dbbed20a178e342 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 14 Nov 2019 12:10:53 +0100 Subject: [PATCH] Airspeed Selector: remove dynamic allocation of airspeed validators (depending on number of connected sensors) but do it statically for the maximum number allowed. Check for number of connected sensors not only during start up, but always when vehicle is disarmed. Signed-off-by: Silvan Fuhrer --- .../airspeed_selector_main.cpp | 73 ++++++++++--------- 1 file changed, 37 insertions(+), 36 deletions(-) diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 5e5c0a6d0b18..e39a7df3a944 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -130,7 +130,8 @@ class AirspeedModule : public ModuleBase, public ModuleParams, int _airspeed_sub[MAX_NUM_AIRSPEED_SENSORS] {}; /**< raw airspeed topics subscriptions. Max 3 airspeeds sensors. */ int32_t _number_of_airspeed_sensors{0}; /**< number of airspeed sensors in use (detected during initialization)*/ - AirspeedValidator *_airspeed_validator{nullptr}; /**< airspeedValidator instances (one for each sensor, assigned dynamically during startup) */ + 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) */ hrt_abstime _time_now_usec{0}; int _valid_airspeed_index{-2}; /**< index of currently chosen (valid) airspeed sensor */ @@ -166,6 +167,7 @@ class AirspeedModule : public ModuleBase, public ModuleParams, ) 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 */ @@ -190,7 +192,6 @@ AirspeedModule::~AirspeedModule() perf_free(_perf_elapsed); - delete[] _airspeed_validator; } int @@ -214,36 +215,7 @@ AirspeedModule::task_spawn(int argc, char *argv[]) void AirspeedModule::init() { - /* the first time we run through here, initialize N airspeedValidator - * instances (N = number of airspeed sensors detected). - * Do not initialize any airspeed validator instance if ASPD_PRIMARY is set to 0 or -1 (sensorless modes). - */ - if (_param_airspeed_primary_index.get() > 0) { - for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) { - if (orb_exists(ORB_ID(airspeed), i) != PX4_OK) { - break; - } - - _number_of_airspeed_sensors = i + 1; - } - - } else { - _number_of_airspeed_sensors = 0; //user has selected groundspeed-windspeed as primary source, or disabled airspeed - } - - _airspeed_validator = new AirspeedValidator[_number_of_airspeed_sensors]; - - if (_number_of_airspeed_sensors != 0 && _airspeed_validator == nullptr) { - // airspeed validator allocation failed (e.g. no enough RAM). Set number of sensors to 0. - _number_of_airspeed_sensors = 0; - mavlink_log_critical(&_mavlink_log_pub, "Airspeed Validator init failed. Don't use airspeed sensor. "); - } - - if (_number_of_airspeed_sensors > 0) { - for (int i = 0; i < _number_of_airspeed_sensors; i++) { - _airspeed_sub[i] = orb_subscribe_multi(ORB_ID(airspeed), i); - } - } + check_for_connected_airspeed_sensors(); /* Set the default sensor */ if (_param_airspeed_primary_index.get() > _number_of_airspeed_sensors) { @@ -267,6 +239,27 @@ AirspeedModule::init() _prev_airspeed_index = _valid_airspeed_index; // needed to detect a switching } +void +AirspeedModule::check_for_connected_airspeed_sensors() +{ + if (_param_airspeed_primary_index.get() > 0) { + for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) { + if (orb_exists(ORB_ID(airspeed), i) == PX4_OK) { + _airspeed_sub[i] = orb_subscribe_multi(ORB_ID(airspeed), i); + + } else { + break; + } + + _number_of_airspeed_sensors = i + 1; + } + + } else { + _number_of_airspeed_sensors = 0; //user has selected groundspeed-windspeed as primary source, or disabled airspeed + } + +} + void AirspeedModule::Run() @@ -286,13 +279,19 @@ AirspeedModule::Run() _time_now_usec = hrt_absolute_time(); //hrt time of the current cycle + bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + + /* Check for new connected airspeed sensors as long as we're disarmed */ + if (!armed) { + check_for_connected_airspeed_sensors(); + } + poll_topics(); update_wind_estimator_sideslip(); update_ground_minus_wind_airspeed(); if (_number_of_airspeed_sensors > 0) { - bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); bool fixed_wing = !_vtol_vehicle_status.vtol_in_rw_mode; bool in_air = !_vehicle_land_detected.landed; @@ -480,13 +479,14 @@ void AirspeedModule::update_ground_minus_wind_airspeed() void AirspeedModule::select_airspeed_and_publish() { - /* Find new valid index if airspeed currently is invalid, but we have sensors, primary sensor is real sensor and checks are enabled. */ + /* Find new valid index if airspeed currently is invalid, but we have sensors, primary sensor is real sensor and checks are enabled or new sensor was added. */ bool airspeed_sensor_switching_necessary = _prev_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX || !_airspeed_validator[_prev_airspeed_index - 1].get_airspeed_valid(); bool airspeed_sensor_switching_allowed = _number_of_airspeed_sensors > 0 && - _param_airspeed_primary_index.get() > airspeed_index::GROUND_MINUS_WIND_INDEX && _param_airspeed_checks_on.get(); + _param_airspeed_primary_index.get() > airspeed_index::GROUND_MINUS_WIND_INDEX && _param_airspeed_checks_on.get(); + bool airspeed_sensor_added = _prev_number_of_airspeed_sensors < _number_of_airspeed_sensors; - if (airspeed_sensor_switching_necessary && airspeed_sensor_switching_allowed) { + if (airspeed_sensor_switching_necessary && (airspeed_sensor_switching_allowed || airspeed_sensor_added)) { _valid_airspeed_index = airspeed_index::DISABLED_INDEX; // set to disabled @@ -524,6 +524,7 @@ void AirspeedModule::select_airspeed_and_publish() } _prev_airspeed_index = _valid_airspeed_index; + _prev_number_of_airspeed_sensors = _number_of_airspeed_sensors; /* fill out airspeed_validated message for publishing it */ airspeed_validated_s airspeed_validated = {};