Compare commits
3 Commits
b020f8cd5d
...
f0a3c9df44
Author | SHA1 | Date |
---|---|---|
Martin Krauser | f0a3c9df44 | |
Martin Krauser | 7c93796221 | |
Martin Krauser | 618547af52 |
|
@ -58,6 +58,7 @@ uint8_t devStatus; // return status after each device operation (0 = succes
|
||||||
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
|
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
|
||||||
uint16_t fifoCount; // count of all bytes currently in FIFO
|
uint16_t fifoCount; // count of all bytes currently in FIFO
|
||||||
uint8_t fifoBuffer[64]; // FIFO storage buffer
|
uint8_t fifoBuffer[64]; // FIFO storage buffer
|
||||||
|
uint32_t time = 0;
|
||||||
|
|
||||||
// orientation/motion vars
|
// orientation/motion vars
|
||||||
Quaternion q; // [w, x, y, z] quaternion container
|
Quaternion q; // [w, x, y, z] quaternion container
|
||||||
|
@ -160,15 +161,20 @@ void loop() {
|
||||||
|
|
||||||
#ifdef OUTPUT_READABLE_QUATERNION
|
#ifdef OUTPUT_READABLE_QUATERNION
|
||||||
// display quaternion values in easy matrix form: w x y z
|
// display quaternion values in easy matrix form: w x y z
|
||||||
|
Quaternion previousQ(q.w,q.x,q.y,q.z);
|
||||||
|
|
||||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||||
|
|
||||||
//oscmsg = qOSC(q.w, q.x, q.y, q.z);
|
//compute the differential rotation between the previous and new orientation
|
||||||
|
Quaternion diff = q.getProduct(previousQ.getConjugate());
|
||||||
|
|
||||||
|
//oscmsg = qOSC(q.w, q.x, q.y, q.z, diff.w, diff.x, diff.y, diff,z);
|
||||||
|
|
||||||
qmsg.add(q.w);
|
qmsg.add(q.w);
|
||||||
qmsg.add(q.x);
|
qmsg.add(q.x);
|
||||||
qmsg.add(q.y);
|
qmsg.add(q.y);
|
||||||
qmsg.add(q.z);
|
qmsg.add(q.z);
|
||||||
|
|
||||||
SLIPBTSerial.beginPacket();
|
SLIPBTSerial.beginPacket();
|
||||||
qmsg.send(SLIPBTSerial);
|
qmsg.send(SLIPBTSerial);
|
||||||
SLIPBTSerial.endPacket();
|
SLIPBTSerial.endPacket();
|
||||||
|
@ -227,10 +233,14 @@ void loop() {
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// Send (accel) over serial
|
// Send (accel) and time elapsed over serial
|
||||||
|
int prevTime = time;
|
||||||
|
time = millis();
|
||||||
msg.add(AcX);
|
msg.add(AcX);
|
||||||
msg.add(AcY);
|
msg.add(AcY);
|
||||||
msg.add(AcZ);
|
msg.add(AcZ);
|
||||||
|
msg.add(time - prevTime);
|
||||||
|
|
||||||
|
|
||||||
SLIPSerial.beginPacket();
|
SLIPSerial.beginPacket();
|
||||||
msg.send(SLIPSerial);
|
msg.send(SLIPSerial);
|
||||||
|
|
Loading…
Reference in New Issue