fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 08:20:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

uORB: introduce SubscriptionMultiArray for working with multi-instances

Browse Source
This commit is contained in:
Daniel Agar
2020-09-02 12:46:47 -04:00
committed by GitHub
parent 71f381ada9
commit 6ff361479c
20 changed files with 234 additions and 169 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/airspeed_selector_main.cpp
+28 -25
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);
}
}