Merge remote-tracking branch 'upstream/master' into offboard2_externalsetpointmessages

Conflicts:
	src/modules/navigator/navigator.h
	src/modules/navigator/navigator_main.cpp

1 files changed, 132 insertions, 9 deletions

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index f11ae72c5..c6ec96dcb 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -40,6 +40,7 @@
  * @author Jean Cyr <jean.m.cyr@gmail.com>
  * @author Julian Oes <julian@oes.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <nuttx/config.h>
@@ -67,6 +68,7 @@
 #include <uORB/topics/mission.h>
 #include <uORB/topics/fence.h>
 #include <uORB/topics/navigation_capabilities.h>
+#include <drivers/drv_baro.h>
 
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
@@ -84,6 +86,7 @@
  */
 extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
 
+#define GEOFENCE_CHECK_INTERVAL 200000
 
 namespace navigator
 {
@@ -97,6 +100,7 @@ Navigator::Navigator() :
 	_navigator_task(-1),
 	_mavlink_fd(-1),
 	_global_pos_sub(-1),
+	_gps_pos_sub(-1),
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
 	_capabilities_sub(-1),
@@ -105,32 +109,47 @@ Navigator::Navigator() :
 	_offboard_mission_sub(-1),
 	_param_update_sub(-1),
 	_pos_sp_triplet_pub(-1),
+	_mission_result_pub(-1),
+	_att_sp_pub(-1),
 	_vstatus{},
 	_control_mode{},
 	_global_pos{},
+	_gps_pos{},
+	_sensor_combined{},
 	_home_pos{},
 	_mission_item{},
 	_nav_caps{},
 	_pos_sp_triplet{},
+	_mission_result{},
+	_att_sp{},
 	_mission_item_valid(false),
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
 	_geofence{},
 	_geofence_violation_warning_sent(false),
-	_fence_valid(false),
 	_inside_fence(true),
 	_navigation_mode(nullptr),
 	_mission(this, "MIS"),
 	_loiter(this, "LOI"),
 	_rtl(this, "RTL"),
+	_rcLoss(this, "RCL"),
+	_dataLinkLoss(this, "DLL"),
+	_engineFailure(this, "EF"),
+	_gpsFailure(this, "GPSF"),
 	_can_loiter_at_sp(false),
 	_pos_sp_triplet_updated(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
-	_param_acceptance_radius(this, "ACC_RAD")
+	_param_acceptance_radius(this, "ACC_RAD"),
+	_param_datalinkloss_obc(this, "DLL_OBC"),
+	_param_rcloss_obc(this, "RCL_OBC")
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;
 	_navigation_mode_array[1] = &_loiter;
 	_navigation_mode_array[2] = &_rtl;
+	_navigation_mode_array[3] = &_dataLinkLoss;
+	_navigation_mode_array[4] = &_engineFailure;
+	_navigation_mode_array[5] = &_gpsFailure;
+	_navigation_mode_array[6] = &_rcLoss;
 
 	updateParams();
 }
@@ -167,6 +186,18 @@ Navigator::global_position_update()
 }
 
 void
+Navigator::gps_position_update()
+{
+	orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos);
+}
+
+void
+Navigator::sensor_combined_update()
+{
+	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
+}
+
+void
 Navigator::home_position_update()
 {
 	orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
@@ -217,6 +248,7 @@ Navigator::task_main()
 	warnx("Initializing..");
 
 	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	_geofence.setMavlinkFd(_mavlink_fd);
 
 	/* Try to load the geofence:
 	 * if /fs/microsd/etc/geofence.txt load from this file
@@ -228,6 +260,7 @@ Navigator::task_main()
 		_geofence.loadFromFile(GEOFENCE_FILENAME);
 
 	} else {
+		mavlink_log_critical(_mavlink_fd, "#audio: No geofence file");
 		if (_geofence.clearDm() > 0)
 			warnx("Geofence cleared");
 		else
@@ -236,6 +269,8 @@ Navigator::task_main()
 
 	/* do subscriptions */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@@ -248,6 +283,8 @@ Navigator::task_main()
 	vehicle_status_update();
 	vehicle_control_mode_update();
 	global_position_update();
+	gps_position_update();
+	sensor_combined_update();
 	home_position_update();
 	navigation_capabilities_update();
 	params_update();
@@ -259,7 +296,7 @@ Navigator::task_main()
 	const hrt_abstime mavlink_open_interval = 500000;
 
 	/* wakeup source(s) */
-	struct pollfd fds[6];
+	struct pollfd fds[8];
 
 	/* Setup of loop */
 	fds[0].fd = _global_pos_sub;
@@ -274,6 +311,10 @@ Navigator::task_main()
 	fds[4].events = POLLIN;
 	fds[5].fd = _param_update_sub;
 	fds[5].events = POLLIN;
+	fds[6].fd = _sensor_combined_sub;
+	fds[6].events = POLLIN;
+	fds[7].fd = _gps_pos_sub;
+	fds[7].events = POLLIN;
 
 	while (!_task_should_exit) {
 
@@ -298,6 +339,21 @@ Navigator::task_main()
 			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 		}
 
+		static bool have_geofence_position_data = false;
+
+		/* gps updated */
+		if (fds[7].revents & POLLIN) {
+			gps_position_update();
+			if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
+				have_geofence_position_data = true;
+			}
+		}
+
+		/* sensors combined updated */
+		if (fds[6].revents & POLLIN) {
+			sensor_combined_update();
+		}
+
 		/* parameters updated */
 		if (fds[5].revents & POLLIN) {
 			params_update();
@@ -327,9 +383,23 @@ Navigator::task_main()
 		/* global position updated */
 		if (fds[0].revents & POLLIN) {
 			global_position_update();
+			static int gposcounter = 0;
+			if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
+				have_geofence_position_data = true;
+			}
+			gposcounter++;
+		}
 
-			/* Check geofence violation */
-			if (!_geofence.inside(&_global_pos)) {
+		/* Check geofence violation */
+		static hrt_abstime last_geofence_check = 0;
+		if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) {
+			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
+			last_geofence_check = hrt_absolute_time();
+			have_geofence_position_data = false;
+			if (!inside) {
+				/* inform other apps via the mission result */
+				_mission_result.geofence_violated = true;
+				publish_mission_result();
 
 				/* Issue a warning about the geofence violation once */
 				if (!_geofence_violation_warning_sent) {
@@ -337,6 +407,9 @@ Navigator::task_main()
 					_geofence_violation_warning_sent = true;
 				}
 			} else {
+				/* inform other apps via the mission result */
+				_mission_result.geofence_violated = false;
+				publish_mission_result();
 				/* Reset the _geofence_violation_warning_sent field */
 				_geofence_violation_warning_sent = false;
 			}
@@ -360,11 +433,30 @@ Navigator::task_main()
 			case NAVIGATION_STATE_AUTO_LOITER:
 				_navigation_mode = &_loiter;
 				break;
+			case NAVIGATION_STATE_AUTO_RCRECOVER:
+				if (_param_rcloss_obc.get() != 0) {
+					_navigation_mode = &_rcLoss;
+				} else {
+					_navigation_mode = &_rtl;
+				}
+				break;
 			case NAVIGATION_STATE_AUTO_RTL:
-				_navigation_mode = &_rtl;
+					_navigation_mode = &_rtl;
 				break;
 			case NAVIGATION_STATE_AUTO_RTGS:
-				_navigation_mode = &_rtl; /* TODO: change this to something else */
+				/* Use complex data link loss mode only when enabled via param
+				* otherwise use rtl */
+				if (_param_datalinkloss_obc.get() != 0) {
+					_navigation_mode = &_dataLinkLoss;
+				} else {
+					_navigation_mode = &_rtl;
+				}
+				break;
+			case NAVIGATION_STATE_AUTO_LANDENGFAIL:
+				_navigation_mode = &_engineFailure;
+				break;
+			case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+				_navigation_mode = &_gpsFailure;
 				break;
 			default:
 				_navigation_mode = nullptr;
@@ -435,7 +527,7 @@ Navigator::status()
 	// 	warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
 	// }
 
-	if (_fence_valid) {
+	if (_geofence.valid()) {
 		warnx("Geofence is valid");
 		/* TODO: needed? */
 //		warnx("Vertex longitude latitude");
@@ -443,7 +535,7 @@ Navigator::status()
 //		warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat);
 
 	} else {
-		warnx("Geofence not set");
+		warnx("Geofence not set (no /etc/geofence.txt on microsd) or not valid");
 	}
 }
 
@@ -527,3 +619,34 @@ int navigator_main(int argc, char *argv[])
 
 	return 0;
 }
+
+void
+Navigator::publish_mission_result()
+{
+	/* lazily publish the mission result only once available */
+	if (_mission_result_pub > 0) {
+		/* publish mission result */
+		orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
+
+	} else {
+		/* advertise and publish */
+		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
+	}
+	/* reset reached bool */
+	_mission_result.reached = false;
+	_mission_result.finished = false;
+}
+
+void
+Navigator::publish_att_sp()
+{
+	/* lazily publish the attitude sp only once available */
+	if (_att_sp_pub > 0) {
+		/* publish att sp*/
+		orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
+
+	} else {
+		/* advertise and publish */
+		_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+	}
+}