From 6ff361479c86954e5b85ebab628410097e2be260 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 2 Sep 2020 12:46:47 -0400 Subject: [PATCH] uORB: introduce SubscriptionMultiArray for working with multi-instances --- src/drivers/uavcan/sensors/sensor_bridge.hpp | 2 +- src/drivers/uavcannode/UavcanNode.cpp | 2 +- src/drivers/uavcannode/UavcanNode.hpp | 3 +- .../CollisionPrevention.cpp | 9 +- .../CollisionPrevention.hpp | 3 +- .../airspeed_selector_main.cpp | 53 ++++---- src/modules/commander/Commander.cpp | 33 ++--- src/modules/commander/Commander.hpp | 18 +-- .../commander/accelerometer_calibration.cpp | 9 +- src/modules/ekf2/ekf2_main.cpp | 14 +- src/modules/mavlink/mavlink_high_latency2.cpp | 10 +- src/modules/mavlink/mavlink_high_latency2.h | 3 +- src/modules/mavlink/mavlink_messages.cpp | 98 ++++---------- src/modules/sensors/voted_sensors_update.cpp | 2 +- src/modules/sensors/voted_sensors_update.h | 7 +- src/modules/simulator/simulator.h | 5 +- src/modules/uORB/CMakeLists.txt | 1 + src/modules/uORB/Subscription.hpp | 2 +- src/modules/uORB/SubscriptionInterval.hpp | 1 + src/modules/uORB/SubscriptionMultiArray.hpp | 128 ++++++++++++++++++ 20 files changed, 234 insertions(+), 169 deletions(-) create mode 100644 src/modules/uORB/SubscriptionMultiArray.hpp diff --git a/src/drivers/uavcan/sensors/sensor_bridge.hpp b/src/drivers/uavcan/sensors/sensor_bridge.hpp index 04384a3980..fe51a02238 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.hpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.hpp @@ -105,7 +105,7 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD bool _out_of_channels = false; protected: - static constexpr unsigned DEFAULT_MAX_CHANNELS = ORB_MULTI_MAX_INSTANCES; + static constexpr unsigned DEFAULT_MAX_CHANNELS = 4; const unsigned _max_channels; UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index d0a1480a9d..4946e908e2 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -380,7 +380,7 @@ void UavcanNode::Run() } // distance_sensor[] -> uavcan::equipment::range_sensor::Measurement - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + for (int i = 0; i < MAX_INSTANCES; i++) { distance_sensor_s dist; if (_distance_sensor_sub[i].update(&dist)) { diff --git a/src/drivers/uavcannode/UavcanNode.hpp b/src/drivers/uavcannode/UavcanNode.hpp index 1ebeef2715..a3d4165b3e 100644 --- a/src/drivers/uavcannode/UavcanNode.hpp +++ b/src/drivers/uavcannode/UavcanNode.hpp @@ -183,7 +183,8 @@ private: uORB::SubscriptionCallbackWorkItem _battery_status_sub{this, ORB_ID(battery_status)}; uORB::SubscriptionCallbackWorkItem _diff_pressure_sub{this, ORB_ID(differential_pressure)}; - uORB::SubscriptionCallbackWorkItem _distance_sensor_sub[ORB_MULTI_MAX_INSTANCES] { + static constexpr int MAX_INSTANCES = 4; + uORB::SubscriptionCallbackWorkItem _distance_sensor_sub[MAX_INSTANCES] { {this, ORB_ID(distance_sensor), 0}, {this, ORB_ID(distance_sensor), 1}, {this, ORB_ID(distance_sensor), 2}, diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index eb5a32368c..32f6d28c26 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -200,13 +200,10 @@ CollisionPrevention::_updateObstacleMap() _sub_vehicle_attitude.update(); // add distance sensor data - for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - - // if a new distance sensor message has arrived - if (_sub_distance_sensor[i].updated()) { - distance_sensor_s distance_sensor {}; - _sub_distance_sensor[i].copy(&distance_sensor); + for (auto &dist_sens_sub : _distance_sensor_subs) { + distance_sensor_s distance_sensor; + if (dist_sens_sub.update(&distance_sensor)) { // consider only instances with valid data and orientations useful for collision prevention if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) && (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) && diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 0b79c19894..7af7a41a53 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -51,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -131,8 +132,8 @@ private: uORB::PublicationQueued _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do publication */ uORB::SubscriptionData _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */ - uORB::Subscription _sub_distance_sensor[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; /**< distance data received from onboard rangefinders */ uORB::SubscriptionData _sub_vehicle_attitude{ORB_ID(vehicle_attitude)}; + uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms}; static constexpr uint64_t TIMEOUT_HOLD_US{5_s}; diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 93da2c0df4..53cb92367e 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -44,6 +44,7 @@ #include #include +#include #include #include #include @@ -113,7 +114,7 @@ private: uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; - uORB::Subscription _airspeed_sub[MAX_NUM_AIRSPEED_SENSORS] {{ORB_ID(airspeed), 0}, {ORB_ID(airspeed), 1}, {ORB_ID(airspeed), 2}}; /**< raw airspeed topics subscriptions. */ + uORB::SubscriptionMultiArray _airspeed_subs{ORB_ID::airspeed}; estimator_status_s _estimator_status {}; vehicle_acceleration_s _accel {}; @@ -244,8 +245,8 @@ AirspeedModule::check_for_connected_airspeed_sensors() if (_param_airspeed_primary_index.get() > 0) { - for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) { - if (!_airspeed_sub[i].advertised()) { + for (int i = 0; i < _airspeed_subs.size(); i++) { + if (!_airspeed_subs[i].advertised()) { break; } @@ -323,30 +324,32 @@ AirspeedModule::Run() /* iterate through all airspeed sensors, poll new data from them and update their validators */ for (int i = 0; i < _number_of_airspeed_sensors; i++) { - /* poll airspeed data */ - airspeed_s airspeed_raw = {}; - _airspeed_sub[i].copy(&airspeed_raw); // poll raw airspeed topic of the i-th sensor - input_data.airspeed_indicated_raw = airspeed_raw.indicated_airspeed_m_s; - input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s; - input_data.airspeed_timestamp = airspeed_raw.timestamp; - input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius; + // poll raw airspeed topic of the i-th sensor + airspeed_s airspeed_raw; - /* update in_fixed_wing_flight for the current airspeed sensor validator */ - /* takeoff situation is active from start till one of the sensors' IAS or groundspeed_EAS is above stall speed */ - if (airspeed_raw.indicated_airspeed_m_s > _airspeed_stall.get() || _ground_minus_wind_EAS > _airspeed_stall.get()) { - _in_takeoff_situation = false; + if (_airspeed_subs[i].update(&airspeed_raw)) { + + input_data.airspeed_indicated_raw = airspeed_raw.indicated_airspeed_m_s; + input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s; + input_data.airspeed_timestamp = airspeed_raw.timestamp; + input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius; + + /* update in_fixed_wing_flight for the current airspeed sensor validator */ + /* takeoff situation is active from start till one of the sensors' IAS or groundspeed_EAS is above stall speed */ + if (airspeed_raw.indicated_airspeed_m_s > _airspeed_stall.get() || _ground_minus_wind_EAS > _airspeed_stall.get()) { + _in_takeoff_situation = false; + } + + /* reset takeoff_situation to true when not in air or not in fixed-wing mode */ + if (!in_air || !fixed_wing) { + _in_takeoff_situation = true; + } + + input_data.in_fixed_wing_flight = (armed && fixed_wing && in_air && !_in_takeoff_situation); + + /* push input data into airspeed validator */ + _airspeed_validator[i].update_airspeed_validator(input_data); } - - /* reset takeoff_situation to true when not in air or not in fixed-wing mode */ - if (!in_air || !fixed_wing) { - _in_takeoff_situation = true; - } - - input_data.in_fixed_wing_flight = (armed && fixed_wing && in_air && !_in_takeoff_situation); - - /* push input data into airspeed validator */ - _airspeed_validator[i].update_airspeed_validator(input_data); - } } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 6e37e6e579..0e27fe0841 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3755,7 +3755,7 @@ void Commander::mission_init() void Commander::data_link_check() { - for (auto &telemetry_status : _telemetry_status_sub) { + for (auto &telemetry_status : _telemetry_status_subs) { telemetry_status_s telemetry; if (telemetry_status.update(&telemetry)) { @@ -3924,14 +3924,13 @@ void Commander::data_link_check() void Commander::avoidance_check() { + for (auto &dist_sens_sub : _distance_sensor_subs) { + distance_sensor_s distance_sensor; - for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - if (_sub_distance_sensor[i].updated()) { - distance_sensor_s distance_sensor {}; - _sub_distance_sensor[i].copy(&distance_sensor); - + if (dist_sens_sub.update(&distance_sensor)) { if ((distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) && (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) { + _valid_distance_sensor_time_us = distance_sensor.timestamp; } } @@ -3961,28 +3960,24 @@ void Commander::avoidance_check() void Commander::battery_status_check() { - bool battery_sub_updated = false; + // We need to update the status flag if ANY battery is updated, because the system source might have + // changed, or might be nothing (if there is no battery connected) + if (!_battery_status_subs.updated()) { + // Nothing has changed since the last time this function was called, so nothing needs to be done now. + return; + } - battery_status_s batteries[ORB_MULTI_MAX_INSTANCES]; + battery_status_s batteries[_battery_status_subs.size()]; size_t num_connected_batteries = 0; - for (size_t i = 0; i < sizeof(_battery_subs) / sizeof(_battery_subs[0]); i++) { - // We need to update the status flag if ANY battery is updated, because the system source might have - // changed, or might be nothing (if there is no battery connected) - battery_sub_updated |= _battery_subs[i].updated(); - - if (_battery_subs[i].copy(&batteries[num_connected_batteries])) { + for (auto &battery_sub : _battery_status_subs) { + if (battery_sub.copy(&batteries[num_connected_batteries])) { if (batteries[num_connected_batteries].connected) { num_connected_batteries++; } } } - if (!battery_sub_updated) { - // Nothing has changed since the last time this function was called, so nothing needs to be done now. - return; - } - // There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest // option is to check if ANY of them have a warning, and specifically find which one has the most // urgent warning. diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 3b0026ac61..83eebb0d56 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -56,6 +56,7 @@ // subscriptions #include +#include #include #include #include @@ -384,21 +385,8 @@ private: // Subscriptions uORB::Subscription _actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS}; -#if BOARD_NUMBER_BRICKS > 1 - uORB::Subscription _battery_subs[ORB_MULTI_MAX_INSTANCES] { - uORB::Subscription(ORB_ID(battery_status), 0), - uORB::Subscription(ORB_ID(battery_status), 1), - uORB::Subscription(ORB_ID(battery_status), 2), - uORB::Subscription(ORB_ID(battery_status), 3), - }; -#else - uORB::Subscription _battery_subs[1] { - uORB::Subscription(ORB_ID(battery_status), 0) - }; -#endif uORB::Subscription _cmd_sub {ORB_ID(vehicle_command)}; uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; - uORB::Subscription _sub_distance_sensor[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; /**< distance data received from onboard rangefinders */ uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)}; uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; @@ -408,9 +396,11 @@ private: uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _subsys_sub{ORB_ID(subsystem_info)}; uORB::Subscription _system_power_sub{ORB_ID(system_power)}; - uORB::Subscription _telemetry_status_sub[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(telemetry_status), 0}, {ORB_ID(telemetry_status), 1}, {ORB_ID(telemetry_status), 2}, {ORB_ID(telemetry_status), 3}}; uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; + uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; + uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; + uORB::SubscriptionMultiArray _telemetry_status_subs{ORB_ID::telemetry_status}; #if defined(BOARD_HAS_POWER_CONTROL) uORB::Subscription _power_button_state_sub {ORB_ID(power_button_state)}; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 2bfaa81909..7e26f63fe9 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -141,6 +141,7 @@ #include #include #include +#include #include #include @@ -437,11 +438,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub) sensor_correction_s sensor_correction{}; sensor_correction_sub.copy(&sensor_correction); - uORB::Subscription accel_sub[MAX_ACCEL_SENS] { - {ORB_ID(sensor_accel), 0}, - {ORB_ID(sensor_accel), 1}, - {ORB_ID(sensor_accel), 2}, - }; + uORB::SubscriptionMultiArray accel_subs{ORB_ID::sensor_accel}; /* use the first sensor to pace the readout, but do per-sensor counts */ for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) { @@ -449,7 +446,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub) Vector3f accel_sum{}; unsigned count = 0; - while (accel_sub[accel_index].update(&arp)) { + while (accel_subs[accel_index].update(&arp)) { // fetch optional thermal offset corrections in sensor/board frame if ((arp.timestamp > 0) && (arp.device_id != 0)) { Vector3f offset{0, 0, 0}; diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 3bf620d78c..93260f72d6 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -256,8 +257,7 @@ private: int _lockstep_component{-1}; // because we can have several distance sensor instances with different orientations - static constexpr int MAX_RNG_SENSOR_COUNT = 4; - uORB::Subscription _range_finder_subs[MAX_RNG_SENSOR_COUNT] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; + uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; int _range_finder_sub_index = -1; // index for downward-facing range finder subscription // because we can have multiple GPS instances @@ -1078,10 +1078,10 @@ void Ekf2::Run() if (_range_finder_sub_index >= 0) { - if (_range_finder_subs[_range_finder_sub_index].updated()) { + if (_distance_sensor_subs[_range_finder_sub_index].updated()) { distance_sensor_s range_finder; - if (_range_finder_subs[_range_finder_sub_index].copy(&range_finder)) { + if (_distance_sensor_subs[_range_finder_sub_index].copy(&range_finder)) { rangeSample range_sample {}; range_sample.rng = range_finder.current_distance; range_sample.quality = range_finder.signal_quality; @@ -1789,10 +1789,10 @@ void Ekf2::resetPreFlightChecks() int Ekf2::getRangeSubIndex() { - for (unsigned i = 0; i < MAX_RNG_SENSOR_COUNT; i++) { - distance_sensor_s report{}; + for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) { + distance_sensor_s report; - if (_range_finder_subs[i].update(&report)) { + if (_distance_sensor_subs[i].update(&report)) { // only use the first instace which has the correct orientation if (report.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) { PX4_INFO("Found range finder with instance %d", i); diff --git a/src/modules/mavlink/mavlink_high_latency2.cpp b/src/modules/mavlink/mavlink_high_latency2.cpp index 88df8bbab6..4509508df7 100644 --- a/src/modules/mavlink/mavlink_high_latency2.cpp +++ b/src/modules/mavlink/mavlink_high_latency2.cpp @@ -72,7 +72,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t) bool updated = _airspeed.valid(); updated |= _airspeed_sp.valid(); - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + for (int i = 0; i < MAX_BATTERIES; i++) { updated |= _batteries[i].analyzer.valid(); } @@ -112,7 +112,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t) int lowest = 0; - for (int i = 1; i < ORB_MULTI_MAX_INSTANCES; i++) { + for (int i = 1; i < MAX_BATTERIES; i++) { const bool battery_connected = _batteries[i].connected && _batteries[i].analyzer.valid(); const bool battery_is_lowest = _batteries[i].analyzer.get_scaled(100.0f) <= _batteries[lowest].analyzer.get_scaled( 100.0f); @@ -183,7 +183,7 @@ void MavlinkStreamHighLatency2::reset_analysers(const hrt_abstime t) _airspeed.reset(); _airspeed_sp.reset(); - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + for (int i = 0; i < MAX_BATTERIES; i++) { _batteries[i].analyzer.reset(); } @@ -230,7 +230,7 @@ bool MavlinkStreamHighLatency2::write_battery_status(mavlink_high_latency2_t *ms struct battery_status_s battery; bool updated = false; - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + for (int i = 0; i < MAX_BATTERIES; i++) { if (_batteries[i].subscription.update(&battery)) { updated = true; _batteries[i].connected = battery.connected; @@ -486,7 +486,7 @@ void MavlinkStreamHighLatency2::update_battery_status() { battery_status_s battery; - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + for (int i = 0; i < MAX_BATTERIES; i++) { if (_batteries[i].subscription.update(&battery)) { _batteries[i].connected = battery.connected; _batteries[i].analyzer.add_value(battery.remaining, _update_rate_filtered); diff --git a/src/modules/mavlink/mavlink_high_latency2.h b/src/modules/mavlink/mavlink_high_latency2.h index 24b36f1a9e..7bc125bfbf 100644 --- a/src/modules/mavlink/mavlink_high_latency2.h +++ b/src/modules/mavlink/mavlink_high_latency2.h @@ -138,7 +138,8 @@ private: hrt_abstime _last_update_time = 0; float _update_rate_filtered = 0.0f; - PerBatteryData _batteries[ORB_MULTI_MAX_INSTANCES] {0, 1, 2, 3}; + static constexpr int MAX_BATTERIES = 4; + PerBatteryData _batteries[MAX_BATTERIES] {0, 1, 2, 3}; /* do not allow top copying this class */ MavlinkStreamHighLatency2(MavlinkStreamHighLatency2 &); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 030cbb0115..2c1ce54af0 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -61,6 +61,7 @@ #include #include +#include #include #include #include @@ -559,9 +560,7 @@ public: private: uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; - uORB::Subscription _battery_status_sub[ORB_MULTI_MAX_INSTANCES] { - {ORB_ID(battery_status), 0}, {ORB_ID(battery_status), 1}, {ORB_ID(battery_status), 2}, {ORB_ID(battery_status), 3} - }; + uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; /* do not allow top copying this class */ MavlinkStreamSysStatus(MavlinkStreamSysStatus &) = delete; @@ -574,15 +573,7 @@ protected: bool send(const hrt_abstime t) override { - bool updated_battery = false; - - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - if (_battery_status_sub[i].updated()) { - updated_battery = true; - } - } - - if (_status_sub.updated() || _cpuload_sub.updated() || updated_battery) { + if (_status_sub.updated() || _cpuload_sub.updated() || _battery_status_subs.updated()) { vehicle_status_s status{}; _status_sub.copy(&status); @@ -591,13 +582,13 @@ protected: battery_status_s battery_status[ORB_MULTI_MAX_INSTANCES] {}; - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - _battery_status_sub[i].copy(&battery_status[i]); + for (int i = 0; i < _battery_status_subs.size(); i++) { + _battery_status_subs[i].copy(&battery_status[i]); } int lowest_battery_index = 0; - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + for (int i = 0; i < _battery_status_subs.size(); i++) { if (battery_status[i].connected && (battery_status[i].remaining < battery_status[lowest_battery_index].remaining)) { lowest_battery_index = i; } @@ -668,21 +659,12 @@ public: unsigned get_size() override { - unsigned total_size = 0; - - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - if (_battery_status_sub[i].advertised()) { - total_size += MAVLINK_MSG_ID_BATTERY_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - } - } - - return total_size; + static constexpr unsigned size_per_battery = MAVLINK_MSG_ID_BATTERY_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return size_per_battery * _battery_status_subs.advertised_count(); } private: - uORB::Subscription _battery_status_sub[ORB_MULTI_MAX_INSTANCES] { - {ORB_ID(battery_status), 0}, {ORB_ID(battery_status), 1}, {ORB_ID(battery_status), 2}, {ORB_ID(battery_status), 3} - }; + uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; /* do not allow top copying this class */ MavlinkStreamBatteryStatus(MavlinkStreamSysStatus &) = delete; @@ -697,10 +679,10 @@ protected: { bool updated = false; - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + for (auto &battery_sub : _battery_status_subs) { battery_status_s battery_status; - if (_battery_status_sub[i].update(&battery_status)) { + if (battery_sub.update(&battery_status)) { /* battery status message with higher resolution */ mavlink_battery_status_t bat_msg{}; // TODO: Determine how to better map between battery ID within the firmware and in MAVLink @@ -764,7 +746,6 @@ protected: updated = true; } - } return updated; @@ -802,21 +783,12 @@ public: unsigned get_size() override { - unsigned total_size = 0; - - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - if (_battery_status_sub[i].advertised()) { - total_size += MAVLINK_MSG_ID_BATTERY_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - } - } - - return total_size; + static constexpr unsigned size_per_battery = MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return size_per_battery * _battery_status_subs.advertised_count(); } private: - uORB::Subscription _battery_status_sub[ORB_MULTI_MAX_INSTANCES] { - {ORB_ID(battery_status), 0}, {ORB_ID(battery_status), 1}, {ORB_ID(battery_status), 2}, {ORB_ID(battery_status), 3} - }; + uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; /* do not allow top copying this class */ MavlinkStreamSmartBatteryInfo(MavlinkStreamSysStatus &) = delete; @@ -831,10 +803,10 @@ protected: { bool updated = false; - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + for (auto &battery_sub : _battery_status_subs) { battery_status_s battery_status; - if (_battery_status_sub[i].update(&battery_status)) { + if (battery_sub.update(&battery_status)) { if (battery_status.serial_number == 0) { //This is not smart battery continue; @@ -899,7 +871,7 @@ public: } private: - uORB::Subscription _vehicle_imu_sub[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(vehicle_imu), 0}, {ORB_ID(vehicle_imu), 1}, {ORB_ID(vehicle_imu), 2}, {ORB_ID(vehicle_imu), 3}}; + uORB::SubscriptionMultiArray _vehicle_imu_subs{ORB_ID::vehicle_imu}; uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _bias_sub{ORB_ID(estimator_sensor_bias)}; uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)}; @@ -923,7 +895,7 @@ protected: vehicle_imu_s imu; - for (auto &imu_sub : _vehicle_imu_sub) { + for (auto &imu_sub : _vehicle_imu_subs) { if (imu_sub.update(&imu)) { if (imu.accel_device_id == sensor_selection.accel_device_id) { updated = true; @@ -2923,10 +2895,8 @@ public: return size; } - for (auto &x : _vehicle_imu_status_sub) { - if (x.advertised()) { - return size; - } + if (_vehicle_imu_status_subs.advertised()) { + return size; } return 0; @@ -2934,13 +2904,7 @@ public: private: uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; - - uORB::Subscription _vehicle_imu_status_sub[ORB_MULTI_MAX_INSTANCES] { - {ORB_ID(vehicle_imu_status), 0}, - {ORB_ID(vehicle_imu_status), 1}, - {ORB_ID(vehicle_imu_status), 2}, - {ORB_ID(vehicle_imu_status), 3}, - }; + uORB::SubscriptionMultiArray _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status}; /* do not allow top copying this class */ MavlinkStreamVibration(MavlinkStreamVibration &) = delete; @@ -2952,19 +2916,7 @@ protected: bool send(const hrt_abstime t) override { - bool updated = _sensor_selection_sub.updated(); - - // check for vehicle_imu_status update - if (!updated) { - for (auto &sub : _vehicle_imu_status_sub) { - if (sub.updated()) { - updated = true; - break; - } - } - } - - if (updated) { + if (_sensor_selection_sub.updated() || _vehicle_imu_status_subs.updated()) { mavlink_vibration_t msg{}; msg.time_usec = hrt_absolute_time(); @@ -2979,7 +2931,7 @@ protected: // primary accel high frequency vibration metric if (sensor_selection.accel_device_id != 0) { - for (auto &x : _vehicle_imu_status_sub) { + for (auto &x : _vehicle_imu_status_subs) { vehicle_imu_status_s status; if (x.copy(&status)) { @@ -2994,10 +2946,10 @@ protected: } // accel 0, 1, 2 cumulative clipping - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + for (int i = 0; i < math::min(static_cast(3), _vehicle_imu_status_subs.size()); i++) { vehicle_imu_status_s status; - if (_vehicle_imu_status_sub[i].copy(&status)) { + if (_vehicle_imu_status_subs[i].copy(&status)) { const uint32_t clipping = status.accel_clipping[0] + status.accel_clipping[1] + status.accel_clipping[2]; diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index f61376e2f2..70d7094917 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -145,7 +145,7 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) // copy corresponding vehicle_imu_status for accel & gyro error counts vehicle_imu_status_s imu_status{}; - _vehicle_imu_status_sub[uorb_index].copy(&imu_status); + _vehicle_imu_status_subs[uorb_index].copy(&imu_status); _accel_device_id[uorb_index] = imu_report.accel_device_id; _gyro_device_id[uorb_index] = imu_report.gyro_device_id; diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index de4b010248..75aa0aebc2 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -48,6 +48,7 @@ #include #include #include +#include #include #include #include @@ -168,11 +169,7 @@ private: uORB::PublicationQueued _info_pub{ORB_ID(subsystem_info)}; /* subsystem info publication */ uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[3]; - uORB::Subscription _vehicle_imu_status_sub[ACCEL_COUNT_MAX] { - {ORB_ID(vehicle_imu_status), 0}, - {ORB_ID(vehicle_imu_status), 1}, - {ORB_ID(vehicle_imu_status), 2}, - }; + uORB::SubscriptionMultiArray _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status}; sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */ diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 9d3ffb4014..77fda598d1 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -225,8 +225,9 @@ private: uORB::Publication _input_rc_pub{ORB_ID(input_rc)}; // HIL GPS - uORB::PublicationMulti *_vehicle_gps_position_pubs[ORB_MULTI_MAX_INSTANCES] {}; - uint8_t _gps_ids[ORB_MULTI_MAX_INSTANCES] {}; + static constexpr int MAX_GPS = 3; + uORB::PublicationMulti *_vehicle_gps_position_pubs[MAX_GPS] {}; + uint8_t _gps_ids[MAX_GPS] {}; std::default_random_engine _gen{}; // uORB subscription handlers diff --git a/src/modules/uORB/CMakeLists.txt b/src/modules/uORB/CMakeLists.txt index 48d0f94131..8c97b527ba 100644 --- a/src/modules/uORB/CMakeLists.txt +++ b/src/modules/uORB/CMakeLists.txt @@ -49,6 +49,7 @@ if(NOT PX4_BOARD MATCHES "px4_io") # TODO: fix this hack (move uORB to platform Subscription.hpp SubscriptionCallback.hpp SubscriptionInterval.hpp + SubscriptionMultiArray.hpp uORB.cpp uORB.h uORBCommon.hpp diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 3809429f8c..06c8fa45a1 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -112,7 +112,7 @@ public: /** * Check if there is a new update. - * */ + */ bool updated() { return advertised() && (_node->published_message_count() != _last_generation); } /** diff --git a/src/modules/uORB/SubscriptionInterval.hpp b/src/modules/uORB/SubscriptionInterval.hpp index 7d721a4db0..36e629f26d 100644 --- a/src/modules/uORB/SubscriptionInterval.hpp +++ b/src/modules/uORB/SubscriptionInterval.hpp @@ -86,6 +86,7 @@ public: ~SubscriptionInterval() = default; bool subscribe() { return _subscription.subscribe(); } + void unsubscribe() { _subscription.unsubscribe(); } bool advertised() { return _subscription.advertised(); } diff --git a/src/modules/uORB/SubscriptionMultiArray.hpp b/src/modules/uORB/SubscriptionMultiArray.hpp new file mode 100644 index 0000000000..65ff9b2dbc --- /dev/null +++ b/src/modules/uORB/SubscriptionMultiArray.hpp @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +/** + * @file SubscriptionMultiArray.hpp + * + */ + +#pragma once + +#include + +#include +#include + +#include "SubscriptionInterval.hpp" + +namespace uORB +{ + +/** + * An array of uORB::Subscriptions of the same topic + */ +template +class SubscriptionMultiArray +{ +public: + static_assert(SIZE <= ORB_MULTI_MAX_INSTANCES, "size must be <= uORB max instances"); + + static constexpr uint8_t size() { return SIZE; } + + /** + * Constructor + * + * @param id The uORB ORB_ID enum for the topic. + */ + explicit SubscriptionMultiArray(ORB_ID id) + { + for (uint8_t i = 0; i < SIZE; i++) { + _subscriptions[i] = SubscriptionInterval{id, i}; + _subscriptions[i].subscribe(); + } + } + + ~SubscriptionMultiArray() = default; + + SubscriptionInterval &operator [](int i) { return _subscriptions[i]; } + const SubscriptionInterval &operator [](int i) const { return _subscriptions[i]; } + + SubscriptionInterval *begin() { return &_subscriptions[0]; } + SubscriptionInterval *end() { return &_subscriptions[SIZE - 1]; } + + const SubscriptionInterval *begin() const { return &_subscriptions[0]; } + const SubscriptionInterval *end() const { return &_subscriptions[SIZE - 1]; } + + // true if any instance is advertised + bool advertised() + { + for (auto &s : _subscriptions) { + if (s.advertised()) { + return true; + } + } + + return false; + } + + // return the number of instances currently advertised + uint8_t advertised_count() + { + uint8_t count = 0; + + for (auto &s : _subscriptions) { + if (s.advertised()) { + count++; + } + } + + return count; + } + + // true if any instance is updated + bool updated() + { + for (auto &s : _subscriptions) { + if (s.updated()) { + return true; + } + } + + return false; + } + +private: + SubscriptionInterval _subscriptions[SIZE]; +}; + +} // namespace uORB