From 973cc3eea9522cad2c0206690b7fc2b22716bcb5 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 11 Jan 2024 16:26:06 +0530 Subject: [PATCH] minor changes to data structure --- src/MPU6050_6Axis_MotionApps20.cpp | 53 +++++++++++++++--------------- 1 file changed, 27 insertions(+), 26 deletions(-) diff --git a/src/MPU6050_6Axis_MotionApps20.cpp b/src/MPU6050_6Axis_MotionApps20.cpp index 4ec91c0..b8decc0 100644 --- a/src/MPU6050_6Axis_MotionApps20.cpp +++ b/src/MPU6050_6Axis_MotionApps20.cpp @@ -408,43 +408,44 @@ bool MPU6050_6Axis_MotionApps20::dmpPacketAvailable() { uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccel(int32_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[28] << 24) | ((uint32_t)packet[29] << 16) | ((uint32_t)packet[30] << 8) | packet[31]); - data[1] = (((uint32_t)packet[32] << 24) | ((uint32_t)packet[33] << 16) | ((uint32_t)packet[34] << 8) | packet[35]); - data[2] = (((uint32_t)packet[36] << 24) | ((uint32_t)packet[37] << 16) | ((uint32_t)packet[38] << 8) | packet[39]); + data[0] = (((int32_t)packet[28] << 24) | ((int32_t)packet[29] << 16) | ((int32_t)packet[30] << 8) | (int32_t)packet[31]); + data[1] = (((int32_t)packet[32] << 24) | ((int32_t)packet[33] << 16) | ((int32_t)packet[34] << 8) | (int32_t)packet[35]); + data[2] = (((int32_t)packet[36] << 24) | ((int32_t)packet[37] << 16) | ((int32_t)packet[38] << 8) | (int32_t)packet[39]); return 0; } uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccel(int16_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[28] << 8) | packet[29]; - data[1] = (packet[32] << 8) | packet[33]; - data[2] = (packet[36] << 8) | packet[37]; + data[0] = ((int16_t)packet[28] << 8) | (int16_t)packet[29]; + data[1] = ((int16_t)packet[32] << 8) | (int16_t)packet[33]; + data[2] = ((int16_t)packet[36] << 8) | (int16_t)packet[37]; return 0; } + uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; - v -> x = (packet[28] << 8) | packet[29]; - v -> y = (packet[32] << 8) | packet[33]; - v -> z = (packet[36] << 8) | packet[37]; + v -> x = ((int16_t)packet[28] << 8) | (int16_t)packet[29]; + v -> y = ((int16_t)packet[32] << 8) | (int16_t)packet[33]; + v -> z = ((int16_t)packet[36] << 8) | (int16_t)packet[37]; return 0; } uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); - data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); - data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); - data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); + data[0] = (((int32_t)packet[0] << 24) | ((int32_t)packet[1] << 16) | ((int32_t)packet[2] << 8) | (int32_t)packet[3]); + data[1] = (((int32_t)packet[4] << 24) | ((int32_t)packet[5] << 16) | ((int32_t)packet[6] << 8) | (int32_t)packet[7]); + data[2] = (((int32_t)packet[8] << 24) | ((int32_t)packet[9] << 16) | ((int32_t)packet[10] << 8) | (int32_t)packet[11]); + data[3] = (((int32_t)packet[12] << 24) | ((int32_t)packet[13] << 16) | ((int32_t)packet[14] << 8) | (int32_t)packet[15]); return 0; } uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; - data[0] = ((packet[0] << 8) | packet[1]); - data[1] = ((packet[4] << 8) | packet[5]); - data[2] = ((packet[8] << 8) | packet[9]); - data[3] = ((packet[12] << 8) | packet[13]); + data[0] = (((int16_t)packet[0] << 8) | (int16_t)packet[1]); + data[1] = (((int16_t)packet[4] << 8) | (int16_t)packet[5]); + data[2] = (((int16_t)packet[8] << 8) | (int16_t)packet[9]); + data[3] = (((int16_t)packet[12] << 8) | (int16_t)packet[13]); return 0; } uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { @@ -465,25 +466,25 @@ uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternion(Quaternion *q, const uint8_ uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyro(int32_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); - data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); - data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]); + data[0] = (((int32_t)packet[16] << 24) | ((int32_t)packet[17] << 16) | ((int32_t)packet[18] << 8) | (int32_t)packet[19]); + data[1] = (((int32_t)packet[20] << 24) | ((int32_t)packet[21] << 16) | ((int32_t)packet[22] << 8) | (int32_t)packet[23]); + data[2] = (((int32_t)packet[24] << 24) | ((int32_t)packet[25] << 16) | ((int32_t)packet[26] << 8) | (int32_t)packet[27]); return 0; } uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyro(int16_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[16] << 8) | packet[17]; - data[1] = (packet[20] << 8) | packet[21]; - data[2] = (packet[24] << 8) | packet[25]; + data[0] = ((int16_t)packet[16] << 8) | (int16_t)packet[17]; + data[1] = ((int16_t)packet[20] << 8) | (int16_t)packet[21]; + data[2] = ((int16_t)packet[24] << 8) | (int16_t)packet[25]; return 0; } uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; - v -> x = (packet[16] << 8) | packet[17]; - v -> y = (packet[20] << 8) | packet[21]; - v -> z = (packet[24] << 8) | packet[25]; + v -> x = ((int16_t)packet[16] << 8) | (int16_t)packet[17]; + v -> y = ((int16_t)packet[20] << 8) | (int16_t)packet[21]; + v -> z = ((int16_t)packet[24] << 8) | (int16_t)packet[25]; return 0; } // uint8_t MPU6050_6Axis_MotionApps20::dmpSetLinearAccelFilterCoefficient(float coef);