From 40cc11a5eda0161458a81f55d1c44a8f288655e1 Mon Sep 17 00:00:00 2001
From: tumbili <bapstr@ethz.ch>
Date: Fri, 26 Jun 2015 15:01:17 +0200
Subject: [PATCH] ported land detector

---
 .../land_detector/FixedwingLandDetector.cpp   |  4 +-
 .../land_detector/land_detector_main.cpp      | 39 +++++++++++--------
 2 files changed, 25 insertions(+), 18 deletions(-)

diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp
index 5f7ded9cb2..b98f3fd4ce 100644
--- a/src/modules/land_detector/FixedwingLandDetector.cpp
+++ b/src/modules/land_detector/FixedwingLandDetector.cpp
@@ -87,12 +87,12 @@ bool FixedwingLandDetector::update()
 	if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) {
 		float val = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx *
 					_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
-		if (isfinite(val)) {
+		if (PX4_ISFINITE(val)) {
 			_velocity_xy_filtered = val;
 		}
 		val = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz);
 
-		if (isfinite(val)) {
+		if (PX4_ISFINITE(val)) {
 			_velocity_z_filtered = val;
 		}
 	}
diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp
index 1ca319ce63..58753d5ca7 100644
--- a/src/modules/land_detector/land_detector_main.cpp
+++ b/src/modules/land_detector/land_detector_main.cpp
@@ -38,6 +38,10 @@
  * @author Johan Jansen <jnsn.johan@gmail.com>
  */
 
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
 #include <unistd.h>					//usleep
 #include <stdio.h>
 #include <string.h>
@@ -80,7 +84,7 @@ static void land_detector_deamon_thread(int argc, char *argv[])
 static void land_detector_stop()
 {
 	if (land_detector_task == nullptr || _landDetectorTaskID == -1) {
-		errx(1, "not running");
+		warnx("not running");
 		return;
 	}
 
@@ -95,7 +99,7 @@ static void land_detector_stop()
 
 		/* if we have given up, kill it */
 		if (++i > 50) {
-			task_delete(_landDetectorTaskID);
+			px4_task_delete(_landDetectorTaskID);
 			break;
 		}
 	} while (land_detector_task->isRunning());
@@ -104,7 +108,7 @@ static void land_detector_stop()
 	delete land_detector_task;
 	land_detector_task = nullptr;
 	_landDetectorTaskID = -1;
-	errx(0, "land_detector has been stopped");
+	warnx("land_detector has been stopped");
 }
 
 /**
@@ -113,7 +117,7 @@ static void land_detector_stop()
 static int land_detector_start(const char *mode)
 {
 	if (land_detector_task != nullptr || _landDetectorTaskID != -1) {
-		errx(1, "already running");
+		warnx("already running");
 		return -1;
 	}
 
@@ -125,13 +129,13 @@ static int land_detector_start(const char *mode)
 		land_detector_task = new MulticopterLandDetector();
 
 	} else {
-		errx(1, "[mode] must be either 'fixedwing' or 'multicopter'");
+		warnx("[mode] must be either 'fixedwing' or 'multicopter'");
 		return -1;
 	}
 
 	//Check if alloc worked
 	if (land_detector_task == nullptr) {
-		errx(1, "alloc failed");
+		warnx("alloc failed");
 		return -1;
 	}
 
@@ -140,11 +144,11 @@ static int land_detector_start(const char *mode)
 					     SCHED_DEFAULT,
 					     SCHED_PRIORITY_DEFAULT,
 					     1000,
-					     (main_t)&land_detector_deamon_thread,
+					     (px4_main_t)&land_detector_deamon_thread,
 					     nullptr);
 
 	if (_landDetectorTaskID < 0) {
-		errx(1, "task start failed: %d", -errno);
+		warnx("task start failed: %d", -errno);
 		return -1;
 	}
 
@@ -163,9 +167,9 @@ static int land_detector_start(const char *mode)
 			usleep(50000);
 
 			if (hrt_absolute_time() > timeout) {
-				err(1, "start failed - timeout");
+				warnx("start failed - timeout");
 				land_detector_stop();
-				exit(1);
+				return 1;
 			}
 		}
 		printf("\n");
@@ -174,7 +178,6 @@ static int land_detector_start(const char *mode)
 	//Remember current active mode
 	strncpy(_currentMode, mode, 12);
 
-	exit(0);
 	return 0;
 }
 
@@ -189,12 +192,15 @@ int land_detector_main(int argc, char *argv[])
 	}
 
 	if (argc >= 2 && !strcmp(argv[1], "start")) {
-		land_detector_start(argv[2]);
+		if (land_detector_start(argv[2]) != 0) {
+			warnx("land_detector start failed");
+			return 1;
+		}
 	}
 
 	if (!strcmp(argv[1], "stop")) {
 		land_detector_stop();
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "status")) {
@@ -204,13 +210,14 @@ int land_detector_main(int argc, char *argv[])
 				warnx("running (%s): %s", _currentMode, (land_detector_task->isLanded()) ? "LANDED" : "IN AIR");
 
 			} else {
-				errx(1, "exists, but not running (%s)", _currentMode);
+				warnx("exists, but not running (%s)", _currentMode);
 			}
 
-			exit(0);
+			return 0;
 
 		} else {
-			errx(1, "not running");
+			warnx("not running");
+			return 1;
 		}
 	}