Skip to content

Commit

Permalink
Merge pull request #374 from jensh/master
Browse files Browse the repository at this point in the history 
Implement MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet);
  • Loading branch information
@jrowberg
jrowberg committed Apr 25, 2018
2 parents 3dc9538 + 9e67902 commit 7ea8b7c
Show file tree
Hide file tree
Showing 6 changed files with 66 additions and 0 deletions.
11 changes: 11 additions & 0 deletions Arduino/MPU6050/MPU6050_6Axis_MotionApps20.h
View file
Expand Up @@ -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);
Expand Down
11 changes: 11 additions & 0 deletions Arduino/MPU6050/MPU6050_9Axis_MotionApps41.h
View file
Expand Up @@ -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);
Expand Down
11 changes: 11 additions & 0 deletions ESP32_ESP-IDF/components/MPU6050/MPU6050_6Axis_MotionApps20.h
View file
Expand Up @@ -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);
Expand Down
11 changes: 11 additions & 0 deletions ESP32_ESP-IDF/components/MPU6050/MPU6050_9Axis_MotionApps41.h
View file
Expand Up @@ -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);
Expand Down
11 changes: 11 additions & 0 deletions MSP430/MPU6050/MPU6050_6Axis_MotionApps20.h
View file
Expand Up @@ -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);
Expand Down
11 changes: 11 additions & 0 deletions MSP430/MPU6050/MPU6050_9Axis_MotionApps41.h
View file
Expand Up @@ -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);
Expand Down

0 comments on commit 7ea8b7c

Please sign in to comment.