yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

Remove attitude setpoint matrix from attitude setpoint topic

Browse Source
This commit is contained in:
Lorenz Meier
2016-10-18 09:46:12 +02:00
committed by Lorenz Meier
parent 5317d29ffd
commit a12780c88f
8 changed files with 47 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
msg/vehicle_attitude_setpoint.msg
View File

@@ -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

29
src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp
View File

@@ -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 {

6
src/modules/mavlink/mavlink_messages.cpp
View File

@@ -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;

2
src/modules/mavlink/mavlink_receiver.cpp
View File

@@ -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;

4
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@@ -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]);

51
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@@ -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();

3
src/modules/vtol_att_control/standard.cpp
View File

@@ -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));
}

7
src/modules/vtol_att_control/tailsitter.cpp
View File

@@ -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));
}