From d2afff1f8e0e8351a654d0ab195720ec9ea65526 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Mar 2018 19:38:40 -0400 Subject: [PATCH 1/2] wind_estimator fix LPWORK/HPWORK confusion and cleanup --- src/modules/wind_estimator/CMakeLists.txt | 2 + .../wind_estimator/wind_estimator_main.cpp | 132 ++++++++++-------- 2 files changed, 78 insertions(+), 56 deletions(-) diff --git a/src/modules/wind_estimator/CMakeLists.txt b/src/modules/wind_estimator/CMakeLists.txt index 6b6ff3e65661..597f629db8b0 100644 --- a/src/modules/wind_estimator/CMakeLists.txt +++ b/src/modules/wind_estimator/CMakeLists.txt @@ -33,6 +33,8 @@ px4_add_module( MODULE modules__wind_estimator MAIN wind_estimator + INCLUDES + ${PX4_SOURCE_DIR}/src/lib/ecl SRCS wind_estimator_main.cpp ) diff --git a/src/modules/wind_estimator/wind_estimator_main.cpp b/src/modules/wind_estimator/wind_estimator_main.cpp index 5d5311596a34..0682f9ab1646 100644 --- a/src/modules/wind_estimator/wind_estimator_main.cpp +++ b/src/modules/wind_estimator/wind_estimator_main.cpp @@ -31,24 +31,28 @@ * ****************************************************************************/ +#include #include - +#include #include #include #include -#include - -#include -#include +#include +#include #include -#include +#include #include #include -#include -#include +#include +#include #define SCHEDULE_INTERVAL 100000 /**< The schedule interval in usec (10 Hz) */ +using matrix::Dcmf; +using matrix::Quatf; +using matrix::Vector2f; +using matrix::Vector3f; + class WindEstimatorModule : public ModuleBase, public ModuleParams { public: @@ -60,20 +64,17 @@ class WindEstimatorModule : public ModuleBase, public Modul /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); - /** @see ModuleBase */ - static WindEstimatorModule *instantiate(int argc, char *argv[]); - /** @see ModuleBase */ static int custom_command(int argc, char *argv[]); /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); - /** - * run the main loop - */ + // run the main loop void cycle(); + int print_status() override; + private: static struct work_s _work; @@ -85,6 +86,9 @@ class WindEstimatorModule : public ModuleBase, public Modul int _airspeed_sub{-1}; int _param_sub{-1}; + perf_counter_t _perf_elapsed{}; + perf_counter_t _perf_interval{}; + DEFINE_PARAMETERS( (ParamFloat) wind_p_noise, (ParamFloat) tas_scale_p_noise, @@ -113,6 +117,8 @@ WindEstimatorModule::WindEstimatorModule(): // initialise parameters update_params(); + _perf_elapsed = perf_alloc_once(PC_ELAPSED, "wind_estimator elapsed"); + _perf_interval = perf_alloc_once(PC_INTERVAL, "wind_estimator interval"); } WindEstimatorModule::~WindEstimatorModule() @@ -123,6 +129,9 @@ WindEstimatorModule::~WindEstimatorModule() orb_unsubscribe(_param_sub); orb_unadvertise(_wind_est_pub); + + perf_free(_perf_elapsed); + perf_free(_perf_interval); } int @@ -134,12 +143,13 @@ WindEstimatorModule::task_spawn(int argc, char *argv[]) // wait until task is up & running if (wait_until_running() < 0) { _task_id = -1; - return -1; - } - _task_id = task_id_is_work_queue; + } else { + _task_id = task_id_is_work_queue; + return PX4_OK; + } - return PX4_OK; + return PX4_ERROR; } void @@ -159,12 +169,17 @@ WindEstimatorModule::cycle_trampoline(void *arg) _object = dev; } - dev->cycle(); + if (dev) { + dev->cycle(); + } } void WindEstimatorModule::cycle() { + perf_count(_perf_interval); + perf_begin(_perf_elapsed); + bool param_updated; orb_check(_param_sub, ¶m_updated); @@ -172,49 +187,52 @@ WindEstimatorModule::cycle() update_params(); } - vehicle_attitude_s vehicle_attitude = {}; - vehicle_local_position_s vehicle_local_position = {}; - airspeed_s airspeed = {}; + bool lpos_valid = false; + bool att_valid = false; + bool airspeed_valid = false; - orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude); - orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed); - orb_copy(ORB_ID(vehicle_local_position), _vehicle_local_position_sub, &vehicle_local_position); + const hrt_abstime time_now_usec = hrt_absolute_time(); - hrt_abstime time_now_usec = hrt_absolute_time(); + // validate required conditions for the filter to fuse measurements - // update wind and airspeed estimator - _wind_estimator.update(time_now_usec); + vehicle_attitude_s att = {}; - // validate required conditions for the filter to fuse measurements - bool fuse = time_now_usec - vehicle_local_position.timestamp < 1000 * 1000 && vehicle_local_position.timestamp; - fuse &= time_now_usec - vehicle_attitude.timestamp < 1000 * 1000; - fuse &= vehicle_local_position.v_xy_valid; + 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); + } - // additionally, for airspeed fusion we need to have recent measurements - bool fuse_airspeed = fuse && time_now_usec - airspeed.timestamp < 1000 * 1000; + 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; + } + + // update wind and airspeed estimator + _wind_estimator.update(time_now_usec); - if (fuse) { + if (lpos_valid && att_valid) { - matrix::Dcmf R_to_earth(matrix::Quatf(vehicle_attitude.q)); - matrix::Vector3f vI(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f vI(lpos.vx, lpos.vy, lpos.vz); + Quatf q(att.q); // sideslip fusion - _wind_estimator.fuse_beta(time_now_usec, &vI._data[0][0], vehicle_attitude.q); + _wind_estimator.fuse_beta(time_now_usec, vI, q); - if (fuse_airspeed) { - matrix::Vector3f vel_var(vehicle_local_position.evh, vehicle_local_position.evh, vehicle_local_position.evv); - vel_var = R_to_earth * vel_var; + // additionally, for airspeed fusion we need to have recent measurements + airspeed_s airspeed = {}; - //airspeed fusion - _wind_estimator.fuse_airspeed(time_now_usec, airspeed.indicated_airspeed_m_s, &vI._data[0][0], - &vel_var._data[0][0]); + if (orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed) == PX4_OK) { + airspeed_valid = (time_now_usec - airspeed.timestamp < 1000 * 1000) && (airspeed.timestamp > 0); } - } + if (airspeed_valid) { + Vector3f vel_var{Dcmf(q) *Vector3f{lpos.evh, lpos.evh, lpos.evv}}; - // if we fused either airspeed or sideslip we publish a wind_estimate message - if (fuse) { + // airspeed fusion + _wind_estimator.fuse_airspeed(time_now_usec, airspeed.indicated_airspeed_m_s, vI, Vector2f{vel_var(0), vel_var(1)}); + } + + // if we fused either airspeed or sideslip we publish a wind_estimate message wind_estimate_s wind_est = {}; wind_est.timestamp = time_now_usec; @@ -236,12 +254,14 @@ WindEstimatorModule::cycle() orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub, &wind_est, &instance, ORB_PRIO_DEFAULT); } + perf_end(_perf_elapsed); + if (should_exit()) { exit_and_cleanup(); } else { /* schedule next cycle */ - work_queue(HPWORK, &_work, (worker_t)&WindEstimatorModule::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL)); + work_queue(LPWORK, &_work, (worker_t)&WindEstimatorModule::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL)); } } @@ -254,18 +274,10 @@ void WindEstimatorModule::update_params() _wind_estimator.set_tas_scale_p_noise(tas_scale_p_noise.get()); _wind_estimator.set_tas_noise(tas_noise.get()); _wind_estimator.set_beta_noise(beta_noise.get()); - -} - -WindEstimatorModule *WindEstimatorModule::instantiate(int argc, char *argv[]) -{ - // No arguments to parse. We also know that we should run as task - return new WindEstimatorModule(); } int WindEstimatorModule::custom_command(int argc, char *argv[]) { - /* start the FMU if not running */ if (!is_running()) { int ret = WindEstimatorModule::task_spawn(argc, argv); @@ -301,6 +313,14 @@ measured_airspeed = scale_factor * real_airspeed. return 0; } +int WindEstimatorModule::print_status() +{ + perf_print_counter(_perf_elapsed); + perf_print_counter(_perf_interval); + + return 0; +} + extern "C" __EXPORT int wind_estimator_main(int argc, char *argv[]); int From df46960b5be33263fef35e62a6f95ab4ea2bf708 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 21 Mar 2018 00:36:04 -0400 Subject: [PATCH 2/2] wind_estimator trim module documentation to fit in limit --- src/modules/wind_estimator/wind_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/wind_estimator/wind_estimator_main.cpp b/src/modules/wind_estimator/wind_estimator_main.cpp index 0682f9ab1646..01ea736a3ec6 100644 --- a/src/modules/wind_estimator/wind_estimator_main.cpp +++ b/src/modules/wind_estimator/wind_estimator_main.cpp @@ -299,9 +299,9 @@ int WindEstimatorModule::print_usage(const char *reason) R"DESCR_STR( ### Description This module runs a combined wind and airspeed scale factor estimator. -If provided the vehicle NED speed and the vehicle attitude it can estimate the horizontal wind components based on a zero +If provided the vehicle NED speed and attitude it can estimate the horizontal wind components based on a zero sideslip assumption. This makes the estimator only suitable for fixed wing vehicles. -If additionally provided an airspeed measurment this module also estimates an airspeed scale factor based on the following model: +If provided an airspeed measurement this module also estimates an airspeed scale factor based on the following model: measured_airspeed = scale_factor * real_airspeed. )DESCR_STR");