File content as of revision 15:b48ac00af0b8:

#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;
}

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

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

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

void DataGlove::Receive()
{
    raw = false;
    numReceived = GloveSocket.GetDataFromBuffer(Buf, 1);
    if (Buf[0] == '$') {
        bcc = '$';
        numReceived = GloveSocket.GetDataFromBuffer(Buf, 2);
        if ((numReceived == 2) && (Buf[0] == 0x0a)) {
            pkglen = Buf[1];
            bcc += Buf[0];
            bcc += Buf[1];
            numReceived = GloveSocket.GetDataFromBuffer(Buf, pkglen);
            if (numReceived < pkglen) {
                notConnectedCount++;
            }
            for (u = 0; u < pkglen - 2; u++) {
                bcc += Buf[u];
            }
            if ((numReceived == pkglen) && (bcc == Buf[pkglen-2])) {
                memcpy(buffer, Buf, numReceived);
                timeOfArrival = (buffer[3] << 24) + (buffer[4] << 16) + (buffer[5] << 8) + buffer[6];
                id = (buffer[1] << 8) + buffer[2];
                pkgtype = buffer[0];
                switch (pkgtype){
                    case STREAM_FINGERS_QUATERNION:
                    {
                        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;
                    }
                    break;
                    case STREAM_QUATERNION:
                    {
                        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;
                        printf("roll: %f\tpitch: %f\tyaw: %f\r\n", gl->roll, gl->pitch, gl->yaw);
                    }
                    break;
                    case STREAM_FINGERS_RAW:
                    {
                        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;
                    }
                    break;
                    case STREAM_RAW:
                    {
                        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;
                        printf("%f %f %f %f %f %f\r\n", gl->gyro[0], gl->gyro[1], gl->gyro[2], gl->accel[0], gl->accel[1], gl->accel[2]);
                    }
                    break;

                    case STREAM_FINGERS:
                    {
                        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;
                    }
                    break;
                }
                notConnectedCount = 0;
            }
            else{
                if (bcc!=Buf[pkglen-2])
                    notConnectedCount++;
            }
        }
    }
    else {
        //printf("couldn't get data %d\r\n", notConnectedCount);
        notConnectedCount++;
    }
    // Re-establishing communication in case no data is received for 1s (20 frames per second*1 = 20)
    if (notConnectedCount > 20) {
        printf("Connection broke! Trying to re-establish...\r\n");
        GloveSocket.Reconnect();
        StreamData();
        notConnectedCount = 0;
    }
}

void DataGlove::Parse()
{
}