Merge pull request #2475 from tumbili/SITL_mission

Sitl mission enable

makefiles/posix/config_posix_sitl.mk

@@ -40,6 +40,7 @@ MODULES		+= modules/position_estimator_inav
 MODULES 	+= modules/navigator
 MODULES 	+= modules/mc_pos_control
 MODULES		+= modules/mc_att_control
+MODULES 	+= modules/land_detector
 
 #
 # Library modules

posix-configs/SITL/init/rcS

@@ -35,6 +35,7 @@ gpssim start
 hil mode_pwm
 commander start
 sensors start
+land_detector start multicopter
 navigator start
 attitude_estimator_q start
 position_estimator_inav start

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;
 		}
 	}

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;
 		}
 	}
 

src/modules/navigator/navigator_main.cpp

@@ -309,7 +309,7 @@ Navigator::task_main()
 	const hrt_abstime mavlink_open_interval = 500000;
 
 	/* wakeup source(s) */
-	struct pollfd fds[8];
+	px4_pollfd_struct_t fds[8];
 
 	/* Setup of loop */
 	fds[0].fd = _global_pos_sub;
@@ -332,7 +332,7 @@ Navigator::task_main()
 	while (!_task_should_exit) {
 
 		/* wait for up to 100ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		if (pret == 0) {
 			/* timed out - periodic check for _task_should_exit, etc. */