From 80f8fcbdf622663040a4916bb35a3977561a2cc2 Mon Sep 17 00:00:00 2001
From: Roman <bapstr@ethz.ch>
Date: Fri, 12 Feb 2016 12:36:08 +0100
Subject: [PATCH] implemented vtol weathervane yaw control for landing and
loiter mission item
---
.../mc_att_control/mc_att_control_main.cpp | 20 ++++++++++++-
.../mc_pos_control/mc_pos_control_main.cpp | 10 ++++++-
src/modules/navigator/mission_block.cpp | 12 +++++++-
src/modules/navigator/mission_block.h | 5 ++++
.../vtol_att_control_params.c | 30 +++++++++++++++++++
5 files changed, 74 insertions(+), 3 deletions(-)
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 6a45167c85..9950e6ef91 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/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;
}
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 d4ab75a545..1e2f3600e4 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/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 */
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index d1d80c541f..cdfb2e96c8 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/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:
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index 3560e6967c..dac3b9b712 100644
--- a/src/modules/navigator/mission_block.h
+++ b/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
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 db5d8d051d..df9de15b31 100644
--- a/src/modules/vtol_att_control/vtol_att_control_params.c
+++ b/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
*
--
GitLab