From 50cac88e5b7c55d01679f2fabf22aa5cb5378838 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 24 Jul 2016 11:18:39 +0200 Subject: [PATCH] land_detector: big refactor, share update function This is a big refactor and general cleanup of the land detector. The main functional change is to share the hysteresis lib across all land detectors. --- .../land_detector/FixedwingLandDetector.cpp | 83 +++------- .../land_detector/FixedwingLandDetector.h | 73 +++------ src/modules/land_detector/LandDetector.cpp | 119 ++++++++++---- src/modules/land_detector/LandDetector.h | 152 +++++++++++------- .../land_detector/MulticopterLandDetector.cpp | 99 ++++-------- .../land_detector/MulticopterLandDetector.h | 57 ++----- .../land_detector/VtolLandDetector.cpp | 51 +++--- src/modules/land_detector/VtolLandDetector.h | 51 +++--- .../land_detector/land_detector_main.cpp | 34 +++- 9 files changed, 337 insertions(+), 382 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index af3cfaa61f..0721e68e8e 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -31,22 +31,23 @@ * ****************************************************************************/ -/** +/* * @file FixedwingLandDetector.cpp - * Land detection algorithm for fixedwings * * @author Johan Jansen * @author Lorenz Meier + * @author Julian Oes */ -#include "FixedwingLandDetector.h" - #include #include #include #include -namespace landdetection +#include "FixedwingLandDetector.h" + + +namespace land_detector { FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), @@ -58,12 +59,10 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _controlState{}, _arming{}, _airspeed{}, - _parameterSub(-1), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), _airspeed_filtered(0.0f), - _accel_horz_lp(0.0f), - _landDetectTrigger(0) + _accel_horz_lp(0.0f) { _paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); _paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX"); @@ -71,54 +70,41 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _paramHandle.maxIntVelocity = param_find("LNDFW_VELI_MAX"); } -void FixedwingLandDetector::initialize() +void FixedwingLandDetector::_initialize_topics() { _controlStateSub = orb_subscribe(ORB_ID(control_state)); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); _airspeedSub = orb_subscribe(ORB_ID(airspeed)); - - updateParameterCache(true); } -void FixedwingLandDetector::updateSubscriptions() +void FixedwingLandDetector::_update_topics() { - orb_update(ORB_ID(control_state), _controlStateSub, &_controlState); - orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); - orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + _orb_update(ORB_ID(control_state), _controlStateSub, &_controlState); + _orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); + _orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); } -LandDetectionResult FixedwingLandDetector::update() +void FixedwingLandDetector::_update_params() { - // First poll for new data from our subscriptions - updateSubscriptions(); - - if (get_freefall_state()) { - _state = LANDDETECTION_RES_FREEFALL; - - } else if (get_landed_state()) { - _state = LANDDETECTION_RES_LANDED; - - } else { - _state = LANDDETECTION_RES_FLYING; - } - - return _state; + param_get(_paramHandle.maxVelocity, &_params.maxVelocity); + param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); + param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); + param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity); } -bool FixedwingLandDetector::get_freefall_state() +bool FixedwingLandDetector::_get_freefall_state() { - //TODO + // TODO return false; } -bool FixedwingLandDetector::get_landed_state() +bool FixedwingLandDetector::_get_landed_state() { // only trigger flight conditions if we are armed if (!_arming.armed) { return true; } - const uint64_t now = hrt_absolute_time(); bool landDetected = false; if (hrt_elapsed_time(&_controlState.timestamp) < 500 * 1000) { @@ -147,14 +133,10 @@ bool FixedwingLandDetector::get_landed_state() && _airspeed_filtered < _params.maxAirSpeed && _accel_horz_lp < _params.maxIntVelocity) { - // these conditions need to be stable for a period of time before we trust them - if (now > _landDetectTrigger) { - landDetected = true; - } + landDetected = true; } else { - // reset land detect trigger - _landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME; + landDetected = false; } } else { @@ -165,23 +147,4 @@ bool FixedwingLandDetector::get_landed_state() return landDetected; } -void FixedwingLandDetector::updateParameterCache(const bool force) -{ - bool updated; - parameter_update_s paramUpdate; - - orb_check(_parameterSub, &updated); - - if (updated) { - orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); - } - - if (updated || force) { - param_get(_paramHandle.maxVelocity, &_params.maxVelocity); - param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); - param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); - param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity); - } -} - -} +} // namespace land_detector diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index f450f94b82..e52ce1d95e 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,23 +33,22 @@ /** * @file FixedwingLandDetector.h - * Land detection algorithm for fixedwing + * Land detector implementation for fixedwing. * * @author Johan Jansen + * @author Morten Lysgaard + * @author Julian Oes */ -#ifndef __FIXED_WING_LAND_DETECTOR_H__ -#define __FIXED_WING_LAND_DETECTOR_H__ +#pragma once -#include "LandDetector.h" #include #include -#include -#include #include -#include -namespace landdetection +#include "LandDetector.h" + +namespace land_detector { class FixedwingLandDetector : public LandDetector @@ -58,46 +57,22 @@ public: FixedwingLandDetector(); protected: - /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz - **/ - LandDetectionResult update() override; + virtual void _initialize_topics() override; - /** - * @brief Initializes the land detection algorithm - **/ - void initialize() override; + virtual void _update_params() override; - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - void updateSubscriptions(); + virtual void _update_topics() override; - /** - * @brief get UAV landed state - **/ - bool get_landed_state(); - - /** - * @brief returns true if UAV is in free-fall state - **/ - bool get_freefall_state(); + virtual bool _get_landed_state() override; + virtual bool _get_freefall_state() override; private: - /** - * @brief download and update local parameter cache - **/ - void updateParameterCache(const bool force); - - /** - * @brief Handles for interesting parameters - **/ struct { param_t maxVelocity; param_t maxClimbRate; param_t maxAirSpeed; param_t maxIntVelocity; - } _paramHandle; + } _paramHandle; struct { float maxVelocity; @@ -106,22 +81,18 @@ private: float maxIntVelocity; } _params; -private: - int _controlStateSub; /**< notification of local position */ - int _armingSub; - int _airspeedSub; - struct control_state_s _controlState; /**< the result from local position subscription */ - struct actuator_armed_s _arming; - struct airspeed_s _airspeed; - int _parameterSub; + int _controlStateSub; + int _armingSub; + int _airspeedSub; + + struct control_state_s _controlState; + struct actuator_armed_s _arming; + struct airspeed_s _airspeed; float _velocity_xy_filtered; float _velocity_z_filtered; float _airspeed_filtered; float _accel_horz_lp; - uint64_t _landDetectTrigger; }; -} - -#endif //__FIXED_WING_LAND_DETECTOR_H__ +} // namespace land_detector diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index a80b1dad48..abceeddfe4 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -31,32 +31,35 @@ * ****************************************************************************/ -/** +/* * @file LandDetector.cpp - * Land detection algorithm * * @author Johan Jansen - * @author Morten Lysgaard + * @author Julian Oes */ -#include "LandDetector.h" -#include //usleep -#include #include #include +#include -namespace landdetection +#include "LandDetector.h" + + +namespace land_detector { + LandDetector::LandDetector() : _landDetectedPub(nullptr), _landDetected{0, false, false}, - _arming_time(0), + _freefall_hysteresis(false), + _landed_hysteresis(true), _taskShouldExit(false), _taskIsRunning(false), _work{} { - // ctor + // Use Trigger time when transitioning from in-air (false) to landed (true). + _landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US); } LandDetector::~LandDetector() @@ -67,60 +70,115 @@ LandDetector::~LandDetector() int LandDetector::start() { + _taskShouldExit = false; + /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0); + work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this, 0); return 0; } -void LandDetector::shutdown() +void LandDetector::stop() { _taskShouldExit = true; } void -LandDetector::cycle_trampoline(void *arg) +LandDetector::_cycle_trampoline(void *arg) { LandDetector *dev = reinterpret_cast(arg); - dev->cycle(); + dev->_cycle(); } -void LandDetector::cycle() +void LandDetector::_cycle() { if (!_taskIsRunning) { - // advertise the first land detected uORB + PX4_INFO("we are doing the init"); + // Advertise the first land detected uORB. _landDetected.timestamp = hrt_absolute_time(); _landDetected.landed = false; _landDetected.freefall = false; - // initialize land detection algorithm - initialize(); + // Initialize uORB topics. + _initialize_topics(); - // task is now running, keep doing so until shutdown() has been called + _check_params(true); + + // Task is now running, keep doing so until we need to stop. _taskIsRunning = true; - _taskShouldExit = false; } - LandDetectionResult current_state = update(); - bool landDetected = (current_state == LANDDETECTION_RES_LANDED); - bool freefallDetected = (current_state == LANDDETECTION_RES_FREEFALL); + _check_params(false); + + _update_topics(); + + _update_state(); + + bool landDetected = (_state == LandDetectionState::LANDED); + bool freefallDetected = (_state == LandDetectionState::FREEFALL); + + // Only publish very first time or when the result has changed. + if ((_landDetectedPub == nullptr) || + (_landDetected.landed != landDetected) || + (_landDetected.freefall != freefallDetected)) { - if (_landDetectedPub == nullptr || _landDetected.landed != landDetected || _landDetected.freefall != freefallDetected) { _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = (current_state == LANDDETECTION_RES_LANDED); - _landDetected.freefall = (current_state == LANDDETECTION_RES_FREEFALL); + _landDetected.landed = (_state == LandDetectionState::LANDED); + _landDetected.freefall = (_state == LandDetectionState::FREEFALL); + int instance; - orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected, &instance, ORB_PRIO_DEFAULT); + orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected, + &instance, ORB_PRIO_DEFAULT); } if (!_taskShouldExit) { - work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, - USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE)); + + // Schedule next cycle. + work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this, + USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE_HZ)); + + } else { + _taskIsRunning = false; } } -bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer) +void LandDetector::_check_params(const bool force) +{ + bool updated; + parameter_update_s paramUpdate; + + orb_check(_parameterSub, &updated); + + if (updated) { + orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); + } + + if (updated || force) { + _update_params(); + } +} + +void LandDetector::_update_state() +{ + bool landed = _get_landed_state(); + _landed_hysteresis.set_state_and_update(landed); + _freefall_hysteresis.set_state_and_update(_get_freefall_state()); + + if (_freefall_hysteresis.get_state()) { + _state = LandDetectionState::FREEFALL; + + } else if (_landed_hysteresis.get_state()) { + _state = LandDetectionState::LANDED; + + } else { + _state = LandDetectionState::FLYING; + } + + return; +} + +bool LandDetector::_orb_update(const struct orb_metadata *meta, int handle, void *buffer) { bool newData = false; @@ -140,4 +198,5 @@ bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void return true; } -} + +} // namespace land_detector diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 05b229b7a5..1b71e8d475 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,101 +31,131 @@ * ****************************************************************************/ -/** +/* * @file LandDetector.h - * Abstract interface for land detector algorithms +Land detector interface for multicopter, fixedwing and VTOL implementations. * * @author Johan Jansen + * @author Julian Oes */ -#ifndef __LAND_DETECTOR_H__ -#define __LAND_DETECTOR_H__ +#pragma once #include +#include #include #include -namespace landdetection +namespace land_detector { -enum LandDetectionResult { - LANDDETECTION_RES_FLYING = 0, /**< UAV is flying */ - LANDDETECTION_RES_LANDED = 1, /**< Land has been detected */ - LANDDETECTION_RES_FREEFALL = 2 /**< Free-fall has been detected */ -}; class LandDetector { public: + enum class LandDetectionState { + FLYING = 0, + LANDED = 1, + FREEFALL = 2 + }; LandDetector(); virtual ~LandDetector(); - /** - * @return true if this task is currently running - **/ - inline bool isRunning() const {return _taskIsRunning;} - - /** - * @return the current landed state + /* + * @return true if this task is currently running. */ - bool isLanded() { return _landDetected.landed; } + inline bool is_running() const + { + return _taskIsRunning; + } - /** - * @brief Tells the Land Detector task that it should exit - **/ - void shutdown(); - /** - * @brief Blocking function that should be called from it's own task thread. This method will - * run the underlying algorithm at the desired update rate and publish if the landing state changes. - **/ + /* + * @return current state. + */ + LandDetectionState get_state() const + { + return _state; + } + + /* + * Tells the task that it should exit. + */ + void stop(); + + /* + * Get the work queue going. + */ int start(); - static void cycle_trampoline(void *arg); - protected: + /* + * Called once to initialize uORB topics. + */ + virtual void _initialize_topics() = 0; - /** - * @brief Pure abstract method that must be overriden by sub-classes. This actually runs the underlying algorithm - * @return true if a landing was detected and this should be broadcast to the rest of the system - **/ - virtual LandDetectionResult update() = 0; + /* + * Update uORB topics. + */ + virtual void _update_topics() = 0; - /** - * @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation, - * uORB topic subscription, creating file descriptors, etc.) - **/ - virtual void initialize() = 0; - /** - * @brief Convenience function for polling uORB subscriptions - * @return true if there was new data and it was successfully copied - **/ - bool orb_update(const struct orb_metadata *meta, int handle, void *buffer); + /* + * Update parameters. + */ + virtual void _update_params() = 0; - static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + /* + * @return true if UAV is in a landed state. + */ + virtual bool _get_landed_state() = 0; - static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold - before triggering a land */ - static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = - 2000000; /**< time interval in which wider acceptance thresholds are used after arming */ + /* + * @return true if UAV is in free-fall state. + */ + virtual bool _get_freefall_state() = 0; -protected: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ - uint64_t _arming_time; /**< timestamp of arming time */ - LandDetectionResult - _state; /**< Result of land detection. Can be LANDDETECTION_RES_FLYING, LANDDETECTION_RES_LANDED or LANDDETECTION_RES_FREEFALL */ + /* + * Convenience function for polling uORB subscriptions. + * + * @return true if there was new data and it was successfully copied + */ + static bool _orb_update(const struct orb_metadata *meta, int handle, void *buffer); + + /* Run main land detector loop at this rate in Hz. */ + static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE_HZ = 50; + + /* Time in us that landing conditions have to hold before triggering a land. */ + static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 2000000; + + /* Time interval in us in which wider acceptance thresholds are used after arming. */ + static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME_US = 2000000; + + orb_advert_t _landDetectedPub; + struct vehicle_land_detected_s _landDetected; + + int _parameterSub; + + LandDetectionState _state; + + systemlib::Hysteresis _freefall_hysteresis; + systemlib::Hysteresis _landed_hysteresis; private: - bool _taskShouldExit; /**< true if it is requested that this task should exit */ - bool _taskIsRunning; /**< task has reached main loop and is currently running */ - struct work_s _work; + static void _cycle_trampoline(void *arg); - void cycle(); + void _cycle(); + + void _check_params(const bool force); + + void _update_state(); + + bool _taskShouldExit; + bool _taskIsRunning; + + struct work_s _work; }; -} -#endif //__LAND_DETECTOR_H__ +} // namespace land_detector diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index f049c85132..7bd347b422 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -33,19 +33,20 @@ /** * @file MulticopterLandDetector.cpp - * Land detection algorithm for multicopters * * @author Johan Jansen * @author Morten Lysgaard + * @author Julian Oes */ -#include "MulticopterLandDetector.h" - #include #include #include -namespace landdetection +#include "MulticopterLandDetector.h" + + +namespace land_detector { MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), @@ -54,7 +55,6 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _vehicleLocalPositionSub(-1), _actuatorsSub(-1), _armingSub(-1), - _parameterSub(-1), _attitudeSub(-1), _manualSub(-1), _ctrl_state_sub(-1), @@ -67,12 +67,8 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _ctrl_state{}, _ctrl_mode{}, _min_trust_start(0), - _freefall_hysteresis(false), - _landed_hysteresis(true) + _arming_time(0) { - // Use Trigger time when transitioning from in-air (false) to landed (true). - _landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME); - _paramHandle.maxRotation = param_find("LNDMC_ROT_MAX"); _paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX"); _paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX"); @@ -82,7 +78,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _paramHandle.ff_trigger_time = param_find("LNDMC_FFALL_TTRI"); } -void MulticopterLandDetector::initialize() +void MulticopterLandDetector::_initialize_topics() { // subscribe to position, attitude, arming and velocity changes _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); @@ -93,46 +89,34 @@ void MulticopterLandDetector::initialize() _manualSub = orb_subscribe(ORB_ID(manual_control_setpoint)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - - // download parameters - updateParameterCache(true); } -void MulticopterLandDetector::updateSubscriptions() +void MulticopterLandDetector::_update_topics() { - orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); - orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude); - orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators); - orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); - orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual); - orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); - orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_ctrl_mode); + _orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); + _orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude); + _orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators); + _orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); + _orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual); + _orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); + _orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_ctrl_mode); } -LandDetectionResult MulticopterLandDetector::update() +void MulticopterLandDetector::_update_params() { - // first poll for new data from our subscriptions - updateSubscriptions(); - - updateParameterCache(false); - - _landed_hysteresis.set_state_and_update(get_landed_state()); - _freefall_hysteresis.set_state_and_update(get_freefall_state()); - - if (_freefall_hysteresis.get_state()) { - _state = LANDDETECTION_RES_FREEFALL; - - } else if (_landed_hysteresis.get_state()) { - _state = LANDDETECTION_RES_LANDED; - - } else { - _state = LANDDETECTION_RES_FLYING; - } - - return _state; + param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); + param_get(_paramHandle.maxVelocity, &_params.maxVelocity); + param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s); + _params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s); + param_get(_paramHandle.maxThrottle, &_params.maxThrottle); + param_get(_paramHandle.minManThrottle, &_params.minManThrottle); + param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2); + param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time); + _freefall_hysteresis.set_hysteresis_time_from(false, 1e6f * _params.ff_trigger_time); } -bool MulticopterLandDetector::get_freefall_state() + +bool MulticopterLandDetector::_get_freefall_state() { if (_params.acc_threshold_m_s2 < 0.1f || _params.acc_threshold_m_s2 > 10.0f) { //if parameter is set to zero or invalid, disable free-fall detection. @@ -152,7 +136,7 @@ bool MulticopterLandDetector::get_freefall_state() return (acc_norm < _params.acc_threshold_m_s2); //true if we are currently falling } -bool MulticopterLandDetector::get_landed_state() +bool MulticopterLandDetector::_get_landed_state() { // Time base for this function const uint64_t now = hrt_absolute_time(); @@ -177,6 +161,7 @@ bool MulticopterLandDetector::get_landed_state() // only trigger flight conditions if we are armed if (!_arming.armed) { _arming_time = 0; + return true; } else if (_arming_time == 0) { @@ -203,6 +188,7 @@ bool MulticopterLandDetector::get_landed_state() // quite acrobatic flight. if ((_min_trust_start > 0) && (hrt_elapsed_time(&_min_trust_start) > 8 * 1000 * 1000)) { + return true; } else { @@ -214,7 +200,7 @@ bool MulticopterLandDetector::get_landed_state() // Widen acceptance thresholds for landed state right after arming // so that motor spool-up and other effects do not trigger false negatives. - if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME) { + if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME_US) { armThresholdFactor = 2.5f; } @@ -243,28 +229,5 @@ bool MulticopterLandDetector::get_landed_state() return true; } -void MulticopterLandDetector::updateParameterCache(const bool force) -{ - bool updated; - parameter_update_s paramUpdate; - - orb_check(_parameterSub, &updated); - - if (updated) { - orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); - } - - if (updated || force) { - param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); - param_get(_paramHandle.maxVelocity, &_params.maxVelocity); - param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s); - _params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s); - param_get(_paramHandle.maxThrottle, &_params.maxThrottle); - param_get(_paramHandle.minManThrottle, &_params.minManThrottle); - param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2); - param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time); - _freefall_hysteresis.set_hysteresis_time_from(false, 1e6f * _params.ff_trigger_time); - } -} } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index d9e7b11879..dbfaf9aa75 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -33,29 +33,28 @@ /** * @file MulticopterLandDetector.h - * Land detection algorithm for multicopters + * Land detection implementation for multicopters. * * @author Johan Jansen * @author Morten Lysgaard + * @author Julian Oes */ -#ifndef __MULTICOPTER_LAND_DETECTOR_H__ -#define __MULTICOPTER_LAND_DETECTOR_H__ +#pragma once -#include "LandDetector.h" +#include #include #include -#include #include #include #include #include #include #include -#include -#include -namespace landdetection +#include "LandDetector.h" + +namespace land_detector { class MulticopterLandDetector : public LandDetector @@ -64,36 +63,15 @@ public: MulticopterLandDetector(); protected: - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - virtual void updateSubscriptions(); + virtual void _initialize_topics() override; - /** - * @brief Runs one iteration of the land detection algorithm - **/ - virtual LandDetectionResult update() override; + virtual void _update_params() override; - /** - * @brief Initializes the land detection algorithm - **/ - virtual void initialize() override; + virtual void _update_topics() override; - /** - * @brief download and update local parameter cache - **/ - virtual void updateParameterCache(const bool force); - - /** - * @brief get multicopter landed state - **/ - bool get_landed_state(); - - /** - * @brief returns true if multicopter is in free-fall state - **/ - bool get_freefall_state(); + virtual bool _get_landed_state() override; + virtual bool _get_freefall_state() override; private: /** @@ -107,7 +85,7 @@ private: param_t minManThrottle; param_t acc_threshold_m_s2; param_t ff_trigger_time; - } _paramHandle; + } _paramHandle; struct { float maxClimbRate; @@ -119,11 +97,9 @@ private: float ff_trigger_time; } _params; -private: int _vehicleLocalPositionSub; int _actuatorsSub; int _armingSub; - int _parameterSub; int _attitudeSub; int _manualSub; int _ctrl_state_sub; @@ -138,11 +114,8 @@ private: struct vehicle_control_mode_s _ctrl_mode; uint64_t _min_trust_start; ///< timestamp when minimum trust was applied first - - systemlib::Hysteresis _freefall_hysteresis; - systemlib::Hysteresis _landed_hysteresis; + uint64_t _arming_time; }; -} -#endif //__MULTICOPTER_LAND_DETECTOR_H__ +} // namespace land_detector diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp index 50a39c5fea..1acd99bd83 100644 --- a/src/modules/land_detector/VtolLandDetector.cpp +++ b/src/modules/land_detector/VtolLandDetector.cpp @@ -36,12 +36,14 @@ * Land detection algorithm for VTOL * * @author Roman Bapst + * @author Julian Oes */ -#include "VtolLandDetector.h" #include -namespace landdetection +#include "VtolLandDetector.h" + +namespace land_detector { VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(), @@ -56,29 +58,24 @@ VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(), _paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX"); } -void VtolLandDetector::initialize() +void VtolLandDetector::_initialize_topics() { - MulticopterLandDetector::initialize(); + MulticopterLandDetector::_initialize_topics(); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); - - // download parameters - updateParameterCache(true); } -void VtolLandDetector::updateSubscriptions() +void VtolLandDetector::_update_topics() { - MulticopterLandDetector::updateSubscriptions(); + MulticopterLandDetector::_update_topics(); - orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + _orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); } -LandDetectionResult VtolLandDetector::update() +bool VtolLandDetector::_get_landed_state() { - updateSubscriptions(); - updateParameterCache(false); - // this is returned from the mutlicopter land detector - bool landed = get_landed_state(); + bool landed = MulticopterLandDetector::_get_landed_state(); // for vtol we additionally consider airspeed if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) { @@ -97,27 +94,19 @@ LandDetectionResult VtolLandDetector::update() _was_in_air = !landed; - _state = (landed) ? LANDDETECTION_RES_LANDED : LANDDETECTION_RES_FLYING; - - return _state; + return landed; } -void VtolLandDetector::updateParameterCache(const bool force) +bool VtolLandDetector::_get_freefall_state() { - MulticopterLandDetector::updateParameterCache(force); + return MulticopterLandDetector::_get_freefall_state(); +} - bool updated; - parameter_update_s paramUpdate; +void VtolLandDetector::_update_params() +{ + MulticopterLandDetector::_update_params(); - orb_check(_parameterSub, &updated); - - if (updated) { - orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); - } - - if (updated || force) { - param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); - } + param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); } } diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h index a9b409f610..54ca2735fd 100644 --- a/src/modules/land_detector/VtolLandDetector.h +++ b/src/modules/land_detector/VtolLandDetector.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,18 +33,19 @@ /** * @file VtolLandDetector.h - * Land detection algorithm for vtol + * Land detection implementation for VTOL also called hybrids. * * @author Roman Bapst + * @author Julian Oes */ -#ifndef __VTOL_LAND_DETECTOR_H__ -#define __VTOL_LAND_DETECTOR_H__ +#pragma once -#include "MulticopterLandDetector.h" #include -namespace landdetection +#include "MulticopterLandDetector.h" + +namespace land_detector { class VtolLandDetector : public MulticopterLandDetector @@ -52,34 +53,21 @@ class VtolLandDetector : public MulticopterLandDetector public: VtolLandDetector(); +protected: + virtual void _initialize_topics() override; + + virtual void _update_params() override; + + virtual void _update_topics() override; + + virtual bool _get_landed_state() override; + + virtual bool _get_freefall_state() override; private: - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - void updateSubscriptions() override; - - /** - * @brief Runs one iteration of the land detection algorithm - **/ - LandDetectionResult update() override; - - /** - * @brief Initializes the land detection algorithm - **/ - void initialize() override; - - /** - * @brief download and update local parameter cache - **/ - void updateParameterCache(const bool force) override; - - /** - * @brief Handles for interesting parameters - **/ struct { param_t maxAirSpeed; - } _paramHandle; + } _paramHandle; struct { float maxAirSpeed; @@ -94,6 +82,5 @@ private: float _airspeed_filtered; /**< low pass filtered airspeed */ }; -#endif -} +} // namespace land_detector diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index bc9b9f8d76..3ccb81ea1e 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -55,7 +55,8 @@ #include "MulticopterLandDetector.h" #include "VtolLandDetector.h" -namespace landdetection + +namespace land_detector { //Function prototypes @@ -83,7 +84,7 @@ static void land_detector_stop() return; } - land_detector_task->shutdown(); + land_detector_task->stop(); // Wait for task to die int i = 0; @@ -92,7 +93,7 @@ static void land_detector_stop() /* wait 20ms */ usleep(20000); - } while (land_detector_task->isRunning() && ++i < 50); + } while (land_detector_task->is_running() && ++i < 50); delete land_detector_task; @@ -146,8 +147,8 @@ static int land_detector_start(const char *mode) usleep(10000); /* check if the waiting involving dots and a newline are still needed */ - if (!land_detector_task->isRunning()) { - while (!land_detector_task->isRunning()) { + if (!land_detector_task->is_running()) { + while (!land_detector_task->is_running()) { usleep(50000); if (hrt_absolute_time() > timeout) { @@ -191,8 +192,27 @@ int land_detector_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (land_detector_task) { - if (land_detector_task->isRunning()) { - PX4_WARN("running (%s): %s", _currentMode, (land_detector_task->isLanded()) ? "LANDED" : "IN AIR"); + if (land_detector_task->is_running()) { + PX4_INFO("running (%s): %s", _currentMode); + LandDetector::LandDetectionState state = land_detector_task->get_state(); + + switch (state) { + case LandDetector::LandDetectionState::FLYING: + PX4_INFO("State: Flying"); + break; + + case LandDetector::LandDetectionState::LANDED: + PX4_INFO("State: Landed"); + break; + + case LandDetector::LandDetectionState::FREEFALL: + PX4_INFO("State: Freefall"); + break; + + default: + PX4_ERR("State: unknown"); + break; + } } else { PX4_WARN("exists, but not running (%s)", _currentMode);