From 96f3feb08853aef5eecf59cbbd653e926b1bfb0a Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 18 Sep 2018 20:06:55 +0200 Subject: [PATCH] weathervane: get rid of passive strategy Signed-off-by: Roman --- msg/vehicle_attitude_setpoint.msg | 1 - src/modules/mc_att_control/mc_att_control.hpp | 4 +- .../mc_att_control/mc_att_control_main.cpp | 12 ------ .../mc_pos_control/mc_pos_control_main.cpp | 2 - .../vtol_att_control_main.cpp | 15 -------- .../vtol_att_control/vtol_att_control_main.h | 3 -- .../vtol_att_control_params.c | 38 ------------------- src/modules/vtol_att_control/vtol_type.cpp | 18 --------- src/modules/vtol_att_control/vtol_type.h | 3 -- 9 files changed, 1 insertion(+), 95 deletions(-) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 9a7014a938..b3b27874af 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/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 diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 50a3b1b3ae..343410ab8e 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -234,9 +234,7 @@ private: (ParamFloat) _board_offset_x, (ParamFloat) _board_offset_y, - (ParamFloat) _board_offset_z, - - (ParamFloat) _vtol_wv_yaw_rate_scale /**< Scale value [0, 1] for yaw rate setpoint */ + (ParamFloat) _board_offset_z ) matrix::Vector3f _attitude_p; /**< P gain for attitude control */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 16479dbf83..3c1d058965 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/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; - } - } } /* diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index b1c0b5a4b8..c633d574b9 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/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); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index e0ae2caf77..a6e673b50a 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/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); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 917e67ff81..4429886664 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/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; diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 495c7e9f6e..f27da247d4 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/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. * diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 46cd1b96f2..2700dc4df0 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/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() diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 6bed95a002..1a10d4c108 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/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;