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

wind_estimator status print last publication

Browse Source
This commit is contained in:
Daniel Agar
2018-08-02 08:07:01 +02:00
committed by Lorenz Meier
parent ad41744670
commit 253323601a
1 changed files with 22 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/wind_estimator/wind_estimator_main.cpp
+22 -8
View File
@@ -32,13 +32,14 @@
****************************************************************************/
#include <drivers/drv_hrt.h>
#include <matrix/math.hpp>
#include <ecl/airdata/WindEstimator.hpp>
#include <matrix/math.hpp>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <px4_module.h>
#include <px4_module_params.h>
#include <px4_workqueue.h>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -46,6 +47,8 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/wind_estimate.h>
using namespace time_literals;
#define SCHEDULE_INTERVAL 100000 /**< The schedule interval in usec (10 Hz) */
using matrix::Dcmf;
@@ -89,6 +92,8 @@ private:
perf_counter_t _perf_elapsed{};
perf_counter_t _perf_interval{};
int _instance{-1};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::WEST_W_P_NOISE>) wind_p_noise,
(ParamFloat<px4::params::WEST_SC_P_NOISE>) tas_scale_p_noise,
@@ -200,13 +205,13 @@ WindEstimatorModule::cycle()
vehicle_attitude_s att = {};
if (orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &att) == PX4_OK) {
att_valid = (time_now_usec - att.timestamp < 1000 * 1000) && (att.timestamp > 0);
att_valid = (time_now_usec - att.timestamp < 1_s) && (att.timestamp > 0);
}
vehicle_local_position_s lpos = {};
if (orb_copy(ORB_ID(vehicle_local_position), _vehicle_local_position_sub, &lpos) == PX4_OK) {
lpos_valid = (time_now_usec - lpos.timestamp < 1000 * 1000) && (lpos.timestamp > 0) && lpos.v_xy_valid;
lpos_valid = (time_now_usec - lpos.timestamp < 1_s) && (lpos.timestamp > 0) && lpos.v_xy_valid;
}
// update wind and airspeed estimator
@@ -224,7 +229,7 @@ WindEstimatorModule::cycle()
airspeed_s airspeed = {};
if (orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed) == PX4_OK) {
airspeed_valid = (time_now_usec - airspeed.timestamp < 1000 * 1000) && (airspeed.timestamp > 0);
airspeed_valid = (time_now_usec - airspeed.timestamp < 1_s) && (airspeed.timestamp > 0);
}
if (airspeed_valid) {
@@ -252,8 +257,7 @@ WindEstimatorModule::cycle()
wind_est.beta_innov_var = _wind_estimator.get_beta_innov_var();
wind_est.tas_scale = _wind_estimator.get_tas_scale();
int instance;
orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub, &wind_est, &instance, ORB_PRIO_DEFAULT);
orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub, &wind_est, &_instance, ORB_PRIO_DEFAULT);
}
perf_end(_perf_elapsed);
@@ -322,6 +326,16 @@ int WindEstimatorModule::print_status()
perf_print_counter(_perf_elapsed);
perf_print_counter(_perf_interval);
if (_instance > -1) {
unsigned instance = _instance;
uORB::Subscription<wind_estimate_s> est{ORB_ID(wind_estimate), instance};
est.update();
print_message(est.get());
} else {
PX4_INFO("never published");
}
return 0;
}