fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/modules/airspeed_selector/airspeed_selector_main.cpp
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);
}
}

21
src/modules/ekf2/ekf2_main.cpp
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);

2
src/modules/logger/logger.cpp
View File

@ -549,13 +549,13 @@ void Logger::add_default_topics()
add_topic("vehicle_status", 200);
add_topic("vehicle_status_flags");
add_topic("vtol_vehicle_status", 200);
add_topic("wind_estimate", 200);
add_topic_multi("actuator_outputs", 100);
add_topic_multi("battery_status", 500);
add_topic_multi("distance_sensor", 100);
add_topic_multi("telemetry_status");
add_topic_multi("vehicle_gps_position");
add_topic_multi("wind_estimate", 200);
#ifdef CONFIG_ARCH_BOARD_PX4_SITL