fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 13:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

select single system-wide wind estimate message (current best)

Browse Source 
- publish wind estimate only from EKF, and wind speeds from airspeed selector to new separate topic (airspeed_wind)
 - rename message wind_estimate to wind
 - publish wind from currently used ekf instance (ekf2selector)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-02-20 13:15:01 -05:00
committed by GitHub
parent 9d65e9a980
commit 0ea8104344
17 changed files with 119 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/airspeed_selector_main.cpp
+8 -8
View File
@@ -60,7 +60,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/airspeed_wind.h>
using namespace time_literals;
@@ -103,7 +103,7 @@ private:
};
uORB::Publication<airspeed_validated_s> _airspeed_validated_pub {ORB_ID(airspeed_validated)}; /**< airspeed validated topic*/
uORB::PublicationMulti<wind_estimate_s> _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {{ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */
uORB::PublicationMulti<airspeed_wind_s> _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {{ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */
orb_advert_t _mavlink_log_pub {nullptr}; /**< mavlink log topic*/
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@@ -129,7 +129,7 @@ private:
vtol_vehicle_status_s _vtol_vehicle_status {};
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 */
airspeed_wind_s _wind_estimate_sideslip {}; /**< wind estimate message for wind estimator instance only fusing sideslip */
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)*/
@@ -489,7 +489,7 @@ void AirspeedModule::update_wind_estimator_sideslip()
_wind_estimate_sideslip.beta_innov = _wind_estimator_sideslip.get_beta_innov();
_wind_estimate_sideslip.beta_innov_var = _wind_estimator_sideslip.get_beta_innov_var();
_wind_estimate_sideslip.tas_scale = _wind_estimator_sideslip.get_tas_scale();
_wind_estimate_sideslip.source = wind_estimate_s::SOURCE_AS_BETA_ONLY;
_wind_estimate_sideslip.source = airspeed_wind_s::SOURCE_AS_BETA_ONLY;
}
void AirspeedModule::update_ground_minus_wind_airspeed()
@@ -602,16 +602,16 @@ void AirspeedModule::select_airspeed_and_publish()
// publish the wind estimator states from all airspeed validators
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
wind_estimate_s wind_est = _airspeed_validator[i].get_wind_estimator_states(_time_now_usec);
airspeed_wind_s wind_est = _airspeed_validator[i].get_wind_estimator_states(_time_now_usec);
if (i == 0) {
wind_est.source = wind_estimate_s::SOURCE_AS_SENSOR_1;
wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_1;
} else if (i == 1) {
wind_est.source = wind_estimate_s::SOURCE_AS_SENSOR_2;
wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_2;
} else {
wind_est.source = wind_estimate_s::SOURCE_AS_SENSOR_3;
wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_3;
}
_wind_est_pub[i + 1].publish(wind_est);