diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-05 09:02:06 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-05 09:02:06 +0100 |
commit | 2f646e7082b49ef92bf9be1defd6fb7fdeec8480 (patch) | |
tree | 07caf9c50fcd0d965314f8416dd5bef3a13a5b1f /src/modules/mc_att_control/mc_att_control_sim.cpp | |
parent | 4c950eb76b0d63be7936dfcd68eb6eed266b91ad (diff) | |
download | px4-firmware-2f646e7082b49ef92bf9be1defd6fb7fdeec8480.tar.gz px4-firmware-2f646e7082b49ef92bf9be1defd6fb7fdeec8480.tar.bz2 px4-firmware-2f646e7082b49ef92bf9be1defd6fb7fdeec8480.zip |
mc att control: ran fix code style script
Diffstat (limited to 'src/modules/mc_att_control/mc_att_control_sim.cpp')
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_sim.cpp | 56 |
1 files changed, 30 insertions, 26 deletions
diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp index 61a4237fc..faf729420 100644 --- a/src/modules/mc_att_control/mc_att_control_sim.cpp +++ b/src/modules/mc_att_control/mc_att_control_sim.cpp @@ -46,7 +46,8 @@ using namespace std; #endif -void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude) { +void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude) +{ math::Quaternion quat; quat(0) = (float)attitude.w(); quat(1) = (float)attitude.x(); @@ -58,48 +59,51 @@ void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> _v_att.q[2] = quat(2); _v_att.q[3] = quat(3); - math::Matrix<3,3> Rot = quat.to_dcm(); - _v_att.R[0][0] = Rot(0,0); - _v_att.R[1][0] = Rot(1,0); - _v_att.R[2][0] = Rot(2,0); - _v_att.R[0][1] = Rot(0,1); - _v_att.R[1][1] = Rot(1,1); - _v_att.R[2][1] = Rot(2,1); - _v_att.R[0][2] = Rot(0,2); - _v_att.R[1][2] = Rot(1,2); - _v_att.R[2][2] = Rot(2,2); + math::Matrix<3, 3> Rot = quat.to_dcm(); + _v_att.R[0][0] = Rot(0, 0); + _v_att.R[1][0] = Rot(1, 0); + _v_att.R[2][0] = Rot(2, 0); + _v_att.R[0][1] = Rot(0, 1); + _v_att.R[1][1] = Rot(1, 1); + _v_att.R[2][1] = Rot(2, 1); + _v_att.R[0][2] = Rot(0, 2); + _v_att.R[1][2] = Rot(1, 2); + _v_att.R[2][2] = Rot(2, 2); _v_att.R_valid = true; } -void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d& angular_rate) { +void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d &angular_rate) +{ // check if this is consistent !!! _v_att.rollspeed = angular_rate(0); _v_att.pitchspeed = angular_rate(1); _v_att.yawspeed = angular_rate(2); } -void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { +void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference) +{ _v_att_sp.roll_body = control_attitude_thrust_reference(0); _v_att_sp.pitch_body = control_attitude_thrust_reference(1); _v_att_sp.yaw_body = control_attitude_thrust_reference(2); - _v_att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; + _v_att_sp.thrust = (control_attitude_thrust_reference(3) - 30) * (-1) / 30; // setup rotation matrix - math::Matrix<3,3> Rot_sp; - Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); - _v_att_sp.R_body[0][0] = Rot_sp(0,0); - _v_att_sp.R_body[1][0] = Rot_sp(1,0); - _v_att_sp.R_body[2][0] = Rot_sp(2,0); - _v_att_sp.R_body[0][1] = Rot_sp(0,1); - _v_att_sp.R_body[1][1] = Rot_sp(1,1); - _v_att_sp.R_body[2][1] = Rot_sp(2,1); - _v_att_sp.R_body[0][2] = Rot_sp(0,2); - _v_att_sp.R_body[1][2] = Rot_sp(1,2); - _v_att_sp.R_body[2][2] = Rot_sp(2,2); + math::Matrix<3, 3> Rot_sp; + Rot_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + _v_att_sp.R_body[0][0] = Rot_sp(0, 0); + _v_att_sp.R_body[1][0] = Rot_sp(1, 0); + _v_att_sp.R_body[2][0] = Rot_sp(2, 0); + _v_att_sp.R_body[0][1] = Rot_sp(0, 1); + _v_att_sp.R_body[1][1] = Rot_sp(1, 1); + _v_att_sp.R_body[2][1] = Rot_sp(2, 1); + _v_att_sp.R_body[0][2] = Rot_sp(0, 2); + _v_att_sp.R_body[1][2] = Rot_sp(1, 2); + _v_att_sp.R_body[2][2] = Rot_sp(2, 2); } -void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d& motor_inputs) { +void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d &motor_inputs) +{ motor_inputs(0) = _actuators.control[0]; motor_inputs(1) = _actuators.control[1]; motor_inputs(2) = _actuators.control[2]; |