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

adapted to new vehicle attitude message

Browse Source
This commit is contained in:
tumbili
2016-04-20 17:51:47 +02:00
committed by Lorenz Meier
parent 215bfaa377
commit 5e0e522903
16 changed files with 142 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/bst/bst.cpp
+6 -3
View File
@@ -49,6 +49,7 @@
#include <math.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/battery_status.h>
#include <mathlib/math/Quaternion.hpp>
#define BST_DEVICE_PATH "/dev/bst0"
@@ -289,11 +290,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);
BSTPacket<BSTAttitude> bst_att = {};
bst_att.type = 0x1E;
bst_att.payload.roll = swap_int32(att.roll * 10000);
bst_att.payload.pitch = swap_int32(att.pitch * 10000);
bst_att.payload.yaw = swap_int32(att.yaw * 10000);
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);
send_packet(bst_att);
}