dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

uORB: introduce SubscriptionMultiArray for working with multi-instances

This commit is contained in:
Daniel Agar 2020-09-02 12:46:47 -04:00 committed by GitHub
parent 71f381ada9
commit 6ff361479c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
20 changed files with 234 additions and 169 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/drivers/uavcan/sensors/sensor_bridge.hpp
View File

@ -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,

2
src/drivers/uavcannode/UavcanNode.cpp
View File

@ -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)) {

3
src/drivers/uavcannode/UavcanNode.hpp
View File

@ -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},

9
src/lib/collision_prevention/CollisionPrevention.cpp
View File

@ -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) &&

3
src/lib/collision_prevention/CollisionPrevention.hpp
View File

@ -51,6 +51,7 @@
#include <systemlib/mavlink_log.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/collision_constraints.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/mavlink_log.h>
@ -131,8 +132,8 @@ private:
uORB::PublicationQueued<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do publication */
uORB::SubscriptionData<obstacle_distance_s> _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<vehicle_attitude_s> _sub_vehicle_attitude{ORB_ID(vehicle_attitude)};
uORB::SubscriptionMultiArray<distance_sensor_s> _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};

53
src/modules/airspeed_selector/airspeed_selector_main.cpp
View File

@ -44,6 +44,7 @@
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/estimator_status.h>
@ -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_s, MAX_NUM_AIRSPEED_SENSORS> _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);
}
}

33
src/modules/commander/Commander.cpp
View File

@ -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.

18
src/modules/commander/Commander.hpp
View File

@ -56,6 +56,7 @@
// subscriptions
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
@ -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_s> _battery_status_subs{ORB_ID::battery_status};
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
#if defined(BOARD_HAS_POWER_CONTROL)
uORB::Subscription _power_button_state_sub {ORB_ID(power_button_state)};

9
src/modules/commander/accelerometer_calibration.cpp
View File

@ -141,6 +141,7 @@
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionBlocking.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/vehicle_attitude.h>
@ -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<sensor_accel_s, MAX_ACCEL_SENS> 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};

14
src/modules/ekf2/ekf2_main.cpp
View File

@ -54,6 +54,7 @@
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/ekf2_timestamps.h>
@ -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_s> _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);

10
src/modules/mavlink/mavlink_high_latency2.cpp
View File

@ -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);

3
src/modules/mavlink/mavlink_high_latency2.h
View File

@ -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 &);

98
src/modules/mavlink/mavlink_messages.cpp
View File

@ -61,6 +61,7 @@
#include <math.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
@ -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_s> _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_s> _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_s> _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_s> _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_s> _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<uint8_t>(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];

2
src/modules/sensors/voted_sensors_update.cpp
View File

@ -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;

7
src/modules/sensors/voted_sensors_update.h
View File

@ -48,6 +48,7 @@
#include <matrix/math.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
@ -168,11 +169,7 @@ private:
uORB::PublicationQueued<subsystem_info_s> _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_s, ACCEL_COUNT_MAX> _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 */

5
src/modules/simulator/simulator.h
View File

@ -225,8 +225,9 @@ private:
uORB::Publication<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};
// HIL GPS
uORB::PublicationMulti<vehicle_gps_position_s> *_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_s> *_vehicle_gps_position_pubs[MAX_GPS] {};
uint8_t _gps_ids[MAX_GPS] {};
std::default_random_engine _gen{};
// uORB subscription handlers

1
src/modules/uORB/CMakeLists.txt
View File

@ -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

2
src/modules/uORB/Subscription.hpp
View File

@ -112,7 +112,7 @@ public:
/**
* Check if there is a new update.
* */
*/
bool updated() { return advertised() && (_node->published_message_count() != _last_generation); }
/**

1
src/modules/uORB/SubscriptionInterval.hpp
View File

@ -86,6 +86,7 @@ public:
~SubscriptionInterval() = default;
bool subscribe() { return _subscription.subscribe(); }
void unsubscribe() { _subscription.unsubscribe(); }
bool advertised() { return _subscription.advertised(); }

128
src/modules/uORB/SubscriptionMultiArray.hpp Normal file
View File

@ -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 <uORB/uORB.h>
#include <px4_platform_common/defines.h>
#include <lib/mathlib/mathlib.h>
#include "SubscriptionInterval.hpp"
namespace uORB
{
/**
* An array of uORB::Subscriptions of the same topic
*/
template<typename T, uint8_t SIZE = ORB_MULTI_MAX_INSTANCES>
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