weathervane: get rid of passive strategy

Signed-off-by: Roman <bapstroman@gmail.com>

msg/vehicle_attitude_setpoint.msg

@@ -19,7 +19,6 @@ bool pitch_reset_integral	# Reset pitch integral part (navigation logic change)
 bool yaw_reset_integral	# Reset yaw integral part (navigation logic change)
 
 bool fw_control_yaw		# control heading with rudder (used for auto takeoff on runway)
-bool disable_mc_yaw_control	# control yaw for mc (used for vtol weather-vane mode)
 
 uint8 apply_flaps       	# flap config specifier
 uint8 FLAPS_OFF = 0     	# no flaps

src/modules/mc_att_control/mc_att_control.hpp

@@ -234,9 +234,7 @@ private:
 
 		(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _board_offset_x,
 		(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _board_offset_y,
-		(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _board_offset_z,
-
-		(ParamFloat<px4::params::VT_WV_YAWR_SCL>) _vtol_wv_yaw_rate_scale		/**< Scale value [0, 1] for yaw rate setpoint  */
+		(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _board_offset_z
 	)
 
 	matrix::Vector3f _attitude_p;		/**< P gain for attitude control */

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -450,18 +450,6 @@ MulticopterAttitudeControl::control_attitude(float dt)
 			_rates_sp(i) = math::constrain(_rates_sp(i), -_mc_rate_max(i), _mc_rate_max(i));
 		}
 	}
-
-	/* VTOL weather-vane mode, dampen yaw rate */
-	if (_vehicle_status.is_vtol && _v_att_sp.disable_mc_yaw_control) {
-		if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) {
-
-			const float wv_yaw_rate_max = _auto_rate_max(2) * _vtol_wv_yaw_rate_scale.get();
-			_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;
-		}
-	}
 }
 
 /*

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -753,7 +753,6 @@ MulticopterPositionControl::task_main()
 			_att_sp = ControlMath::thrustToAttitude(thr_sp, _control.getYawSetpoint());
 			_att_sp.yaw_sp_move_rate = _control.getYawspeedSetpoint();
 			_att_sp.fw_control_yaw = false;
-			_att_sp.disable_mc_yaw_control = false;
 			_att_sp.apply_flaps = false;
 
 			if (!constraints.landing_gear) {
@@ -779,7 +778,6 @@ MulticopterPositionControl::task_main()
 			_att_sp.yaw_body = _local_pos.yaw;
 			_att_sp.yaw_sp_move_rate = 0.0f;
 			_att_sp.fw_control_yaw = false;
-			_att_sp.disable_mc_yaw_control = false;
 			_att_sp.apply_flaps = false;
 			matrix::Quatf q_sp = matrix::Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
 			q_sp.copyTo(_att_sp.q_d);

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -87,11 +87,6 @@ VtolAttitudeControl::VtolAttitudeControl()
 	_params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE");
 	_params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID");
 
-
-	_params_handles.wv_takeoff = param_find("VT_WV_TKO_EN");
-	_params_handles.wv_land = param_find("VT_WV_LND_EN");
-	_params_handles.wv_loiter = param_find("VT_WV_LTR_EN");
-
 	/* fetch initial parameter values */
 	parameters_update();
 
@@ -479,16 +474,6 @@ VtolAttitudeControl::parameters_update()
 	_params.front_trans_time_min = math::min(_params.front_trans_time_openloop * 0.9f,
 				       _params.front_trans_time_min);
 
-	/* weathervane */
-	param_get(_params_handles.wv_takeoff, &l);
-	_params.wv_takeoff = (l == 1);
-
-	param_get(_params_handles.wv_loiter, &l);
-	_params.wv_loiter = (l == 1);
-
-	param_get(_params_handles.wv_land, &l);
-	_params.wv_land = (l == 1);
-
 
 	param_get(_params_handles.front_trans_duration, &_params.front_trans_duration);
 	param_get(_params_handles.back_trans_duration, &_params.back_trans_duration);

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -189,9 +189,6 @@ private:
 		param_t fw_qc_max_roll;
 		param_t front_trans_time_openloop;
 		param_t front_trans_time_min;
-		param_t wv_takeoff;
-		param_t wv_loiter;
-		param_t wv_land;
 		param_t front_trans_duration;
 		param_t back_trans_duration;
 		param_t transition_airspeed;

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -279,44 +279,6 @@ PARAM_DEFINE_INT32(VT_FW_QC_R, 0);
  */
 PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f);
 
-/**
- * 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.0
- * @max 1.0
- * @increment 0.01
- * @decimal 3
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_FLOAT(VT_WV_YAWR_SCL, 0.15f);
-
-/**
- * Enable weather-vane mode takeoff for missions
- *
- * @boolean
- * @group Mission
- */
-PARAM_DEFINE_INT32(VT_WV_TKO_EN, 0);
-
-/**
- * Weather-vane mode for loiter
- *
- * @boolean
- * @group Mission
- */
-PARAM_DEFINE_INT32(VT_WV_LTR_EN, 0);
-
-/**
- * Weather-vane mode landings for missions
- *
- * @boolean
- * @group Mission
- */
-PARAM_DEFINE_INT32(VT_WV_LND_EN, 0);
-
 /**
  * The channel number of motors that must be turned off in fixed wing mode.
  *

src/modules/vtol_att_control/vtol_type.cpp

@@ -124,24 +124,6 @@ void VtolType::update_mc_state()
 	_mc_roll_weight = 1.0f;
 	_mc_pitch_weight = 1.0f;
 	_mc_yaw_weight = 1.0f;
-
-	// VTOL weathervane
-	_v_att_sp->disable_mc_yaw_control = false;
-
-	if (_attc->get_pos_sp_triplet()->current.valid &&
-	    !_v_control_mode->flag_control_manual_enabled) {
-
-		if (_params->wv_takeoff && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
-			_v_att_sp->disable_mc_yaw_control = true;
-
-		} else if (_params->wv_loiter
-			   && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
-			_v_att_sp->disable_mc_yaw_control = true;
-
-		} else if (_params->wv_land && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
-			_v_att_sp->disable_mc_yaw_control = true;
-		}
-	}
 }
 
 void VtolType::update_fw_state()

src/modules/vtol_att_control/vtol_type.h

@@ -58,9 +58,6 @@ struct Params {
 	float fw_qc_max_roll;		// maximum roll angle FW mode (QuadChute)
 	float front_trans_time_openloop;
 	float front_trans_time_min;
-	bool wv_takeoff;
-	bool wv_loiter;
-	bool wv_land;
 	float front_trans_duration;
 	float back_trans_duration;
 	float transition_airspeed;