diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index b1106685d1b..22e5b446923 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -15,9 +15,6 @@ float32 yaw_body # body angle in NED frame float32 yaw_sp_move_rate # rad/s (commanded by user) -float32[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame -bool R_valid # Set to true if rotation matrix is valid - # For quaternion-based attitude control float32[4] q_d # Desired quaternion for quaternion control bool q_d_valid # Set to true if quaternion vector is valid diff --git a/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp index 29c0791faaf..ab661b92e6a 100644 --- a/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -640,8 +640,10 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4 && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ _R.identity(); - memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); - _att_sp_msg.data().R_valid = true; + math::Quaternion qd; + qd.from_dcm(_R); + memcpy(&_att_sp_msg.data().q_d[0], qd.data, sizeof(_att_sp_msg.data().q_d)); + _att_sp_msg.data().q_d_valid = true; _att_sp_msg.data().roll_body = 0.0f; _att_sp_msg.data().pitch_body = 0.0f; @@ -930,12 +932,14 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4 _R(i, 2) = body_z(i); } - /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); - _att_sp_msg.data().R_valid = true; - /* calculate euler angles, for logging only, must not be used for control */ math::Vector<3> eul = _R.to_euler(); + + math::Quaternion q; + q.from_dcm(_R); + memcpy(_att_sp_msg.data().q_d, q.data, sizeof(_att_sp_msg.data().q_d)); + _att_sp_msg.data().q_d_valid = true; + _att_sp_msg.data().roll_body = eul(0); _att_sp_msg.data().pitch_body = eul(1); /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ @@ -945,9 +949,10 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4 * force level attitude, don't change yaw */ _R.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body); - /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); - _att_sp_msg.data().R_valid = true; + math::Quaternion q; + q.from_dcm(_R); + memcpy(_att_sp_msg.data().q_d, q.data, sizeof(_att_sp_msg.data().q_d)); + _att_sp_msg.data().q_d_valid = true; _att_sp_msg.data().roll_body = 0.0f; _att_sp_msg.data().pitch_body = 0.0f; @@ -1042,8 +1047,10 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4 /* Construct attitude setpoint rotation matrix */ math::Matrix<3, 3> R_sp; R_sp.from_euler(_att_sp_msg.data().roll_body, _att_sp_msg.data().pitch_body, _att_sp_msg.data().yaw_body); - _att_sp_msg.data().R_valid = true; - memcpy(&_att_sp_msg.data().R_body[0], R_sp.data, sizeof(_att_sp_msg.data().R_body)); + math::Quaternion q; + q.from_dcm(R_sp); + memcpy(_att_sp_msg.data().q_d, q.data, sizeof(_att_sp_msg.data().q_d)); + _att_sp_msg.data().q_d_valid = true; _att_sp_msg.data().timestamp = get_time_micros(); } else { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fed9cd2db3c..1b2484c86a2 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2584,7 +2584,11 @@ protected: mavlink_attitude_target_t msg{}; msg.time_boot_ms = att_sp.timestamp / 1000; - mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q); + if (att_sp.q_d_valid) { + memcpy(&msg.q[0], &att_sp.q_d[0], sizeof(msg.q)); + } else { + mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q); + } msg.body_roll_rate = att_rates_sp.roll; msg.body_pitch_rate = att_rates_sp.pitch; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a6b39b3cc0e..d9fc66e7faa 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1079,8 +1079,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (!ignore_attitude_msg) { // only copy att sp if message contained new data mavlink_quaternion_to_euler(set_attitude_target.q, &_att_sp.roll_body, &_att_sp.pitch_body, &_att_sp.yaw_body); - mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])_att_sp.R_body); - _att_sp.R_valid = true; _att_sp.yaw_sp_move_rate = 0.0; memcpy(_att_sp.q_d, set_attitude_target.q, sizeof(_att_sp.q_d)); _att_sp.q_d_valid = true; 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 10335a12231..b5f3dc3b268 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -669,8 +669,8 @@ MulticopterAttitudeControl::control_attitude(float dt) _thrust_sp = _v_att_sp.thrust; /* construct attitude setpoint rotation matrix */ - math::Matrix<3, 3> R_sp; - R_sp.set(_v_att_sp.R_body); + math::Quaternion q_sp(_v_att_sp.q_d[0], _v_att_sp.q_d[1], _v_att_sp.q_d[2], _v_att_sp.q_d[3]); + math::Matrix<3, 3> R_sp = q_sp.to_dcm(); /* get current rotation matrix from control state quaternions */ math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); 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 87c73aeaccf..29c420edb95 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1241,7 +1241,7 @@ MulticopterPositionControl::task_main() thrust_int.zero(); - math::Matrix<3, 3> R; + matrix::Dcmf R; R.identity(); /* wakeup source */ @@ -1391,8 +1391,9 @@ MulticopterPositionControl::task_main() && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); - memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); - _att_sp.R_valid = true; + matrix::Quatf qd = R; + memcpy(&_att_sp.q_d[0], qd.data(), sizeof(_att_sp.q_d)); + _att_sp.q_d_valid = true; _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; @@ -1419,8 +1420,6 @@ MulticopterPositionControl::task_main() reset_int_xy = true; R.identity(); - memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); - _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; @@ -1651,13 +1650,15 @@ MulticopterPositionControl::task_main() // if yes, then correct xy velocity setpoint such that the attitude setpoint is continuous if (!control_vel_enabled_prev && _control_mode.flag_control_velocity_enabled) { + matrix::Dcmf Rb = matrix::Quatf(_att_sp.q_d[0], _att_sp.q_d[1], _att_sp.q_d[2], _att_sp.q_d[3]); + // choose velocity xyz setpoint such that the resulting thrust setpoint has the direction // given by the last attitude setpoint - _vel_sp(0) = _vel(0) + (-PX4_R(_att_sp.R_body, 0, + _vel_sp(0) = _vel(0) + (-Rb(0, 2) * _att_sp.thrust - thrust_int(0) - _vel_err_d(0) * _params.vel_d(0)) / _params.vel_p(0); - _vel_sp(1) = _vel(1) + (-PX4_R(_att_sp.R_body, 1, + _vel_sp(1) = _vel(1) + (-Rb(1, 2) * _att_sp.thrust - thrust_int(1) - _vel_err_d(1) * _params.vel_d(1)) / _params.vel_p(1); - _vel_sp(2) = _vel(2) + (-PX4_R(_att_sp.R_body, 2, + _vel_sp(2) = _vel(2) + (-Rb(2, 2) * _att_sp.thrust - thrust_int(2) - _vel_err_d(2) * _params.vel_d(2)) / _params.vel_p(2); _vel_sp_prev(0) = _vel_sp(0); _vel_sp_prev(1) = _vel_sp(1); @@ -1904,17 +1905,13 @@ MulticopterPositionControl::task_main() R(i, 2) = body_z(i); } - /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); - _att_sp.R_valid = true; - /* copy quaternion setpoint to attitude setpoint topic */ - math::Quaternion q_sp; - q_sp.from_dcm(R); - memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); + matrix::Quatf q_sp = R; + memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d)); + _att_sp.q_d_valid = true; /* calculate euler angles, for logging only, must not be used for control */ - math::Vector<3> euler = R.to_euler(); + matrix::Eulerf euler = R; _att_sp.roll_body = euler(0); _att_sp.pitch_body = euler(1); /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ @@ -1922,16 +1919,12 @@ MulticopterPositionControl::task_main() } else if (!_control_mode.flag_control_manual_enabled) { /* autonomous altitude control without position control (failsafe landing), * force level attitude, don't change yaw */ - R.from_euler(0.0f, 0.0f, _att_sp.yaw_body); - - /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); - _att_sp.R_valid = true; + R = matrix::Eulerf(0.0f, 0.0f, _att_sp.yaw_body); /* copy quaternion setpoint to attitude setpoint topic */ - math::Quaternion q_sp; - q_sp.from_dcm(R); - memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); + matrix::Quatf q_sp = R; + memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d)); + _att_sp.q_d_valid = true; _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; @@ -2065,14 +2058,10 @@ MulticopterPositionControl::task_main() _att_sp.roll_body = -atan2f(z_roll_pitch_sp(1), z_roll_pitch_sp(2)); } - math::Matrix<3, 3> R_sp; - R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); - memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); - /* copy quaternion setpoint to attitude setpoint topic */ - math::Quaternion q_sp; - q_sp.from_dcm(R_sp); - memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); + matrix::Quatf q_sp = matrix::Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); + memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d)); + _att_sp.q_d_valid = true; } _att_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index d93870acc6b..b4eafaa044c 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -345,7 +345,7 @@ void Standard::update_mc_state() } matrix::Dcmf R(matrix::Quatf(_v_att->q)); - matrix::Dcmf R_sp(&_v_att_sp->R_body[0]); + matrix::Dcmf R_sp(matrix::Quatf(_v_att_sp->q_d)); matrix::Eulerf euler(R); matrix::Eulerf euler_sp(R_sp); _pusher_throttle = 0.0f; @@ -390,7 +390,6 @@ void Standard::update_mc_state() float roll = -atan2f(tilt_new(1), tilt_new(2)); R_sp = matrix::Eulerf(roll, pitch, euler_sp(2)); matrix::Quatf q_sp(R_sp); - memcpy(&_v_att_sp->R_body[0], &R_sp._data[0], sizeof(_v_att_sp->R_body)); memcpy(&_v_att_sp->q_d[0], &q_sp._data[0], sizeof(_v_att_sp->q_d)); } diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index d8e875a9535..ebbf16f5097 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -347,14 +347,9 @@ void Tailsitter::update_transition_state() _v_att_sp->timestamp = hrt_absolute_time(); _v_att_sp->roll_body = 0.0f; _v_att_sp->yaw_body = _yaw_transition; - _v_att_sp->R_valid = true; - - math::Matrix<3, 3> R_sp; - R_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body); - memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body)); math::Quaternion q_sp; - q_sp.from_dcm(R_sp); + q_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body); memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d)); }