fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 17:48:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink disable conversion helpers and use Matrix

Browse Source
This commit is contained in:
Daniel Agar
2018-06-09 21:19:33 +02:00
committed by Lorenz Meier
parent a76c82f5f2
commit 619cc6aedc
3 changed files with 12 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_bridge_header.h
+2
View File
@@ -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 */
src/modules/mavlink/mavlink_messages.cpp
+2 -1
View File
@@ -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;
src/modules/mavlink/mavlink_receiver.cpp
+8 -3
View File
@@ -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