From 96f3feb08853aef5eecf59cbbd653e926b1bfb0a Mon Sep 17 00:00:00 2001
From: Roman <bapstroman@gmail.com>
Date: Tue, 18 Sep 2018 20:06:55 +0200
Subject: [PATCH] weathervane: get rid of passive strategy
Signed-off-by: Roman <bapstroman@gmail.com>
---
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<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 */
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;
--
GitLab