fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 17:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Land detector: Run in work queue

Browse Source
This commit is contained in:
Lorenz Meier
2015-10-10 22:23:34 +02:00
parent 129a4c3b53
commit 0318c1b330
3 changed files with 56 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/land_detector_main.cpp
+6 -26
View File
@@ -67,47 +67,32 @@ extern "C" __EXPORT int land_detector_main(int argc, char *argv[]);
//Private variables
static LandDetector *land_detector_task = nullptr;
static int _landDetectorTaskID = -1;
static char _currentMode[12];
/**
* Deamon thread function
**/
static void land_detector_deamon_thread(int argc, char *argv[])
{
land_detector_task->start();
}
/**
* Stop the task, force killing it if it doesn't stop by itself
**/
static void land_detector_stop()
{
if (land_detector_task == nullptr || _landDetectorTaskID == -1) {
if (land_detector_task == nullptr) {
warnx("not running");
return;
}
land_detector_task->shutdown();
//Wait for task to die
// Wait for task to die
int i = 0;
do {
/* wait 20ms */
usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
px4_task_delete(_landDetectorTaskID);
break;
}
} while (land_detector_task->isRunning());
} while (land_detector_task->isRunning() && ++i < 50);
delete land_detector_task;
land_detector_task = nullptr;
_landDetectorTaskID = -1;
warnx("land_detector has been stopped");
}
@@ -116,7 +101,7 @@ static void land_detector_stop()
**/
static int land_detector_start(const char *mode)
{
if (land_detector_task != nullptr || _landDetectorTaskID != -1) {
if (land_detector_task != nullptr) {
warnx("already running");
return -1;
}
@@ -140,14 +125,9 @@ static int land_detector_start(const char *mode)
}
//Start new thread task
_landDetectorTaskID = px4_task_spawn_cmd("land_detector",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
1000,
(px4_main_t)&land_detector_deamon_thread,
nullptr);
int ret = land_detector_task->start();
if (_landDetectorTaskID < 0) {
if (ret) {
warnx("task start failed: %d", -errno);
return -1;
}