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.

1 files changed, 84 insertions, 2 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)
 {