File content as of revision 22:0cc5b7e55419:

#include "DataGlove.h"

#define STREAM_FINGERS_QUATERNION 1
#define STREAM_QUATERNION 2
#define STREAM_FINGERS_RAW 3
#define STREAM_RAW 4
#define STREAM_FINGERS 5

DataGlove::DataGlove()
{
	notConnectedCount = 0;
	gl = new Glove;
	correction = new Glove;
	corrected = new Glove;
	ReceiveCount = 0;
    numReceived = 0;
    //Initialize correction values to 0
    correction->roll = 0.0;
    correction->pitch = 0.0;
    correction->yaw = 0.0;
    for (int i = 0; i < 5; i++)
    	correction->fingers[i] = 0.0;
}

DataGlove::~DataGlove()
{
	delete(gl);
}

void DataGlove::Init()
{
	GloveSocket.Init();
	GloveSocket.Connect();
	StreamData(STREAM_FINGERS_QUATERNION);
}

void DataGlove::StreamData(uint8_t DataType)
{
    SendBuf[0] = '$';
    SendBuf[1] = 0x0A;
    SendBuf[2] = 0x03;
    // Streaming quaternion data
    SendBuf[3] = DataType;
    SendBuf[4] = (SendBuf[0]+SendBuf[1]+SendBuf[2]+SendBuf[3])%256;
    SendBuf[5] = '#';
    GloveSocket.SendDataToGlove(SendBuf, 6);
}

void DataGlove::StopSampling()
{
    SendBuf[0] = '$';
    SendBuf[1] = 0x0B;
    SendBuf[2] = 0x02;
    // Streaming quaternion data
    SendBuf[3] = (SendBuf[0]+SendBuf[1]+SendBuf[2])%256;
    SendBuf[4] = '#';
    GloveSocket.SendDataToGlove(SendBuf, 6);
}

void DataGlove::Receive()
{
    raw = false;
    numReceived += GloveSocket.GetDataFromBuffer(Buf, 1);
    //if(ReceiveCount++%25 == 0) printf("%d %c\r\n", ReceiveCount, Buf[0]);
    if (Buf[0] == '$' && numReceived > 0) 
    {
        bcc = '$';
        numReceived += GloveSocket.GetDataFromBuffer((Buf+numReceived), 2);
        if ((numReceived > 2) && (Buf[1] == 0x0a)) 
        {
            pkglen = Buf[2];
            bcc += Buf[1];
            bcc += Buf[2];
            numReceived += GloveSocket.GetDataFromBuffer((Buf+numReceived), pkglen - numReceived + 3);
            if ((numReceived  - 3 )< pkglen) 
            {
                if(numReceived == 0) notConnectedCount++;
                return;
            }
            for (u = 3; u <= pkglen; u++)
            {
                bcc += Buf[u];
            }
            if ((numReceived - 3 == pkglen) && (bcc == Buf[pkglen+1])) 
            {
                memcpy(buffer, (Buf + 3), numReceived - 3);
                /*timeOfArrival = (buffer[3] << 24) + (buffer[4] << 16) + (buffer[5] << 8) + buffer[6];
                id = (buffer[1] << 8) + buffer[2];
                pkgtype = buffer[0];*/
                timeOfArrival = (Buf[6] << 24) + (Buf[7] << 16) + (Buf[8] << 8) + Buf[9];
                id = (Buf[4] << 8) + Buf[5];
                pkgtype = Buf[3];
                if(ReceiveCount > 501 && ReceiveCount++%25 == 0) {//printf("%d %c\r\n", ReceiveCount, Buf[0]);
                	printf("%f %f %f %f %f %f %f %f\r\n", corrected->roll, corrected->pitch, corrected->yaw, 
                	corrected->fingers[0], corrected->fingers[1], corrected->fingers[2], 
                	corrected->fingers[3], corrected->fingers[4]);
                }
                switch (pkgtype){
                    case STREAM_FINGERS_QUATERNION:
                    {
                        ExtractFingersQuat();
                    }
                    break;
                    case STREAM_QUATERNION:
                    {
                        ExtractQuat();
                    }
                    break;
                    case STREAM_FINGERS_RAW:
                    {
                        ExtractFingersRaw();
                    }
                    break;
                    case STREAM_RAW:
                    {
                        ExtractRaw();
                    }
                    break;

                    case STREAM_FINGERS:
                    {
                        ExtractFingers();
                    }
                    break;
                }
                Buf[0] = 0; //Clear Token so no repeats.
                notConnectedCount = 0;
                numReceived = 0;
            }
            else
            {
                if (bcc!=Buf[pkglen+1])
                    notConnectedCount++;
            }
        }
        // Normalize the data for the first 1000 samples
        if (ReceiveCount < 500) {
        	correction->roll = (correction->roll*ReceiveCount + gl->roll)/(ReceiveCount+1);
        	correction->pitch = (correction->pitch*ReceiveCount + gl->pitch)/(ReceiveCount+1);
        	correction->yaw = (correction->yaw*ReceiveCount + gl->yaw)/(ReceiveCount+1);
        	for (int iter = 0; iter < 5; iter++)
        		correction->fingers[iter] = (correction->fingers[iter]*ReceiveCount + gl->fingers[iter])/(ReceiveCount+1);
        }
        else {
        	corrected->roll = gl->roll - correction->roll;
        	corrected->pitch = gl->pitch - correction->pitch;
        	corrected->yaw = gl->yaw - correction->yaw;
        	for (int iter = 0; iter < 5; iter++)
        		corrected->fingers[iter] = gl->fingers[iter] - correction->fingers[iter];
        }
    }
    else {
        //printf("couldn't get data %d\r\n", notConnectedCount);
        // Re-establishing communication in case no data is received for 1s (20 frames per second*1 = 20)
        if (notConnectedCount > 200) {
            printf("Connection broke! Trying to re-establish... %d %c\r\n",numReceived,Buf[0]);
            //GloveSocket.Reconnect();
            //StreamData(STREAM_QUATERNION);
            notConnectedCount = 0;
        }
        numReceived = 0;
        notConnectedCount++;
    }
    
}

void DataGlove::ExtractFingersQuat()
{
    k = 7;
    q0 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
    k += 4;
    q1 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
    k += 4;
    q2 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
    k += 4;
    q3 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
    k += 4;
    for (y = 0; y < 5; y++) 
    {
        gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[k + 1]);
        k += 2;
    }
    q00 = q0 / 32768.0;
    q11 = q1 / 32768.0;
    q22 = q2 / 32768.0;
    q33 = q3 / 32768.0;

    norm = sqrt(q00 * q00 + q11 * q11 + q22 * q22 + q33 * q33);
    test = q00 * q11 + q22 * q33;

    roll = 180.0 * atan2(2 * (q00 * q11 + q22 * q33), 1 - 2 * (q11 * q11 + q22 * q22)) / 3.1415;       
    pitch = 180.0 * asin(2 * (q00 * q22 - q33 * q11)) / 3.1415;
    yaw = 180.0 * atan2(2 * (q00 * q33 + q11 * q22), 1 - 2 * (q22 * q22 + q33 * q33)) / 3.1415;

    gl->roll = roll;
    gl->pitch = pitch;
    gl->yaw = yaw;
    gl->lastPkgTime = timeOfArrival;

}
void DataGlove::ExtractQuat()
{
    k = 7;
    q0 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
    k += 4;
    q1 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
    k += 4;
    q2 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
    k += 4;
    q3 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
    k += 4;
    for (y = 0; y < 5; y++) 
    {
        gl->fingers[y] = 0.0;
    }
    q00 = q0 / 32768.0;
    q11 = q1 / 32768.0;
    q22 = q2 / 32768.0;
    q33 = q3 / 32768.0;

    norm = sqrt(q00 * q00 + q11 * q11 + q22 * q22 + q33 * q33);
    test = q00 * q11 + q22 * q33;
    roll = 180.0 * atan2(2 * (q00 * q11 + q22 * q33), 1 - 2 * (q11 * q11 + q22 * q22)) / 3.1415;       
    pitch = 180.0 * asin(2 * (q00 * q22 - q33 * q11)) / 3.1415;
    yaw = 180.0 * atan2(2 * (q00 * q33 + q11 * q22), 1 - 2 * (q22 * q22 + q33 * q33)) / 3.1415;

    gl->roll = roll;
    gl->pitch = pitch;
    gl->yaw = yaw;
    gl->lastPkgTime = timeOfArrival;

}

void DataGlove::ExtractFingersRaw()
{
    k = 7;
    for (j = 0; j < 3; j++)
    {
        gl->gyro[j] = (buffer[k] << 8) + (buffer[k+1]);
        if (gl->gyro[j] > 0x7fff) 
            gl->gyro[j] -= 0x10000;
        k += 2;
    }
    for (j = 0; j < 3; j++)
    {
        gl->magn[j] = (buffer[k] << 8) + (buffer[k+1]);
        if (gl->magn[j] > 0x7fff) 
            gl->magn[j] -= 0x10000;
        k += 2;
    }
    for (j = 0; j < 3; j++)
    {
        gl->accel[j] = (buffer[k] << 8) + (buffer[k+1]);
        if (gl->accel[j]>0x7fff) 
            gl->accel[j] -= 0x10000;
        k += 2;
    }
    for (y = 0; y < 5; y++)
    {
        gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[k+1]);
        k += 2;
    }

    gl->roll = 0.0;
    gl->pitch = 0.0;
    gl->yaw = 0.0;
    gl->lastPkgTime = timeOfArrival;
}

void DataGlove::ExtractRaw()
{
    k = 7;
    for (j = 0; j < 3; j++){
        gl->gyro[j] = (buffer[k] << 8) + (buffer[k+1]);
        if (gl->gyro[j] > 0x7fff) 
            gl->gyro[j] -= 0x10000;
        k += 2;
    }
    for (j = 0; j < 3; j++){
        gl->magn[j] = (buffer[k] << 8) + (buffer[k+1]);
        if (gl->magn[j] > 0x7fff) 
            gl->magn[j] -= 0x10000;
        k += 2;
    }
    for (j = 0; j < 3; j++){
        gl->accel[j] = (buffer[k] << 8) + (buffer[k+1]);
        if (gl->accel[j] > 0x7fff) 
            gl->accel[j] -= 0x10000;
        k += 2;
    }
    int y = 0;
    for (y = 0; y < 5; y++){
        gl->fingers[y] = 0.0;
    }
    
    gl->roll = 0.0;
    gl->pitch = 0.0;
    gl->yaw = 0.0;
    gl->lastPkgTime = timeOfArrival;
}

void DataGlove::ExtractFingers()
{
    k = 7;
    for (y = 0; y < 5; y++){
        gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[k+1]);
        k += 2;
    }
    
    gl->roll = 0.0;
    gl->pitch = 0.0;
    gl->yaw = 0.0;
    gl->lastPkgTime = timeOfArrival;
}