From 3ae19f254c0959e534cf3b4788148c50deca1280 Mon Sep 17 00:00:00 2001 From: Azarakhsh Keipour Date: Sat, 28 Dec 2019 17:51:29 -0500 Subject: [PATCH] Omni Att-Ctrl: Manual mode works smoother now and based on the MC_OMNI_MODE parameter - Some observations: - It's still not as smooth as expected (or maybe I'm not a good pilot) - Acceleration on the body X axis feels lower than the body Y axis (no idea why) - Flight tested in Gazebo sim in both Manual and Autonomous modes --- src/modules/mc_att_control/mc_att_control.hpp | 4 +- .../mc_att_control/mc_att_control_main.cpp | 61 +++++++++---------- 2 files changed, 33 insertions(+), 32 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 701cfb356e..bcfec33d84 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -157,7 +157,9 @@ 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 + (ParamInt) _param_mc_airmode, + + (ParamInt) _param_mc_omni_mode ) 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 fb29e0d6f3..f7b9e2b97c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -218,43 +218,42 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z); - ///////// Added for Omni //////////////// - // set the euler angles, for logging only, must not be used for control - attitude_setpoint.roll_body = 0; - attitude_setpoint.pitch_body = 0; + /* modify roll/pitch if we're in omni-directional mode */ + if (_param_mc_omni_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; + 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; + 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(); + // 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; + if (h_thrust_norm > 1.0F) { + h_thrust /= h_thrust_norm; + } + + const float max_xy_thrust_ratio = 0.3; + h_thrust *= max_xy_thrust_ratio; + + // 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)); } - const float max_xy_thrust_ratio = 0.3; - h_thrust *= max_xy_thrust_ratio; - - // Check if the total thrust has exceeded the maximum - Vector3f total_thrust = Vector3f(h_thrust(0), h_thrust(1), z_thrust); - float total_thrust_norm = total_thrust.norm(); - - if (total_thrust_norm > 1.0F) { - total_thrust /= total_thrust_norm; - } - - attitude_setpoint.thrust_body[0] = throttle_curve(total_thrust(0)); - attitude_setpoint.thrust_body[1] = throttle_curve(total_thrust(1)); - attitude_setpoint.thrust_body[2] = -throttle_curve(total_thrust(2)); - ///////// End of added for omni ///////////// - attitude_setpoint.timestamp = hrt_absolute_time(); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint);