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

EKF and AirspeedSelector: publish multiple wind estimate topic instances and log them all

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2019-08-29 15:48:46 +02:00
committed by Daniel Agar
parent 10ed88db57
commit ff6577ce5f
3 changed files with 20 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/airspeed_selector_main.cpp
+7 -16
View File
@@ -49,6 +49,8 @@
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -93,8 +95,8 @@ public:
private:
static constexpr int MAX_NUM_AIRSPEED_SENSORS = 3; /**< Support max 3 airspeed sensors */
orb_advert_t _airspeed_validated_pub {nullptr}; /**< airspeed validated topic*/
orb_advert_t _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */
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) */
orb_advert_t _mavlink_log_pub {nullptr}; /**< mavlink log topic*/
uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
@@ -181,15 +183,6 @@ AirspeedModule::~AirspeedModule()
{
ScheduleClear();
for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
if (_wind_est_pub[i] != nullptr) {
orb_unadvertise(_wind_est_pub[i]);
}
}
orb_unadvertise(_airspeed_validated_pub);
perf_free(_perf_elapsed);
perf_free(_perf_interval);
@@ -524,17 +517,15 @@ void AirspeedModule::select_airspeed_and_publish()
}
/* publish airspeed validated topic */
int instance;
orb_publish_auto(ORB_ID(airspeed_validated), &_airspeed_validated_pub, &airspeed_validated, &instance,
ORB_PRIO_DEFAULT);
_airspeed_validated_pub.publish(airspeed_validated);
/* publish sideslip-only-fusion wind topic */
orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub[0], &_wind_estimate_sideslip, &instance, ORB_PRIO_LOW);
_wind_est_pub[0].publish(_wind_estimate_sideslip);
/* 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(hrt_absolute_time());
orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub[i + 1], &wind_est, &instance, ORB_PRIO_LOW);
_wind_est_pub[i + 1].publish(wind_est);
}
}