Diff: main.cpp

Revision:

0:07a3605167e8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Sep 09 14:19:33 2020 +0000
@@ -0,0 +1,421 @@
+#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 ;
+}
\ No newline at end of file