Model-Based Team / Mbed 2 deprecated CubicHand

Cubic Hand project for EECS 249A course.

Dependencies:   MMA8451Q TSI cc3000_hostdriver_mbedsocket NVIC_set_all_priorities mbed Multi_WS2811

DataGlove.cpp@22:0cc5b7e55419, 2014-12-11 (annotated)

Committer:
naren
Date:
Thu Dec 11 04:07:54 2014 +0000
Revision:
22:0cc5b7e55419
Parent:
19:e751d075e2e6
Child:
23:a8de4f87cada
Added moving average filter; ; correction, corrected are two new structures used for this purpose

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kalbers 3:5f5d75cba8e1 1 #include "DataGlove.h"
kalbers 3:5f5d75cba8e1 2
naren 14:0c4a26dc6873 3 #define STREAM_FINGERS_QUATERNION 1
naren 14:0c4a26dc6873 4 #define STREAM_QUATERNION 2
naren 14:0c4a26dc6873 5 #define STREAM_FINGERS_RAW 3
naren 14:0c4a26dc6873 6 #define STREAM_RAW 4
naren 14:0c4a26dc6873 7 #define STREAM_FINGERS 5
naren 14:0c4a26dc6873 8
kalbers 13:c701f1122797 9 DataGlove::DataGlove()
kalbers 3:5f5d75cba8e1 10 {
kalbers 13:c701f1122797 11 notConnectedCount = 0;
naren 14:0c4a26dc6873 12 gl = new Glove;
naren 22:0cc5b7e55419 13 correction = new Glove;
naren 22:0cc5b7e55419 14 corrected = new Glove;
kalbers 18:6737bf53ccd1 15 ReceiveCount = 0;
kalbers 19:e751d075e2e6 16 numReceived = 0;
naren 22:0cc5b7e55419 17 //Initialize correction values to 0
naren 22:0cc5b7e55419 18 correction->roll = 0.0;
naren 22:0cc5b7e55419 19 correction->pitch = 0.0;
naren 22:0cc5b7e55419 20 correction->yaw = 0.0;
naren 22:0cc5b7e55419 21 for (int i = 0; i < 5; i++)
naren 22:0cc5b7e55419 22 correction->fingers[i] = 0.0;
kalbers 3:5f5d75cba8e1 23 }
kalbers 3:5f5d75cba8e1 24
kalbers 3:5f5d75cba8e1 25 DataGlove::~DataGlove()
kalbers 3:5f5d75cba8e1 26 {
naren 14:0c4a26dc6873 27 delete(gl);
kalbers 3:5f5d75cba8e1 28 }
kalbers 3:5f5d75cba8e1 29
kalbers 3:5f5d75cba8e1 30 void DataGlove::Init()
kalbers 3:5f5d75cba8e1 31 {
naren 14:0c4a26dc6873 32 GloveSocket.Init();
kalbers 3:5f5d75cba8e1 33 GloveSocket.Connect();
naren 22:0cc5b7e55419 34 StreamData(STREAM_FINGERS_QUATERNION);
kalbers 15:b48ac00af0b8 35 }
kalbers 15:b48ac00af0b8 36
kalbers 18:6737bf53ccd1 37 void DataGlove::StreamData(uint8_t DataType)
kalbers 15:b48ac00af0b8 38 {
kalbers 3:5f5d75cba8e1 39 SendBuf[0] = '$';
kalbers 3:5f5d75cba8e1 40 SendBuf[1] = 0x0A;
kalbers 3:5f5d75cba8e1 41 SendBuf[2] = 0x03;
naren 14:0c4a26dc6873 42 // Streaming quaternion data
kalbers 18:6737bf53ccd1 43 SendBuf[3] = DataType;
kalbers 3:5f5d75cba8e1 44 SendBuf[4] = (SendBuf[0]+SendBuf[1]+SendBuf[2]+SendBuf[3])%256;
kalbers 3:5f5d75cba8e1 45 SendBuf[5] = '#';
kalbers 3:5f5d75cba8e1 46 GloveSocket.SendDataToGlove(SendBuf, 6);
kalbers 3:5f5d75cba8e1 47 }
kalbers 3:5f5d75cba8e1 48
kalbers 19:e751d075e2e6 49 void DataGlove::StopSampling()
kalbers 19:e751d075e2e6 50 {
kalbers 19:e751d075e2e6 51 SendBuf[0] = '$';
kalbers 19:e751d075e2e6 52 SendBuf[1] = 0x0B;
kalbers 19:e751d075e2e6 53 SendBuf[2] = 0x02;
kalbers 19:e751d075e2e6 54 // Streaming quaternion data
kalbers 19:e751d075e2e6 55 SendBuf[3] = (SendBuf[0]+SendBuf[1]+SendBuf[2])%256;
kalbers 19:e751d075e2e6 56 SendBuf[4] = '#';
kalbers 19:e751d075e2e6 57 GloveSocket.SendDataToGlove(SendBuf, 6);
kalbers 19:e751d075e2e6 58 }
kalbers 19:e751d075e2e6 59
kalbers 3:5f5d75cba8e1 60 void DataGlove::Receive()
kalbers 3:5f5d75cba8e1 61 {
kalbers 15:b48ac00af0b8 62 raw = false;
kalbers 19:e751d075e2e6 63 numReceived += GloveSocket.GetDataFromBuffer(Buf, 1);
kalbers 19:e751d075e2e6 64 //if(ReceiveCount++%25 == 0) printf("%d %c\r\n", ReceiveCount, Buf[0]);
kalbers 19:e751d075e2e6 65 if (Buf[0] == '$' && numReceived > 0)
kalbers 19:e751d075e2e6 66 {
naren 14:0c4a26dc6873 67 bcc = '$';
kalbers 19:e751d075e2e6 68 numReceived += GloveSocket.GetDataFromBuffer((Buf+numReceived), 2);
kalbers 19:e751d075e2e6 69 if ((numReceived > 2) && (Buf[1] == 0x0a))
kalbers 19:e751d075e2e6 70 {
kalbers 19:e751d075e2e6 71 pkglen = Buf[2];
naren 14:0c4a26dc6873 72 bcc += Buf[1];
kalbers 19:e751d075e2e6 73 bcc += Buf[2];
kalbers 19:e751d075e2e6 74 numReceived += GloveSocket.GetDataFromBuffer((Buf+numReceived), pkglen - numReceived + 3);
kalbers 19:e751d075e2e6 75 if ((numReceived - 3 )< pkglen)
kalbers 19:e751d075e2e6 76 {
kalbers 19:e751d075e2e6 77 if(numReceived == 0) notConnectedCount++;
kalbers 19:e751d075e2e6 78 return;
naren 14:0c4a26dc6873 79 }
kalbers 19:e751d075e2e6 80 for (u = 3; u <= pkglen; u++)
kalbers 19:e751d075e2e6 81 {
naren 14:0c4a26dc6873 82 bcc += Buf[u];
naren 14:0c4a26dc6873 83 }
kalbers 19:e751d075e2e6 84 if ((numReceived - 3 == pkglen) && (bcc == Buf[pkglen+1]))
kalbers 19:e751d075e2e6 85 {
kalbers 19:e751d075e2e6 86 memcpy(buffer, (Buf + 3), numReceived - 3);
kalbers 19:e751d075e2e6 87 /*timeOfArrival = (buffer[3] << 24) + (buffer[4] << 16) + (buffer[5] << 8) + buffer[6];
naren 14:0c4a26dc6873 88 id = (buffer[1] << 8) + buffer[2];
kalbers 19:e751d075e2e6 89 pkgtype = buffer[0];*/
kalbers 19:e751d075e2e6 90 timeOfArrival = (Buf[6] << 24) + (Buf[7] << 16) + (Buf[8] << 8) + Buf[9];
kalbers 19:e751d075e2e6 91 id = (Buf[4] << 8) + Buf[5];
kalbers 19:e751d075e2e6 92 pkgtype = Buf[3];
naren 22:0cc5b7e55419 93 if(ReceiveCount > 501 && ReceiveCount++%25 == 0) {//printf("%d %c\r\n", ReceiveCount, Buf[0]);
naren 22:0cc5b7e55419 94 printf("%f %f %f %f %f %f %f %f\r\n", corrected->roll, corrected->pitch, corrected->yaw,
naren 22:0cc5b7e55419 95 corrected->fingers[0], corrected->fingers[1], corrected->fingers[2],
naren 22:0cc5b7e55419 96 corrected->fingers[3], corrected->fingers[4]);
naren 22:0cc5b7e55419 97 }
naren 14:0c4a26dc6873 98 switch (pkgtype){
naren 14:0c4a26dc6873 99 case STREAM_FINGERS_QUATERNION:
naren 14:0c4a26dc6873 100 {
kalbers 18:6737bf53ccd1 101 ExtractFingersQuat();
naren 14:0c4a26dc6873 102 }
naren 14:0c4a26dc6873 103 break;
naren 14:0c4a26dc6873 104 case STREAM_QUATERNION:
naren 14:0c4a26dc6873 105 {
kalbers 18:6737bf53ccd1 106 ExtractQuat();
naren 14:0c4a26dc6873 107 }
naren 14:0c4a26dc6873 108 break;
naren 14:0c4a26dc6873 109 case STREAM_FINGERS_RAW:
naren 14:0c4a26dc6873 110 {
kalbers 18:6737bf53ccd1 111 ExtractFingersRaw();
naren 14:0c4a26dc6873 112 }
naren 14:0c4a26dc6873 113 break;
naren 14:0c4a26dc6873 114 case STREAM_RAW:
naren 14:0c4a26dc6873 115 {
kalbers 18:6737bf53ccd1 116 ExtractRaw();
naren 14:0c4a26dc6873 117 }
naren 14:0c4a26dc6873 118 break;
naren 14:0c4a26dc6873 119
naren 14:0c4a26dc6873 120 case STREAM_FINGERS:
naren 14:0c4a26dc6873 121 {
kalbers 18:6737bf53ccd1 122 ExtractFingers();
naren 14:0c4a26dc6873 123 }
naren 14:0c4a26dc6873 124 break;
naren 14:0c4a26dc6873 125 }
kalbers 19:e751d075e2e6 126 Buf[0] = 0; //Clear Token so no repeats.
naren 14:0c4a26dc6873 127 notConnectedCount = 0;
kalbers 19:e751d075e2e6 128 numReceived = 0;
naren 14:0c4a26dc6873 129 }
kalbers 19:e751d075e2e6 130 else
kalbers 19:e751d075e2e6 131 {
kalbers 19:e751d075e2e6 132 if (bcc!=Buf[pkglen+1])
naren 14:0c4a26dc6873 133 notConnectedCount++;
naren 14:0c4a26dc6873 134 }
naren 14:0c4a26dc6873 135 }
naren 22:0cc5b7e55419 136 // Normalize the data for the first 1000 samples
naren 22:0cc5b7e55419 137 if (ReceiveCount < 500) {
naren 22:0cc5b7e55419 138 correction->roll = (correction->roll*ReceiveCount + gl->roll)/(ReceiveCount+1);
naren 22:0cc5b7e55419 139 correction->pitch = (correction->pitch*ReceiveCount + gl->pitch)/(ReceiveCount+1);
naren 22:0cc5b7e55419 140 correction->yaw = (correction->yaw*ReceiveCount + gl->yaw)/(ReceiveCount+1);
naren 22:0cc5b7e55419 141 for (int iter = 0; iter < 5; iter++)
naren 22:0cc5b7e55419 142 correction->fingers[iter] = (correction->fingers[iter]*ReceiveCount + gl->fingers[iter])/(ReceiveCount+1);
naren 22:0cc5b7e55419 143 }
naren 22:0cc5b7e55419 144 else {
naren 22:0cc5b7e55419 145 corrected->roll = gl->roll - correction->roll;
naren 22:0cc5b7e55419 146 corrected->pitch = gl->pitch - correction->pitch;
naren 22:0cc5b7e55419 147 corrected->yaw = gl->yaw - correction->yaw;
naren 22:0cc5b7e55419 148 for (int iter = 0; iter < 5; iter++)
naren 22:0cc5b7e55419 149 corrected->fingers[iter] = gl->fingers[iter] - correction->fingers[iter];
naren 22:0cc5b7e55419 150 }
kalbers 3:5f5d75cba8e1 151 }
kalbers 3:5f5d75cba8e1 152 else {
kalbers 15:b48ac00af0b8 153 //printf("couldn't get data %d\r\n", notConnectedCount);
kalbers 19:e751d075e2e6 154 // Re-establishing communication in case no data is received for 1s (20 frames per second*1 = 20)
kalbers 19:e751d075e2e6 155 if (notConnectedCount > 200) {
naren 22:0cc5b7e55419 156 printf("Connection broke! Trying to re-establish... %d %c\r\n",numReceived,Buf[0]);
kalbers 19:e751d075e2e6 157 //GloveSocket.Reconnect();
kalbers 19:e751d075e2e6 158 //StreamData(STREAM_QUATERNION);
kalbers 19:e751d075e2e6 159 notConnectedCount = 0;
kalbers 19:e751d075e2e6 160 }
kalbers 19:e751d075e2e6 161 numReceived = 0;
naren 14:0c4a26dc6873 162 notConnectedCount++;
kalbers 3:5f5d75cba8e1 163 }
kalbers 19:e751d075e2e6 164
kalbers 3:5f5d75cba8e1 165 }
kalbers 3:5f5d75cba8e1 166
kalbers 18:6737bf53ccd1 167 void DataGlove::ExtractFingersQuat()
kalbers 18:6737bf53ccd1 168 {
kalbers 18:6737bf53ccd1 169 k = 7;
kalbers 18:6737bf53ccd1 170 q0 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
kalbers 18:6737bf53ccd1 171 k += 4;
kalbers 18:6737bf53ccd1 172 q1 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
kalbers 18:6737bf53ccd1 173 k += 4;
kalbers 18:6737bf53ccd1 174 q2 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
kalbers 18:6737bf53ccd1 175 k += 4;
kalbers 18:6737bf53ccd1 176 q3 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
kalbers 18:6737bf53ccd1 177 k += 4;
kalbers 18:6737bf53ccd1 178 for (y = 0; y < 5; y++)
kalbers 18:6737bf53ccd1 179 {
kalbers 18:6737bf53ccd1 180 gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[k + 1]);
kalbers 18:6737bf53ccd1 181 k += 2;
kalbers 18:6737bf53ccd1 182 }
kalbers 18:6737bf53ccd1 183 q00 = q0 / 32768.0;
kalbers 18:6737bf53ccd1 184 q11 = q1 / 32768.0;
kalbers 18:6737bf53ccd1 185 q22 = q2 / 32768.0;
kalbers 18:6737bf53ccd1 186 q33 = q3 / 32768.0;
kalbers 18:6737bf53ccd1 187
kalbers 18:6737bf53ccd1 188 norm = sqrt(q00 * q00 + q11 * q11 + q22 * q22 + q33 * q33);
kalbers 18:6737bf53ccd1 189 test = q00 * q11 + q22 * q33;
kalbers 18:6737bf53ccd1 190
kalbers 18:6737bf53ccd1 191 roll = 180.0 * atan2(2 * (q00 * q11 + q22 * q33), 1 - 2 * (q11 * q11 + q22 * q22)) / 3.1415;
kalbers 18:6737bf53ccd1 192 pitch = 180.0 * asin(2 * (q00 * q22 - q33 * q11)) / 3.1415;
kalbers 18:6737bf53ccd1 193 yaw = 180.0 * atan2(2 * (q00 * q33 + q11 * q22), 1 - 2 * (q22 * q22 + q33 * q33)) / 3.1415;
kalbers 18:6737bf53ccd1 194
kalbers 18:6737bf53ccd1 195 gl->roll = roll;
kalbers 18:6737bf53ccd1 196 gl->pitch = pitch;
kalbers 18:6737bf53ccd1 197 gl->yaw = yaw;
kalbers 18:6737bf53ccd1 198 gl->lastPkgTime = timeOfArrival;
kalbers 18:6737bf53ccd1 199
kalbers 18:6737bf53ccd1 200 }
kalbers 18:6737bf53ccd1 201 void DataGlove::ExtractQuat()
kalbers 18:6737bf53ccd1 202 {
kalbers 18:6737bf53ccd1 203 k = 7;
kalbers 18:6737bf53ccd1 204 q0 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
kalbers 18:6737bf53ccd1 205 k += 4;
kalbers 18:6737bf53ccd1 206 q1 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
kalbers 18:6737bf53ccd1 207 k += 4;
kalbers 18:6737bf53ccd1 208 q2 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
kalbers 18:6737bf53ccd1 209 k += 4;
kalbers 18:6737bf53ccd1 210 q3 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
kalbers 18:6737bf53ccd1 211 k += 4;
kalbers 18:6737bf53ccd1 212 for (y = 0; y < 5; y++)
kalbers 18:6737bf53ccd1 213 {
kalbers 18:6737bf53ccd1 214 gl->fingers[y] = 0.0;
kalbers 18:6737bf53ccd1 215 }
kalbers 18:6737bf53ccd1 216 q00 = q0 / 32768.0;
kalbers 18:6737bf53ccd1 217 q11 = q1 / 32768.0;
kalbers 18:6737bf53ccd1 218 q22 = q2 / 32768.0;
kalbers 18:6737bf53ccd1 219 q33 = q3 / 32768.0;
kalbers 18:6737bf53ccd1 220
kalbers 18:6737bf53ccd1 221 norm = sqrt(q00 * q00 + q11 * q11 + q22 * q22 + q33 * q33);
kalbers 18:6737bf53ccd1 222 test = q00 * q11 + q22 * q33;
kalbers 18:6737bf53ccd1 223 roll = 180.0 * atan2(2 * (q00 * q11 + q22 * q33), 1 - 2 * (q11 * q11 + q22 * q22)) / 3.1415;
kalbers 18:6737bf53ccd1 224 pitch = 180.0 * asin(2 * (q00 * q22 - q33 * q11)) / 3.1415;
kalbers 18:6737bf53ccd1 225 yaw = 180.0 * atan2(2 * (q00 * q33 + q11 * q22), 1 - 2 * (q22 * q22 + q33 * q33)) / 3.1415;
kalbers 18:6737bf53ccd1 226
kalbers 18:6737bf53ccd1 227 gl->roll = roll;
kalbers 18:6737bf53ccd1 228 gl->pitch = pitch;
kalbers 18:6737bf53ccd1 229 gl->yaw = yaw;
kalbers 18:6737bf53ccd1 230 gl->lastPkgTime = timeOfArrival;
kalbers 18:6737bf53ccd1 231
kalbers 18:6737bf53ccd1 232 }
kalbers 18:6737bf53ccd1 233
kalbers 18:6737bf53ccd1 234 void DataGlove::ExtractFingersRaw()
kalbers 3:5f5d75cba8e1 235 {
kalbers 18:6737bf53ccd1 236 k = 7;
kalbers 18:6737bf53ccd1 237 for (j = 0; j < 3; j++)
kalbers 18:6737bf53ccd1 238 {
kalbers 18:6737bf53ccd1 239 gl->gyro[j] = (buffer[k] << 8) + (buffer[k+1]);
kalbers 18:6737bf53ccd1 240 if (gl->gyro[j] > 0x7fff)
kalbers 18:6737bf53ccd1 241 gl->gyro[j] -= 0x10000;
kalbers 18:6737bf53ccd1 242 k += 2;
kalbers 18:6737bf53ccd1 243 }
kalbers 18:6737bf53ccd1 244 for (j = 0; j < 3; j++)
kalbers 18:6737bf53ccd1 245 {
kalbers 18:6737bf53ccd1 246 gl->magn[j] = (buffer[k] << 8) + (buffer[k+1]);
kalbers 18:6737bf53ccd1 247 if (gl->magn[j] > 0x7fff)
kalbers 18:6737bf53ccd1 248 gl->magn[j] -= 0x10000;
kalbers 18:6737bf53ccd1 249 k += 2;
kalbers 18:6737bf53ccd1 250 }
kalbers 18:6737bf53ccd1 251 for (j = 0; j < 3; j++)
kalbers 18:6737bf53ccd1 252 {
kalbers 18:6737bf53ccd1 253 gl->accel[j] = (buffer[k] << 8) + (buffer[k+1]);
kalbers 18:6737bf53ccd1 254 if (gl->accel[j]>0x7fff)
kalbers 18:6737bf53ccd1 255 gl->accel[j] -= 0x10000;
kalbers 18:6737bf53ccd1 256 k += 2;
kalbers 18:6737bf53ccd1 257 }
kalbers 18:6737bf53ccd1 258 for (y = 0; y < 5; y++)
kalbers 18:6737bf53ccd1 259 {
kalbers 18:6737bf53ccd1 260 gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[k+1]);
kalbers 18:6737bf53ccd1 261 k += 2;
kalbers 18:6737bf53ccd1 262 }
kalbers 18:6737bf53ccd1 263
kalbers 18:6737bf53ccd1 264 gl->roll = 0.0;
kalbers 18:6737bf53ccd1 265 gl->pitch = 0.0;
kalbers 18:6737bf53ccd1 266 gl->yaw = 0.0;
kalbers 18:6737bf53ccd1 267 gl->lastPkgTime = timeOfArrival;
kalbers 18:6737bf53ccd1 268 }
kalbers 18:6737bf53ccd1 269
kalbers 18:6737bf53ccd1 270 void DataGlove::ExtractRaw()
kalbers 18:6737bf53ccd1 271 {
kalbers 18:6737bf53ccd1 272 k = 7;
kalbers 18:6737bf53ccd1 273 for (j = 0; j < 3; j++){
kalbers 18:6737bf53ccd1 274 gl->gyro[j] = (buffer[k] << 8) + (buffer[k+1]);
kalbers 18:6737bf53ccd1 275 if (gl->gyro[j] > 0x7fff)
kalbers 18:6737bf53ccd1 276 gl->gyro[j] -= 0x10000;
kalbers 18:6737bf53ccd1 277 k += 2;
kalbers 18:6737bf53ccd1 278 }
kalbers 18:6737bf53ccd1 279 for (j = 0; j < 3; j++){
kalbers 18:6737bf53ccd1 280 gl->magn[j] = (buffer[k] << 8) + (buffer[k+1]);
kalbers 18:6737bf53ccd1 281 if (gl->magn[j] > 0x7fff)
kalbers 18:6737bf53ccd1 282 gl->magn[j] -= 0x10000;
kalbers 18:6737bf53ccd1 283 k += 2;
kalbers 18:6737bf53ccd1 284 }
kalbers 18:6737bf53ccd1 285 for (j = 0; j < 3; j++){
kalbers 18:6737bf53ccd1 286 gl->accel[j] = (buffer[k] << 8) + (buffer[k+1]);
kalbers 18:6737bf53ccd1 287 if (gl->accel[j] > 0x7fff)
kalbers 18:6737bf53ccd1 288 gl->accel[j] -= 0x10000;
kalbers 18:6737bf53ccd1 289 k += 2;
kalbers 18:6737bf53ccd1 290 }
kalbers 18:6737bf53ccd1 291 int y = 0;
kalbers 18:6737bf53ccd1 292 for (y = 0; y < 5; y++){
kalbers 18:6737bf53ccd1 293 gl->fingers[y] = 0.0;
kalbers 18:6737bf53ccd1 294 }
kalbers 18:6737bf53ccd1 295
kalbers 18:6737bf53ccd1 296 gl->roll = 0.0;
kalbers 18:6737bf53ccd1 297 gl->pitch = 0.0;
kalbers 18:6737bf53ccd1 298 gl->yaw = 0.0;
kalbers 18:6737bf53ccd1 299 gl->lastPkgTime = timeOfArrival;
kalbers 18:6737bf53ccd1 300 }
kalbers 18:6737bf53ccd1 301
kalbers 18:6737bf53ccd1 302 void DataGlove::ExtractFingers()
kalbers 18:6737bf53ccd1 303 {
kalbers 18:6737bf53ccd1 304 k = 7;
kalbers 18:6737bf53ccd1 305 for (y = 0; y < 5; y++){
kalbers 18:6737bf53ccd1 306 gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[k+1]);
kalbers 18:6737bf53ccd1 307 k += 2;
kalbers 18:6737bf53ccd1 308 }
kalbers 18:6737bf53ccd1 309
kalbers 18:6737bf53ccd1 310 gl->roll = 0.0;
kalbers 18:6737bf53ccd1 311 gl->pitch = 0.0;
kalbers 18:6737bf53ccd1 312 gl->yaw = 0.0;
kalbers 18:6737bf53ccd1 313 gl->lastPkgTime = timeOfArrival;
kalbers 3:5f5d75cba8e1 314 }