Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 15:30:16 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

fw_att_control move to matrix lib math

Browse Source
This commit is contained in:
Daniel Agar
2018-03-19 00:23:45 -04:00
parent 1b174eeca2
commit b7bfeb442e
2 changed files with 14 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+14 -23
View File
@@ -480,14 +480,7 @@ void FixedwingAttitudeControl::run()
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
/* get current rotation matrix and euler angles from control state quaternions */ /* get current rotation matrix and euler angles from control state quaternions */
math::Quaternion q_att(_att.q[0], _att.q[1], _att.q[2], _att.q[3]); matrix::Dcmf R = matrix::Quatf(_att.q);
_R = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R.to_euler();
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
if (_vehicle_status.is_vtol && _parameters.vtol_type == vtol_type::TAILSITTER) { if (_vehicle_status.is_vtol && _parameters.vtol_type == vtol_type::TAILSITTER) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
@@ -506,29 +499,25 @@ void FixedwingAttitudeControl::run()
* Rxy Ryy Rzy -Rzy Ryy Rxy * Rxy Ryy Rzy -Rzy Ryy Rxy
* Rxz Ryz Rzz -Rzz Ryz Rxz * Rxz Ryz Rzz -Rzz Ryz Rxz
* */ * */
math::Matrix<3, 3> R_adapted = _R; //modified rotation matrix matrix::Dcmf R_adapted = R; //modified rotation matrix
/* move z to x */ /* move z to x */
R_adapted(0, 0) = _R(0, 2); R_adapted(0, 0) = R(0, 2);
R_adapted(1, 0) = _R(1, 2); R_adapted(1, 0) = R(1, 2);
R_adapted(2, 0) = _R(2, 2); R_adapted(2, 0) = R(2, 2);
/* move x to z */ /* move x to z */
R_adapted(0, 2) = _R(0, 0); R_adapted(0, 2) = R(0, 0);
R_adapted(1, 2) = _R(1, 0); R_adapted(1, 2) = R(1, 0);
R_adapted(2, 2) = _R(2, 0); R_adapted(2, 2) = R(2, 0);
/* change direction of pitch (convert to right handed system) */ /* change direction of pitch (convert to right handed system) */
R_adapted(0, 0) = -R_adapted(0, 0); R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0); R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0); R_adapted(2, 0) = -R_adapted(2, 0);
euler_angles = R_adapted.to_euler(); //adapted euler angles for fixed wing operation
/* fill in new attitude data */ /* fill in new attitude data */
_R = R_adapted; R = R_adapted;
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
/* lastly, roll- and yawspeed have to be swaped */ /* lastly, roll- and yawspeed have to be swaped */
float helper = _att.rollspeed; float helper = _att.rollspeed;
@@ -536,6 +525,8 @@ void FixedwingAttitudeControl::run()
_att.yawspeed = helper; _att.yawspeed = helper;
} }
matrix::Eulerf euler_angles(R);
updateSubscriptions(); updateSubscriptions();
vehicle_setpoint_poll(); vehicle_setpoint_poll();
vehicle_control_mode_poll(); vehicle_control_mode_poll();
@@ -634,9 +625,9 @@ void FixedwingAttitudeControl::run()
/* Prepare data for attitude controllers */ /* Prepare data for attitude controllers */
struct ECL_ControlData control_input = {}; struct ECL_ControlData control_input = {};
control_input.roll = _roll; control_input.roll = euler_angles.phi();
control_input.pitch = _pitch; control_input.pitch = euler_angles.theta();
control_input.yaw = _yaw; control_input.yaw = euler_angles.psi();
control_input.body_x_rate = _att.rollspeed; control_input.body_x_rate = _att.rollspeed;
control_input.body_y_rate = _att.pitchspeed; control_input.body_y_rate = _att.pitchspeed;
control_input.body_z_rate = _att.yawspeed; control_input.body_z_rate = _att.yawspeed;
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
-6
View File
@@ -274,12 +274,6 @@ private:
} _parameter_handles{}; /**< handles for interesting parameters */ } _parameter_handles{}; /**< handles for interesting parameters */
// Rotation matrix and euler angles to extract from control state
math::Matrix<3, 3> _R{};
float _roll{0.0f};
float _pitch{0.0f};
float _yaw{0.0f};
ECL_RollController _roll_ctrl; ECL_RollController _roll_ctrl;
ECL_PitchController _pitch_ctrl; ECL_PitchController _pitch_ctrl;
ECL_YawController _yaw_ctrl; ECL_YawController _yaw_ctrl;