diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-19 14:10:25 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-19 14:10:25 +0400 |
| commit | ba612c3ee8b88b9352e7cfa723997887dd736b76 (patch) | |
| tree | 8b101b50259ffebd4de12891f4f684d6d84c3f53 /src/modules/mavlink/mavlink_receiver.cpp | |
| parent | e3a5a384d7b3678d1cbef63dc28fbe9a8f1de940 (diff) | |
| download | px4-firmware-ba612c3ee8b88b9352e7cfa723997887dd736b76.tar.gz px4-firmware-ba612c3ee8b88b9352e7cfa723997887dd736b76.tar.bz2 px4-firmware-ba612c3ee8b88b9352e7cfa723997887dd736b76.zip | |
mathlib fixes
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.cpp')
| -rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 11 |
1 files changed, 6 insertions, 5 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7b6fad658..653d4b6b3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -683,8 +683,9 @@ handle_message(mavlink_message_t *msg) /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); - math::Dcm C_nb(q); - math::EulerAngles euler(C_nb); + math::Matrix<3,3> C_nb; + C_nb.from_quaternion(q); + math::Vector<3> euler = C_nb.to_euler(); /* set rotation matrix */ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) @@ -699,9 +700,9 @@ handle_message(mavlink_message_t *msg) hil_attitude.q[3] = q(3); hil_attitude.q_valid = true; - hil_attitude.roll = euler.getPhi(); - hil_attitude.pitch = euler.getTheta(); - hil_attitude.yaw = euler.getPsi(); + hil_attitude.roll = euler(0); + hil_attitude.pitch = euler(1); + hil_attitude.yaw = euler(2); hil_attitude.rollspeed = hil_state.rollspeed; hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.yawspeed = hil_state.yawspeed; |