Merge pull request #1349 from PX4/fohaltitude

FOH mode for altitude (optional, enable with parameter)

3 files changed, 123 insertions, 3 deletions

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index c0e37a3ed..b33b2049f 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,75 @@ 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, however make sure that
+	 * _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */
+	_min_current_sp_distance_xy = math::min(math::min(d_current, _min_current_sp_distance_xy),
+			_distance_current_previous);
+
+	/* 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..a91c89968 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);