Merge pull request #2261 from PX4/navigator_port

Navigator port

makefiles/posix/config_posix_default.mk

@@ -34,6 +34,7 @@ MODULES		+= modules/ekf_att_pos_estimator
 #
 # Vehicle Control
 #
+MODULES 	+= modules/navigator
 MODULES 	+= modules/mc_pos_control
 MODULES		+= modules/mc_att_control
 
@@ -47,6 +48,7 @@ MODULES		+= modules/dataman
 MODULES		+= modules/sdlog2
 MODULES		+= modules/simulator
 MODULES		+= modules/commander
+MODULES 	+= modules/controllib
 
 #
 # Libraries

src/modules/controllib/blocks.hpp

@@ -39,6 +39,7 @@
 
 #pragma once
 
+#include <px4_defines.h>
 #include <assert.h>
 #include <time.h>
 #include <stdlib.h>

src/modules/controllib/uorb/blocks.cpp

@@ -61,16 +61,16 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
 
 	// heading to waypoint
 	float psiTrack = get_bearing_to_next_waypoint(
-				 (double)pos.lat / (double)1e7d,
-				 (double)pos.lon / (double)1e7d,
+				 (double)pos.lat / (double)1e7,
+				 (double)pos.lon / (double)1e7,
 				 missionCmd.lat,
 				 missionCmd.lon);
 
 	// cross track
 	struct crosstrack_error_s xtrackError;
 	get_distance_to_line(&xtrackError,
-			     (double)pos.lat / (double)1e7d,
-			     (double)pos.lon / (double)1e7d,
+			     (double)pos.lat / (double)1e7,
+			     (double)pos.lon / (double)1e7,
 			     lastMissionCmd.lat,
 			     lastMissionCmd.lon,
 			     missionCmd.lat,

src/modules/navigator/mission.cpp

@@ -533,7 +533,7 @@ Mission::heading_sp_update()
 
 	/* Don't change setpoint if last and current waypoint are not valid */
 	if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid ||
-			!isfinite(_on_arrival_yaw)) {
+			!PX4_ISFINITE(_on_arrival_yaw)) {
 		return;
 	}
 
@@ -581,7 +581,7 @@ Mission::altitude_sp_foh_update()
 
 	/* Don't change setpoint if last and current waypoint are not valid */
 	if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid ||
-			!isfinite(_mission_item_previous_alt)) {
+			!PX4_ISFINITE(_mission_item_previous_alt)) {
 		return;
 	}
 

src/modules/navigator/mission_block.cpp

@@ -80,8 +80,8 @@ MissionBlock::is_mission_item_reached()
 	}
 
 	/* TODO: count turns */
-	if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) {
+	if (/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
 		return false;
 	}
 

src/modules/navigator/navigator_main.cpp

@@ -44,6 +44,9 @@
  */
 
 #include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
 
 #include <stdio.h>
 #include <stdlib.h>
@@ -174,7 +177,7 @@ Navigator::~Navigator()
 
 			/* if we have given up, kill it */
 			if (++i > 50) {
-				task_delete(_navigator_task);
+				px4_task_delete(_navigator_task);
 				break;
 			}
 		} while (_navigator_task != -1);
@@ -505,7 +508,7 @@ Navigator::task_main()
 	warnx("exiting.");
 
 	_navigator_task = -1;
-	_exit(0);
+	return;
 }
 
 int
@@ -516,9 +519,9 @@ Navigator::start()
 	/* start the task */
 	_navigator_task = px4_task_spawn_cmd("navigator",
 					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_DEFAULT + 20,
+					 SCHED_PRIORITY_MAX -5,
 					 1700,
-					 (main_t)&Navigator::task_main_trampoline,
+					 (px4_main_t)&Navigator::task_main_trampoline,
 					 nullptr);
 
 	if (_navigator_task < 0) {
@@ -584,54 +587,57 @@ void Navigator::load_fence_from_file(const char *filename)
 
 static void usage()
 {
-	errx(1, "usage: navigator {start|stop|status|fence|fencefile}");
+	warnx("usage: navigator {start|stop|status|fence|fencefile}");
 }
 
 int navigator_main(int argc, char *argv[])
 {
 	if (argc < 2) {
 		usage();
+		return 1;
 	}
 
 	if (!strcmp(argv[1], "start")) {
 
 		if (navigator::g_navigator != nullptr) {
-			errx(1, "already running");
+			warnx("already running");
+			return 1;
 		}
 
 		navigator::g_navigator = new Navigator;
 
 		if (navigator::g_navigator == nullptr) {
-			errx(1, "alloc failed");
+			warnx("alloc failed");
+			return 1;
 		}
 
 		if (OK != navigator::g_navigator->start()) {
 			delete navigator::g_navigator;
 			navigator::g_navigator = nullptr;
-			err(1, "start failed");
+			warnx("start failed");
+			return 1;
 		}
 
 		return 0;
 	}
 
-	if (navigator::g_navigator == nullptr)
-		errx(1, "not running");
+	if (navigator::g_navigator == nullptr) {
+		warnx("not running");
+		return 1;
+	}
 
 	if (!strcmp(argv[1], "stop")) {
 		delete navigator::g_navigator;
 		navigator::g_navigator = nullptr;
-
 	} else if (!strcmp(argv[1], "status")) {
 		navigator::g_navigator->status();
-
 	} else if (!strcmp(argv[1], "fence")) {
 		navigator::g_navigator->add_fence_point(argc - 2, argv + 2);
-
 	} else if (!strcmp(argv[1], "fencefile")) {
 		navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME);
-
 	} else {
 		usage();
+		return 1;
 	}
 
 	return 0;

src/platforms/px4_defines.h

@@ -146,6 +146,8 @@ __END_DECLS
 #define OK 0
 #define ERROR -1
 
+#define MAX_RAND 32767
+
 #if defined(__PX4_QURT)
 #define M_PI			3.14159265358979323846
 #define M_PI_2			1.57079632679489661923
@@ -209,6 +211,7 @@ __END_DECLS
 #define fminf(x, y) ((x) > (y) ? y : x)
 #endif
 
+
 #endif
 
 /*