fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 14:30: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/ekf2/ekf2_main.cpp
+12 -9
View File
@@ -51,6 +51,7 @@
#include <px4_tasks.h>
#include <px4_time.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/distance_sensor.h>
@@ -283,7 +284,7 @@ private:
uORB::Publication<sensor_bias_s> _sensor_bias_pub{ORB_ID(sensor_bias)};
uORB::Publication<vehicle_attitude_s> _att_pub{ORB_ID(vehicle_attitude)};
uORB::Publication<vehicle_odometry_s> _vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
uORB::Publication<wind_estimate_s> _wind_pub{ORB_ID(wind_estimate)};
uORB::PublicationMulti<wind_estimate_s> _wind_pub{ORB_ID(wind_estimate)};
uORB::PublicationData<vehicle_global_position_s> _vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
uORB::PublicationData<vehicle_local_position_s> _vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
@@ -1757,20 +1758,22 @@ bool Ekf2::publish_attitude(const sensor_combined_s &sensors, const hrt_abstime
bool Ekf2::publish_wind_estimate(const hrt_abstime &timestamp)
{
if (_ekf.get_wind_status()) {
float velNE_wind[2];
_ekf.get_wind_velocity(velNE_wind);
float wind_var[2];
_ekf.get_wind_velocity_var(wind_var);
// Publish wind estimate
// Publish wind estimate only if ekf declares them valid
wind_estimate_s wind_estimate{};
float velNE_wind[2];
float wind_var[2];
_ekf.get_wind_velocity(velNE_wind);
_ekf.get_wind_velocity_var(wind_var);
_ekf.get_airspeed_innov(&wind_estimate.tas_innov);
_ekf.get_airspeed_innov_var(&wind_estimate.tas_innov_var);
_ekf.get_beta_innov(&wind_estimate.beta_innov);
_ekf.get_beta_innov_var(&wind_estimate.beta_innov_var);
wind_estimate.timestamp = timestamp;
wind_estimate.windspeed_north = velNE_wind[0];
wind_estimate.windspeed_east = velNE_wind[1];
wind_estimate.variance_north = wind_var[0];
wind_estimate.variance_east = wind_var[1];
wind_estimate.tas_scale = 1.0f; //fix to 1 as scale not estimated in ekf
wind_estimate.tas_scale = 0.0f; //leave at 0 as scale is not estimated in ekf
_wind_pub.publish(wind_estimate);