File content as of revision 4:5982d904f7aa:

#include "Pixy.h"
/**
 * Constructeur de l'objet
 */

PIXY::PIXY(PinName tx, PinName rx, int debit)
{
    Pixy_CCFrameIsNew = -1;
    Pixy_NMFrameIsNew = -1;
    FlagPixy = 0;
    FlagPixyOverflow = 0;
    Pixy_check = -1;

    _Pixy = new Serial (tx, rx, debit);
    _Pixy->attach (callback(this,&PIXY::getPixyByte));

}

/**
 * Programme d'interruption gérant la réception des octets envoyés par la caméra sur la liaison série
 */
void PIXY::getPixyByte ()
{
    static T_msgBuffer      msgBuffer;                              // Buffer de réception 32 bits
    static T_wordBuffer     wordBuffer;                             // Buffer de réception 16 bits
    static T_pixyState      PIXY_state = none;                      // Variable d'etat de la liaison 
    static Byte             byteCount = 0;                          // Nb d'octets lu
    static int              PIXY_synced = 0;                        // Etat de la liaison Pixy <-> carte
    Word                    i, somme;
    Byte                    dummy;
    static Byte             PIXY_nbCCObjet = 0, PIXY_wCCObjet = 0;  // Variable interne de la FIFO Color Code
    static Byte             PIXY_nbNMObjet = 0, PIXY_wNMObjet = 0;  // Variable interne de la FIFO normale

    Pixy_check = 0;

    dummy = _Pixy->getc();                                                              // On stocke l'octet reçu dans une variable temporaire

    if (!PIXY_synced) {                                                                 // Si la camera et la carte ne sont pas synchronisées
        msgBuffer.octet[byteCount] = dummy;                                             // On stocke l'octet reçu dans la première case dispo du tableau temporaire

        if (byteCount < 3) {                                                            // Tant qu'on n'a pas encore reçu les 4 premier octets
            byteCount++;                                                                    // On passe à la case suivante du tableau temporaire
        } else {                                                                        // Lorsqu'on a 4 octets
            if ((msgBuffer.motlong == 0xaa550000)||(msgBuffer.motlong == 0xaa560000)) { // On teste pour voir si on a reçu un entête
                PIXY_synced = 1;                                                            // Si oui : On passe en mode synchronisé
                PIXY_state = begin;                                                         // Et on va à l'aiguillage
                byteCount = 0;                                                              // Et on remet le comptage d'octet à 0
            } else {                                                                    // Si on n'a pas trouvé d'entête
                for (i=1; i<4; i++) msgBuffer.octet[i-1] = msgBuffer.octet[i];              // On décalle les cases du tableau
                byteCount = 3;                                                              // Et on attend le caractère suivant
            }
        }
    } else {
        switch (PIXY_state) {
            case begin :                                                        // l'aiguillage est là !
                wordBuffer.octet[byteCount%2] = dummy;                          // on stocke les octets reçus
                byteCount++;
                if (byteCount == 2) {                                           // Quand on a 2 octets
                    switch (wordBuffer.mot) {
                        case 0xaa55 :                                           // Si c'est un bloc normal (code = 0xAA55)
                            PIXY_state = normal;                                // On part vers le traitement spécifique
                            break;

                        case 0xaa56 :                                           // Si c'est un bloc color Code (code = 0xAA56)
                            PIXY_state = colorCode;                             // On part vers le traitement spécifique
                            break;

                        case 0 :                                                // Si c'est un debut de trame (code = 0x0000)
                            PIXY_state = doubleZero;                            // On part vers le traitement spécifique
                            break;

                        default :                                               // Si c'est autre chose
                            PIXY_synced = 0;                                    // C'est qu'on est perdu donc plus synchronisé.
                            PIXY_state = none;                                  // Ceinture et bretelle
                            break;
                    }
                    byteCount = 0;
                }
                break;

            case normal :                                                       // Si on a un bloc normal

                Pixy_NMFIFO[PIXY_wNMObjet].tab[byteCount] = dummy;              // On stocke les octets un à un dans la structure Bloc
                if (byteCount < 11) {                                           // Tant que la structure n'est pas pleine
                    byteCount++;                                                // On passe à l'octet suivant
                } else {                                                        // Quand elle est pleine
                    byteCount = 0;                                              // On réinitialise
                    PIXY_state = begin;                                         // On prépare le retour à l'aiguillage

                    // On calcule la somme de contrôle
                    somme = Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.signature + Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.x + Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.y + Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.width + Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.height;

                    if (somme == Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.checksum) {  // Si le checksum est bon, on valide la réception
                        if (PIXY_wNMObjet < 19)     PIXY_wNMObjet++;            // On incrémente le pointeur d'écriture dans la FIFO Objet
                        else                        PIXY_wNMObjet = 0;
                        if (PIXY_nbNMObjet < 19)    PIXY_nbNMObjet++;           // On dit que l'on a un objet de plus
                        else                        FlagPixyOverflow = 1;       // Si on a plus de 20 objets en attente => Overflow
                    }
                }
                break;

            case colorCode :                                                    // Si on a un bloc colorCode

                Pixy_CCFIFO[PIXY_wCCObjet].tab[byteCount] = dummy;              // On stocke les octets un à un dans la structure CCBloc
                if (byteCount < 13) byteCount++;                                // tant que la structure n'est pas pleine on passe à l'octet suivant
                else {                                                          // Quand elle est pleine
                    byteCount = 0;                                              // On réinitialise
                    PIXY_state = begin;                                         // On prepare le retour à l'aiguillage

                    // On calcule la somme de contrôle
                    somme = Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.signature + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.x + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.y + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.width + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.height + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.angle;

                    if (somme == Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.checksum) {  // Si le checksum est bon
                        if (PIXY_wCCObjet < 19)     PIXY_wCCObjet++;            // On incrémente le pointeur d'écriture dans la FIFO CCObjet
                        else                        PIXY_wCCObjet = 0;
                        if (PIXY_nbCCObjet < 19)    PIXY_nbCCObjet++;           // On dit que l'on a un objet CC de plus à traiter
                        else                        FlagPixyOverflow = 1;       // Si on a plus de 20 CC objets (en attente) => Overflow
                    }
                }
                break;

            case doubleZero :                                                   // Si on a reçu le code de début d'une nouvelle trame (0000).

                Pixy_NMObjet = PIXY_nbNMObjet;                                  // On met à jour les variables pour le traitement extérieur
                Pixy_CCObjet = PIXY_nbCCObjet;
                Pixy_CCFrameIsNew = 1;
                Pixy_FirstCCObjet = PIXY_wCCObjet;
                Pixy_NMFrameIsNew = 1;
                Pixy_FirstNMObjet = PIXY_wNMObjet;
                FlagPixy = 1;                                                   // On valide le traitement de la trame précédente.

                PIXY_nbCCObjet = 0;                                             // On remet à 0 les variables internes
                PIXY_nbNMObjet = 0;
                byteCount = 0;

                // Comme on est dans une interruption, si on est ici c'est qu'on a reçu le code 0000, mais aussi le premier octet de la suite (le 0x55 du start)
                // On va donc le stocker ici dans le tableau de réception du start.
                wordBuffer.octet[byteCount%2] = dummy;                          // on stocke l'octet reçu
                byteCount++;

                PIXY_state = waitForStart;                                      // On passe en attente de la deuxième partie du code
                break;

            case waitForStart :                                                 // Si on a reçu le code de début d'une nouvelle trame.

                wordBuffer.octet[byteCount%2] = dummy;                          // on stocke l'octet reçu (ça en fait 2 avec celui reçu precedemment
                byteCount++;
                if (byteCount == 2) {                                           // On verifie quand même si on a bien 2 octets
                    byteCount = 0;                                              // Si oui, on remet à 0 le comptage
                    if (wordBuffer.mot == 0xaa55) {                             // On vérifie qu'on a bien reçu l'entête de depart
                        PIXY_state = begin;                                     // Si c'est le cas, alors tout va bien et on va à l'aiguillage
                    } else {                                                    // Si ce n'est pas le cas
                        PIXY_synced = 0;                                        // On n'est plus synchronisés
                        PIXY_state = none;                                      // Ceinture et Bretelles
                    }
                }
                break;
        }
    }
}

/**
 *  Transmet le nombre d'objets par catégorie dans la dernière trame reçue
 */
int PIXY::detectedObject (int* nbNM, int* nbCC)
{
    *nbNM = (int)Pixy_NMObjet;
    *nbCC = (int)Pixy_CCObjet;
    if (Pixy_check!=0) return -1;
    if (FlagPixyOverflow) return -2;
    return 0;
}

/**
 *  Permet la lecture du plus ancien bloc CC de la FIFO se trouvant dans la dernière trame reçue
 */
T_pixyCCBloc PIXY::getCCBloc (void)
{
    T_pixyCCBloc dummy;
    static Byte PIXY_rCCObjet = 0;

    if (Pixy_CCObjet !=0) {
        if (Pixy_CCFrameIsNew) {                                                //Si on est au début d'une nouvelle trame, on initialise le pointeur de lecture
            if (Pixy_FirstCCObjet <= Pixy_CCObjet)  PIXY_rCCObjet = 20 - Pixy_CCObjet + Pixy_FirstCCObjet ;
            else                                    PIXY_rCCObjet = Pixy_FirstCCObjet - Pixy_CCObjet ;
            Pixy_CCFrameIsNew = 0;
        }
        dummy = Pixy_CCFIFO[PIXY_rCCObjet].CCbloc;
        if (PIXY_rCCObjet < 19)     PIXY_rCCObjet++;                            // On incrémente le pointeur de lecture dans la FIFO CCObjet
        else                        PIXY_rCCObjet = 0;
        Pixy_CCObjet--;                                                         // On dit que l'on a un objet CC de moins à traiter
    } else {
        dummy.signature = 0xFFFF;
    }
    return dummy;
}

/**
 *  Permet la lecture du plus ancien bloc NM de la FIFO se trouvant dans la dernière trame reçue
 */
T_pixyNMBloc PIXY::getNMBloc (void)
{
    T_pixyNMBloc dummy;
    static Byte PIXY_rNMObjet = 0;

    if (Pixy_NMObjet !=0) {
        if (Pixy_NMFrameIsNew) {                                                //Si on est au début d'une nouvelle trame, on initialise le pointeur de lecture
            if (Pixy_FirstNMObjet <= Pixy_NMObjet)  PIXY_rNMObjet = 20 - Pixy_NMObjet + Pixy_FirstNMObjet ;
            else                                    PIXY_rNMObjet = Pixy_FirstNMObjet - Pixy_NMObjet ;
            Pixy_NMFrameIsNew = 0;
        }
        dummy = Pixy_NMFIFO[PIXY_rNMObjet].NMbloc;
        if (PIXY_rNMObjet < 19)     PIXY_rNMObjet++;                            // On incrémente le pointeur de lecture dans la FIFO CCObjet
        else                        PIXY_rNMObjet = 0;
        Pixy_NMObjet--;                                                         // On dit que l'on a un objet CC de moins à traiter
    } else {
        dummy.signature = 0xFFFF;
    }
    return dummy;
}

/**
 * Permet le réglage de la luminosité
 */
void PIXY::setBrightness (Byte brightness)
{
    _Pixy->putc(0); //Start
    _Pixy->putc(0xFE); //Brightness Level
    _Pixy->putc(brightness);
}

/**
 * Indique si une nouvelle image est disponible pour le traitement
 */
int PIXY::checkNewImage (void)
{
    if (FlagPixy) {
        FlagPixy = 0;
        return 1;
    } else {
        return 0;
    }
}

/**
 * Indique l'état de la liaison entre la carte et la camera
 */
int PIXY::checkPixy (void)
{
    return Pixy_check;
}