fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-31 03:51:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

airspeed_selector: fix airspeed subscription

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2019-11-22 18:25:22 -05:00
committed by Daniel Agar
parent 75da6e351b
commit 3f92bc26ce
1 changed files with 11 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/airspeed_selector_main.cpp
+11 -9
View File
@@ -115,6 +115,7 @@ private:
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_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. */
estimator_status_s _estimator_status {}; estimator_status_s _estimator_status {};
vehicle_acceleration_s _accel {}; vehicle_acceleration_s _accel {};
@@ -128,7 +129,6 @@ private:
WindEstimator _wind_estimator_sideslip; /**< wind estimator instance only fusing sideslip */ WindEstimator _wind_estimator_sideslip; /**< wind estimator instance only fusing sideslip */
wind_estimate_s _wind_estimate_sideslip {}; /**< wind estimate message for wind estimator instance only fusing sideslip */ wind_estimate_s _wind_estimate_sideslip {}; /**< wind estimate message for wind estimator instance only fusing sideslip */
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)*/ int32_t _number_of_airspeed_sensors{0}; /**< number of airspeed sensors in use (detected during initialization)*/
int32_t _prev_number_of_airspeed_sensors{0}; /**< number of airspeed sensors in previous loop (to detect a new added sensor)*/ 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) */ AirspeedValidator _airspeed_validator[MAX_NUM_AIRSPEED_SENSORS] {}; /**< airspeedValidator instances (one for each sensor) */
@@ -242,22 +242,24 @@ AirspeedModule::init()
void void
AirspeedModule::check_for_connected_airspeed_sensors() AirspeedModule::check_for_connected_airspeed_sensors()
{ {
if (_param_airspeed_primary_index.get() > 0) { /* check for new connected airspeed sensor */
for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) { int detected_airspeed_sensors = 0;
if (orb_exists(ORB_ID(airspeed), i) == PX4_OK) {
_airspeed_sub[i] = orb_subscribe_multi(ORB_ID(airspeed), i);
} else { if (_param_airspeed_primary_index.get() > 0) {
for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
if (!_airspeed_sub[i].published()) {
break; break;
} }
_number_of_airspeed_sensors = i + 1; detected_airspeed_sensors = i + 1;
} }
} else { } else {
_number_of_airspeed_sensors = 0; //user has selected groundspeed-windspeed as primary source, or disabled airspeed detected_airspeed_sensors = 0; //user has selected groundspeed-windspeed as primary source, or disabled airspeed
} }
_number_of_airspeed_sensors = detected_airspeed_sensors;
} }
@@ -318,7 +320,7 @@ AirspeedModule::Run()
/* poll airspeed data */ /* poll airspeed data */
airspeed_s airspeed_raw = {}; airspeed_s airspeed_raw = {};
orb_copy(ORB_ID(airspeed), _airspeed_sub[i], &airspeed_raw); // poll raw airspeed topic of the i-th sensor _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_indicated_raw = airspeed_raw.indicated_airspeed_m_s;
input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s; input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s;
input_data.airspeed_timestamp = airspeed_raw.timestamp; input_data.airspeed_timestamp = airspeed_raw.timestamp;