fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 22:23:35 +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/mavlink/streams/ALTITUDE.hpp
+1 -1
View File
@@ -35,7 +35,7 @@
#define ALTITUDE_HPP
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/wind.h>
class MavlinkStreamAltitude : public MavlinkStream
{
src/modules/mavlink/streams/HIGH_LATENCY2.hpp
+8 -8
View File
@@ -49,7 +49,7 @@
#include <uORB/topics/mission_result.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/wind.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
@@ -134,7 +134,7 @@ private:
updated |= write_tecs_status(&msg);
updated |= write_vehicle_status(&msg);
updated |= write_vehicle_status_flags(&msg);
updated |= write_wind_estimate(&msg);
updated |= write_wind(&msg);
if (updated) {
msg.timestamp = t / 1000;
@@ -477,9 +477,9 @@ private:
return false;
}
bool write_wind_estimate(mavlink_high_latency2_t *msg)
bool write_wind(mavlink_high_latency2_t *msg)
{
wind_estimate_s wind;
wind_s wind;
if (_wind_sub.update(&wind)) {
msg->wind_heading = static_cast<uint8_t>(math::degrees(matrix::wrap_2pi(atan2f(wind.windspeed_east,
@@ -506,7 +506,7 @@ private:
update_local_position();
update_gps();
update_vehicle_status();
update_wind_estimate();
update_wind();
}
void update_airspeed()
@@ -586,9 +586,9 @@ private:
}
}
void update_wind_estimate()
void update_wind()
{
wind_estimate_s wind;
wind_s wind;
if (_wind_sub.update(&wind)) {
_windspeed.add_value(sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east),
@@ -642,7 +642,7 @@ private:
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _status_flags_sub{ORB_ID(vehicle_status_flags)};
uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)};
uORB::Subscription _wind_sub{ORB_ID(wind_estimate)};
uORB::Subscription _wind_sub{ORB_ID(wind)};
SimpleAnalyzer _airspeed;
SimpleAnalyzer _airspeed_sp;
src/modules/mavlink/streams/WIND_COV.hpp
+9 -9
View File
@@ -35,7 +35,7 @@
#define WIND_COV_HPP
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/wind.h>
class MavlinkStreamWindCov : public MavlinkStream
{
@@ -50,29 +50,29 @@ public:
unsigned get_size() override
{
return _wind_estimate_sub.advertised() ? MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
return _wind_sub.advertised() ? MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
explicit MavlinkStreamWindCov(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _wind_estimate_sub{ORB_ID(wind_estimate)};
uORB::Subscription _wind_sub{ORB_ID(wind)};
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)};
bool send() override
{
wind_estimate_s wind_estimate;
wind_s wind;
if (_wind_estimate_sub.update(&wind_estimate)) {
if (_wind_sub.update(&wind)) {
mavlink_wind_cov_t msg{};
msg.time_usec = wind_estimate.timestamp;
msg.time_usec = wind.timestamp;
msg.wind_x = wind_estimate.windspeed_north;
msg.wind_y = wind_estimate.windspeed_east;
msg.wind_x = wind.windspeed_north;
msg.wind_y = wind.windspeed_east;
msg.wind_z = 0.0f;
msg.var_horiz = wind_estimate.variance_north + wind_estimate.variance_east;
msg.var_horiz = wind.variance_north + wind.variance_east;
msg.var_vert = 0.0f;
vehicle_local_position_s lpos{};