Skip to content

Commit

Permalink
minor changes to data structure
Browse files Browse the repository at this point in the history
  • Loading branch information
@KalanaRatnayake
KalanaRatnayake committed Jan 11, 2024
1 parent 23f1637 commit 973cc3e
Showing 1 changed file with 27 additions and 26 deletions.
53 changes: 27 additions & 26 deletions src/MPU6050_6Axis_MotionApps20.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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);
Expand Down

0 comments on commit 973cc3e

Please sign in to comment.