|
|
|
@ -76,7 +76,7 @@ static char _currentMode[12];
@@ -76,7 +76,7 @@ static char _currentMode[12];
|
|
|
|
|
static void land_detector_stop() |
|
|
|
|
{ |
|
|
|
|
if (land_detector_task == nullptr) { |
|
|
|
|
warnx("not running"); |
|
|
|
|
PX4_WARN("not running"); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -94,7 +94,7 @@ static void land_detector_stop()
@@ -94,7 +94,7 @@ static void land_detector_stop()
|
|
|
|
|
|
|
|
|
|
delete land_detector_task; |
|
|
|
|
land_detector_task = nullptr; |
|
|
|
|
warnx("land_detector has been stopped"); |
|
|
|
|
PX4_WARN("land_detector has been stopped"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
@ -103,7 +103,7 @@ static void land_detector_stop()
@@ -103,7 +103,7 @@ static void land_detector_stop()
|
|
|
|
|
static int land_detector_start(const char *mode) |
|
|
|
|
{ |
|
|
|
|
if (land_detector_task != nullptr) { |
|
|
|
|
warnx("already running"); |
|
|
|
|
PX4_WARN("already running"); |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -118,13 +118,13 @@ static int land_detector_start(const char *mode)
@@ -118,13 +118,13 @@ static int land_detector_start(const char *mode)
|
|
|
|
|
land_detector_task = new VtolLandDetector(); |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
warnx("[mode] must be either 'fixedwing' or 'multicopter'"); |
|
|
|
|
PX4_WARN("[mode] must be either 'fixedwing' or 'multicopter'"); |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
//Check if alloc worked
|
|
|
|
|
if (land_detector_task == nullptr) { |
|
|
|
|
warnx("alloc failed"); |
|
|
|
|
PX4_WARN("alloc failed"); |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -132,7 +132,7 @@ static int land_detector_start(const char *mode)
@@ -132,7 +132,7 @@ static int land_detector_start(const char *mode)
|
|
|
|
|
int ret = land_detector_task->start(); |
|
|
|
|
|
|
|
|
|
if (ret) { |
|
|
|
|
warnx("task start failed: %d", -errno); |
|
|
|
|
PX4_WARN("task start failed: %d", -errno); |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -145,19 +145,14 @@ static int land_detector_start(const char *mode)
@@ -145,19 +145,14 @@ static int land_detector_start(const char *mode)
|
|
|
|
|
/* check if the waiting involving dots and a newline are still needed */ |
|
|
|
|
if (!land_detector_task->isRunning()) { |
|
|
|
|
while (!land_detector_task->isRunning()) { |
|
|
|
|
|
|
|
|
|
printf("."); |
|
|
|
|
fflush(stdout); |
|
|
|
|
usleep(50000); |
|
|
|
|
|
|
|
|
|
if (hrt_absolute_time() > timeout) { |
|
|
|
|
warnx("start failed - timeout"); |
|
|
|
|
PX4_WARN("start failed - timeout"); |
|
|
|
|
land_detector_stop(); |
|
|
|
|
return 1; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
printf("\n"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
//Remember current active mode
|
|
|
|
@ -178,7 +173,7 @@ int land_detector_main(int argc, char *argv[])
@@ -178,7 +173,7 @@ int land_detector_main(int argc, char *argv[])
|
|
|
|
|
|
|
|
|
|
if (argc >= 2 && !strcmp(argv[1], "start")) { |
|
|
|
|
if (land_detector_start(argv[2]) != 0) { |
|
|
|
|
warnx("land_detector start failed"); |
|
|
|
|
PX4_WARN("land_detector start failed"); |
|
|
|
|
return 1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -194,22 +189,22 @@ int land_detector_main(int argc, char *argv[])
@@ -194,22 +189,22 @@ int land_detector_main(int argc, char *argv[])
|
|
|
|
|
if (land_detector_task) { |
|
|
|
|
|
|
|
|
|
if (land_detector_task->isRunning()) { |
|
|
|
|
warnx("running (%s): %s", _currentMode, (land_detector_task->isLanded()) ? "LANDED" : "IN AIR"); |
|
|
|
|
PX4_WARN("running (%s): %s", _currentMode, (land_detector_task->isLanded()) ? "LANDED" : "IN AIR"); |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
warnx("exists, but not running (%s)", _currentMode); |
|
|
|
|
PX4_WARN("exists, but not running (%s)", _currentMode); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return 0; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
warnx("not running"); |
|
|
|
|
PX4_WARN("not running"); |
|
|
|
|
return 1; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
exiterr: |
|
|
|
|
warnx("usage: land_detector {start|stop|status} [mode]"); |
|
|
|
|
warnx("mode can either be 'fixedwing' or 'multicopter'"); |
|
|
|
|
PX4_WARN("usage: land_detector {start|stop|status} [mode]"); |
|
|
|
|
PX4_WARN("mode can either be 'fixedwing' or 'multicopter'"); |
|
|
|
|
return 1; |
|
|
|
|
} |
|
|
|
|