diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index d6a750f89a3f..6e2d107e92ee 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() @@ -74,6 +71,7 @@ LandDetector::~LandDetector() void LandDetector::start() { + _check_params(true); ScheduleOnInterval(LAND_DETECTOR_UPDATE_INTERVAL); } @@ -82,7 +80,7 @@ void LandDetector::Run() perf_begin(_cycle_perf); _check_params(false); - _armingSub.update(&_arming); + _actuator_armed_sub.update(&_arming); _update_topics(); _update_state(); @@ -98,7 +96,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 +118,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 +148,14 @@ void LandDetector::Run() exit_and_cleanup(); } } + void LandDetector::_check_params(const bool force) { - parameter_update_s paramUpdate; + parameter_update_s param_update; - if (_parameterSub.update(¶mUpdate) || force) { + if (_param_update_sub.update(¶m_update) || force) { _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; + _update_total_flight_time(); } } @@ -186,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 dd0a30516a40..9278307c7509 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 Run() override; + void _update_state(); - param_t _p_total_flight_time_high{PARAM_INVALID}; - param_t _p_total_flight_time_low{PARAM_INVALID}; + 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 + 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