From 431a0cb11e6f1cf6044e7ec79ae934e23aaa2876 Mon Sep 17 00:00:00 2001 From: Azarakhsh Keipour Date: Tue, 28 Jul 2020 21:30:23 -0400 Subject: [PATCH] Omni Pos-Ctrl: Added a parameter for the attitude change rate (OMNI_ATT_MODE=6) --- .../PositionControl/ControlMath.cpp | 27 ++++++++++++------- .../PositionControl/ControlMath.hpp | 18 ++++++++----- .../PositionControl/PositionControl.cpp | 5 ++-- .../PositionControl/PositionControl.hpp | 14 +++++----- .../mc_pos_control/mc_pos_control_main.cpp | 6 +++-- .../mc_pos_control/mc_pos_control_params.c | 14 ++++++++++ 6 files changed, 57 insertions(+), 27 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp index 01e02795ba..6d9d00846f 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp @@ -48,7 +48,8 @@ namespace ControlMath { void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, const int omni_att_mode, const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir, - float &omni_att_roll, float &omni_att_pitch, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp) + float &omni_att_roll, float &omni_att_pitch, const float omni_att_rate, int omni_proj_axes, + vehicle_attitude_setpoint_s &att_sp) { // Print an error if the omni_att_mode parameter is out of range @@ -76,7 +77,7 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix:: } case 6: { // Attitude is changed very slowly given the rate - float tilt_rate = 0.01; + float tilt_rate = math::radians(omni_att_rate); thrustToSlowAttitude(thr_sp, yaw_sp, att, tilt_rate, omni_proj_axes, att_sp); break; } @@ -88,17 +89,23 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix:: // Estimate the optimal tilt angle and direction to counteract the wind if (omni_att_mode == 5 || omni_att_mode == 6) { - // Calculate the tilt angle - omni_att_tilt_angle = asinf(Vector2f(thr_sp(0), thr_sp(1)).norm() / thr_sp.norm()); - // Calculate the tilt direction - omni_att_tilt_dir = wrap_2pi(atan2f(thr_sp(1), thr_sp(0))); + // Calculate the current z axis + Vector3f curr_z; + matrix::Dcmf R_body = att; - // Set the roll angle - omni_att_roll = att_sp.roll_body; + for (int i = 0; i < 3; i++) { + curr_z(i) = R_body(i, 2); + } - // Set the pitch angle - omni_att_pitch = att_sp.pitch_body; + // Calculate the tilt angle and direction + omni_att_tilt_angle = asinf(Vector2f(curr_z(0), curr_z(1)).norm() / curr_z.norm()); + omni_att_tilt_dir = wrap_2pi(atan2f(-curr_z(1), -curr_z(0))); + + // Calculate the roll and pitch + Eulerf euler = R_body; + omni_att_roll = euler(0); + omni_att_pitch = euler(1); } } diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp index efcc49a547..60685a7476 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp @@ -50,17 +50,21 @@ namespace ControlMath * @param thr_sp desired 3D thrust vector * @param yaw_sp the desired yaw * @param att current attitude of the robot - * @param omni_att_mode attitude mode for omnidirectional vehicles: 0-tilted 1-min-tilt 2-zero-tilt + * @param omni_att_mode attitude mode for omnidirectional vehicles * @param omni_dfc_max_thrust maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles - * @param omni_att_tilt_angle the desired tilt for the vehicle in mode=3, is output for mode=5 (in degrees) - * @param omni_att_tilt_dir the direction of the desired tilt with respec to North in mode=3, is output for mode=5 (in degrees) - * @param omni_att_roll the desired roll for the vehicle in mode=4, is output for mode=6 (in degrees) - * @param omni_att_pitch the desired pitch for the vehicle in mode=4, is output for mode=6 (in degrees) + * @param omni_att_tilt_angle the desired tilt for the vehicle in mode=3, is output for mode > 5 (in radians) + * @param omni_att_tilt_dir the direction of the desired tilt with respec to North in mode=3, is output for mode > 5 (in radians) + * @param omni_att_roll the desired roll for the vehicle in mode=4, is output for mode > 5 (in radians) + * @param omni_att_pitch the desired pitch for the vehicle in mode=4, is output for mode=6 (in radians) + * @param omni_att_rate the attitude change rate for mode=6 + * @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current) * @param att_sp attitude setpoint to fill */ void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, const int omni_att_mode, const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir, - float &omni_att_roll, float &omni_att_pitch, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp); + float &omni_att_roll, float &omni_att_pitch, const float omni_att_rate, const int omni_proj_axes, + vehicle_attitude_setpoint_s &att_sp); + /** * Converts a body z vector and yaw set-point to a desired attitude. * @param body_z a world frame 3D vector in direction of the desired body z axis @@ -122,7 +126,7 @@ void thrustToFixedRollPitch(const matrix::Vector3f &thr_sp, const float yaw_sp, * @param thr_sp a 3D vector * @param yaw_sp the desired yaw * @param att current attitude of the robot - * @param tilt_rate rate for the tilt change (0-1) + * @param tilt_rate rate for the tilt change (non-negative in radians per loop) * @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current) * @param att_sp attitude setpoint to fill */ diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index edd2aaee72..a99d567086 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -363,9 +363,10 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s void PositionControl::getAttitudeSetpoint(const matrix::Quatf &att, const int omni_att_mode, const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir, float &omni_att_roll, - float &omni_att_pitch, int omni_proj_axes, vehicle_attitude_setpoint_s &attitude_setpoint) const + float &omni_att_pitch, const float omni_att_rate, const int omni_proj_axes, + vehicle_attitude_setpoint_s &attitude_setpoint) const { ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, att, omni_att_mode, omni_dfc_max_thrust, omni_att_tilt_angle, - omni_att_tilt_dir, omni_att_roll, omni_att_pitch, omni_proj_axes, attitude_setpoint); + omni_att_tilt_dir, omni_att_roll, omni_att_pitch, omni_att_rate, omni_proj_axes, 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 267f7180a7..680f0e19de 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -170,17 +170,19 @@ public: * 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 att current attitude of the robot - * @param omni_att_mode attitude mode for omnidirectional vehicles: 0-tilted 1-min-tilt 2-zero-tilt + * @param omni_att_mode attitude mode for omnidirectional vehicles * @param omni_dfc_max_thrust maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles - * @param omni_att_tilt_angle the desired tilt for the vehicle in mode=3, is output for mode=5 (in degrees) - * @param omni_att_tilt_dir the direction of the desired tilt with respec to North in mode=3, is output for mode=5 (in degrees) - * @param omni_att_roll the desired roll for the vehicle in mode=4, is output for mode=6 (in degrees) - * @param omni_att_pitch the desired pitch for the vehicle in mode=4, is output for mode=6 (in degrees) + * @param omni_att_tilt_angle the desired tilt for the vehicle in mode=3, is output for mode > 5 (in radians) + * @param omni_att_tilt_dir the direction of the desired tilt with respec to North in mode=3, is output for mode > 5 (in radians) + * @param omni_att_roll the desired roll for the vehicle in mode=4, is output for mode > 5 (in radians) + * @param omni_att_pitch the desired pitch for the vehicle in mode=4, is output for mode=6 (in radians) + * @param omni_att_rate the attitude change rate for mode=6 + * @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current) * @param attitude_setpoint reference to struct to fill up */ void getAttitudeSetpoint(const matrix::Quatf &att, const int omni_att_mode, const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir, float &omni_att_roll, float &omni_att_pitch, - int omni_proj_axes, vehicle_attitude_setpoint_s &attitude_setpoint) const; + const float omni_att_rate, const int omni_proj_axes, 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 41aac99c0f..05ba55f2bc 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -187,7 +187,8 @@ private: (ParamFloat) _param_omni_att_tilt_dir, (ParamFloat) _param_omni_att_roll, (ParamFloat) _param_omni_att_pitch, - (ParamInt) _param_omni_proj_axes + (ParamInt) _param_omni_proj_axes, + (ParamFloat) _param_omni_att_rate ); control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ @@ -686,7 +687,8 @@ MulticopterPositionControl::Run() vehicle_attitude_setpoint_s attitude_setpoint{}; attitude_setpoint.timestamp = time_stamp_now; _control.getAttitudeSetpoint(matrix::Quatf(att.q), _param_omni_att_mode.get(), _param_omni_dfc_max_thr.get(), - _tilt_angle, _tilt_dir, _tilt_roll, _tilt_pitch, _param_omni_proj_axes.get(), attitude_setpoint); + _tilt_angle, _tilt_dir, _tilt_roll, _tilt_pitch, _param_omni_att_rate.get(), _param_omni_proj_axes.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 61aa15fb06..e2b5bc5300 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -856,3 +856,17 @@ PARAM_DEFINE_FLOAT(OMNI_ATT_PITCH, 0.0f); * @group Multicopter Attitude Control */ PARAM_DEFINE_INT32(OMNI_PROJ_AXES, 1); + + +/** + * Rate for change of attitude + * + * Specifies the rate in which the attitude can change in slow attitude mode + * (OMNI_ATT_MODE = 6), rotating the Z axis from the current attitude to the + * optimal attitude. The value is in degrees per position control loop. + * + * @min 0 + * @decimal 3 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(OMNI_ATT_RATE, 0.5f);