File content as of revision 0:07a3605167e8:

#include "mbed.h"
#include "LCD_DISCO_F746NG.h"
#include "math.h"


//**************************
// Définition des constantes
//**************************

// L'ensemble des commandes possibles du LIDAR
#define LIDAR_STOP        0x25
#define LIDAR_RESET       0x40
#define LIDAR_SCAN        0x20
#define EXPRESS_SCAN      0x82
#define GET_INFO          0x50
#define GET_HEALTH        0x52
#define GET_SAMPLERATE    0x59

// Les commandes doivent toutes démarrer par ce caractère
#define START_FLAG        0xA5

//**************************
// instantiation des objets
//**************************
// Instantiation de l'objet écran LCD
LCD_DISCO_F746NG lcd;

// Instantiation liaison série ; liaison avec le LIDAR
Serial LIDAR_link(D1,D0);

// Instatiation d'une sortie PWM sur la pin D3. Son nom est mypwm
PwmOut pwm_Lidar(D6);

// Instantiation d'un Timer pour l'attente des réponses
Timer horloge ;

// Instantiation led1
DigitalOut led1(LED1);


//*******************
// Variables globales
//*******************
float temps_debut, temps_fin;

//*************************
// Prototypes des Fonctions
//*************************
bool initialisation();
bool get_health(char* pointeur_etat_sante_lidar);
void radar(void);
bool scan(int8_t* ptr_buffer_donnees_lidar, int16_t Nbre_echantillons);

// ************************
// Le main
//*************************
int main()
{
    bool test;
    const int16_t Nbre_echantillons = 200;
    int8_t buffer_donnees_lidar[Nbre_echantillons*5] = {0};
    int8_t* ptr_buffer_donnees_lidar;

    led1 = 1;

    test = initialisation();

    if (!test) {
        lcd.Clear(LCD_COLOR_WHITE);
        lcd.SetTextColor(LCD_COLOR_ORANGE);
        lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"Probleme etat de sante Lidar", CENTER_MODE);
        wait(2);
        return 0;
    }

    ptr_buffer_donnees_lidar = buffer_donnees_lidar;

    while(1) {
        test = scan(ptr_buffer_donnees_lidar, Nbre_echantillons);
        wait_ms(200);

    }
}

//***************************************
//Initialisation : configuration initiale
//***************************************
bool initialisation()
{
    bool test ;
    char etat_sante_lidar ;
    char affichage[3];
    char Vidage_buffer_liaison_serie;

    // Initialisation Communication LIDAR
    LIDAR_link.baud(115200);
    LIDAR_link.format(8,SerialBase::None,1);

    // premier message pour vérifier que tout est OK
    lcd.Clear(LCD_COLOR_WHITE);
    lcd.SetTextColor(LCD_COLOR_ORANGE);
    lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"Initialisation OK", CENTER_MODE);

    // Initialisation PWM
    pwm_Lidar.period_us(40);
    pwm_Lidar.write(0.7);

    // Vidage buffer liaison série
    while (LIDAR_link.readable() == true) {
        Vidage_buffer_liaison_serie = LIDAR_link.getc();
    }
    // test lidar
    test = get_health(&etat_sante_lidar);

    if (test == false) {
        lcd.Clear(LCD_COLOR_BLUE);
        lcd.SetTextColor(LCD_COLOR_ORANGE);
        lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"Problemes lidar", CENTER_MODE);
        wait(2);

        return false;

    } else {
        sprintf(affichage,"%x",etat_sante_lidar);
        lcd.Clear(LCD_COLOR_YELLOW);
        lcd.SetTextColor(LCD_COLOR_ORANGE);
        lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"Lidar OK", CENTER_MODE);
        lcd.DisplayStringAt(0, LINE(6), (uint8_t *)affichage, CENTER_MODE);
        wait(2);

        radar();

        return true;
    }

} //End Initialisation

//********************************************
//Fonction lecture de l'état de santé du LIDAR
//********************************************

bool get_health(char *pointeur_etat_sante_lidar)
{
    char response_descriptor[7] = {0,0,0,0,0,0,0};
    char response[3];
    bool test_time_out;
    char ii;
    char affichage[3] = {0};
    char Vidage_buffer_liaison_serie;

    // Vidage buffer liaison série
    while (LIDAR_link.readable() == true) {
        Vidage_buffer_liaison_serie = LIDAR_link.getc();
    }

    // On attend que la liaison série soit prête en éccriture
    while (LIDAR_link.writeable() == false) {
    }

    // Envoi de la commande GET_HEALTH
    LIDAR_link.putc(START_FLAG);
    LIDAR_link.putc(GET_HEALTH);


    // Attente de la réponse
    /*    temps_debut = horloge.read();
        test_time_out = true ;

        while ((LIDAR_link.readable() == false) & (test_time_out == true)) {
            temps_fin = horloge.read();
            test_time_out = (temps_fin - temps_debut) < 2;
        }

        if (!test_time_out) {
            return false ;
        }
        */

    // si le délai de réponse n'est pas dépassé

    // acquisition des données
    // tout d'abord le descripteur de réponse : 7 octets
    for (ii = 0 ; ii <=6 ; ii++) {

        response_descriptor[ii] = LIDAR_link.getc();
    }

    // Et on lit 3 octets pour cette requête
    for (ii = 0 ; ii <= 2 ; ii++) {
        response[ii] = LIDAR_link.getc();
    }

 /*   lcd.Clear(LCD_COLOR_WHITE);
    lcd.SetTextColor(LCD_COLOR_ORANGE);
    for (ii = 0 ; ii <= 6 ; ii++) {
        sprintf(affichage,"%x",response_descriptor[ii]);
        lcd.DisplayStringAt(0, LINE(ii),(uint8_t*) affichage, LEFT_MODE);
    }

    for (ii = 0 ; ii <= 2 ; ii++) {
        sprintf(affichage,"%x",response[ii]);
        lcd.DisplayStringAt(20, LINE(ii),(uint8_t*) affichage, LEFT_MODE);
    }
*/
    wait_ms(2);

    // l'état de santé du Lidar est donné par le premier octet de la réponse : si 0 pas de problème
    *pointeur_etat_sante_lidar = response[0] ;

    return true ;
}

//********************************************
// Fonction Tracé radar sur l'écran
//********************************************
void radar (void)
{
    float angle[13] = {0,30, 60, 90,120,150, 180, 210,240,270, 300, 330, 360};
    float distance[13] = {0,1, 2, 3,4,5, 6, 7,8,9, 10, 11, 12};
    int8_t jj;
    float x, y;

    lcd.Clear(LCD_COLOR_WHITE);
    lcd.SetTextColor(LCD_COLOR_BLACK);
    lcd.DrawCircle(240,136,136);
    lcd.SetTextColor(LCD_COLOR_BLACK);
    lcd.DrawCircle(240,136,102);
    lcd.SetTextColor(LCD_COLOR_BLACK);
    lcd.DrawCircle(240,136,68);
    lcd.SetTextColor(LCD_COLOR_BLACK);
    lcd.DrawCircle(240,136,34);
    lcd.SetTextColor(LCD_COLOR_BLACK);
    lcd.DrawLine(104,136,376,136);
    lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
    lcd.DrawLine(240,0,240,136);
    lcd.SetTextColor(LCD_COLOR_BLACK);
    lcd.DrawLine(240,136,240,272);
    lcd.SetTextColor(LCD_COLOR_BLACK);
    lcd.DrawLine(240,136,358,68);
    lcd.SetTextColor(LCD_COLOR_BLACK);
    lcd.DrawLine(240,136,308,18);
    lcd.SetTextColor(LCD_COLOR_BLACK);
    lcd.DrawLine(240,136,172,18);
    lcd.SetTextColor(LCD_COLOR_BLACK);
    lcd.DrawLine(240,136,122,68);
    lcd.SetTextColor(LCD_COLOR_BLACK);
    lcd.DrawLine(240,136,172,254);
    lcd.SetTextColor(LCD_COLOR_BLACK);
    lcd.DrawLine(240,136,122,204);
    lcd.SetTextColor(LCD_COLOR_BLACK);
    lcd.DrawLine(240,136,308,254);
    lcd.SetTextColor(LCD_COLOR_BLACK);
    lcd.DrawLine(240,136,358,204);
    /**********************************************************************************************************************/
//****************
// test affichage
//****************

    /*    for (jj = 0 ; jj <= 12 ; jj++) {
            angle[jj] = angle[jj] * (3.141592 / 180);
            x= 240 + distance[jj] * 9 * sin(angle[jj]);
            y= 136 - distance[jj] * 9 * cos(angle[jj]);
            lcd.SetTextColor(LCD_COLOR_RED);
            lcd.FillCircle(x,y,3);
        }

        wait(10);
    */
    return;
}

//********************************************
// Fonction Scan du LIDAR
//********************************************

bool scan(int8_t *ptr_buffer_donnees_lidar, int16_t Nbre_echantillons)
{
    char response_descriptor[7] = {0};
    char response[5];

    char Vidage_buffer_liaison_serie;
    char ii, jj;
    char data, data1, data2;

    char affichage[8];

    float angle, distance, x, y;
    uint16_t decodage_angle ;
    uint16_t decodage_distance;
    uint8_t  decodage_qualite, S_S;

    bool test_time_out;


    // On attend que la liaison série soit prête en écriture
    while (LIDAR_link.writeable() == false) {
    }

// Envoi de la commande SCAN
    LIDAR_link.putc(START_FLAG);
    LIDAR_link.putc(LIDAR_SCAN);

    for (ii = 0 ; ii <=6 ; ii++) {
        // attente de la réponse
        temps_debut = horloge.read();
        test_time_out = true ;
        while ((LIDAR_link.readable() == false) & (test_time_out == true)) {
            temps_fin = horloge.read();
            test_time_out = (temps_fin - temps_debut) < 2e-3;
        }

        if (test_time_out == false) {
            return false;
            lcd.Clear(LCD_COLOR_GREEN);
        }
        response_descriptor[ii] = LIDAR_link.getc();
    }

    for (jj = 0 ; jj <= (Nbre_echantillons - 1) ; jj++) {
        // Et on lit des paquets de 5 octets pour cette requête
        for (ii = 0 ; ii <= 4 ; ii++) {
            // attente de la réponse
            temps_debut = horloge.read();
            test_time_out = true ;
            while ((LIDAR_link.readable() == false) & (test_time_out == true)) {
                temps_fin = horloge.read();
                test_time_out = (temps_fin - temps_debut) < 2e-3;
            }

            if (test_time_out == false) {
                return false;
                lcd.Clear(LCD_COLOR_GREEN);
            }
            *(ptr_buffer_donnees_lidar+ ii + 5 * jj) = LIDAR_link.getc();
        }
    }

    //On arrête l'acquisition une fois le nombre d'échantillons demandés atteints
    LIDAR_link.putc(LIDAR_STOP);
    wait_ms(2);

    // Vidage buffer liaison série
    while (LIDAR_link.readable() == true) {
        Vidage_buffer_liaison_serie = LIDAR_link.getc();
    }

    /*    lcd.Clear(LCD_COLOR_WHITE);
        lcd.SetTextColor(LCD_COLOR_BLACK);

       for (ii = 0 ; ii <= 6 ; ii++) {
          sprintf(affichage,"%x",response_descriptor[ii]);
         lcd.DisplayStringAt(0, LINE(ii),(uint8_t*) affichage, LEFT_MODE);
            }

          for (jj = 0 ; jj <= (Nbre_echantillons - 1) ; jj++) {
                for (ii = 0 ; ii <= 4 ; ii++) {
                    data = (*(ptr_buffer_donnees_lidar + ii + 5*jj));
                    sprintf(affichage,"%x",data);
                    lcd.DisplayStringAt(50+80*jj, LINE(ii),(uint8_t*) affichage, LEFT_MODE);
                }
            }
     */

    radar();
    if ((response_descriptor[0] == 0xA5)&(Nbre_echantillons >=2)) {

        for (jj = 1 ; jj <= (Nbre_echantillons - 1) ; jj++) {

            // test erreur transmission S et _S
            data = (*(ptr_buffer_donnees_lidar + 5 * jj));
            S_S = data & 0x03;

            // decodage qualité

            decodage_qualite = data & 0xFD ;
            decodage_qualite = (decodage_qualite >> 2);
//            sprintf(affichage,"%x",decodage_qualite);
//            lcd.DisplayStringAt(50+80*jj, LINE(6),(uint8_t*) affichage, LEFT_MODE);

            if ((decodage_qualite >= 0x08) && ((S_S == 0x01)|(S_S == 0x02))) {


                //decodage angle
                decodage_angle = 0x0000;
                data2 = (*(ptr_buffer_donnees_lidar + 2 + 5 * jj));
                data1 = (*(ptr_buffer_donnees_lidar + 1 + 5 * jj)) ;
                decodage_angle = data2;
                decodage_angle = (((decodage_angle << 8) | data1) >> 1);

                angle = (decodage_angle/64.0) ; // en degré
//              sprintf(affichage,"%4.0f",angle);
//                lcd.DisplayStringAt(50+80*jj, LINE(7),(uint8_t*) affichage, LEFT_MODE);


                //decodage distance
                decodage_distance = 0x0000;
                data2 = (*(ptr_buffer_donnees_lidar + 4 + 5 * jj));
                data1 = (*(ptr_buffer_donnees_lidar + 3 + 5 * jj)) ;
                decodage_distance = data2;
                decodage_distance = ((decodage_distance << 8) | data1);


                distance = (decodage_distance/4000.0) ; // en mètres
//                sprintf(affichage,"%4.0f",distance);
//                lcd.DisplayStringAt(50+80*jj, LINE(8),(uint8_t*) affichage, LEFT_MODE);

                // affichage du point
                angle = angle * (3.141592 / 180);
                x= 240 + distance * 45 * sin(angle);
                y= 136 - distance * 45 * cos(angle);
                lcd.SetTextColor(LCD_COLOR_RED);
                lcd.FillCircle(x,y,1);


            }// fin if

        } // for jj
    } // fin if

    return 1 ;
}