From b7c721f3f61776c5b67336cbefa9bfa5f035357a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 3 Jun 2022 09:52:45 +0200 Subject: [PATCH] TEST PositionControl: use aux2 to switch between thrust vs attitude --- .../PositionControl/PositionControl.cpp | 35 +++++++++++-------- 1 file changed, 20 insertions(+), 15 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 1984d44ee2..826fe9adc5 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -252,28 +252,33 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, bool landed) { - // ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint); - manual_control_setpoint_s manual_control_setpoint; _manual_control_setpoint_sub.copy(&manual_control_setpoint); - if (landed) { + if (manual_control_setpoint.aux2 > 0.3f) { + ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint); _roll_angle = 0.f; _pitch_angle = 0.f; } else { - _roll_angle += _dt * manual_control_setpoint.aux1 * 2.f * M_PI_F / 2.f; - // _pitch_angle += _dt * powf(manual_control_setpoint.aux1, 2.f) * 2.f * M_PI_F / 2.f; // TODO: aux2 + + if (landed) { + _roll_angle = 0.f; + _pitch_angle = 0.f; + + } else { + _roll_angle += _dt * manual_control_setpoint.aux1 * 2.f * M_PI_F / 2.f; + // _pitch_angle += _dt * powf(manual_control_setpoint.aux1, 2.f) * 2.f * M_PI_F / 2.f; // TODO: aux2 + } + + Quatf q_sp = Eulerf(_roll_angle, _pitch_angle, _yaw_sp); + q_sp.copyTo(attitude_setpoint.q_d); + attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp; + + // Rotate thrust by negative attitude + Dcmf att_sp_dcm{q_sp}; + Vector3f thrust_sp_body = att_sp_dcm.transpose() * _thr_sp; + thrust_sp_body.copyTo(attitude_setpoint.thrust_body); } - - Quatf q_sp = Eulerf(_roll_angle, _pitch_angle, _yaw_sp); - q_sp.copyTo(attitude_setpoint.q_d); - attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp; - - // Rotate thrust by negative attitude - Dcmf att_sp_dcm{q_sp}; - Vector3f thrust_sp_body = att_sp_dcm.transpose() * _thr_sp; - thrust_sp_body.copyTo(attitude_setpoint.thrust_body); - }