File content as of revision 1:dd81f4065b6b:

#include "pixy2.h"

PIXY2::PIXY2(PinName tx, PinName rx, int debit)
{
    _Pixy2 = new Serial (tx, rx, debit);
    _Pixy2->attach (callback(this,&PIXY2::pixy2_getByte));
    etat = idle;
    rPointer = 0;
    Pixy2_buffer = (Byte*) malloc (0x100);
}

void PIXY2::pixy2_getByte ()    // Interruption de la pixy2
{
    static Byte             startPoint;
    T_Word                  *buffer;
    
    Pixy2_buffer[wPointer] = _Pixy2->getc();                                 // On stocke l'octet reçu dans la première case dispo du tableau temporaire
    wPointer++;
    
    switch (etat) {
        case messageSent : // Si on a envoyé une requete => on attend un entête
            if ( (wPointer - rPointer) >= 2) { // On attend d'avoir reçu 2 octets
                startPoint = wPointer - 1;
                buffer = (T_Word*) &Pixy2_buffer[startPoint]; //On pointe la structure sur les 2 derniers octets reçus
                if ((buffer->mot == PIXY2_CSSYNC) || (buffer->mot == PIXY2_SYNC)) {
                    etat = receivingHeader;     // On passe à l'état réception de l'entête
                    rPointer = startPoint;
                    if (buffer->mot == PIXY2_SYNC)      frameContainChecksum = 0;   // On mémorise qu'il n'y a pas de checksum à vérifier
                    else                                frameContainChecksum = 1;   // On mémorise qu'il y a un checksum à vérifier
                }
            }
            break;

        case receivingHeader :  //Si on est en train de recevoir un entête (entre le SYNC et... La fin de l'entête)
            if ((frameContainChecksum && ((wPointer - rPointer) == PIXY2_CSHEADERSIZE)) || (!frameContainChecksum && ((wPointer - rPointer) == PIXY2_NCSHEADERSIZE))) {
                //Si on a reçu 6 octets pour une trame avec checksum ou 4 pour une trame sans checksum
                etat = headerReceived;          //On passe à entête reçu
                dataSize = Pixy2_buffer[startPoint+3]; // Et on lit la taille de la payload
            }
            break;

        case headerReceived :   // Si on vient de recevoir le premier octet de données
            startPoint = wPointer;  // On enregistre sa position
            etat = receivingData;   // Et on dit que l'on est en train de recevoir des données
            break;

        case receivingData :    // Si on est en train de recevoir des données.
            if (wPointer == (dataSize + startPoint)) etat = dataReceived; //Quand on a reçu toutes les données promises on dit que c'est OK
            break;

        default : // On ne traite volontairement ici pas tous les cas, en particulier idle et dataReceived. C'est à la fonction de le faire ! Le reste est lié à des réceptions de données.
            break;
    }
}

int PIXY2::pixy2_sndGetVersion (void){
    T_pixy2sendBuffer   msg;
    int                 i = 0, dataSize = 0;
    msg.frame.header.pixSync = PIXY2_SYNC;
    msg.frame.header.pixType = PIXY2_ASK_VERS;
    msg.frame.header.pixLength = 0;
    do {
        while(!_Pixy2->writable());
        _Pixy2->putc(msg.data[i]);
        i++;
    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
    return 0;
}

int PIXY2::pixy2_sndGetResolution (void){
    T_pixy2sendBuffer   msg;
    int                 i = 0, dataSize = 1;
    msg.frame.header.pixSync = PIXY2_SYNC;
    msg.frame.header.pixType = PIXY2_ASK_RESOL;
    msg.frame.header.pixLength = 1;
    msg.frame.data[0] = 0;
    do {
        while(!_Pixy2->writable());
        _Pixy2->putc(msg.data[i]);
        i++;
    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
    return 0;
}

int PIXY2::pixy2_sndSetCameraBrightness (Byte brightness){
    T_pixy2sendBuffer   msg;
    int                 i = 0, dataSize = 1;
    msg.frame.header.pixSync = PIXY2_SYNC;
    msg.frame.header.pixType = PIXY2_SET_BRIGHT;
    msg.frame.header.pixLength = 1;
    msg.frame.data[0] = brightness;
    do {
        while(!_Pixy2->writable());
        _Pixy2->putc(msg.data[i]);
        i++;
    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
    return 0;
}

int PIXY2::pixy2_sndSetServo (Word s0, Word s1){
    T_pixy2sendBuffer   msg;
    int                 i = 0, dataSize = 4;
    T_Word              tmp;
    msg.frame.header.pixSync = PIXY2_SYNC;
    msg.frame.header.pixType = PIXY2_SET_SERVOS;
    msg.frame.header.pixLength = 1;
    tmp.mot = s0;
    msg.frame.data[0] = tmp.octet[0];
    msg.frame.data[1] = tmp.octet[1];
    tmp.mot = s1;
    msg.frame.data[2] = tmp.octet[0];
    msg.frame.data[3] = tmp.octet[1];
    do {
        while(!_Pixy2->writable());
        _Pixy2->putc(msg.data[i]);
        i++;
    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
    return 0;
}

int PIXY2::pixy2_sndSetLED (Byte red, Byte green, Byte blue){
    T_pixy2sendBuffer   msg;
    int                 i = 0, dataSize = 3;
    msg.frame.header.pixSync = PIXY2_SYNC;
    msg.frame.header.pixType = PIXY2_SET_LED;
    msg.frame.header.pixLength = 1;
    msg.frame.data[0] = red;
    msg.frame.data[1] = green;
    msg.frame.data[2] = blue;
    do {
        while(!_Pixy2->writable());
        _Pixy2->putc(msg.data[i]);
        i++;
    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
    return 0;
}

int PIXY2::pixy2_sndSetLamp (Byte upper, Byte lower){
    T_pixy2sendBuffer   msg;
    int                 i = 0, dataSize = 2;
    msg.frame.header.pixSync = PIXY2_SYNC;
    msg.frame.header.pixType = PIXY2_SET_LED;
    msg.frame.header.pixLength = 1;
    msg.frame.data[0] = upper;
    msg.frame.data[1] = lower;
    do {
        while(!_Pixy2->writable());
        _Pixy2->putc(msg.data[i]);
        i++;
    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
    return 0;
}

int PIXY2::pixy2_sndGetFPS (void){
    T_pixy2sendBuffer   msg;
    int                 i = 0, dataSize = 0;
    msg.frame.header.pixSync = PIXY2_SYNC;
    msg.frame.header.pixType = PIXY2_ASK_FPS;
    msg.frame.header.pixLength = 0;
    do {
        while(!_Pixy2->writable());
        _Pixy2->putc(msg.data[i]);
        i++;
    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
    return 0;
}

int PIXY2::pixy2_sndGetBlocks (Byte sigmap, Byte maxBloc){
    T_pixy2sendBuffer   msg;
    int                 i = 0, dataSize = 2;
    msg.frame.header.pixSync = PIXY2_SYNC;
    msg.frame.header.pixType = PIXY2_ASK_BLOC;
    msg.frame.header.pixLength = 1;
    msg.frame.data[0] = sigmap;
    msg.frame.data[1] = maxBloc;
    do {
        while(!_Pixy2->writable());
        _Pixy2->putc(msg.data[i]);
        i++;
    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
    return 0;
}

int PIXY2::pixy2_sndGetMainFeature (Byte type, Byte feature){
    T_pixy2sendBuffer   msg;
    int                 i = 0, dataSize = 2;
    msg.frame.header.pixSync = PIXY2_SYNC;
    msg.frame.header.pixType = PIXY2_ASK_LINE;
    msg.frame.header.pixLength = 1;
    msg.frame.data[0] = type;
    msg.frame.data[1] = feature;
    do {
        while(!_Pixy2->writable());
        _Pixy2->putc(msg.data[i]);
        i++;
    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
    return 0;
}

int PIXY2::pixy2_sndSetMode (Byte mode){
    T_pixy2sendBuffer   msg;
    int                 i = 0, dataSize = 1;
    msg.frame.header.pixSync = PIXY2_SYNC;
    msg.frame.header.pixType = PIXY2_SET_MODE;
    msg.frame.header.pixLength = 1;
    msg.frame.data[0] = mode;
    do {
        while(!_Pixy2->writable());
        _Pixy2->putc(msg.data[i]);
        i++;
    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
    return 0;
}

int PIXY2::pixy2_sndSetNexTurn (Word angle){
    T_pixy2sendBuffer   msg;
    int                 i = 0, dataSize = 2;
    T_Word              tmp;
    tmp.mot = angle;
    msg.frame.header.pixSync = PIXY2_SYNC;
    msg.frame.header.pixType = PIXY2_SET_TURN;
    msg.frame.header.pixLength = 1;
    tmp.mot = angle;
    msg.frame.data[0] = tmp.octet[0];
    msg.frame.data[1] = tmp.octet[1];
    do {
        while(!_Pixy2->writable());
        _Pixy2->putc(msg.data[i]);
        i++;
    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
    return 0;
}

int PIXY2::pixy2_sndSetDefaultTurn (Word angle){
    T_pixy2sendBuffer   msg;
    int                 i = 0, dataSize = 2;
    T_Word              tmp;
    tmp.mot = angle;
    msg.frame.header.pixSync = PIXY2_SYNC;
    msg.frame.header.pixType = PIXY2_SET_DEFTURN;
    msg.frame.header.pixLength = 1;
    tmp.mot = angle;
    msg.frame.data[0] = tmp.octet[0];
    msg.frame.data[1] = tmp.octet[1];
    do {
        while(!_Pixy2->writable());
        _Pixy2->putc(msg.data[i]);
        i++;
    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
    return 0;
}

int PIXY2::pixy2_sndSetVector (Byte vectorIndex){
    T_pixy2sendBuffer   msg;
    int                 i = 0, dataSize = 1;
    msg.frame.header.pixSync = PIXY2_SYNC;
    msg.frame.header.pixType = PIXY2_SET_VECTOR;
    msg.frame.header.pixLength = 1;
    msg.frame.data[0] = vectorIndex;
    do {
        while(!_Pixy2->writable());
        _Pixy2->putc(msg.data[i]);
        i++;
    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
    return 0;
}

int PIXY2::pixy2_sndReverseVector (void){
    T_pixy2sendBuffer   msg;
    int                 i = 0, dataSize = 0;
    msg.frame.header.pixSync = PIXY2_SYNC;
    msg.frame.header.pixType = PIXY2_SET_REVERSE;
    msg.frame.header.pixLength = 0;
    do {
        while(!_Pixy2->writable());
        _Pixy2->putc(msg.data[i]);
        i++;
    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
    return 0;
}

int PIXY2::pixy2_sndGetRGB (Word x, Word y, Byte saturate){
    T_pixy2sendBuffer   msg;
    int                 i = 0, dataSize = 3;
    msg.frame.header.pixSync = PIXY2_SYNC;
    msg.frame.header.pixType = PIXY2_ASK_VIDEO;
    msg.frame.header.pixLength = 1;
    msg.frame.data[0] = x;
    msg.frame.data[1] = y;
    msg.frame.data[2] = saturate;
    do {
        while(!_Pixy2->writable());
        _Pixy2->putc(msg.data[i]);
        i++;
    } while (i<(PIXY2_NCSHEADERSIZE+dataSize));
    return 0;
}

/*  La fonction est bloquante à l'envoi (pas vraiment le choix), mais elle est non bloquante en réception. On essayera de faire une fonction non bloquante en envoi avec write, mais c'est pas la priorité.
Le principe c'est de stocker dans un buffer circulaire les données au fur et à mesure qu'elle sont reçues et de traiter uniquement en castant les infos. Pour cela, il faut recevoir et stocker. */

int PIXY2::pixy2_getVersion (T_Pixy2Version *version){

    T_pixy2RcvHeader    *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[rPointer];
    int                 cr = 0;
    
    switch (etat) {
        case idle :
            PIXY2::pixy2_sndGetVersion();
            etat = messageSent;
            rPointer = wPointer;
            cr = -2;                        //Busy
            break;
            
        case dataReceived :
            if (frameContainChecksum) {
                if ( pixy2_validateChecksum (&Pixy2_buffer[rPointer]) ) {
                    if (msg->pixType == PIXY2_REP_VERS) {
                        version = (T_Pixy2Version*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
                        etat = idle;
                    } else cr = -7;   // Type Error
                } else cr = -3;       // Checksum Error
            } else {
                if (msg->pixType == PIXY2_REP_VERS) {
                    version = (T_Pixy2Version*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
                    etat = idle;
                } else cr = -7;       // Type Error
            }
            break;

        default :
            cr = -1;                // Busy
            break;
    }
    return cr;
}

int PIXY2::pixy2_getResolution (T_Pixy2Resolution *resolution){return 0;}
int PIXY2::pixy2_setCameraBrightness (Byte brightness){return 0;}
int PIXY2::pixy2_setServos (Word s0, Word s1){return 0;} 
int PIXY2::pixy2_setLED (Byte red, Byte green, Byte blue){return 0;}
int PIXY2::pixy2_setLamp (Byte upper, Byte lower){return 0;}
int PIXY2::pixy2_getFPS (T_pixy2returnCode *framerate){return 0;}
int PIXY2::pixy2_getBlocks (Byte sigmap, Byte maxBloc){return 0;}
int PIXY2::pixy2_getMainFeature (Byte type, Byte features){return 0;}
int PIXY2::pixy2_getAllFeature (Byte features){return 0;}
int PIXY2::pixy2_setMode (Byte mode){return 0;}
int PIXY2::pixy2_setNexTurn (Word angle){return 0;}
int PIXY2::pixy2_setDefaultTurn (Word angle){return 0;}
int PIXY2::pixy2_setVector (Byte vectorIndex){return 0;}
int PIXY2::pixy2_ReverseVector (void){return 0;}
int PIXY2::pixy2_getRGB (Word x, Word y, Byte saturate, T_pixy2Pixel *pixel){return 0;}

int PIXY2::pixy2_validateChecksum (Byte* tab){
    Word    i, sum = 0;
    T_Word  *tmp;
    
    for (i=0; i<*(tab+3);i++)   sum = sum + *(tab + PIXY2_CSHEADERSIZE + i);
    tmp = (T_Word*) (tab+4);
    if (tmp->mot == sum) return 0;
    else return -1;
}