From a11cced4b2f46d6714b215026aee6bc413a00424 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 11 Apr 2023 14:20:30 -0400 Subject: [PATCH] drivers/ins/vectornav: fix attitude (quaternion) and GPS packet extract --- src/drivers/ins/vectornav/VectorNav.cpp | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/src/drivers/ins/vectornav/VectorNav.cpp b/src/drivers/ins/vectornav/VectorNav.cpp index bb38ab7e8fc..8b3bc951450 100644 --- a/src/drivers/ins/vectornav/VectorNav.cpp +++ b/src/drivers/ins/vectornav/VectorNav.cpp @@ -195,10 +195,10 @@ void VectorNav::sensorCallback(VnUartPacket *packet) // publish attitude vehicle_attitude_s attitude{}; attitude.timestamp_sample = time_now_us; - attitude.q[0] = quaternion.c[0]; - attitude.q[1] = quaternion.c[1]; - attitude.q[2] = quaternion.c[2]; - attitude.q[3] = quaternion.c[3]; + attitude.q[0] = quaternion.c[3]; + attitude.q[1] = quaternion.c[0]; + attitude.q[2] = quaternion.c[1]; + attitude.q[3] = quaternion.c[2]; attitude.timestamp = hrt_absolute_time(); _attitude_pub.publish(attitude); @@ -282,14 +282,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet) (void)time_startup; // GPSGROUP_UTC - // TimeUtc timeUtc; - // timeUtc.year = VnUartPacket_extractInt8(packet); - // 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); + TimeUtc timeUtc = VnUartPacket_extractTimeUtc(packet); + (void)timeUtc; // GPSGROUP_NUMSATS const uint8_t numSats = VnUartPacket_extractUint8(packet);