From f65daf72599b13ae30ff98ec22c38838996c2c49 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 2 Mar 2023 16:45:20 +0100 Subject: [PATCH] mc att: do not pass by euler angles to generate att sp from sticks --- .../mc_att_control/mc_att_control_main.cpp | 64 ++++++++----------- 1 file changed, 27 insertions(+), 37 deletions(-) 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 29ee08248c..40013ae520 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -149,57 +149,47 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, v *= _man_tilt_max / v_norm; } - Quatf q_sp_rpy = AxisAnglef(v(0), v(1), 0.f); - Eulerf euler_sp = q_sp_rpy; - attitude_setpoint.roll_body = euler_sp(0); - attitude_setpoint.pitch_body = euler_sp(1); + Quatf q_sp_rp = AxisAnglef(v(0), v(1), 0.f); // The axis angle can change the yaw as well (noticeable at higher tilt angles). // This is the formula by how much the yaw changes: // let a := tilt angle, b := atan(y/x) (direction of maximum tilt) // yaw = atan(-2 * sin(b) * cos(b) * sin^2(a/2) / (1 - 2 * cos^2(b) * sin^2(a/2))). - attitude_setpoint.yaw_body = _man_yaw_sp + euler_sp(2); + const Quatf q_sp_yaw(cosf(_man_yaw_sp / 2.f), 0.f, 0.f, sinf(_man_yaw_sp / 2.f)); - /* modify roll/pitch only if we're a VTOL */ if (_vtol) { - // Construct attitude setpoint rotation matrix. Modify the setpoints for roll - // and pitch such that they reflect the user's intention even if a large yaw error - // (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix - // from the pure euler angle setpoints will lead to unexpected attitude behaviour from - // the user's view as the euler angle sequence uses the yaw setpoint and not the current - // heading of the vehicle. - // However there's also a coupling effect that causes oscillations for fast roll/pitch changes - // at higher tilt angles, so we want to avoid using this on multicopters. - // The effect of that can be seen with: - // - roll/pitch into one direction, keep it fixed (at high angle) - // - apply a fast yaw rotation - // - look at the roll and pitch angles: they should stay pretty much the same as when not yawing + // Modify the setpoints for roll and pitch such that they reflect the user's intention even + // if a large yaw error(yaw_sp - yaw) is present. In the presence of a yaw error constructing + // an attitude setpoint from the yaw setpoint will lead to unexpected attitude behaviour from + // the user's view as the tilt will not be aligned with the heading of the vehicle. - // calculate our current yaw error - float yaw_error = wrap_pi(attitude_setpoint.yaw_body - yaw); + const Vector3f z_unit(0.f, 0.f, 1.f); - // compute the vector obtained by rotating a z unit vector by the rotation - // given by the roll and pitch commands of the user - Vector3f zB = {0.0f, 0.0f, 1.0f}; - Dcmf R_sp_roll_pitch = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, 0.0f); - Vector3f z_roll_pitch_sp = R_sp_roll_pitch * zB; + // Extract yaw from the current attitude + Vector3f att_z = q.dcm_z(); + Quatf q_delta = Quatf(z_unit, att_z); + Quatf q_yaw = q_delta.inversed() * q; // This is not euler yaw - // transform the vector into a new frame which is rotated around the z axis - // by the current yaw error. this vector defines the desired tilt when we look - // into the direction of the desired heading - Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error); - z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp; + // Find the quaternion that creates a tilt aligned with the body frame + // when rotated by the yaw setpoint + // To do so, solve q_yaw * q_sp_rp = q_sp_yaw * q_sp_rp_compensated + Quatf q_sp_rp_compensated = q_sp_yaw.inversed() * q_yaw * q_sp_rp; - // use the formula z_roll_pitch_sp = R_tilt * [0;0;1] - // R_tilt is computed from_euler; only true if cos(roll) not equal zero - // -> valid if roll is not +-pi/2; - attitude_setpoint.roll_body = -asinf(z_roll_pitch_sp(1)); - attitude_setpoint.pitch_body = atan2f(z_roll_pitch_sp(0), z_roll_pitch_sp(2)); + // Extract the corrected tilt as we still want to include the yaw setpoint + Vector3f att_sp_z = q_sp_rp_compensated.dcm_z(); + q_sp_rp = Quatf(z_unit, att_sp_z); } - /* copy quaternion setpoint to attitude setpoint topic */ - Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body); + // Align the desired tilt with the yaw setpoint + Quatf q_sp = q_sp_yaw * q_sp_rp; + q_sp.copyTo(attitude_setpoint.q_d); + // Transform to euler angles for logging only + const Eulerf euler_sp(q_sp); + attitude_setpoint.roll_body = euler_sp(0); + attitude_setpoint.pitch_body = euler_sp(1); + attitude_setpoint.yaw_body = euler_sp(2); + attitude_setpoint.thrust_body[2] = -throttle_curve((_manual_control_setpoint.throttle + 1.f) * .5f); attitude_setpoint.timestamp = hrt_absolute_time();