Skip to content

Commit 9e67902

Browse files
jenshjensh
committed
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.
1 parent 3dc9538 commit 9e67902

File tree

6 files changed

+66
-0
lines changed

6 files changed

+66
-0
lines changed

‎Arduino/MPU6050/MPU6050_6Axis_MotionApps20.h

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -675,6 +675,17 @@ uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Qu
675675
// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
676676
// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
677677
// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
678+
uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) {
679+
/* +1g corresponds to +8192, sensitivity is 2g. */
680+
int16_t qI[4];
681+
uint8_t status = dmpGetQuaternion(qI, packet);
682+
data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384;
683+
data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384;
684+
data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1]
685+
- (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384);
686+
return status;
687+
}
688+
678689
uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
679690
v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
680691
v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);

‎Arduino/MPU6050/MPU6050_9Axis_MotionApps41.h

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -780,6 +780,17 @@ uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Qu
780780
// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
781781
// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
782782
// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
783+
uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) {
784+
/* +1g corresponds to +8192, sensitivity is 2g. */
785+
int16_t qI[4];
786+
uint8_t status = dmpGetQuaternion(qI, packet);
787+
data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384;
788+
data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384;
789+
data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1]
790+
- (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384);
791+
return status;
792+
}
793+
783794
uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
784795
v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
785796
v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);

‎ESP32_ESP-IDF/components/MPU6050/MPU6050_6Axis_MotionApps20.h

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -680,6 +680,17 @@ uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Qu
680680
// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
681681
// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
682682
// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
683+
uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) {
684+
/* +1g corresponds to +8192, sensitivity is 2g. */
685+
int16_t qI[4];
686+
uint8_t status = dmpGetQuaternion(qI, packet);
687+
data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384;
688+
data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384;
689+
data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1]
690+
- (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384);
691+
return status;
692+
}
693+
683694
uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
684695
v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
685696
v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);

‎ESP32_ESP-IDF/components/MPU6050/MPU6050_9Axis_MotionApps41.h

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -780,6 +780,17 @@ uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Qu
780780
// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
781781
// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
782782
// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
783+
uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) {
784+
/* +1g corresponds to +8192, sensitivity is 2g. */
785+
int16_t qI[4];
786+
uint8_t status = dmpGetQuaternion(qI, packet);
787+
data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384;
788+
data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384;
789+
data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1]
790+
- (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384);
791+
return status;
792+
}
793+
783794
uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
784795
v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
785796
v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);

‎MSP430/MPU6050/MPU6050_6Axis_MotionApps20.h

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -625,6 +625,17 @@ uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Qu
625625
// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
626626
// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
627627
// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
628+
uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) {
629+
/* +1g corresponds to +8192, sensitivity is 2g. */
630+
int16_t qI[4];
631+
uint8_t status = dmpGetQuaternion(qI, packet);
632+
data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384;
633+
data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384;
634+
data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1]
635+
- (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384);
636+
return status;
637+
}
638+
628639
uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
629640
v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
630641
v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);

‎MSP430/MPU6050/MPU6050_9Axis_MotionApps41.h

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -736,6 +736,17 @@ uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Qu
736736
// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
737737
// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
738738
// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
739+
uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) {
740+
/* +1g corresponds to +8192, sensitivity is 2g. */
741+
int16_t qI[4];
742+
uint8_t status = dmpGetQuaternion(qI, packet);
743+
data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384;
744+
data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384;
745+
data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1]
746+
- (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384);
747+
return status;
748+
}
749+
739750
uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
740751
v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
741752
v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);

0 commit comments

Comments
 (0)