added position and speed calculation and messaging
Martin
parent
5d24919125
commit
21b5674b27
|
|
@ -63,7 +63,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 fifoCount; // count of all bytes currently in FIFO
|
||||
uint8_t fifoBuffer[64]; // FIFO storage buffer
|
||||
unsigned long timeOn = 0;
|
||||
unsigned long timeOn;
|
||||
|
||||
// orientation/motion vars
|
||||
Quaternion q; // [w, x, y, z] quaternion container
|
||||
|
|
@ -74,6 +74,8 @@ VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measure
|
|||
VectorFloat gravity; // [x, y, z] gravity vector
|
||||
float euler[3]; // [psi, theta, phi] Euler angle container
|
||||
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
|
||||
Matrix<3> position; // [x,y,z] tracks position of device
|
||||
Matrix<3> speed; // [x,y,z] tracks speed of device
|
||||
|
||||
|
||||
// Sem dobimo vrednosti
|
||||
|
|
@ -91,6 +93,9 @@ OSCMessage emsg("/error/");
|
|||
OSCMessage kmsg("/keys/");
|
||||
OSCMessage quaternionMessage("/quaternion/");
|
||||
OSCMessage quaternionDiffMessage("/quaternionDiff/");
|
||||
OSCMessage eulerDiffMessage("/eulerDiff/");
|
||||
OSCMessage positionMessage("/position/");
|
||||
OSCMessage speedMessage("/speed/");
|
||||
|
||||
void setup() {
|
||||
Wire.begin();
|
||||
|
|
@ -101,6 +106,11 @@ void setup() {
|
|||
pinMode(keys[i], INPUT_PULLUP);
|
||||
}
|
||||
|
||||
//Set position to origin, speed to nothing, and uptime to 0
|
||||
timeOn = 0;
|
||||
position.Fill(0);
|
||||
speed.Fill(0);
|
||||
|
||||
Serial.begin(115200); // set this as high as you can reliably run on your platform
|
||||
SerialBT.begin("wavey wind");
|
||||
|
||||
|
|
@ -158,6 +168,50 @@ void setup() {
|
|||
}
|
||||
}
|
||||
|
||||
BLA::Matrix<3> eulerFromQuaternion(Quaternion q) {
|
||||
float x2 = q.x + q.x; float y2 = q.y + q.y; float z2 = q.z + q.z;
|
||||
float xx = q.x * x2; float xy = q.x * y2; float xz = q.x * z2;
|
||||
float yy = q.y * y2; float yz = q.y * z2; float zz = q.z * z2;
|
||||
float wx = q.w * x2; float wy = q.w * y2; float wz = q.w * z2;
|
||||
|
||||
BLA::Matrix<4,4> rotationMatrix = {
|
||||
1 - (yy + zz), xy + wz, xz - wy, 0,
|
||||
xy - wz, 1 - ( xx + zz ), yz + wx, 0,
|
||||
xz + wy, yz - wx, 1 - ( xx + yy ), 0,
|
||||
0, 0, 0, 1
|
||||
};
|
||||
|
||||
//TODO: test whether BLA library uses column-major matrix notation in code
|
||||
BLA::Matrix<3> eulerVector;
|
||||
eulerVector.Fill(0);
|
||||
eulerVector(1) = asin(clamp(rotationMatrix(1,3),-1,1));
|
||||
if (fabsf(rotationMatrix(1,3)) < 0.9999999) {
|
||||
eulerVector(0) = atan2f(-rotationMatrix(2,3), rotationMatrix(3,3));
|
||||
eulerVector(2) = atan2f( -rotationMatrix(1,2), rotationMatrix(1,1));
|
||||
} else {
|
||||
eulerVector(0) = atan2f(rotationMatrix(3,2), rotationMatrix(2,2));
|
||||
eulerVector(2) = 0;
|
||||
}
|
||||
return eulerVector;
|
||||
}
|
||||
|
||||
void streamAndClearMessage(OSCMessage msg) {
|
||||
SLIPSerial.beginPacket();
|
||||
msg.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
|
||||
SLIPBTSerial.beginPacket();
|
||||
msg.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
|
||||
msg.empty();
|
||||
}
|
||||
|
||||
float clamp(float value,float min,float max) {
|
||||
return fmaxf( min, fminf(max, value));
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
// if programming failed, don't try to do anything
|
||||
if (!dmpReady) return;
|
||||
|
|
@ -180,47 +234,23 @@ void loop() {
|
|||
quaternionMessage.add(q.x);
|
||||
quaternionMessage.add(q.y);
|
||||
quaternionMessage.add(q.z);
|
||||
|
||||
SLIPBTSerial.beginPacket();
|
||||
quaternionMessage.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
|
||||
SLIPSerial.beginPacket();
|
||||
quaternionMessage.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
|
||||
quaternionMessage.empty();
|
||||
streamAndClearMessage(quaternionMessage);
|
||||
|
||||
quaternionDiffMessage.add(diff.w);
|
||||
quaternionDiffMessage.add(diff.x);
|
||||
quaternionDiffMessage.add(diff.y);
|
||||
quaternionDiffMessage.add(diff.z);
|
||||
|
||||
SLIPBTSerial.beginPacket();
|
||||
quaternionDiffMessage.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
|
||||
SLIPSerial.beginPacket();
|
||||
quaternionDiffMessage.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
|
||||
quaternionDiffMessage.empty();
|
||||
streamAndClearMessage(quaternionDiffMessage);
|
||||
|
||||
Matrix<3> eulerDiffVector = eulerFromQuaternion(diff);
|
||||
|
||||
eulerDiffMessage.add(eulerDiffVector(0));
|
||||
eulerDiffMessage.add(eulerDiffVector(1));
|
||||
eulerDiffMessage.add(eulerDiffVector(2));
|
||||
|
||||
SLIPBTSerial.beginPacket();
|
||||
eulerDiffMessage.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
|
||||
SLIPSerial.beginPacket();
|
||||
eulerDiffMessage.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
|
||||
eulerDiffMessage.empty();
|
||||
streamAndClearMessage(eulerDiffMessage);
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -275,6 +305,11 @@ void loop() {
|
|||
int prevTime = timeOn;
|
||||
timeOn = millis();
|
||||
int elapsedTime = timeOn - prevTime;
|
||||
Matrix<3> speedGain = {AcX * elapsedTime, AcY * elapsedTime, AcZ * elapsedTime};
|
||||
|
||||
//Assume linear acceleration over measured time window, multiply time by halfpoint between last-known and current speed
|
||||
position = position + (((speed + speedGain) + speed) /2 * elapsedTime);
|
||||
speed += speedGain;
|
||||
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
|
|
@ -283,63 +318,27 @@ void loop() {
|
|||
msg.add(AcY);
|
||||
msg.add(AcZ);
|
||||
msg.add(elapsedTime);
|
||||
|
||||
streamAndClearMessage(msg);
|
||||
|
||||
positionMessage.add(position(0));
|
||||
positionMessage.add(position(1));
|
||||
positionMessage.add(position(2));
|
||||
|
||||
streamAndClearMessage(positionMessage);
|
||||
|
||||
speedMessage.add(speed(0));
|
||||
speedMessage.add(speed(1));
|
||||
speedMessage.add(speed(2));
|
||||
|
||||
streamAndClearMessage(speedMessage);
|
||||
|
||||
|
||||
SLIPSerial.beginPacket();
|
||||
msg.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
|
||||
SLIPBTSerial.beginPacket();
|
||||
msg.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
|
||||
msg.empty();
|
||||
|
||||
// Send keys
|
||||
for(int i = 0; i < KEYLEN; i++) {
|
||||
pressed[i] = !digitalRead(keys[i]);
|
||||
kmsg.add(pressed[i]);
|
||||
}
|
||||
|
||||
SLIPSerial.beginPacket();
|
||||
kmsg.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
|
||||
SLIPBTSerial.beginPacket();
|
||||
kmsg.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
|
||||
kmsg.empty();
|
||||
streamAndClearMessage(kmsg);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
BLA::Matrix<3> eulerFromQuaternion(Quaternion q) {
|
||||
float x2 = q.x + q.x; float y2 = q.y + q.y; float z2 = q.z + q.z;
|
||||
float xx = q.x * x2; float xy = q.x * y2; float xz = q.x * z2;
|
||||
float yy = q.y * y2; float yz = q.y * z2; float zz = q.z * z2;
|
||||
float wx = q.w * x2; float wy = q.w * y2; float wz = q.w * z2;
|
||||
|
||||
BLA::Matrix<4,4> rotationMatrix = {
|
||||
1 - (yy + zz), xy + wz, xz - wy, 0,
|
||||
xy - wz, 1 - ( xx + zz ), yz + wx, 0,
|
||||
xz + wy, yz - wx, 1 - ( xx + yy ), 0,
|
||||
0, 0, 0, 1
|
||||
};
|
||||
|
||||
//TODO: test whether BLA library uses column-major matrix notation in code
|
||||
BLA::Matrix<3> eulerVector;
|
||||
eulerVector.Fill(0);
|
||||
eulerVector(1) = asin(clamp(rotationMatrix(1,3),-1,1));
|
||||
if (fabsf(rotationMatrix(1,3)) < 0.9999999) {
|
||||
eulerVector(0) = atan2f(-rotationMatrix(2,3), rotationMatrix(3,3));
|
||||
eulerVector(2) = atan2f( -rotationMatrix(1,2), rotationMatrix(1,1));
|
||||
} else {
|
||||
eulerVector(0) = atan2f(rotationMatrix(3,2), rotationMatrix(2,2));
|
||||
eulerVector(2) = 0;
|
||||
}
|
||||
return eulerVector;
|
||||
}
|
||||
|
||||
float clamp(float value,float min,float max) {
|
||||
return fmaxf( min, fminf(max, value));
|
||||
}
|
||||
Loading…
Reference in New Issue