fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 17:43:34 +08: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-19 10:17:41 +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
src/modules/mc_pos_control/mc_pos_control_main.cpp
+20 -31
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();