From 71d58c9278b34ec6c90ec9497e292897f8a256a5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 4 Jun 2019 12:15:01 -0400 Subject: [PATCH] wind_estimator move to new WQ (lp_default) and uORB::Subscription --- .../wind_estimator/wind_estimator_main.cpp | 93 ++++++------------- 1 file changed, 30 insertions(+), 63 deletions(-) diff --git a/src/modules/wind_estimator/wind_estimator_main.cpp b/src/modules/wind_estimator/wind_estimator_main.cpp index 2a70b40730..f961cd79fa 100644 --- a/src/modules/wind_estimator/wind_estimator_main.cpp +++ b/src/modules/wind_estimator/wind_estimator_main.cpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include #include #include @@ -49,14 +49,14 @@ using namespace time_literals; -#define SCHEDULE_INTERVAL 100000 /**< The schedule interval in usec (10 Hz) */ +static constexpr uint32_t SCHEDULE_INTERVAL{100_ms}; /**< 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 +class WindEstimatorModule : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: @@ -74,20 +74,19 @@ public: static int print_usage(const char *reason = nullptr); // run the main loop - void cycle(); + void Run() override; int print_status() override; private: - static struct work_s _work; WindEstimator _wind_estimator; orb_advert_t _wind_est_pub{nullptr}; /**< wind estimate topic */ - int _vehicle_attitude_sub{-1}; - int _vehicle_local_position_sub{-1}; - int _airspeed_sub{-1}; - int _param_sub{-1}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; + uORB::Subscription _param_sub{ORB_ID(parameter_update)}; perf_counter_t _perf_elapsed{}; perf_counter_t _perf_interval{}; @@ -103,7 +102,6 @@ private: (ParamInt) _param_west_beta_gate ) - static void cycle_trampoline(void *arg); int start(); void update_params(); @@ -111,16 +109,10 @@ private: bool subscribe_topics(); }; -work_s WindEstimatorModule::_work = {}; - WindEstimatorModule::WindEstimatorModule(): - ModuleParams(nullptr) + ModuleParams(nullptr), + ScheduledWorkItem(px4::wq_configurations::lp_default) { - _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - _param_sub = orb_subscribe(ORB_ID(parameter_update)); - // initialise parameters update_params(); @@ -130,10 +122,7 @@ WindEstimatorModule::WindEstimatorModule(): WindEstimatorModule::~WindEstimatorModule() { - orb_unsubscribe(_vehicle_attitude_sub); - orb_unsubscribe(_vehicle_local_position_sub); - orb_unsubscribe(_airspeed_sub); - orb_unsubscribe(_param_sub); + ScheduleClear(); orb_unadvertise(_wind_est_pub); @@ -145,13 +134,18 @@ int WindEstimatorModule::task_spawn(int argc, char *argv[]) { /* schedule a cycle to start things */ - work_queue(LPWORK, &_work, (worker_t)&WindEstimatorModule::cycle_trampoline, nullptr, 0); + WindEstimatorModule *dev = new WindEstimatorModule(); - // wait until task is up & running - if (wait_until_running() < 0) { - _task_id = -1; + // check if the trampoline is called for the first time + if (!dev) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } - } else { + _object.store(dev); + + if (dev) { + dev->ScheduleOnInterval(SCHEDULE_INTERVAL, 10000); _task_id = task_id_is_work_queue; return PX4_OK; } @@ -160,37 +154,14 @@ WindEstimatorModule::task_spawn(int argc, char *argv[]) } void -WindEstimatorModule::cycle_trampoline(void *arg) -{ - WindEstimatorModule *dev = reinterpret_cast(arg); - - // check if the trampoline is called for the first time - if (!dev) { - dev = new WindEstimatorModule(); - - if (!dev) { - PX4_ERR("alloc failed"); - return; - } - - _object.store(dev); - } - - if (dev) { - dev->cycle(); - } -} - -void -WindEstimatorModule::cycle() +WindEstimatorModule::Run() { perf_count(_perf_interval); perf_begin(_perf_elapsed); - bool param_updated; - orb_check(_param_sub, ¶m_updated); + parameter_update_s param{}; - if (param_updated) { + if (_param_sub.update(¶m)) { update_params(); } @@ -201,15 +172,15 @@ WindEstimatorModule::cycle() // validate required conditions for the filter to fuse measurements - vehicle_attitude_s att = {}; + vehicle_attitude_s att{}; - if (orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &att) == PX4_OK) { + if (_vehicle_attitude_sub.update(&att)) { att_valid = (time_now_usec - att.timestamp < 1_s) && (att.timestamp > 0); } - vehicle_local_position_s lpos = {}; + vehicle_local_position_s lpos{}; - if (orb_copy(ORB_ID(vehicle_local_position), _vehicle_local_position_sub, &lpos) == PX4_OK) { + if (_vehicle_local_position_sub.update(&lpos)) { lpos_valid = (time_now_usec - lpos.timestamp < 1_s) && (lpos.timestamp > 0) && lpos.v_xy_valid; } @@ -225,9 +196,9 @@ WindEstimatorModule::cycle() _wind_estimator.fuse_beta(time_now_usec, vI, q); // additionally, for airspeed fusion we need to have recent measurements - airspeed_s airspeed = {}; + airspeed_s airspeed{}; - if (orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed) == PX4_OK) { + if (_airspeed_sub.update(&airspeed)) { if ((time_now_usec - airspeed.timestamp < 1_s) && (airspeed.timestamp > 0)) { const Vector3f vel_var{Dcmf(q) *Vector3f{lpos.evh, lpos.evh, lpos.evv}}; @@ -261,10 +232,6 @@ WindEstimatorModule::cycle() if (should_exit()) { exit_and_cleanup(); - - } else { - /* schedule next cycle */ - work_queue(LPWORK, &_work, (worker_t)&WindEstimatorModule::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL)); } }