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 3beb1bbe64..1c9c95bc4c 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -200,7 +200,7 @@ PARAM_DEFINE_INT32(OMNI_ATT_MODE, 0); * @decimal 2 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(OMNI_ATT_TLT_ANG, 15.0f); +PARAM_DEFINE_FLOAT(OMNI_ATT_TLT_ANG, 0.0f); /** * Omni-directional attitude setpoint tilt direction angle diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp index be3be34988..40367b6d59 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp @@ -45,7 +45,8 @@ using namespace matrix; 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, vehicle_attitude_setpoint_s &att_sp) + const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir, + float &omni_att_roll, float &omni_att_pitch, vehicle_attitude_setpoint_s &att_sp) { // Print an error if the omni_att_mode parameter is out of range if (omni_att_mode > 6 || omni_att_mode < 0) { @@ -62,15 +63,15 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix:: break; case 3: { // Attitude is set to a fixed tilt at a fixed global direction (used for omnidirectional vehicles) - float tilt_angle = math::radians(10.F); - float tilt_dir = math::radians(270.F); + float tilt_angle = math::radians(omni_att_tilt_angle); + float tilt_dir = math::radians(omni_att_tilt_dir); thrustToFixedTiltAttitude(thr_sp, yaw_sp, att, tilt_angle, tilt_dir, att_sp); break; } case 4: { // Attitude is set to a fixed roll and pitch (used for omnidirectional vehicles) - float roll_angle = math::radians(5.F); - float pitch_angle = math::radians(5.F); + float roll_angle = math::radians(omni_att_roll); + float pitch_angle = math::radians(omni_att_pitch); thrustToFixedRollPitch(thr_sp, yaw_sp, att, roll_angle, pitch_angle, att_sp); break; } @@ -182,7 +183,7 @@ void thrustToMinTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const f Vector2f thr_sp_h(thr_sp(0), thr_sp(1)); if (thr_sp_h.norm() <= omni_dfc_max_thrust) { - thrustToAttitude(thr_sp, yaw_sp, matrix::Quatf(), 2, omni_dfc_max_thrust, att_sp); + thrustToZeroTiltAttitude(thr_sp, yaw_sp, att_sp); return; } diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp index 4dd9ecd5aa..6d1fc9eb24 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp @@ -49,14 +49,18 @@ namespace ControlMath * Converts thrust vector and yaw set-point to a desired attitude. * @param thr_sp desired 3D thrust vector * @param yaw_sp the desired yaw - * @param att_sp attitude setpoint to fill + * @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_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 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, - vehicle_attitude_setpoint_s &att_sp); + 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, 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 diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 1f0a6de67e..8dc81abfcd 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -362,10 +362,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 &omnni_att_tilt_angle, float &omnni_att_tilt_dir, float &omnni_att_roll, - float &omnni_att_pitch, - vehicle_attitude_setpoint_s &attitude_setpoint) const + const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir, float &omni_att_roll, + float &omni_att_pitch, vehicle_attitude_setpoint_s &attitude_setpoint) const { - ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, att, omni_att_mode, omni_dfc_max_thrust, attitude_setpoint); + 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, 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 9b54aa0aff..8cd71aef97 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -169,12 +169,17 @@ 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 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_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 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 &omnni_att_tilt_angle, float &omnni_att_tilt_dir, float &omnni_att_roll, float &omnni_att_pitch, + float &omni_att_tilt_angle, float &omni_att_tilt_dir, float &omni_att_roll, float &omni_att_pitch, 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 c0221bbe62..22c78ca39a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -662,9 +662,24 @@ MulticopterPositionControl::Run() vehicle_attitude_setpoint_s attitude_setpoint{}; attitude_setpoint.timestamp = time_stamp_now; - float omnni_att_tilt_angle, omnni_att_tilt_dir, omnni_att_roll, omnni_att_pitch; + float omni_att_tilt_angle = _param_omni_att_tilt_angle.get(); + float omni_att_tilt_dir = _param_omni_att_tilt_dir.get(); + float omni_att_roll = _param_omni_att_roll.get(); + float omni_att_pitch = _param_omni_att_pitch.get(); _control.getAttitudeSetpoint(matrix::Quatf(att.q), _param_omni_att_mode.get(), _param_omni_dfc_max_thr.get(), - omnni_att_tilt_angle, omnni_att_tilt_dir, omnni_att_roll, omnni_att_pitch, attitude_setpoint); + omni_att_tilt_angle, omni_att_tilt_dir, omni_att_roll, omni_att_pitch, attitude_setpoint); + + // Update the tilt angle and direciton parameters if we are in tilt estimation mode + if (_param_omni_att_mode.get() == 5) { + _param_omni_att_tilt_angle.set(omni_att_tilt_angle); + _param_omni_att_tilt_dir.set(omni_att_tilt_dir); + } + + // Update the roll and pitch angle parameters if we are in roll/pitch estimation mode + if (_param_omni_att_mode.get() == 6) { + _param_omni_att_roll.set(omni_att_roll); + _param_omni_att_pitch.set(omni_att_pitch); + } // 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.