FOH mode for altitude

This introduces a parameter for the navigator. When enabled the navigator
publishes a first order old (FOH) type altitude setpoint instead of the
default zero order hold. For takeoff and landing the FOH mode is not
active. The FOH altitude is calculated such that the sp reaches the
altitude of the waypoint when the system is at a horizontal distance
equal to the acceptance radius. Also the altitude setpoint will only
converge towards the waypoint altitude but never diverge.

3 files changed, 121 insertions, 3 deletions

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index c0e37a3ed..7000dd6c0 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -36,12 +36,14 @@
  * Helper class to access missions
  *
  * @author Julian Oes <julian@oes.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <sys/types.h>
 #include <string.h>
 #include <stdlib.h>
 #include <unistd.h>
+#include <float.h>
 
 #include <drivers/drv_hrt.h>
 
@@ -49,6 +51,7 @@
 #include <mavlink/mavlink_log.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
+#include <lib/mathlib/mathlib.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
@@ -62,6 +65,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_param_onboard_enabled(this, "ONBOARD_EN"),
 	_param_takeoff_alt(this, "TAKEOFF_ALT"),
 	_param_dist_1wp(this, "DIST_1WP"),
+	_param_altmode(this, "ALTMODE"),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
 	_current_onboard_mission_index(-1),
@@ -72,7 +76,11 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_mission_result({0}),
 	_mission_type(MISSION_TYPE_NONE),
 	_inited(false),
-	_dist_1wp_ok(false)
+	_dist_1wp_ok(false),
+	_missionFeasiblityChecker(),
+	_min_current_sp_distance_xy(FLT_MAX),
+	_mission_item_previous_alt(NAN),
+	_distance_current_previous(0.0f)
 {
 	/* load initial params */
 	updateParams();
@@ -144,6 +152,8 @@ Mission::on_active()
 		advance_mission();
 		set_mission_items();
 
+	} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
+		altitude_sp_foh_update();
 	} else {
 		/* if waypoint position reached allow loiter on the setpoint */
 		if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
@@ -202,7 +212,7 @@ Mission::update_offboard_mission()
 		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
 		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 
-		missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
+		_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
 				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
 				_navigator->get_home_position()->alt);
 
@@ -313,11 +323,19 @@ Mission::set_mission_items()
 	/* make sure param is up to date */
 	updateParams();
 
+	/* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */
+	altitude_sp_foh_reset();
+
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
 	/* set previous position setpoint to current */
 	set_previous_pos_setpoint();
 
+	/* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */
+	if (pos_sp_triplet->previous.valid) {
+		_mission_item_previous_alt = _mission_item.altitude;
+	}
+
 	/* get home distance state */
 	bool home_dist_ok = check_dist_1wp();
 	/* the home dist check provides user feedback, so we initialize it to this */
@@ -446,9 +464,73 @@ Mission::set_mission_items()
 		}
 	}
 
+	/* Save the distance between the current sp and the previous one */
+	if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
+		_distance_current_previous = get_distance_to_next_waypoint(pos_sp_triplet->current.lat,
+				pos_sp_triplet->current.lon,
+				pos_sp_triplet->previous.lat,
+				pos_sp_triplet->previous.lon);
+	}
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+Mission::altitude_sp_foh_update()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	/* 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)) {
+		return;
+	}
+
+	/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
+	if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) {
+		return;
+	}
+
+	/* Don't do FOH for landing and takeoff waypoints, the ground may be near
+	 * and the FW controller has a custom landing logic */
+	if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
+		return;
+	}
+
+
+	/* Calculate distance to current waypoint */
+	float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
+			_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
+
+	/* Save distance to waypoint if it is the smallest ever achieved */
+	_min_current_sp_distance_xy = math::min(d_current, _min_current_sp_distance_xy);
+
+	/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
+	 * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
+	if (_min_current_sp_distance_xy < _mission_item.acceptance_radius) {
+		pos_sp_triplet->current.alt = _mission_item.altitude;
+	} else {
+		/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
+		* of the mission item as it is used to check if the mission item is reached
+		* The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
+		* radius around the current waypoint
+		**/
+		float delta_alt = (_mission_item.altitude - _mission_item_previous_alt);
+		float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius);
+		float a = _mission_item_previous_alt - grad * _distance_current_previous;
+		pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
+
+	}
+
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
+void
+Mission::altitude_sp_foh_reset()
+{
+	_min_current_sp_distance_xy = FLT_MAX;
+}
+
 bool
 Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item)
 {
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 4da6a1155..f7d12da70 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -36,6 +36,7 @@
  * Navigator mode to access missions
  *
  * @author Julian Oes <julian@oes.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #ifndef NAVIGATOR_MISSION_H
@@ -75,6 +76,11 @@ public:
 
 	virtual void on_active();
 
+	enum mission_altitude_mode {
+		MISSION_ALTMODE_ZOH = 0,
+		MISSION_ALTMODE_FOH =1
+	};
+
 private:
 	/**
 	 * Update onboard mission topic
@@ -103,6 +109,16 @@ private:
 	void set_mission_items();
 
 	/**
+	 * Updates the altitude sp to follow a foh
+	 */
+	void altitude_sp_foh_update();
+
+	/**
+	 * Resets the altitude sp foh logic
+	 */
+	void altitude_sp_foh_reset();
+
+	/**
 	 * Read current or next mission item from the dataman and watch out for DO_JUMPS
 	 * @return true if successful
 	 */
@@ -136,6 +152,7 @@ private:
 	control::BlockParamInt _param_onboard_enabled;
 	control::BlockParamFloat _param_takeoff_alt;
 	control::BlockParamFloat _param_dist_1wp;
+	control::BlockParamInt _param_altmode;
 
 	struct mission_s _onboard_mission;
 	struct mission_s _offboard_mission;
@@ -157,7 +174,13 @@ private:
 	bool _inited;
 	bool _dist_1wp_ok;
 
-	MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
+	MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */
+
+	float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint  */
+	float _mission_item_previous_alt; /**< holds the altitude of the previous mission item,
+					    can be replaced by a full copy of the previous mission item if needed*/
+	float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet,
+					    only use if current and previous are valid */
 };
 
 #endif
diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
index d15e1e771..04c01fe51 100644
--- a/src/modules/navigator/mission_params.c
+++ b/src/modules/navigator/mission_params.c
@@ -82,3 +82,16 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
+
+/**
+ * Altitude setpoint mode
+ *
+ * 0: the system will follow a zero order hold altitude setpoint
+ * 1: the system will follow a first order hold altitude setpoint
+ * values follow the definition in enum mission_altitude_mode
+ *
+ * @min 0
+ * @max 1
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(MIS_ALTMODE, 0);