From f3cf2d309edf458552dc7a28c4bdef6c4af49689 Mon Sep 17 00:00:00 2001 From: Azarakhsh Keipour Date: Tue, 29 Sep 2020 14:01:24 -0400 Subject: [PATCH] Omni Att Ctrl: Manual mode no longer uses the attitude mode and acts like an underactuated UAV --- src/modules/mc_att_control/mc_att_control.hpp | 6 +--- .../mc_att_control/mc_att_control_main.cpp | 36 ------------------- 2 files changed, 1 insertion(+), 41 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 097921e538..701cfb356e 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -157,11 +157,7 @@ private: _param_mpc_thr_hover, /**< throttle at which vehicle is at hover equilibrium */ (ParamInt) _param_mpc_thr_curve, /**< throttle curve behavior */ - (ParamInt) _param_mc_airmode, - - /* Omnidirectional vehicle params */ - (ParamInt) _param_omni_att_mode, - (ParamFloat) _param_omni_dfc_max_thr + (ParamInt) _param_mc_airmode ) bool _is_tailsitter{false}; 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 3c1cc279d8..827118156b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -217,42 +217,6 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ attitude_setpoint.q_d_valid = true; attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z); - - /* modify roll/pitch if we're in omni-directional mode */ - if (_param_omni_att_mode.get() == 2) { - // set the euler angles, for logging only, must not be used for control - attitude_setpoint.roll_body = 0; - attitude_setpoint.pitch_body = 0; - attitude_setpoint.yaw_body = _man_yaw_sp; - - Quatf q_sp_omni = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body); - q_sp_omni.copyTo(attitude_setpoint.q_d); - attitude_setpoint.q_d_valid = true; - - float x_thrust = _manual_control_sp.x; - float y_thrust = _manual_control_sp.y; - float z_thrust = _manual_control_sp.z; - - // Check if the total horizontal thrust has exceeded the maximum - Vector2f h_thrust = Vector2f(x_thrust, y_thrust); - float h_thrust_norm = h_thrust.norm(); - - if (h_thrust_norm > 1.0F) { - h_thrust /= h_thrust_norm; - } - - h_thrust *= _param_omni_dfc_max_thr.get(); - - // Check if the total thrust has exceeded the maximum - Vector3f total_thrust = Vector3f(h_thrust(0), h_thrust(1), z_thrust); - - total_thrust = total_thrust / total_thrust.norm() * z_thrust; - - attitude_setpoint.thrust_body[0] = throttle_curve(math::abs_t(total_thrust(0))) * math::sign(total_thrust(0)); - attitude_setpoint.thrust_body[1] = throttle_curve(math::abs_t(total_thrust(1))) * math::sign(total_thrust(1)); - attitude_setpoint.thrust_body[2] = -throttle_curve(total_thrust(2)); - } - attitude_setpoint.timestamp = hrt_absolute_time(); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint);