File content as of revision 11:68ef9f85e40e:

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

/* Le programme utilise l'interruption de la liaison série pour recevoir le message et avancer la machine d'état pour permettre au programme de ne pas être bloquant en réception.
   Lorsqu'on appelle une fonction, elle retourne le code -2 pour signifier que le message de réponse de la caméra n'est pas completement reçu...
   Les étapes de fonctionnement sont les suivantes :
   
   * idle : La caméra n'a pas été solicité => on envoi le message de requête.
   * messageSent : La requête a été transmise, mais la réponse n'est pas encore arrivée.
   * receivingHeader : Le mot de synchro a été reçu, le reste de l'entête est en cours de réception.
   * headerReceived : L'entête a été intégralement reçu et peut être traité (tache interne) pour définir la payload.
   * receivingData : On est en train de recevoir la payload.
   * dataReceived : La réponse a été intégralement reçue et est disponible pour le traitement (et la libération).
*/

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

T_pixy2ErrorCode 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 PIXY2_OK;
}

T_pixy2ErrorCode 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 PIXY2_OK;
}

T_pixy2ErrorCode 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 PIXY2_OK;
}

T_pixy2ErrorCode 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 PIXY2_OK;
}

T_pixy2ErrorCode 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 PIXY2_OK;
}

T_pixy2ErrorCode 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 PIXY2_OK;
}

T_pixy2ErrorCode 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 PIXY2_OK;
}

T_pixy2ErrorCode 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 PIXY2_OK;
}

T_pixy2ErrorCode 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 PIXY2_OK;
}

T_pixy2ErrorCode 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 PIXY2_OK;
}

T_pixy2ErrorCode 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 PIXY2_OK;
}

T_pixy2ErrorCode 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 PIXY2_OK;
}

T_pixy2ErrorCode 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 PIXY2_OK;
}

T_pixy2ErrorCode 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 PIXY2_OK;
}

T_pixy2ErrorCode 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 PIXY2_OK;
}

/*  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. */

T_pixy2ErrorCode PIXY2::pixy2_getVersion (T_pixy2Version *version){

    T_pixy2RcvHeader    *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[rPointer];
    T_pixy2ErrorCode    cr = PIXY2_OK;
    
    switch (etat) {
        case idle :                                                                 // Si la caméra est inactive
            PIXY2::pixy2_sndGetVersion();                                           // On envoie la trame de demande de la version
            etat = messageSent;                                                     // On passe à l'attente du message de réponse
            rPointer = wPointer;                                                    // On enregistre la position du pointeur de FIFO
            cr = PIXY2_BUSY;                                                        // On signale à l'utilisateur que la caméra est maintenant occupée
            break;
            
        case dataReceived :                                                         // Quand on a reçu l'intégralité du message
            if (frameContainChecksum) {                                             // Si la trame contient un checksum
                if ( pixy2_validateChecksum (&Pixy2_buffer[rPointer]) ) {           // On lance la validation du checksum           
                    if (msg->pixType == PIXY2_REP_VERS) {                           // On vérifie que la trame est du type convenable
                        version = (T_pixy2Version*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
                                                                                    // On mappe le pointeur de version sur la donnée stockée dans la FIFO (ligne précédente)                        
                        etat = idle;                                                // Et on annoce que la pixy est libre
                    } else {
                        if (msg->pixType == PIXY2_ERROR) {                          // Si on reçoit une trame d'erreur
                            cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
                                                                                    // On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)    
                        }else cr = PIXY2_TYPE_ERROR;                                // Si le type ne correspond à rien de normal on signale une erreur
                    }                                   
                } else cr = PIXY2_BAD_CHECKSUM;                                     // Si le checksum est faux on retourne une erreur aussi !
            } else {                                                                // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
                if (msg->pixType == PIXY2_REP_VERS) {                               // On vérifie que la trame est du type convenable                               
                    version = (T_pixy2Version*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
                                                                                    // Si le checksum et le type sont bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)                        
                    etat = idle;                                                    // Et on annoce que la pixy est libre
                } else {
                    if (msg->pixType == PIXY2_ERROR) {                              // Si on reçoit une trame d'erreur
                        cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
                                                                                    // On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)    
                    }else cr = PIXY2_TYPE_ERROR;                                    // Si le type ne correspond à rien de normal on signale une erreur
                }
            }
            break;

        default :                                                                   // Dans tous les autres cas
            cr = PIXY2_BUSY;                                                        // On signale que la caméra est occupée.
            break;
    }
    return cr;
}

T_pixy2ErrorCode PIXY2::pixy2_getResolution (T_pixy2Resolution *resolution){

    T_pixy2RcvHeader    *msg = (T_pixy2RcvHeader*) &Pixy2_buffer[rPointer];
    T_pixy2ErrorCode    cr = PIXY2_OK;
    
    switch (etat) {
        case idle :                                                                 // Si la caméra est inactive
            PIXY2::pixy2_sndGetResolution();                                        // On envoie la trame de demande de la résolution
            etat = messageSent;                                                     // On passe à l'attente du message de réponse
            rPointer = wPointer;                                                    // On enregistre la position du pointeur de FIFO
            cr = PIXY2_BUSY;                                                        // On signale à l'utilisateur que la caméra est maintenant occupée
            break;
            
        case dataReceived :                                                         // Quand on a reçu l'intégralité du message
            if (frameContainChecksum) {                                             // Si la trame contient un checksum
                if ( pixy2_validateChecksum (&Pixy2_buffer[rPointer]) ) {           // On lance la validation du checksum           
                    if (msg->pixType == PIXY2_REP_RESOL) {                          // On vérifie que la trame est du type convenable
                        resolution = (T_pixy2Resolution*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
                                                                                    // Si le checksum et le type sont bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)                        
                        etat = idle;                                                // Et on annoce que la pixy est libre
                    } else {
                        if (msg->pixType == PIXY2_ERROR) {                          // Si on reçoit une trame d'erreur
                            cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_CSHEADERSIZE];
                                                                                    // On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)    
                        }else cr = PIXY2_TYPE_ERROR;                                // Si le type ne correspond à rien de normal on signale une erreur
                    }                                   
                } else cr = PIXY2_BAD_CHECKSUM;                                     // Si le checksum est faux on retourne une erreur aussi !
            } else {                                                                // S'il n'y a pas de checksum (pas normal mais on ne sait jamais)
                if (msg->pixType == PIXY2_REP_RESOL) {                              // On vérifie que la trame est du type convenable                               
                    resolution = (T_pixy2Resolution*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
                                                                                    // Si le checksum et le type sont bon, on mappe le pointeur de résultat sur la FIFO (ligne précédente)                        
                    etat = idle;                                                    // Et on annoce que la pixy est libre
                } else {
                    if (msg->pixType == PIXY2_ERROR) {                              // Si on reçoit une trame d'erreur
                        cr = *(T_pixy2ErrorCode*) &Pixy2_buffer[rPointer + PIXY2_NCSHEADERSIZE];
                                                                                    // On mappe le résultat sur la donnée stockée dans la FIFO (ligne précédente)    
                    }else cr = PIXY2_TYPE_ERROR;                                    // Si le type ne correspond à rien de normal on signale une erreur
                }
            }
            break;

        default :                                                                   // Dans tous les autres cas
            cr = PIXY2_BUSY;                                                        // On signale à l'utilisateur que la caméra est maintenant occupée
            break;
    }
    return cr;
}


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

T_pixy2ErrorCode 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 PIXY2_BAD_CHECKSUM;
}