Add yaw modes that define multirotor heading behaviour during missions.

src/modules/navigator/mission.cpp

@@ -39,6 +39,7 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  * @author Anton Babushkin <anton.babushkin@me.com>
  * @author Ban Siesta <bansiesta@gmail.com>
+ * @author Simon Wilks <simon@uaventure.com>
  */
 
 #include <sys/types.h>
@@ -68,6 +69,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false),
 	_param_dist_1wp(this, "MIS_DIST_1WP", false),
 	_param_altmode(this, "MIS_ALTMODE", false),
+	_param_yawmode(this, "MIS_YAWMODE", false),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
 	_current_onboard_mission_index(-1),
@@ -80,6 +82,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_missionFeasiblityChecker(),
 	_min_current_sp_distance_xy(FLT_MAX),
 	_mission_item_previous_alt(NAN),
+  	_on_arrival_yaw(NAN),
 	_distance_current_previous(0.0f)
 {
 	/* load initial params */
@@ -166,6 +169,13 @@ Mission::on_active()
 			_navigator->set_can_loiter_at_sp(true);
 		}
 	}
+
+	/* see if we need to update the current yaw heading for rotary wing types */
+	if (_navigator->get_vstatus()->is_rotary_wing 
+			&& _param_yawmode.get() != MISSION_YAWMODE_NONE
+			&& _mission_type != MISSION_TYPE_NONE) {
+		heading_sp_update();
+	}
 }
 
 void
@@ -275,7 +285,7 @@ Mission::check_dist_1wp()
 						&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
 
 					/* check only items with valid lat/lon */
-					if (	mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
+					if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
 							mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
 							mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
 							mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
@@ -362,7 +372,6 @@ Mission::set_mission_items()
 			mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running");
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
-
 	} else {
 		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
@@ -396,6 +405,10 @@ Mission::set_mission_items()
 		return;
 	}
 
+	if (pos_sp_triplet->current.valid) {
+		_on_arrival_yaw = _mission_item.yaw;
+	}
+
 	/* do takeoff on first waypoint for rotary wing vehicles */
 	if (_navigator->get_vstatus()->is_rotary_wing) {
 		/* force takeoff if landed (additional protection) */
@@ -442,6 +455,7 @@ Mission::set_mission_items()
 			_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
 			_mission_item.lat = _navigator->get_global_position()->lat;
 			_mission_item.lon = _navigator->get_global_position()->lon;
+			_mission_item.yaw = NAN;
 			_mission_item.altitude = takeoff_alt;
 			_mission_item.altitude_is_relative = false;
 			_mission_item.autocontinue = true;
@@ -481,7 +495,6 @@ Mission::set_mission_items()
 		if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) {
 			/* got next mission item, update setpoint triplet */
 			mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next);
-
 		} else {
 			/* next mission item is not available */
 			pos_sp_triplet->next.valid = false;
@@ -503,6 +516,59 @@ Mission::set_mission_items()
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
+void
+Mission::heading_sp_update()
+{
+	if (_takeoff) {
+		/* we don't want to be yawing during takeoff */
+		return;
+	}
+
+	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(_on_arrival_yaw)) {
+		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;
+	}
+
+	/* set yaw angle for the waypoint iff a loiter time has been specified */
+	if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) {
+		_mission_item.yaw = _on_arrival_yaw;
+	/* always keep the front of the rotary wing pointing to the next waypoint */
+	} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) {
+		_mission_item.yaw = get_bearing_to_next_waypoint(
+		        _navigator->get_global_position()->lat,
+		        _navigator->get_global_position()->lon,
+		        _mission_item.lat,
+		        _mission_item.lon);
+	/* always keep the back of the rotary wing pointing towards home */
+	} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) {
+		_mission_item.yaw = get_bearing_to_next_waypoint(
+		        _navigator->get_global_position()->lat,
+		        _navigator->get_global_position()->lon,
+		        _navigator->get_home_position()->lat,
+		        _navigator->get_home_position()->lon);
+	/* always keep the back of the rotary wing pointing towards home */
+	} else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
+		_mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint(
+		        _navigator->get_global_position()->lat,
+		        _navigator->get_global_position()->lon,
+		        _navigator->get_home_position()->lat,
+		        _navigator->get_home_position()->lon) + M_PI_F);
+	}
+
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+
 void
 Mission::altitude_sp_foh_update()
 {

src/modules/navigator/mission.h

@@ -83,6 +83,13 @@ public:
 		MISSION_ALTMODE_FOH = 1
 	};
 
+	enum mission_yaw_mode {
+		MISSION_YAWMODE_NONE = 0,
+		MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1,
+		MISSION_YAWMODE_FRONT_TO_HOME = 2,
+		MISSION_YAWMODE_BACK_TO_HOME = 3
+	};
+
 private:
 	/**
 	 * Update onboard mission topic
@@ -110,6 +117,11 @@ private:
 	 */
 	void set_mission_items();
 
+	/**
+	 * Updates the heading of the vehicle. Rotary wings only.
+	 */
+	void heading_sp_update();
+
 	/**
 	 * Updates the altitude sp to follow a foh
 	 */
@@ -155,6 +167,7 @@ private:
 	control::BlockParamFloat _param_takeoff_alt;
 	control::BlockParamFloat _param_dist_1wp;
 	control::BlockParamInt _param_altmode;
+	control::BlockParamInt _param_yawmode;
 
 	struct mission_s _onboard_mission;
 	struct mission_s _offboard_mission;
@@ -177,7 +190,8 @@ private:
 
 	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*/
+					    can be replaced by a full copy of the previous mission item if needed */
+	float _on_arrival_yaw; /**< holds the yaw value that should be applied when the current waypoint is reached */
 	float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet,
 					    only use if current and previous are valid */
 };

src/modules/navigator/mission_block.cpp

@@ -134,6 +134,7 @@ MissionBlock::is_mission_item_reached()
 		}
 	}
 
+	/* Check if the waypoint and the requested yaw setpoint. */
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 
 		/* TODO: removed takeoff, why? */
@@ -151,7 +152,7 @@ MissionBlock::is_mission_item_reached()
 		}
 	}
 
-	/* check if the current waypoint was reached */
+	/* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */
 	if (_waypoint_position_reached && _waypoint_yaw_reached) {
 
 		if (_time_first_inside_orbit == 0) {

src/modules/navigator/mission_params.c

@@ -95,3 +95,19 @@ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
  * @group Mission
  */
 PARAM_DEFINE_INT32(MIS_ALTMODE, 0);
+
+/**
+ * Multirotor only. Yaw setpoint mode.
+ *
+ * 0: Set the yaw heading to the yaw value specified for the destination waypoint.
+ * 1: Maintain a yaw heading pointing towards the next waypoint.
+ * 2: Maintain a yaw heading that always points to the home location.
+ * 3: Maintain a yaw heading that always points away from the home location (ie: back always faces home).
+ *
+ * The values are defined in the enum mission_altitude_mode
+ *
+ * @min 0
+ * @max 3
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(MIS_YAWMODE, 0);