From 81d554ef71a889f922864af1c435a183b97bdca4 Mon Sep 17 00:00:00 2001 From: Azarakhsh Keipour Date: Mon, 30 Dec 2019 23:18:48 -0500 Subject: [PATCH] Omni Pos-Ctrl: The maximum dfc thrust is defined as a parameter - The parameter is shared with the manual mode's maximum horizontal thrust (renamed from OMNI_MAX_HOR_THR to OMNI_DFC_MAX_THR) defined in the mc_att_control module - The definition for the OMNI_ATT_MODE moved from mc_pos_control module to mc_att_control - The thrustToAttitude function now has additional omni_dfc_max_thrust parameter - Test modules are fixed to call the new thrustToAttitude function appropriately - The code is tested in Gazebo for both manual and (semi-)autonomous modes --- src/modules/mc_att_control/mc_att_control.hpp | 2 +- .../mc_att_control/mc_att_control_main.cpp | 2 +- .../mc_att_control/mc_att_control_params.c | 28 ++++++++- .../PositionControl/ControlMath.cpp | 57 +++++++++++-------- .../PositionControl/ControlMath.hpp | 8 ++- .../PositionControl/ControlMathTest.cpp | 7 ++- .../PositionControl/PositionControl.cpp | 5 +- .../PositionControl/PositionControl.hpp | 6 +- .../mc_pos_control/mc_pos_control_main.cpp | 5 +- .../mc_pos_control/mc_pos_control_params.c | 22 ------- 10 files changed, 80 insertions(+), 62 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index aa73ace4b4..097921e538 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -161,7 +161,7 @@ private: /* Omnidirectional vehicle params */ (ParamInt) _param_omni_att_mode, - (ParamFloat) _param_omni_max_hor_thr + (ParamFloat) _param_omni_dfc_max_thr ) 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 8bf343b44a..3c1cc279d8 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -241,7 +241,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ h_thrust /= h_thrust_norm; } - h_thrust *= _param_omni_max_hor_thr.get(); + 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); 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 646cff5cba..cd1766b33f 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -146,16 +146,38 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 200.0f); PARAM_DEFINE_FLOAT(MC_RATT_TH, 0.8f); /** - * Maximum horizontal thrust ratio for omnidirectional vehicles + * Maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles * * Specifies the maximum horizontal thrust compared to the maximum possible * thrust generated by the vehicle for an omnidirectional multirotor. The * value of this parameter does not affect the behavior if the attitude mode - * is not set to one of omni-directional modes. + * is not set to one of omni-directional modes (if OMNI_ATT_MODE is 0). * * @min 0 * @max 1 * @decimal 2 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(OMNI_MAX_HOR_THR, 0.15f); +PARAM_DEFINE_FLOAT(OMNI_DFC_MAX_THR, 0.15f); + +/** + * Omni-directional attitude setpoint mode + * + * Specifies the type of attitude setpoint sent to the attitude controller. + * The parameter can be set to a normal attitude setpoint, where the tilt + * of the vehicle (roll and pitch) are calculated from the desired thrust + * vector. This should be the behavior for the non-omnidirectional vehicles, + * such as quadrotors and other multirotors with coplanar rotors. + * The "constant zero tilt" mode enforces zero roll and pitch and can only be + * used for omnidirectional vehicles. The min-tilt mode enforces zero tilt + * up to a maximum set acceleration (thrust) and tilts the vehicle as small + * as possible if the thrust vector is larger than the maximum. + * + * @min 0 + * @max 2 + * @value 0 tilted attitude + * @value 1 min-tilt attitude + * @value 2 constant zero tilt + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_INT32(OMNI_ATT_MODE, 0); diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp index 566af00d39..6523695307 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp @@ -45,7 +45,7 @@ using namespace matrix; namespace ControlMath { void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const int omni_att_mode, - vehicle_attitude_setpoint_s &att_sp) + const float omni_dfc_max_thrust, vehicle_attitude_setpoint_s &att_sp) { // Print an error if the omni_att_mode parameter is out of range if (omni_att_mode > 2 || omni_att_mode < 0) { @@ -54,7 +54,7 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const int omni switch (omni_att_mode) { case 1: // Attitude is set to the minimum roll and pitch (used for omnidirectional vehicles) - thrustToMinTiltAttitude(thr_sp, yaw_sp, att_sp); + thrustToMinTiltAttitude(thr_sp, yaw_sp, omni_dfc_max_thrust, att_sp); break; case 2: // Attitude is set to the fixed zero roll and pitch (used for omnidirectional vehicles) @@ -157,32 +157,41 @@ void thrustToZeroTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicl att_sp.thrust_body[2] = thr_sp(2); } -void thrustToMinTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp) +void thrustToMinTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const float omni_dfc_max_thrust, + vehicle_attitude_setpoint_s &att_sp) { - // TEMP: Define the maximum dfc horizontal thrust - const float omni_dfc_max_thrust = 0.15f; + Vector3f body_z; + float lambda = 0.f; - // Check if the horizontal force is less than the maximum possible - Vector2f thr_sp_h(thr_sp(0), thr_sp(1)); - if (thr_sp_h.norm() <= omni_dfc_max_thrust) { - return thrustToAttitude(thr_sp, yaw_sp, 2, att_sp); + // zero vector, no direction, set safe level value + if (thr_sp.norm_squared() < FLT_EPSILON) { + body_z(2) = 1.f; + + } else { + // Check if the horizontal force is less than the maximum possible + Vector2f thr_sp_h(thr_sp(0), thr_sp(1)); + + if (thr_sp_h.norm() <= omni_dfc_max_thrust) { + thrustToAttitude(thr_sp, yaw_sp, 2, omni_dfc_max_thrust, att_sp); + return; + } + + // Calculate the tilt angle + float thr_sp_norm = thr_sp.norm(); + float xi = asinf(Vector2f(thr_sp(0), + thr_sp(1)).norm() / thr_sp_norm); // angle between upward direction and the desired thrust + float mu = asinf(omni_dfc_max_thrust / thr_sp_norm); // angle between the Z thrust and the desired thrust + lambda = xi - mu; // the desired tilt angle + + // Calculate the direction of the body Z axis + Vector3f v_hat(0.f, 0.f, -1.f); // upward direction + Vector3f p_hat = v_hat % thr_sp; // the axis of rotation for lambda + p_hat.normalize(); + body_z = -(1 - cosf(lambda)) * p_hat * (p_hat.dot(v_hat)) + cosf(lambda) * v_hat - sinf(lambda) * + (v_hat % p_hat); // Rodrigues' rotation formula + body_z = -body_z; } - // Calculate the tilt angle - float thr_sp_norm = thr_sp.norm(); - float xi = asinf(Vector2f(thr_sp(0), - thr_sp(1)).norm() / thr_sp_norm); // angle between upward direction and the desired thrust - float mu = asinf(omni_dfc_max_thrust / thr_sp_norm); // angle between the Z thrust and the desired thrust - float lambda = xi - mu; // the desired tilt angle - - // Calculate the direction of the body Z axis - Vector3f v_hat(0.f, 0.f, -1.f); // upward direction - Vector3f p_hat = v_hat % thr_sp; // the axis of rotation for lambda - p_hat.normalize(); - Vector3f body_z = -(1 - cosf(lambda)) * p_hat * (p_hat.dot(v_hat)) + cosf(lambda) * v_hat - sinf(lambda) * - (v_hat % p_hat); // Rodrigues' rotation formula - body_z = -body_z; - // vector of desired yaw direction in XY plane, rotated by PI/2 Vector3f y_C(-sinf(yaw_sp), cosf(yaw_sp), 0.0f); diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp index dacda7baae..970ac36d6b 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp @@ -50,9 +50,11 @@ namespace ControlMath * @param thr_sp desired 3D thrust vector * @param yaw_sp the desired yaw * @param att_sp attitude setpoint to fill - * @param omni_att_mode attitude mode for omnidirectional vehicles: 0-tilted 1-daisy-chain 2-zero-tilt + * @param omni_att_mode attitude mode for omnidirectional vehicles: 0-tilted 1-min-tilt 2-zero-tilt + * @param omni_dfc_max_thrust maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles */ void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const int omni_att_mode, + const float omni_dfc_max_thrust, vehicle_attitude_setpoint_s &att_sp); /** * Converts a body z vector and yaw set-point to a desired attitude. @@ -74,9 +76,11 @@ void thrustToZeroTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp * Converts thrust vector and yaw set-point to a minimum-tilt attitude for an omni-directional multirotor. * @param thr_sp a 3D vector * @param yaw_sp the desired yaw + * @param omni_dfc_max_thrust maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles * @param att_sp attitude setpoint to fill */ -void thrustToMinTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp); +void thrustToMinTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const float omni_dfc_max_thrust, + vehicle_attitude_setpoint_s &att_sp); /** * Outputs the sum of two vectors but respecting the limits and priority. diff --git a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp index 5e7dc3a434..83f27c6fd2 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp @@ -46,8 +46,9 @@ TEST(ControlMathTest, ThrottleAttitudeMapping) Vector3f thr{0.f, 0.f, -1.f}; float yaw = 0.f; int omni_att_mode = 0; + float omni_dfc_max_thrust = 0.0f; vehicle_attitude_setpoint_s att{}; - thrustToAttitude(thr, yaw, omni_att_mode, att); + thrustToAttitude(thr, yaw, omni_att_mode, omni_dfc_max_thrust, att); EXPECT_FLOAT_EQ(att.roll_body, 0.f); EXPECT_FLOAT_EQ(att.pitch_body, 0.f); EXPECT_FLOAT_EQ(att.yaw_body, 0.f); @@ -56,7 +57,7 @@ TEST(ControlMathTest, ThrottleAttitudeMapping) /* expected: same as before but with 90 yaw * reason: only yaw changed */ yaw = M_PI_2_F; - thrustToAttitude(thr, yaw, omni_att_mode, att); + thrustToAttitude(thr, yaw, omni_att_mode, omni_dfc_max_thrust, att); EXPECT_FLOAT_EQ(att.roll_body, 0.f); EXPECT_FLOAT_EQ(att.pitch_body, 0.f); EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); @@ -66,7 +67,7 @@ TEST(ControlMathTest, ThrottleAttitudeMapping) * reason: thrust points straight down and order Euler * order is: 1. roll, 2. pitch, 3. yaw */ thr = Vector3f(0.f, 0.f, 1.f); - thrustToAttitude(thr, yaw, omni_att_mode, att); + thrustToAttitude(thr, yaw, omni_att_mode, omni_dfc_max_thrust, att); EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F); EXPECT_FLOAT_EQ(att.pitch_body, 0.f); EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 69999c9313..34873b93e6 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -361,8 +361,9 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s _thr_sp.copyTo(local_position_setpoint.thrust); } -void PositionControl::getAttitudeSetpoint(const int omni_att_mode, vehicle_attitude_setpoint_s &attitude_setpoint) const +void PositionControl::getAttitudeSetpoint(const int omni_att_mode, const float omni_dfc_max_thrust, + vehicle_attitude_setpoint_s &attitude_setpoint) const { - ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, omni_att_mode, attitude_setpoint); + ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, omni_att_mode, omni_dfc_max_thrust, attitude_setpoint); attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp; } diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index 185edde273..a41715260b 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -169,10 +169,12 @@ public: * Get the controllers output attitude setpoint * This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control. * It needs to be executed by the attitude controller to achieve velocity and position tracking. - * @param omni_att_mode attitude mode for omnidirectional vehicles: 0-tilted 1-daisy-chain 2-zero-tilt + * @param omni_att_mode attitude mode for omnidirectional vehicles: 0-tilted 1-min-tilt 2-zero-tilt + * @param omni_dfc_max_thrust maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles * @param attitude_setpoint reference to struct to fill up */ - void getAttitudeSetpoint(const int omni_att_mode, vehicle_attitude_setpoint_s &attitude_setpoint) const; + void getAttitudeSetpoint(const int omni_att_mode, const float omni_dfc_max_thrust, + vehicle_attitude_setpoint_s &attitude_setpoint) const; private: /** 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 d31638aea8..07fe595f0d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -173,7 +173,8 @@ private: (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ (ParamFloat) _param_mpc_thr_min, (ParamFloat) _param_mpc_thr_max, - (ParamInt) _param_omni_att_mode + (ParamInt) _param_omni_att_mode, + (ParamFloat) _param_omni_dfc_max_thr ); control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ @@ -656,7 +657,7 @@ MulticopterPositionControl::Run() vehicle_attitude_setpoint_s attitude_setpoint{}; attitude_setpoint.timestamp = time_stamp_now; - _control.getAttitudeSetpoint(_param_omni_att_mode.get(), attitude_setpoint); + _control.getAttitudeSetpoint(_param_omni_att_mode.get(), _param_omni_dfc_max_thr.get(), attitude_setpoint); // Part of landing logic: if ground-contact/maybe landed was detected, turn off // controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic. diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 7f85d600ec..4f9e153d45 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -745,25 +745,3 @@ PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 1.0f); * @group Mission */ PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); - -/** - * Omni-directional attitude setpoint mode - * - * Specifies the type of attitude setpoint sent to the attitude controller. - * The parameter can be set to a normal attitude setpoint, where the tilt - * of the vehicle (roll and pitch) are calculated from the desired thrust - * vector. This should be the behavior for the non-omnidirectional vehicles, - * such as quadrotors and other multirotors with coplanar rotors. - * The "constant zero tilt" mode enforces zero roll and pitch and can only be - * used for omnidirectional vehicles. The daisy-chain mode enforces zero tilt - * up to a maximum set acceleration (thrust) and tilts the vehicle as small - * as possible if the thrust vector is larger than the maximum. - * - * @min 0 - * @max 2 - * @value 0 tilted attitude - * @value 1 daisy-chain tilt/no-tilt attitude - * @value 2 constant zero tilt - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_INT32(OMNI_ATT_MODE, 0);