implemented vtol weathervane yaw control for landing and loiter mission item

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -201,6 +201,7 @@ private:
 		param_t roll_tc;
 		param_t pitch_tc;
 		param_t vtol_opt_recovery_enabled;
+		param_t vtol_wv_yaw_rate_scale;
 
 	}		_params_handles;		/**< handles for interesting parameters */
 
@@ -222,6 +223,7 @@ private:
 		float rattitude_thres;
 		int vtol_type;						/**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */
 		bool vtol_opt_recovery_enabled;
+		float vtol_wv_yaw_rate_scale;			/**< Scale value [0, 1] for yaw rate setpoint  */
 	}		_params;
 
 	TailsitterRecovery *_ts_opt_recovery;	/**< Computes optimal rates for tailsitter recovery */
@@ -353,6 +355,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params.acro_rate_max.zero();
 	_params.rattitude_thres = 1.0f;
 	_params.vtol_opt_recovery_enabled = false;
+	_params.vtol_wv_yaw_rate_scale = 1.0f;
+
 
 	_rates_prev.zero();
 	_rates_sp.zero();
@@ -390,7 +394,11 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.vtol_type 		= 	param_find("VT_TYPE");
 	_params_handles.roll_tc			= 	param_find("MC_ROLL_TC");
 	_params_handles.pitch_tc		= 	param_find("MC_PITCH_TC");
-	_params_handles.vtol_opt_recovery_enabled = param_find("VT_OPT_RECOV_EN");
+	_params_handles.vtol_opt_recovery_enabled	= param_find("VT_OPT_RECOV_EN");
+	_params_handles.vtol_wv_yaw_rate_scale		= param_find("VT_WV_YAWR_SCL");
+
+
+
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -511,6 +519,8 @@ MulticopterAttitudeControl::parameters_update()
 	param_get(_params_handles.vtol_opt_recovery_enabled, &tmp);
 	_params.vtol_opt_recovery_enabled = (bool)tmp;
 
+	param_get(_params_handles.vtol_wv_yaw_rate_scale, &_params.vtol_wv_yaw_rate_scale);
+
 	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
 
 	return OK;
@@ -724,6 +734,14 @@ MulticopterAttitudeControl::control_attitude(float dt)
 		}
 	}
 
+	/* weather-vane mode, dampen yaw rate */
+	if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
+		float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
+		_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
+		// prevent integrator winding up in weathervane mode
+		_rates_int(2) = 0.0f;
+	}
+
 	/* feed forward yaw setpoint rate */
 	_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
 }

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -192,6 +192,7 @@ private:
 		param_t hold_max_xy;
 		param_t hold_max_z;
 		param_t acc_hor_max;
+
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	struct {
@@ -442,7 +443,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params_handles.hold_max_z = param_find("MPC_HOLD_MAX_Z");
 	_params_handles.acc_hor_max = param_find("MPC_ACC_HOR_MAX");
 
-
 	/* fetch initial parameter values */
 	parameters_update(true);
 }
@@ -1263,6 +1263,14 @@ MulticopterPositionControl::task_main()
 				control_auto(dt);
 			}
 
+			/* weather-vane mode for vtol: disable yaw control */
+			if(_mode_auto && _pos_sp_triplet.current.disable_mc_yaw_control == true) {
+				_att_sp.disable_mc_yaw_control = true;
+			} else {
+				/* reset in case of setpoint updates */
+				_att_sp.disable_mc_yaw_control = false;
+			}
+
 			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
 			    && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 				/* idle state, don't run controller and set zero thrust */

src/modules/navigator/mission_block.cpp

@@ -70,7 +70,9 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
 	_action_start(0),
 	_actuators{},
 	_actuator_pub(nullptr),
-	_cmd_pub(nullptr)
+	_cmd_pub(nullptr),
+	_param_vtol_wv_land(this, "VT_WV_LND_EN", false),
+	_param_vtol_wv_loiter(this, "VT_WV_LTR_EN", false)
 {
 }
 
@@ -175,6 +177,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? */
@@ -310,6 +313,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	sp->loiter_direction = item->loiter_direction;
 	sp->pitch_min = item->pitch_min;
 	sp->acceptance_radius = item->acceptance_radius;
+	sp->disable_mc_yaw_control = false;
 
 	switch (item->nav_cmd) {
 	case NAV_CMD_IDLE:
@@ -322,12 +326,18 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 
 	case NAV_CMD_LAND:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
+		if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()){
+			sp->disable_mc_yaw_control = true;
+		}
 		break;
 
 	case NAV_CMD_LOITER_TIME_LIMIT:
 	case NAV_CMD_LOITER_TURN_COUNT:
 	case NAV_CMD_LOITER_UNLIMITED:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
+		if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()){
+			sp->disable_mc_yaw_control = true;
+		}
 		break;
 
 	default:

src/modules/navigator/mission_block.h

@@ -45,6 +45,9 @@
 
 #include <navigator/navigation.h>
 
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
 #include <uORB/topics/mission.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
@@ -129,6 +132,8 @@ protected:
 	actuator_controls_s _actuators;
 	orb_advert_t    _actuator_pub;
 	orb_advert_t	_cmd_pub;
+	control::BlockParamInt _param_vtol_wv_land;
+	control::BlockParamInt _param_vtol_wv_loiter;
 };
 
 #endif

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -213,6 +213,36 @@ PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f);
  */
 PARAM_DEFINE_INT32(VT_OPT_RECOV_EN, 0);
 
+/**
+ * Enable weather-vane mode landings for missions
+ *
+ * @min 0
+ * @max 1
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_WV_LND_EN, 0);
+
+/**
+ * Weather-vane yaw rate scale.
+ *
+ * The desired yawrate from the controller will be scaled in order to avoid
+ * yaw fighting against the wind.
+ *
+ * @min 0
+ * @max 1
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_WV_YAWR_SCL, 0.15f);
+
+/**
+ * Enable weather-vane mode for loiter
+ *
+ * @min 0
+ * @max 1
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_WV_LTR_EN, 0);
+
 /**
  * Front transition timeout
  *