From d4a19163bb49b20612643b4fd531cf10e0496892 Mon Sep 17 00:00:00 2001
From: Eddy <scott.edward@aurora.aero>
Date: Wed, 21 Oct 2015 17:49:50 -0400
Subject: [PATCH] Added switching from attitude control generated rate
setpoints to manual rate setpoints when in rattitude mode
---
.../mc_att_control/mc_att_control_main.cpp | 77 +++++++++++--------
.../mc_att_control/mc_att_control_params.c | 19 ++++-
2 files changed, 59 insertions(+), 37 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 830f48dff8..8eb573acc7 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -190,12 +190,10 @@ private:
param_t pitch_rate_max;
param_t yaw_rate_max;
- param_t man_roll_max;
- param_t man_pitch_max;
- param_t man_yaw_max;
param_t acro_roll_max;
param_t acro_pitch_max;
param_t acro_yaw_max;
+ param_t rattitude_thres;
} _params_handles; /**< handles for interesting parameters */
@@ -212,11 +210,8 @@ private:
float yaw_rate_max;
math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */
- float man_roll_max;
- float man_pitch_max;
- float man_yaw_max;
math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
-
+ float rattitude_thres;
} _params;
/**
@@ -346,11 +341,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params.roll_rate_max = 0.0f;
_params.pitch_rate_max = 0.0f;
_params.yaw_rate_max = 0.0f;
- _params.man_roll_max = 0.0f;
- _params.man_pitch_max = 0.0f;
- _params.man_yaw_max = 0.0f;
_params.mc_rate_max.zero();
_params.acro_rate_max.zero();
+ _params.rattitude_thres = 1.0f;
_rates_prev.zero();
_rates_sp.zero();
@@ -380,12 +373,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX");
_params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX");
_params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX");
- _params_handles.man_roll_max = param_find("MC_MAN_R_MAX");
- _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX");
- _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX");
_params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX");
_params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX");
- _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
+ _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
+ _params_handles.rattitude_thres = param_find("MC_RATT_TH");
/* fetch initial parameter values */
parameters_update();
@@ -466,14 +457,6 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.mc_rate_max(2) = math::radians(_params.yaw_rate_max);
- /* manual attitude control scale */
- param_get(_params_handles.man_roll_max, &_params.man_roll_max);
- param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
- param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
- _params.man_roll_max = math::radians(_params.man_roll_max);
- _params.man_pitch_max = math::radians(_params.man_pitch_max);
- _params.man_yaw_max = math::radians(_params.man_yaw_max);
-
/* manual rate control scale and auto mode roll/pitch rate limits */
param_get(_params_handles.acro_roll_max, &v);
_params.acro_rate_max(0) = math::radians(v);
@@ -482,6 +465,9 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.acro_yaw_max, &v);
_params.acro_rate_max(2) = math::radians(v);
+ /* stick deflection needed in rattitude mode to control rates not angles */
+ param_get(_params_handles.rattitude_thres, &_params.rattitude_thres);
+
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
return OK;
@@ -810,22 +796,45 @@ MulticopterAttitudeControl::task_main()
vehicle_motor_limits_poll();
if (_v_control_mode.flag_control_attitude_enabled) {
+
control_attitude(dt);
- /* publish attitude rates setpoint */
- _v_rates_sp.roll = _rates_sp(0);
- _v_rates_sp.pitch = _rates_sp(1);
- _v_rates_sp.yaw = _rates_sp(2);
- _v_rates_sp.thrust = _thrust_sp;
- _v_rates_sp.timestamp = hrt_absolute_time();
+ /* Check if we want to directly pass through manual rate setpoints*/
+ if(_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE){
+ /* Calculate the manual rates for all channels */
+ math::Vector<3> man_rates = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max);
+
+ /* Check all channels and replace attitude controler rate setpoint if manual input greater than threshold*/
+ _v_rates_sp.roll = (fabsf(_manual_control_sp.y) > _params.rattitude_thres) ? man_rates(0) : _rates_sp(0);
+ _v_rates_sp.pitch = (fabsf(_manual_control_sp.x) > _params.rattitude_thres) ? man_rates(1) : _rates_sp(1);
+ _v_rates_sp.yaw = (fabsf(_manual_control_sp.r) > _params.rattitude_thres) ? man_rates(2) : _rates_sp(2);
+ _v_rates_sp.thrust = _thrust_sp;
+ _v_rates_sp.timestamp = hrt_absolute_time();
- if (_v_rates_sp_pub != nullptr) {
- orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
+ /* publish attitude rates setpoint */
+ if (_v_rates_sp_pub != nullptr) {
+ orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
+
+ } else if (_rates_sp_id) {
+ _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
+ }
+ }else{
- } else if (_rates_sp_id) {
- _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
- }
+ /* publish attitude rates setpoint */
+ _v_rates_sp.roll = _rates_sp(0);
+ _v_rates_sp.pitch = _rates_sp(1);
+ _v_rates_sp.yaw = _rates_sp(2);
+ _v_rates_sp.thrust = _thrust_sp;
+ _v_rates_sp.timestamp = hrt_absolute_time();
+
+ if (_v_rates_sp_pub != nullptr) {
+ orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
+
+ } else if (_rates_sp_id) {
+ _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
+ }
+ }
} else {
/* attitude controller disabled, poll rates setpoint topic */
if (_v_control_mode.flag_control_manual_enabled) {
@@ -976,4 +985,4 @@ int mc_att_control_main(int argc, char *argv[])
warnx("unrecognized command");
return 1;
-}
+}
\ No newline at end of file
diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c
index 8d9fb6b911..a2a69d7116 100644
--- a/src/modules/mc_att_control/mc_att_control_params.c
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -251,7 +251,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 60.0f);
* @max 360.0
* @group Multicopter Attitude Control
*/
-PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
+PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 360.0f);
/**
* Max acro pitch rate
@@ -261,7 +261,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
* @max 360.0
* @group Multicopter Attitude Control
*/
-PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
+PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 360.0f);
/**
* Max acro yaw rate
@@ -270,4 +270,17 @@ PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
* @min 0.0
* @group Multicopter Attitude Control
*/
-PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f);
+PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 360.0f);
+
+/**
+ * Threshold for Rattitude mode
+ *
+ * Manual input needed in order to override attitude control rate setpoints
+ * and instead pass manual stick inputs as rate setpoints
+ *
+ * @unit
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Attitude Control
+ */
+ PARAM_DEFINE_FLOAT(MC_RATT_TH, 1.0f);
--
GitLab