fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 02:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

attitude setpoint topic: cleanup of matrix class usage

Browse Source 
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2016-09-27 22:03:36 +02:00
committed by Lorenz Meier
parent ee314d2f50
commit 3faaeb06d1
10 changed files with 31 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/bst/bst.cpp
+7 -5
View File
@@ -51,6 +51,8 @@
#include <uORB/topics/battery_status.h>
#include <mathlib/math/Quaternion.hpp>
using namespace matrix;
#define BST_DEVICE_PATH "/dev/bst0"
static const char commandline_usage[] = "usage: bst start|status|stop";
@@ -290,13 +292,13 @@ void BST::cycle()
if (updated) {
vehicle_attitude_s att;
orb_copy(ORB_ID(vehicle_attitude), _attitude_sub, &att);
matrix::Quaternion<float> q(&att.q[0]);
matrix::Euler<float> euler(q);
Quatf q(att.q);
Eulerf euler(q);
BSTPacket<BSTAttitude> bst_att = {};
bst_att.type = 0x1E;
bst_att.payload.roll = swap_int32(euler(0) * 10000);
bst_att.payload.pitch = swap_int32(euler(1) * 10000);
bst_att.payload.yaw = swap_int32(euler(2) * 10000);
bst_att.payload.roll = swap_int32(euler.phi() * 10000);
bst_att.payload.pitch = swap_int32(euler.theta() * 10000);
bst_att.payload.yaw = swap_int32(euler.psi() * 10000);
send_packet(bst_att);
}