Drone yaw set to destination waypoint yaw in LOITER mode if MIS_YAWMODE=0 (#7269)

* Drone does not change orientation while in LOITER mode if MIS_YAWMODE=0

* Fix code style

* Some more code style fixes

* Format checks passed

src/modules/navigator/loiter.cpp

@@ -59,6 +59,7 @@
 Loiter::Loiter(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
 	_param_min_alt(this, "MIS_LTRMIN_ALT", false),
+	_param_yawmode(this, "MIS_YAWMODE", false),
 	_loiter_pos_set(false)
 {
 	// load initial params
@@ -159,19 +160,22 @@ Loiter::reposition()
 		memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current));
 		pos_sp_triplet->next.valid = false;
 
-		// set yaw
-
-		float travel_dist = get_distance_to_next_waypoint(_navigator->get_global_position()->lat,
-				    _navigator->get_global_position()->lon,
-				    pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
-
-		if (travel_dist > 1.0f) {
-			// calculate direction the vehicle should point to.
-			pos_sp_triplet->current.yaw = get_bearing_to_next_waypoint(
-							      _navigator->get_global_position()->lat,
-							      _navigator->get_global_position()->lon,
-							      pos_sp_triplet->current.lat,
-							      pos_sp_triplet->current.lon);
+		// set yaw (depends on the value of parameter MIS_YAWMODE):
+		// MISSION_YAWMODE_NONE: do not change yaw setpoint
+		// MISSION_YAWMODE_FRONT_TO_WAYPOINT: point to next waypoint
+		if (_param_yawmode.get() != MISSION_YAWMODE_NONE) {
+			float travel_dist = get_distance_to_next_waypoint(_navigator->get_global_position()->lat,
+					    _navigator->get_global_position()->lon,
+					    pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
+
+			if (travel_dist > 1.0f) {
+				// calculate direction the vehicle should point to.
+				pos_sp_triplet->current.yaw = get_bearing_to_next_waypoint(
+								      _navigator->get_global_position()->lat,
+								      _navigator->get_global_position()->lon,
+								      pos_sp_triplet->current.lat,
+								      pos_sp_triplet->current.lon);
+			}
 		}
 
 		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);

src/modules/navigator/loiter.h

@@ -60,6 +60,14 @@ public:
 
 	virtual void on_active();
 
+	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,
+		MISSION_YAWMODE_MAX = 4
+	};
+
 private:
 	/**
 	 * Use the stored reposition location of the navigator
@@ -73,6 +81,7 @@ private:
 	void set_loiter_position();
 
 	control::BlockParamFloat _param_min_alt;
+	control::BlockParamInt _param_yawmode;
 	bool _loiter_pos_set;
 };