From 9e67902cf04a4d9330fc9adde9d723399143f782 Mon Sep 17 00:00:00 2001 From: Jens Hauke Date: Wed, 25 Apr 2018 15:40:41 +0200 Subject: [PATCH] Implement MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet); This implements the integer only version of "uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q)". int16_t *data is assumed to be a vector with 3 components int16_t data[3]. +1g corresponds to +8192, sensitivity is 2g. --- Arduino/MPU6050/MPU6050_6Axis_MotionApps20.h | 11 +++++++++++ Arduino/MPU6050/MPU6050_9Axis_MotionApps41.h | 11 +++++++++++ .../components/MPU6050/MPU6050_6Axis_MotionApps20.h | 11 +++++++++++ .../components/MPU6050/MPU6050_9Axis_MotionApps41.h | 11 +++++++++++ MSP430/MPU6050/MPU6050_6Axis_MotionApps20.h | 11 +++++++++++ MSP430/MPU6050/MPU6050_9Axis_MotionApps41.h | 11 +++++++++++ 6 files changed, 66 insertions(+) diff --git a/Arduino/MPU6050/MPU6050_6Axis_MotionApps20.h b/Arduino/MPU6050/MPU6050_6Axis_MotionApps20.h index e3e3c0e8..1fdb2e83 100644 --- a/Arduino/MPU6050/MPU6050_6Axis_MotionApps20.h +++ b/Arduino/MPU6050/MPU6050_6Axis_MotionApps20.h @@ -675,6 +675,17 @@ uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Qu // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +8192, sensitivity is 2g. */ + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; + data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); + return status; +} + uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); diff --git a/Arduino/MPU6050/MPU6050_9Axis_MotionApps41.h b/Arduino/MPU6050/MPU6050_9Axis_MotionApps41.h index 016d0b0d..cf025146 100644 --- a/Arduino/MPU6050/MPU6050_9Axis_MotionApps41.h +++ b/Arduino/MPU6050/MPU6050_9Axis_MotionApps41.h @@ -780,6 +780,17 @@ uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Qu // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +8192, sensitivity is 2g. */ + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; + data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); + return status; +} + uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); diff --git a/ESP32_ESP-IDF/components/MPU6050/MPU6050_6Axis_MotionApps20.h b/ESP32_ESP-IDF/components/MPU6050/MPU6050_6Axis_MotionApps20.h index 8e3d0499..3d018568 100644 --- a/ESP32_ESP-IDF/components/MPU6050/MPU6050_6Axis_MotionApps20.h +++ b/ESP32_ESP-IDF/components/MPU6050/MPU6050_6Axis_MotionApps20.h @@ -680,6 +680,17 @@ uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Qu // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +8192, sensitivity is 2g. */ + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; + data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); + return status; +} + uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); diff --git a/ESP32_ESP-IDF/components/MPU6050/MPU6050_9Axis_MotionApps41.h b/ESP32_ESP-IDF/components/MPU6050/MPU6050_9Axis_MotionApps41.h index 016d0b0d..cf025146 100644 --- a/ESP32_ESP-IDF/components/MPU6050/MPU6050_9Axis_MotionApps41.h +++ b/ESP32_ESP-IDF/components/MPU6050/MPU6050_9Axis_MotionApps41.h @@ -780,6 +780,17 @@ uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Qu // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +8192, sensitivity is 2g. */ + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; + data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); + return status; +} + uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); diff --git a/MSP430/MPU6050/MPU6050_6Axis_MotionApps20.h b/MSP430/MPU6050/MPU6050_6Axis_MotionApps20.h index 6a8afe27..f65807a6 100644 --- a/MSP430/MPU6050/MPU6050_6Axis_MotionApps20.h +++ b/MSP430/MPU6050/MPU6050_6Axis_MotionApps20.h @@ -625,6 +625,17 @@ uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Qu // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +8192, sensitivity is 2g. */ + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; + data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); + return status; +} + uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); diff --git a/MSP430/MPU6050/MPU6050_9Axis_MotionApps41.h b/MSP430/MPU6050/MPU6050_9Axis_MotionApps41.h index aa04e1ba..87151e3b 100644 --- a/MSP430/MPU6050/MPU6050_9Axis_MotionApps41.h +++ b/MSP430/MPU6050/MPU6050_9Axis_MotionApps41.h @@ -736,6 +736,17 @@ uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Qu // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +8192, sensitivity is 2g. */ + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; + data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); + return status; +} + uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);