Skip to content
Snippets Groups Projects
Commit 8a83fb7d authored by Beat Küng's avatar Beat Küng
Browse files

land_detector: use ModuleBase & add module documentation

parent 8d7481f9
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/modules/land_detector/FixedwingLandDetector.cpp
+ 1
− 1
View file @ 8a83fb7d
......@@ -31,7 +31,7 @@
*
****************************************************************************/
/*
/**
* @file FixedwingLandDetector.cpp
*
* @author Johan Jansen <jnsn.johan@gmail.com>
......
src/modules/land_detector/LandDetector.cpp
+ 5
− 19
View file @ 8a83fb7d
......@@ -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)
......
src/modules/land_detector/LandDetector.h
+ 13
− 16
View file @ 8a83fb7d
......@@ -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 <jnsn.johan@gmail.com>
* @author Julian Oes <julian@oes.ch>
......@@ -43,6 +43,7 @@ Land detector interface for multicopter, fixedwing and VTOL implementations.
#pragma once
#include <px4_workqueue.h>
#include <px4_module.h>
#include <systemlib/hysteresis/hysteresis.h>
#include <systemlib/param/param.h>
#include <uORB/uORB.h>
......@@ -52,7 +53,7 @@ namespace land_detector
{
class LandDetector
class LandDetector : public ModuleBase<LandDetector>
{
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;
};
......
src/modules/land_detector/land_detector_main.cpp
+ 70
− 136
View file @ 8a83fb7d
......@@ -50,7 +50,6 @@
#include <errno.h>
#include <drivers/drv_hrt.h>
#include <systemlib/systemlib.h> //Scheduler
#include <systemlib/err.h> //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;
}
/**
* Main entry point for this module
*/
int land_detector_main(int argc, char *argv[])
int LandDetector::print_status()
{
PX4_INFO("running (%s)", _currentMode);
LandDetector::LandDetectionState state = get_state();
if (argc < 2) {
goto exiterr;
}
switch (state) {
case LandDetector::LandDetectionState::FLYING:
PX4_INFO("State: Flying");
break;
if (argc >= 2 && !strcmp(argv[1], "start")) {
if (land_detector_start(argv[2]) != 0) {
PX4_WARN("land_detector start failed");
return 1;
}
case LandDetector::LandDetectionState::LANDED:
PX4_INFO("State: Landed");
break;
return 0;
}
case LandDetector::LandDetectionState::FREEFALL:
PX4_INFO("State: Freefall");
break;
if (!strcmp(argv[1], "stop")) {
land_detector_stop();
return 0;
default:
PX4_ERR("State: unknown");
break;
}
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;
}
return 0;
}
} else {
PX4_WARN("exists, but not running (%s)", _currentMode);
}
int LandDetector::print_usage(const char *reason)
{
if (reason) {
PX4_ERR("%s\n", reason);
}
return 0;
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;
}
} 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;
int land_detector_main(int argc, char *argv[])
{
return LandDetector::main(argc, argv);
}
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment