aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mc_att_control/mc_att_control_sim.cpp
diff options
context:
space:
mode:
authorThomas Gubler <thomasgubler@gmail.com>2014-12-05 09:02:06 +0100
committerThomas Gubler <thomasgubler@gmail.com>2014-12-05 09:02:06 +0100
commit2f646e7082b49ef92bf9be1defd6fb7fdeec8480 (patch)
tree07caf9c50fcd0d965314f8416dd5bef3a13a5b1f /src/modules/mc_att_control/mc_att_control_sim.cpp
parent4c950eb76b0d63be7936dfcd68eb6eed266b91ad (diff)
downloadpx4-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.cpp56
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];