Merge branch 'beta' into rtl_heading

3 files changed, 130 insertions, 44 deletions

diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c
index 20dd1fe2f..5831a0ca9 100644
--- a/src/modules/navigator/geofence_params.c
+++ b/src/modules/navigator/geofence_params.c
@@ -45,11 +45,17 @@
 #include <systemlib/param/param.h>
 
 /*
- * geofence parameters, accessible via MAVLink
- *
+ * Geofence parameters, accessible via MAVLink
  */
 
-// @DisplayName		Switch to enable geofence
-// @Description		if set to 1 geofence is enabled, defaults to 1 because geofence is only enabled when the geofence.txt file is present
-// @Range		0 or 1
+/**
+ * Enable geofence.
+ *
+ * Set to 1 to enable geofence.
+ * Defaults to 1 because geofence is only enabled when the geofence.txt file is present.
+ *
+ * @min 0
+ * @max 1
+ * @group Geofence
+ */
 PARAM_DEFINE_INT32(GF_ON, 1);
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index d7f6fd16a..0ed12d106 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -306,6 +306,12 @@ private:
 	void		start_land_home();
 
 	/**
+	 * Fork for state transitions
+	 */
+	void		request_loiter_or_ready();
+	void		request_mission_if_available();
+
+	/**
 	 * Guards offboard mission
 	 */
 	bool		offboard_mission_available(unsigned relative_index);
@@ -699,24 +705,17 @@ Navigator::task_main()
 					} else {
 						/* MISSION switch */
 						if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
-							dispatch(EVENT_LOITER_REQUESTED);
+							request_loiter_or_ready();
 							stick_mode = true;
 
 						} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
-							/* switch to mission only if available */
-							if (_mission.current_mission_available()) {
-								dispatch(EVENT_MISSION_REQUESTED);
-
-							} else {
-								dispatch(EVENT_LOITER_REQUESTED);
-							}
-
+							request_mission_if_available();
 							stick_mode = true;
 						}
 
 						if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
 							/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
-							dispatch(EVENT_LOITER_REQUESTED);
+							request_mission_if_available();
 							stick_mode = true;
 						}
 					}
@@ -733,17 +732,11 @@ Navigator::task_main()
 							break;
 
 						case NAV_STATE_LOITER:
-							dispatch(EVENT_LOITER_REQUESTED);
+							request_loiter_or_ready();
 							break;
 
 						case NAV_STATE_MISSION:
-							if (_mission.current_mission_available()) {
-								dispatch(EVENT_MISSION_REQUESTED);
-
-							} else {
-								dispatch(EVENT_LOITER_REQUESTED);
-							}
-
+							request_mission_if_available();
 							break;
 
 						case NAV_STATE_RTL:
@@ -770,12 +763,7 @@ Navigator::task_main()
 					} else {
 						/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
 						if (myState == NAV_STATE_NONE) {
-							if (_mission.current_mission_available()) {
-								dispatch(EVENT_MISSION_REQUESTED);
-
-							} else {
-								dispatch(EVENT_LOITER_REQUESTED);
-							}
+							request_mission_if_available();
 						}
 					}
 				}
@@ -1071,7 +1059,7 @@ Navigator::start_loiter()
 		float min_alt_amsl = _parameters.min_altitude + _home_pos.alt;
 
 		/* use current altitude if above min altitude set by parameter */
-		if (_global_pos.alt < min_alt_amsl) {
+		if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) {
 			_pos_sp_triplet.current.alt = min_alt_amsl;
 			mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
 
@@ -1080,9 +1068,8 @@ Navigator::start_loiter()
 			mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
 		}
 
-		_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
 	}
-
+	_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
 	_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
 	_pos_sp_triplet.current.loiter_direction = 1;
 	_pos_sp_triplet.previous.valid = false;
@@ -1406,6 +1393,28 @@ Navigator::set_rtl_item()
 }
 
 void
+Navigator::request_loiter_or_ready()
+{
+	if (_vstatus.condition_landed) {
+		dispatch(EVENT_READY_REQUESTED);
+
+	} else {
+		dispatch(EVENT_LOITER_REQUESTED);
+	}
+}
+
+void
+Navigator::request_mission_if_available()
+{
+	if (_mission.current_mission_available()) {
+		dispatch(EVENT_MISSION_REQUESTED);
+
+	} else {
+		request_loiter_or_ready();
+	}
+}
+
+void
 Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item)
 {
 	sp->valid = true;
@@ -1574,13 +1583,7 @@ Navigator::on_mission_item_reached()
 			/* loiter at last waypoint */
 			_reset_loiter_pos = false;
 			mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
-
-			if (_vstatus.condition_landed) {
-				dispatch(EVENT_READY_REQUESTED);
-
-			} else {
-				dispatch(EVENT_LOITER_REQUESTED);
-			}
+			request_loiter_or_ready();
 		}
 
 	} else if (myState == NAV_STATE_RTL) {
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 1ba159a8e..9ef359c6d 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -50,14 +50,91 @@
 
 /*
  * Navigator parameters, accessible via MAVLink
- *
  */
 
+/**
+ * Minimum altitude (fixed wing only)
+ *
+ * Minimum altitude above home for LOITER.
+ *
+ * @unit meters
+ * @group Navigation
+ */
 PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
+
+/**
+ * Waypoint acceptance radius
+ *
+ * Default value of acceptance radius (if not specified in mission item).
+ *
+ * @unit meters
+ * @min 0.0
+ * @group Navigation
+ */
 PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
+
+/**
+ * Loiter radius (fixed wing only)
+ *
+ * Default value of loiter radius (if not specified in mission item).
+ *
+ * @unit meters
+ * @min 0.0
+ * @group Navigation
+ */
 PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
+
+/**
+ * Enable onboard mission
+ *
+ * @group Navigation
+ */
 PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
-PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);	// default TAKEOFF altitude
-PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);		// slow descend from this altitude when landing
-PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);		// min altitude for going home in RTL mode
-PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);	// delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT
+
+/**
+ * Take-off altitude
+ *
+ * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint.
+ *
+ * @unit meters
+ * @group Navigation
+ */
+PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);
+
+/**
+ * Landing altitude
+ *
+ * Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed.
+ *
+ * @unit meters
+ * @group Navigation
+ */
+PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);
+
+/**
+ * Return-To-Launch altitude
+ *
+ * Minimum altitude above home position for going home in RTL mode.
+ *
+ * @unit meters
+ * @group Navigation
+ */
+PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);
+
+/**
+ * Return-To-Launch delay
+ *
+ * Delay after descend before landing in RTL mode.
+ * If set to -1 the system will not land but loiter at NAV_LAND_ALT.
+ *
+ * @unit seconds
+ * @group Navigation
+ */
+PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);
+
+/**
+ * Enable parachute deployment
+ *
+ * @group Navigation
+ */
+PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0);