From adb10a18f2f201f2ef5dfa632f0be9b3ad2d0c34 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Sun, 1 Sep 2019 22:52:01 -0600 Subject: [PATCH] Cherry pick the directory and voted_sensors_update.h from PR #9756. Consolidate _update_params() methods for improved inheritance from the LandDetector base class. Move common uORB::Subscriptions to the base class for inheritance. Deprecate redundant override methods. --- .../land_detector/FixedwingLandDetector.cpp | 33 ++++---------- .../land_detector/FixedwingLandDetector.h | 14 ++---- src/modules/land_detector/LandDetector.cpp | 28 +++++------- src/modules/land_detector/LandDetector.h | 44 ++++++++++++------- .../land_detector/MulticopterLandDetector.cpp | 8 ++-- .../land_detector/MulticopterLandDetector.h | 10 ++--- .../land_detector/RoverLandDetector.cpp | 13 ------ src/modules/land_detector/RoverLandDetector.h | 15 ++----- 8 files changed, 63 insertions(+), 102 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index f190b5f335d0..aac241829d84 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -39,8 +39,6 @@ * @author Julian Oes */ -#include - #include "FixedwingLandDetector.h" namespace land_detector @@ -55,35 +53,20 @@ FixedwingLandDetector::FixedwingLandDetector() void FixedwingLandDetector::_update_topics() { + LandDetector::_update_topics(); _airspeed_sub.update(&_airspeed); - _vehicle_acceleration_sub.update(&_vehicle_acceleration); - _vehicle_local_position_sub.update(&_vehicle_local_position); -} - -void FixedwingLandDetector::_update_params() -{ - // check for parameter updates - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - // update parameters from storage - _update_params(); - } } float FixedwingLandDetector::_get_max_altitude() { - // TODO - // This means no altitude limit as the limit - // is always current position plus 10000 meters + // TODO: This means no altitude limit as the limit + // is always current position plus 10000 meters. return roundf(-_vehicle_local_position.z + 10000); } bool FixedwingLandDetector::_get_landed_state() { - // only trigger flight conditions if we are armed + // Only trigger flight conditions if we are armed. if (!_actuator_armed.armed) { return true; } @@ -116,11 +99,11 @@ bool FixedwingLandDetector::_get_landed_state() _xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f; - // crude land detector for fixedwing - landDetected = _xy_accel_filtered < _param_lndfw_xyaccel_max.get() - && _airspeed_filtered < _param_lndfw_airspd.get() + // Crude land detector for fixedwing. + landDetected = _airspeed_filtered < _param_lndfw_airspd.get() && _velocity_xy_filtered < _param_lndfw_vel_xy_max.get() - && _velocity_z_filtered < _param_lndfw_vel_z_max.get(); + && _velocity_z_filtered < _param_lndfw_vel_z_max.get() + && _xy_accel_filtered < _param_lndfw_xyaccel_max.get(); } else { // Control state topic has timed out and we need to assume we're landed. diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 843766e7958c..4cadc977372f 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -42,10 +42,9 @@ #pragma once +#include #include #include -#include -#include #include "LandDetector.h" @@ -60,12 +59,13 @@ class FixedwingLandDetector final : public LandDetector FixedwingLandDetector(); protected: - void _update_params() override; - void _update_topics() override; bool _get_landed_state() override; + float _get_max_altitude() override; + void _update_topics() override; + private: /** Time in us that landing conditions have to hold before triggering a land. */ @@ -73,13 +73,8 @@ class FixedwingLandDetector final : public LandDetector static constexpr hrt_abstime FLYING_TRIGGER_TIME_US = 0_us; uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; - uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; airspeed_s _airspeed{}; - vehicle_acceleration_s _vehicle_acceleration{}; - vehicle_local_position_s _vehicle_local_position{}; float _airspeed_filtered{0.0f}; float _velocity_xy_filtered{0.0f}; @@ -93,7 +88,6 @@ class FixedwingLandDetector final : public LandDetector (ParamFloat) _param_lndfw_vel_xy_max, (ParamFloat) _param_lndfw_vel_z_max ); - }; } // namespace land_detector diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 0598a28a4539..809cf1e76433 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -40,14 +40,6 @@ #include "LandDetector.h" -#include -#include - -#include -#include -#include -#include - using namespace time_literals; namespace land_detector @@ -71,7 +63,7 @@ LandDetector::~LandDetector() void LandDetector::start() { - _check_params(true); + _update_params(true); ScheduleOnInterval(LAND_DETECTOR_UPDATE_INTERVAL); } @@ -79,7 +71,7 @@ void LandDetector::Run() { perf_begin(_cycle_perf); - _check_params(false); + _update_params(); _actuator_armed_sub.update(&_actuator_armed); _update_topics(); _update_state(); @@ -149,16 +141,13 @@ void LandDetector::Run() } } -void LandDetector::_check_params(const bool force) +void LandDetector::_update_params(const bool force) { // check for parameter updates if (_parameter_update_sub.updated() || force) { // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - // update parameters from storage - _update_params(); + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); _update_total_flight_time(); } @@ -192,6 +181,13 @@ void LandDetector::_update_state() } } +void LandDetector::_update_topics() +{ + _actuator_armed_sub.update(&_actuator_armed); + _vehicle_acceleration_sub.update(&_vehicle_acceleration); + _vehicle_local_position_sub.update(&_vehicle_local_position); +} + void LandDetector::_update_total_flight_time() { _total_flight_time = static_cast(_param_total_flight_time_high.get()) << 32; diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index a971e3328d45..cfcdf0cca2db 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -42,16 +42,25 @@ #pragma once +#include #include -#include +#include #include +#include +#include +#include +#include #include #include #include #include #include #include +#include +#include #include +#include + using namespace time_literals; @@ -100,14 +109,15 @@ class LandDetector : public ModuleBase, ModuleParams, px4::Schedul protected: /** - * Update parameters. + * Updates parameters if changes have occurred or if forced. + * @var force Forces a parameter update. */ - virtual void _update_params() = 0; + virtual void _update_params(const bool force = false); /** - * Update uORB topics. + * Updates subscribed uORB topics. */ - virtual void _update_topics() = 0; + virtual void _update_topics(); /** * @return true if UAV is in a landed state. @@ -132,7 +142,7 @@ class LandDetector : public ModuleBase, ModuleParams, px4::Schedul /** * @return maximum altitude that can be reached */ - virtual float _get_max_altitude() = 0; + virtual float _get_max_altitude() { return INFINITY; } /** * @return true if vehicle could be in ground effect (close to ground) @@ -142,9 +152,6 @@ class LandDetector : public ModuleBase, ModuleParams, px4::Schedul /** Run main land detector loop at this interval. */ static constexpr uint32_t LAND_DETECTOR_UPDATE_INTERVAL = 20_ms; - orb_advert_t _land_detected_pub{nullptr}; - vehicle_land_detected_s _land_detected{}; - LandDetectionState _state{LandDetectionState::LANDED}; systemlib::Hysteresis _freefall_hysteresis{false}; @@ -153,11 +160,18 @@ class LandDetector : public ModuleBase, ModuleParams, px4::Schedul systemlib::Hysteresis _ground_contact_hysteresis{true}; systemlib::Hysteresis _ground_effect_hysteresis{false}; - actuator_armed_s _actuator_armed {}; + actuator_armed_s _actuator_armed{}; + vehicle_acceleration_s _vehicle_acceleration{}; + vehicle_land_detected_s _land_detected{}; + vehicle_local_position_s _vehicle_local_position{}; -private: + orb_advert_t _land_detected_pub{nullptr}; + + uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - void _check_params(bool force = false); +private: void Run() override; @@ -165,15 +179,13 @@ class LandDetector : public ModuleBase, ModuleParams, px4::Schedul 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 + bool _previous_armed_state{false}; ///< stores the previous actuator_armed.armed state hrt_abstime _takeoff_time{0}; + hrt_abstime _total_flight_time{0}; ///< total vehicle flight time in microseconds perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, "land_detector_cycle")}; - uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; DEFINE_PARAMETERS_CUSTOM_PARENT( diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 11a1d7676b52..72c3dffc7cb1 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -86,17 +86,19 @@ MulticopterLandDetector::MulticopterLandDetector() void MulticopterLandDetector::_update_topics() { + LandDetector::_update_topics(); + _actuator_controls_sub.update(&_actuator_controls); _battery_sub.update(&_battery_status); - _vehicle_acceleration_sub.update(&_vehicle_acceleration); _vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity); _vehicle_control_mode_sub.update(&_vehicle_control_mode); - _vehicle_local_position_sub.update(&_vehicle_local_position); _vehicle_local_position_setpoint_sub.update(&_vehicle_local_position_setpoint); } -void MulticopterLandDetector::_update_params() +void MulticopterLandDetector::_update_params(const bool force) { + LandDetector::_update_params(force); + _freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _param_lndmc_ffall_ttri.get())); param_get(_paramHandle.minThrottle, &_params.minThrottle); diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 8d7f8a095bb0..0c63c0ef4716 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -67,7 +67,7 @@ class MulticopterLandDetector : public LandDetector MulticopterLandDetector(); protected: - void _update_params() override; + void _update_params(const bool force = false) override; void _update_topics() override; bool _get_landed_state() override; @@ -79,7 +79,7 @@ class MulticopterLandDetector : public LandDetector float _get_max_altitude() override; private: - /* get control mode dependent pilot throttle threshold with which we should quit landed state and take off */ + /** Get control mode dependent pilot throttle threshold with which we should quit landed state and take off. */ float _get_takeoff_throttle(); bool _has_low_thrust(); @@ -100,9 +100,7 @@ class MulticopterLandDetector : public LandDetector /** Time interval in us in which wider acceptance thresholds are used after landed. */ static constexpr hrt_abstime LAND_DETECTOR_LAND_PHASE_TIME_US = 2_s; - /** - * @brief Handles for interesting parameters - **/ + /** Handles for interesting parameters. **/ struct { param_t minThrottle; param_t hoverThrottle; @@ -127,10 +125,8 @@ class MulticopterLandDetector : public LandDetector actuator_controls_s _actuator_controls {}; battery_status_s _battery_status {}; - vehicle_acceleration_s _vehicle_acceleration{}; vehicle_angular_velocity_s _vehicle_angular_velocity{}; vehicle_control_mode_s _vehicle_control_mode {}; - vehicle_local_position_s _vehicle_local_position {}; vehicle_local_position_setpoint_s _vehicle_local_position_setpoint {}; hrt_abstime _min_trust_start{0}; ///< timestamp when minimum trust was applied first diff --git a/src/modules/land_detector/RoverLandDetector.cpp b/src/modules/land_detector/RoverLandDetector.cpp index 063f8712b305..5a69f537aa34 100644 --- a/src/modules/land_detector/RoverLandDetector.cpp +++ b/src/modules/land_detector/RoverLandDetector.cpp @@ -44,14 +44,6 @@ namespace land_detector { -void RoverLandDetector::_update_topics() -{ -} - -void RoverLandDetector::_update_params() -{ -} - bool RoverLandDetector::_get_ground_contact_state() { return true; @@ -66,9 +58,4 @@ bool RoverLandDetector::_get_landed_state() return false; } -float RoverLandDetector::_get_max_altitude() -{ - return 0.0f; -} - } // namespace land_detector diff --git a/src/modules/land_detector/RoverLandDetector.h b/src/modules/land_detector/RoverLandDetector.h index f04175938266..bc1fb0a8947c 100644 --- a/src/modules/land_detector/RoverLandDetector.h +++ b/src/modules/land_detector/RoverLandDetector.h @@ -41,8 +41,6 @@ #pragma once -#include - #include "LandDetector.h" namespace land_detector @@ -54,18 +52,11 @@ class RoverLandDetector : public LandDetector RoverLandDetector() = default; protected: - virtual void _update_params() override; - - virtual void _update_topics() override; - - virtual bool _get_landed_state() override; - - virtual bool _get_ground_contact_state() override; - - virtual float _get_max_altitude() override; + bool _get_ground_contact_state() override; + bool _get_landed_state() override; private: -}; +}; } // namespace land_detector