From aabe8da5ecf1bfe33ae9dbc14052f904c9c56bdc Mon Sep 17 00:00:00 2001 From: mcsauder Date: Wed, 5 Jun 2019 16:55:04 -0600 Subject: [PATCH 1/4] Refactor the LandDetector class to - Reduce duplicate code in LandDetector _check_params() method. - Standardize naming cases. - Implement DEFINE_PARAMETERS() macro. --- src/modules/land_detector/LandDetector.cpp | 43 ++++++++++---------- src/modules/land_detector/LandDetector.h | 46 ++++++++++++---------- 2 files changed, 48 insertions(+), 41 deletions(-) diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index d6a750f89a3f..6bbafa2524f6 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -54,17 +54,14 @@ namespace land_detector { LandDetector::LandDetector() : - ScheduledWorkItem(px4::wq_configurations::hp_default), - _cycle_perf(perf_alloc(PC_ELAPSED, "land_detector_cycle")) + ModuleParams(nullptr), + ScheduledWorkItem(px4::wq_configurations::hp_default) { _landDetected.timestamp = hrt_absolute_time(); _landDetected.freefall = false; _landDetected.landed = true; _landDetected.ground_contact = false; _landDetected.maybe_landed = false; - - _p_total_flight_time_high = param_find("LND_FLIGHT_T_HI"); - _p_total_flight_time_low = param_find("LND_FLIGHT_T_LO"); } LandDetector::~LandDetector() @@ -81,8 +78,8 @@ void LandDetector::Run() { perf_begin(_cycle_perf); - _check_params(false); - _armingSub.update(&_arming); + _check_params(); + _actuator_armed_sub.update(&_arming); _update_topics(); _update_state(); @@ -98,7 +95,7 @@ void LandDetector::Run() // publish at 1 Hz, very first time, or when the result has changed if ((hrt_elapsed_time(&_landDetected.timestamp) >= 1_s) || - (_landDetectedPub == nullptr) || + (_land_detected_pub == nullptr) || (_landDetected.landed != landDetected) || (_landDetected.freefall != freefallDetected) || (_landDetected.maybe_landed != maybe_landedDetected) || @@ -120,22 +117,28 @@ void LandDetector::Run() _landDetected.in_ground_effect = in_ground_effect; int instance; - orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected, + orb_publish_auto(ORB_ID(vehicle_land_detected), &_land_detected_pub, &_landDetected, &instance, ORB_PRIO_DEFAULT); } // set the flight time when disarming (not necessarily when landed, because all param changes should // happen on the same event and it's better to set/save params while not in armed state) - if (_takeoff_time != 0 && !_arming.armed && _previous_arming_state) { + if (_takeoff_time != 0 && !_arming.armed && _previous_armed_state) { _total_flight_time += now - _takeoff_time; _takeoff_time = 0; + uint32_t flight_time = (_total_flight_time >> 32) & 0xffffffff; - param_set_no_notification(_p_total_flight_time_high, &flight_time); + + _param_total_flight_time_high.set(flight_time); + _param_total_flight_time_high.commit_no_notification(); + flight_time = _total_flight_time & 0xffffffff; - param_set_no_notification(_p_total_flight_time_low, &flight_time); + + _param_total_flight_time_low.set(flight_time); + _param_total_flight_time_low.commit_no_notification(); } - _previous_arming_state = _arming.armed; + _previous_armed_state = _arming.armed; perf_end(_cycle_perf); @@ -144,17 +147,15 @@ void LandDetector::Run() exit_and_cleanup(); } } -void LandDetector::_check_params(const bool force) + +void LandDetector::_check_params() { - parameter_update_s paramUpdate; + parameter_update_s param_update; - if (_parameterSub.update(¶mUpdate) || force) { + if (_param_update_sub.update(¶m_update)) { _update_params(); - uint32_t flight_time; - param_get(_p_total_flight_time_high, (int32_t *)&flight_time); - _total_flight_time = ((uint64_t)flight_time) << 32; - param_get(_p_total_flight_time_low, (int32_t *)&flight_time); - _total_flight_time |= flight_time; + _total_flight_time = static_cast(_param_total_flight_time_high.get()) << 32; + _total_flight_time |= _param_total_flight_time_low.get(); } } diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index dd0a30516a40..6fbd31b2f5f0 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -43,6 +43,7 @@ #pragma once #include +#include #include #include #include @@ -57,7 +58,7 @@ using namespace time_literals; namespace land_detector { -class LandDetector : public ModuleBase, px4::ScheduledWorkItem +class LandDetector : public ModuleBase, ModuleParams, px4::ScheduledWorkItem { public: enum class LandDetectionState { @@ -71,7 +72,6 @@ class LandDetector : public ModuleBase, px4::ScheduledWorkItem LandDetector(); virtual ~LandDetector(); - static int task_spawn(int argc, char *argv[]); /** @see ModuleBase */ static int custom_command(int argc, char *argv[]) @@ -88,27 +88,26 @@ class LandDetector : public ModuleBase, px4::ScheduledWorkItem /** * @return current state. */ - LandDetectionState get_state() const - { - return _state; - } + LandDetectionState get_state() const { return _state; } /** * Get the work queue going. */ void start(); + static int task_spawn(int argc, char *argv[]); + protected: /** - * Update uORB topics. + * Update parameters. */ - virtual void _update_topics() = 0; + virtual void _update_params() = 0; /** - * Update parameters. + * Update uORB topics. */ - virtual void _update_params() = 0; + virtual void _update_topics() = 0; /** * @return true if UAV is in a landed state. @@ -143,7 +142,7 @@ class LandDetector : public ModuleBase, px4::ScheduledWorkItem /** Run main land detector loop at this interval. */ static constexpr uint32_t LAND_DETECTOR_UPDATE_INTERVAL = 20_ms; - orb_advert_t _landDetectedPub{nullptr}; + orb_advert_t _land_detected_pub{nullptr}; vehicle_land_detected_s _landDetected{}; LandDetectionState _state{LandDetectionState::LANDED}; @@ -154,26 +153,33 @@ class LandDetector : public ModuleBase, px4::ScheduledWorkItem systemlib::Hysteresis _ground_contact_hysteresis{true}; systemlib::Hysteresis _ground_effect_hysteresis{false}; - struct actuator_armed_s _arming {}; + actuator_armed_s _arming {}; private: - void Run() override; - void _check_params(bool force = false); + void _check_params(); + + void Run() override; void _update_state(); - param_t _p_total_flight_time_high{PARAM_INVALID}; - param_t _p_total_flight_time_low{PARAM_INVALID}; + bool _previous_armed_state{false}; ///< stores the previous actuator_armed.armed state + + uint32_t _measure_interval{LAND_DETECTOR_UPDATE_INTERVAL}; + uint64_t _total_flight_time{0}; ///< in microseconds + hrt_abstime _takeoff_time{0}; - perf_counter_t _cycle_perf; + perf_counter_t _cycle_perf{(perf_alloc(PC_ELAPSED, "land_detector_cycle"))}; - bool _previous_arming_state{false}; ///< stores the previous _arming.armed state + uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _param_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _parameterSub{ORB_ID(parameter_update)}; - uORB::Subscription _armingSub{ORB_ID(actuator_armed)}; + DEFINE_PARAMETERS( + (ParamInt) _param_total_flight_time_high, + (ParamInt) _param_total_flight_time_low + ); }; } // namespace land_detector From 55d52c4affac65a80e6b8c454a1f7d60a46855d6 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Mon, 24 Jun 2019 09:21:45 -0600 Subject: [PATCH 2/4] Delete _measure_interval and redundant parentheses in LandDetector.h. --- src/modules/land_detector/LandDetector.h | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 6fbd31b2f5f0..d93db958e5dd 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -165,13 +165,11 @@ class LandDetector : public ModuleBase, ModuleParams, px4::Schedul bool _previous_armed_state{false}; ///< stores the previous actuator_armed.armed state - uint32_t _measure_interval{LAND_DETECTOR_UPDATE_INTERVAL}; - uint64_t _total_flight_time{0}; ///< in microseconds hrt_abstime _takeoff_time{0}; - perf_counter_t _cycle_perf{(perf_alloc(PC_ELAPSED, "land_detector_cycle"))}; + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, "land_detector_cycle")}; uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; uORB::Subscription _param_update_sub{ORB_ID(parameter_update)}; From bf67344943867958d7eb7141a95d872170bf373a Mon Sep 17 00:00:00 2001 From: mcsauder Date: Tue, 25 Jun 2019 07:15:19 -0600 Subject: [PATCH 3/4] Revert changes to the check_params(const bool force) declaration/definition to match current PX4:master. --- src/modules/land_detector/LandDetector.cpp | 6 +++--- src/modules/land_detector/LandDetector.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 6bbafa2524f6..1680589da7c2 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -78,7 +78,7 @@ void LandDetector::Run() { perf_begin(_cycle_perf); - _check_params(); + _check_params(false); _actuator_armed_sub.update(&_arming); _update_topics(); _update_state(); @@ -148,11 +148,11 @@ void LandDetector::Run() } } -void LandDetector::_check_params() +void LandDetector::_check_params(const bool force) { parameter_update_s param_update; - if (_param_update_sub.update(¶m_update)) { + if (_param_update_sub.update(¶m_update) || force) { _update_params(); _total_flight_time = static_cast(_param_total_flight_time_high.get()) << 32; _total_flight_time |= _param_total_flight_time_low.get(); diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index d93db958e5dd..5d9c648a8c3a 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -157,7 +157,7 @@ class LandDetector : public ModuleBase, ModuleParams, px4::Schedul private: - void _check_params(); + void _check_params(bool force = false); void Run() override; From a0d3f512025e23656b19f278fe2d43837fecaae7 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Tue, 25 Jun 2019 15:41:53 -0600 Subject: [PATCH 4/4] Add check_params(true) call to the LandDetector start() method. Break out _update_total_flight_time() method in the LandDetector class. --- src/modules/land_detector/LandDetector.cpp | 10 ++++++++-- src/modules/land_detector/LandDetector.h | 2 ++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 1680589da7c2..6e2d107e92ee 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -71,6 +71,7 @@ LandDetector::~LandDetector() void LandDetector::start() { + _check_params(true); ScheduleOnInterval(LAND_DETECTOR_UPDATE_INTERVAL); } @@ -154,8 +155,7 @@ void LandDetector::_check_params(const bool force) if (_param_update_sub.update(¶m_update) || force) { _update_params(); - _total_flight_time = static_cast(_param_total_flight_time_high.get()) << 32; - _total_flight_time |= _param_total_flight_time_low.get(); + _update_total_flight_time(); } } @@ -187,4 +187,10 @@ void LandDetector::_update_state() } } +void LandDetector::_update_total_flight_time() +{ + _total_flight_time = static_cast(_param_total_flight_time_high.get()) << 32; + _total_flight_time |= _param_total_flight_time_low.get(); +} + } // namespace land_detector diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 5d9c648a8c3a..9278307c7509 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -163,6 +163,8 @@ class LandDetector : public ModuleBase, ModuleParams, px4::Schedul void _update_state(); + void _update_total_flight_time(); + bool _previous_armed_state{false}; ///< stores the previous actuator_armed.armed state uint64_t _total_flight_time{0}; ///< in microseconds