Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

drivers/ins/vectornav: fix attitude (quaternion) and GPS packet extract

Browse Source
This commit is contained in:
Daniel Agar
2023-04-11 14:20:30 -04:00
parent 5c1e0ddd96
commit a11cced4b2
1 changed files with 6 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/ins/vectornav/VectorNav.cpp
+6 -12
View File
@@ -195,10 +195,10 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
// publish attitude // publish attitude
vehicle_attitude_s attitude{}; vehicle_attitude_s attitude{};
attitude.timestamp_sample = time_now_us; attitude.timestamp_sample = time_now_us;
attitude.q[0] = quaternion.c[0]; attitude.q[0] = quaternion.c[3];
attitude.q[1] = quaternion.c[1]; attitude.q[1] = quaternion.c[0];
attitude.q[2] = quaternion.c[2]; attitude.q[2] = quaternion.c[1];
attitude.q[3] = quaternion.c[3]; attitude.q[3] = quaternion.c[2];
attitude.timestamp = hrt_absolute_time(); attitude.timestamp = hrt_absolute_time();
_attitude_pub.publish(attitude); _attitude_pub.publish(attitude);
@@ -282,14 +282,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
(void)time_startup; (void)time_startup;
// GPSGROUP_UTC // GPSGROUP_UTC
// TimeUtc timeUtc; TimeUtc timeUtc = VnUartPacket_extractTimeUtc(packet);
// timeUtc.year = VnUartPacket_extractInt8(packet); (void)timeUtc;
// timeUtc.month = VnUartPacket_extractUint8(packet);
// timeUtc.day = VnUartPacket_extractUint8(packet);
// timeUtc.hour = VnUartPacket_extractUint8(packet);
// timeUtc.min = VnUartPacket_extractUint8(packet);
// timeUtc.sec = VnUartPacket_extractUint8(packet);
// timeUtc.ms = VnUartPacket_extractUint16(packet);
// GPSGROUP_NUMSATS // GPSGROUP_NUMSATS
const uint8_t numSats = VnUartPacket_extractUint8(packet); const uint8_t numSats = VnUartPacket_extractUint8(packet);