diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 89d80a6f10..eefab8f5b3 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -31,7 +31,7 @@ * ****************************************************************************/ -/* +/** * @file FixedwingLandDetector.cpp * * @author Johan Jansen diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 02bdbddfe0..77862203e1 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -58,8 +58,6 @@ LandDetector::LandDetector() : _freefall_hysteresis(false), _landed_hysteresis(true), _ground_contact_hysteresis(true), - _taskShouldExit(false), - _taskIsRunning(false), _total_flight_time{0}, _takeoff_time{0}, _work{} @@ -71,23 +69,12 @@ LandDetector::LandDetector() : LandDetector::~LandDetector() { - work_cancel(HPWORK, &_work); - _taskShouldExit = true; } int LandDetector::start() { - _taskShouldExit = false; - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this, 0); - - return 0; -} - -void LandDetector::stop() -{ - _taskShouldExit = true; + return work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this, 0); } void @@ -100,7 +87,7 @@ LandDetector::_cycle_trampoline(void *arg) void LandDetector::_cycle() { - if (!_taskIsRunning) { + if (!_object) { // not initialized yet // Advertise the first land detected uORB. _landDetected.timestamp = hrt_absolute_time(); _landDetected.freefall = false; @@ -114,8 +101,7 @@ void LandDetector::_cycle() _check_params(true); - // Task is now running, keep doing so until we need to stop. - _taskIsRunning = true; + _object = this; } _check_params(false); @@ -165,14 +151,14 @@ void LandDetector::_cycle() &instance, ORB_PRIO_DEFAULT); } - if (!_taskShouldExit) { + if (!should_exit()) { // Schedule next cycle. work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this, USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE_HZ)); } else { - _taskIsRunning = false; + exit_and_cleanup(); } } void LandDetector::_check_params(const bool force) diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 99ff7ecd3f..87341a07ca 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -33,7 +33,7 @@ /** * @file LandDetector.h -Land detector interface for multicopter, fixedwing and VTOL implementations. + * Land detector interface for multicopter, fixedwing and VTOL implementations. * * @author Johan Jansen * @author Julian Oes @@ -43,6 +43,7 @@ Land detector interface for multicopter, fixedwing and VTOL implementations. #pragma once #include +#include #include #include #include @@ -52,7 +53,7 @@ namespace land_detector { -class LandDetector +class LandDetector : public ModuleBase { public: enum class LandDetectionState { @@ -65,14 +66,19 @@ public: LandDetector(); virtual ~LandDetector(); - /** - * @return true if this task is currently running. - */ - inline bool is_running() const + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]) { - return _taskIsRunning; + return print_usage("unknown command"); } + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; /** * @return current state. @@ -82,11 +88,6 @@ public: return _state; } - /** - * Tells the task that it should exit. - */ - void stop(); - /** * Get the work queue going. */ @@ -170,15 +171,11 @@ private: void _update_state(); - bool _taskShouldExit; - bool _taskIsRunning; - param_t _p_total_flight_time_high; param_t _p_total_flight_time_low; uint64_t _total_flight_time; ///< in microseconds hrt_abstime _takeoff_time; - struct work_s _work; }; diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index a28a6e0bf4..573f1fbc03 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -50,7 +50,6 @@ #include #include #include //Scheduler -#include //print to console #include "FixedwingLandDetector.h" #include "MulticopterLandDetector.h" @@ -61,180 +60,115 @@ namespace land_detector { -// Function prototypes -static int land_detector_start(const char *mode); -static void land_detector_stop(); - -/** - * land detector app start / stop handling function - * This makes the land detector module accessible from the nuttx shell - * @ingroup apps - */ extern "C" __EXPORT int land_detector_main(int argc, char *argv[]); -// Private variables -static LandDetector *land_detector_task = nullptr; static char _currentMode[12]; -/** - * Stop the task, force killing it if it doesn't stop by itself - */ -static void land_detector_stop() +int LandDetector::task_spawn(int argc, char *argv[]) { - if (land_detector_task == nullptr) { - PX4_WARN("not running"); - return; - } - - land_detector_task->stop(); - - // Wait for task to die - int i = 0; - - do { - // wait 20ms at a time - usleep(20000); - - } while (land_detector_task->is_running() && ++i < 50); - - - delete land_detector_task; - land_detector_task = nullptr; - PX4_WARN("land_detector has been stopped"); -} - -/** - * Start new task, fails if it is already running. Returns OK if successful - */ -static int land_detector_start(const char *mode) -{ - if (land_detector_task != nullptr) { - PX4_WARN("already running"); + if (argc < 2) { + print_usage(); return -1; } - //Allocate memory - if (!strcmp(mode, "fixedwing")) { - land_detector_task = new FixedwingLandDetector(); + LandDetector *obj; - } else if (!strcmp(mode, "multicopter")) { - land_detector_task = new MulticopterLandDetector(); + if (!strcmp(argv[1], "fixedwing")) { + obj = new FixedwingLandDetector(); - } else if (!strcmp(mode, "vtol")) { - land_detector_task = new VtolLandDetector(); + } else if (!strcmp(argv[1], "multicopter")) { + obj = new MulticopterLandDetector(); - } else if (!strcmp(mode, "ugv")) { - land_detector_task = new RoverLandDetector(); + } else if (!strcmp(argv[1], "vtol")) { + obj = new VtolLandDetector(); + + } else if (!strcmp(argv[1], "ugv")) { + obj = new RoverLandDetector(); } else { - PX4_WARN("[mode] must be either 'fixedwing', 'multicopter', or 'vtol'"); + print_usage("unknown mode"); return -1; } - // Check if alloc worked - if (land_detector_task == nullptr) { - PX4_WARN("alloc failed"); + if (!obj) { + PX4_ERR("alloc failed"); return -1; } - // Start new thread task - int ret = land_detector_task->start(); + int ret = obj->start(); - if (ret) { - PX4_WARN("task start failed: %d", -errno); - return -1; - } - - // Avoid memory fragmentation by not exiting start handler until the task has fully started - const uint64_t timeout = hrt_absolute_time() + 5000000; // 5 second timeout - - // Do one sleep before the first check - usleep(10000); - - if (!land_detector_task->is_running()) { - while (!land_detector_task->is_running()) { - usleep(50000); - - if (hrt_absolute_time() > timeout) { - PX4_WARN("start failed - timeout"); - land_detector_stop(); - return 1; - } - } + if (ret < 0) { + delete obj; + return ret; } // Remember current active mode - strncpy(_currentMode, mode, sizeof(_currentMode) - 1); + strncpy(_currentMode, argv[1], sizeof(_currentMode) - 1); _currentMode[sizeof(_currentMode) - 1] = '\0'; + wait_until_running(); // this will wait until _object is set from the cycle method + _task_id = task_id_is_work_queue; + return 0; +} + +int LandDetector::print_status() +{ + PX4_INFO("running (%s)", _currentMode); + LandDetector::LandDetectionState state = 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; + } + return 0; } -/** - * Main entry point for this module - */ +int LandDetector::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Module to detect the freefall and landed state of the vehicle, and publishing the `vehicle_land_detected` topic. +Each vehicle type (multirotor, fixedwing, vtol, ...) provides its own algorithm, taking into account various +states, such as commanded thrust, arming state and vehicle motion. + +### Implementation +Every type is implemented in its own class with a common base class. The base class maintains a state (landed, ground +contact, ...). Each possible state is implemented in the derived classes. A hysteresis and a fixed priority of each +state determines the current state. + +The module runs periodically on the HP work queue. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("land_detector", "system"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task"); + PRINT_MODULE_USAGE_ARG("fixedwing|multicopter|vtol|rover", "Select vehicle type", false); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + + int land_detector_main(int argc, char *argv[]) { - - if (argc < 2) { - goto exiterr; - } - - if (argc >= 2 && !strcmp(argv[1], "start")) { - if (land_detector_start(argv[2]) != 0) { - PX4_WARN("land_detector start failed"); - return 1; - } - - return 0; - } - - if (!strcmp(argv[1], "stop")) { - land_detector_stop(); - return 0; - } - - if (!strcmp(argv[1], "status")) { - if (land_detector_task) { - - if (land_detector_task->is_running()) { - PX4_INFO("running (%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); - } - - return 0; - - } else { - PX4_WARN("not running"); - return 1; - } - } - -exiterr: - PX4_WARN("usage: land_detector {start|stop|status} [mode]"); - PX4_WARN("mode can either be 'fixedwing' or 'multicopter'"); - return 1; + return LandDetector::main(argc, argv); } }