From 619cc6aedce1c4a5f72fb5ea1c2a9a8678808938 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sat, 9 Jun 2018 14:07:37 -0400
Subject: [PATCH] mavlink disable conversion helpers and use Matrix
---
src/modules/mavlink/mavlink_bridge_header.h | 2 ++
src/modules/mavlink/mavlink_messages.cpp | 3 ++-
src/modules/mavlink/mavlink_receiver.cpp | 11 ++++++++---
3 files changed, 12 insertions(+), 4 deletions(-)
diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h
index a35573db75..d9c1a66958 100644
--- a/src/modules/mavlink/mavlink_bridge_header.h
+++ b/src/modules/mavlink/mavlink_bridge_header.h
@@ -42,6 +42,8 @@
#ifndef MAVLINK_BRIDGE_HEADER_H
#define MAVLINK_BRIDGE_HEADER_H
+#define MAVLINK_NO_CONVERSION_HELPERS
+
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
/* use efficient approach, see mavlink_helpers.h */
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index c3d931e77a..131fdf0b2f 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -2852,7 +2852,8 @@ protected:
memcpy(&msg.q[0], &att_sp.q_d[0], sizeof(msg.q));
} else {
- mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q);
+ matrix::Quatf q = matrix::Eulerf(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body);
+ memcpy(&msg.q[0], q.data(), sizeof(msg.q));
}
msg.body_roll_rate = att_rates_sp.roll;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index f8c3810464..5bdc543d9d 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1340,10 +1340,15 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
att_sp.timestamp = hrt_absolute_time();
if (!ignore_attitude_msg) { // only copy att sp if message contained new data
- mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
- att_sp.yaw_sp_move_rate = 0.0;
- memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
+ matrix::Quatf q(set_attitude_target.q);
+ q.copyTo(att_sp.q_d);
att_sp.q_d_valid = true;
+
+ matrix::Eulerf euler{q};
+ att_sp.roll_body = euler.phi();
+ att_sp.pitch_body = euler.theta();
+ att_sp.yaw_body = euler.psi();
+ att_sp.yaw_sp_move_rate = 0.0f;
}
if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
--
GitLab