Eric Laboure / Mbed 2 deprecated DISCO_F746G_test_lidar5

E&R IUT Cachan Geii2

Dependencies:   mbed LCD_DISCO_F746NG BSP_DISCO_F746NG

main.cpp@0:07a3605167e8, 2020-09-09 (annotated)

Committer:
elaboure
Date:
Wed Sep 09 14:19:33 2020 +0000
Revision:
0:07a3605167e8
Essai Lidar IUT Cachan v5

Who changed what in which revision?

UserRevisionLine numberNew contents of line
elaboure 0:07a3605167e8 1 #include "mbed.h"
elaboure 0:07a3605167e8 2 #include "LCD_DISCO_F746NG.h"
elaboure 0:07a3605167e8 3 #include "math.h"
elaboure 0:07a3605167e8 4
elaboure 0:07a3605167e8 5
elaboure 0:07a3605167e8 6 //**************************
elaboure 0:07a3605167e8 7 // Définition des constantes
elaboure 0:07a3605167e8 8 //**************************
elaboure 0:07a3605167e8 9
elaboure 0:07a3605167e8 10 // L'ensemble des commandes possibles du LIDAR
elaboure 0:07a3605167e8 11 #define LIDAR_STOP 0x25
elaboure 0:07a3605167e8 12 #define LIDAR_RESET 0x40
elaboure 0:07a3605167e8 13 #define LIDAR_SCAN 0x20
elaboure 0:07a3605167e8 14 #define EXPRESS_SCAN 0x82
elaboure 0:07a3605167e8 15 #define GET_INFO 0x50
elaboure 0:07a3605167e8 16 #define GET_HEALTH 0x52
elaboure 0:07a3605167e8 17 #define GET_SAMPLERATE 0x59
elaboure 0:07a3605167e8 18
elaboure 0:07a3605167e8 19 // Les commandes doivent toutes démarrer par ce caractère
elaboure 0:07a3605167e8 20 #define START_FLAG 0xA5
elaboure 0:07a3605167e8 21
elaboure 0:07a3605167e8 22 //**************************
elaboure 0:07a3605167e8 23 // instantiation des objets
elaboure 0:07a3605167e8 24 //**************************
elaboure 0:07a3605167e8 25 // Instantiation de l'objet écran LCD
elaboure 0:07a3605167e8 26 LCD_DISCO_F746NG lcd;
elaboure 0:07a3605167e8 27
elaboure 0:07a3605167e8 28 // Instantiation liaison série ; liaison avec le LIDAR
elaboure 0:07a3605167e8 29 Serial LIDAR_link(D1,D0);
elaboure 0:07a3605167e8 30
elaboure 0:07a3605167e8 31 // Instatiation d'une sortie PWM sur la pin D3. Son nom est mypwm
elaboure 0:07a3605167e8 32 PwmOut pwm_Lidar(D6);
elaboure 0:07a3605167e8 33
elaboure 0:07a3605167e8 34 // Instantiation d'un Timer pour l'attente des réponses
elaboure 0:07a3605167e8 35 Timer horloge ;
elaboure 0:07a3605167e8 36
elaboure 0:07a3605167e8 37 // Instantiation led1
elaboure 0:07a3605167e8 38 DigitalOut led1(LED1);
elaboure 0:07a3605167e8 39
elaboure 0:07a3605167e8 40
elaboure 0:07a3605167e8 41 //*******************
elaboure 0:07a3605167e8 42 // Variables globales
elaboure 0:07a3605167e8 43 //*******************
elaboure 0:07a3605167e8 44 float temps_debut, temps_fin;
elaboure 0:07a3605167e8 45
elaboure 0:07a3605167e8 46 //*************************
elaboure 0:07a3605167e8 47 // Prototypes des Fonctions
elaboure 0:07a3605167e8 48 //*************************
elaboure 0:07a3605167e8 49 bool initialisation();
elaboure 0:07a3605167e8 50 bool get_health(char* pointeur_etat_sante_lidar);
elaboure 0:07a3605167e8 51 void radar(void);
elaboure 0:07a3605167e8 52 bool scan(int8_t* ptr_buffer_donnees_lidar, int16_t Nbre_echantillons);
elaboure 0:07a3605167e8 53
elaboure 0:07a3605167e8 54 // ************************
elaboure 0:07a3605167e8 55 // Le main
elaboure 0:07a3605167e8 56 //*************************
elaboure 0:07a3605167e8 57 int main()
elaboure 0:07a3605167e8 58 {
elaboure 0:07a3605167e8 59 bool test;
elaboure 0:07a3605167e8 60 const int16_t Nbre_echantillons = 200;
elaboure 0:07a3605167e8 61 int8_t buffer_donnees_lidar[Nbre_echantillons*5] = {0};
elaboure 0:07a3605167e8 62 int8_t* ptr_buffer_donnees_lidar;
elaboure 0:07a3605167e8 63
elaboure 0:07a3605167e8 64 led1 = 1;
elaboure 0:07a3605167e8 65
elaboure 0:07a3605167e8 66 test = initialisation();
elaboure 0:07a3605167e8 67
elaboure 0:07a3605167e8 68 if (!test) {
elaboure 0:07a3605167e8 69 lcd.Clear(LCD_COLOR_WHITE);
elaboure 0:07a3605167e8 70 lcd.SetTextColor(LCD_COLOR_ORANGE);
elaboure 0:07a3605167e8 71 lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"Probleme etat de sante Lidar", CENTER_MODE);
elaboure 0:07a3605167e8 72 wait(2);
elaboure 0:07a3605167e8 73 return 0;
elaboure 0:07a3605167e8 74 }
elaboure 0:07a3605167e8 75
elaboure 0:07a3605167e8 76 ptr_buffer_donnees_lidar = buffer_donnees_lidar;
elaboure 0:07a3605167e8 77
elaboure 0:07a3605167e8 78 while(1) {
elaboure 0:07a3605167e8 79 test = scan(ptr_buffer_donnees_lidar, Nbre_echantillons);
elaboure 0:07a3605167e8 80 wait_ms(200);
elaboure 0:07a3605167e8 81
elaboure 0:07a3605167e8 82 }
elaboure 0:07a3605167e8 83 }
elaboure 0:07a3605167e8 84
elaboure 0:07a3605167e8 85 //***************************************
elaboure 0:07a3605167e8 86 //Initialisation : configuration initiale
elaboure 0:07a3605167e8 87 //***************************************
elaboure 0:07a3605167e8 88 bool initialisation()
elaboure 0:07a3605167e8 89 {
elaboure 0:07a3605167e8 90 bool test ;
elaboure 0:07a3605167e8 91 char etat_sante_lidar ;
elaboure 0:07a3605167e8 92 char affichage[3];
elaboure 0:07a3605167e8 93 char Vidage_buffer_liaison_serie;
elaboure 0:07a3605167e8 94
elaboure 0:07a3605167e8 95 // Initialisation Communication LIDAR
elaboure 0:07a3605167e8 96 LIDAR_link.baud(115200);
elaboure 0:07a3605167e8 97 LIDAR_link.format(8,SerialBase::None,1);
elaboure 0:07a3605167e8 98
elaboure 0:07a3605167e8 99 // premier message pour vérifier que tout est OK
elaboure 0:07a3605167e8 100 lcd.Clear(LCD_COLOR_WHITE);
elaboure 0:07a3605167e8 101 lcd.SetTextColor(LCD_COLOR_ORANGE);
elaboure 0:07a3605167e8 102 lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"Initialisation OK", CENTER_MODE);
elaboure 0:07a3605167e8 103
elaboure 0:07a3605167e8 104 // Initialisation PWM
elaboure 0:07a3605167e8 105 pwm_Lidar.period_us(40);
elaboure 0:07a3605167e8 106 pwm_Lidar.write(0.7);
elaboure 0:07a3605167e8 107
elaboure 0:07a3605167e8 108 // Vidage buffer liaison série
elaboure 0:07a3605167e8 109 while (LIDAR_link.readable() == true) {
elaboure 0:07a3605167e8 110 Vidage_buffer_liaison_serie = LIDAR_link.getc();
elaboure 0:07a3605167e8 111 }
elaboure 0:07a3605167e8 112 // test lidar
elaboure 0:07a3605167e8 113 test = get_health(&etat_sante_lidar);
elaboure 0:07a3605167e8 114
elaboure 0:07a3605167e8 115 if (test == false) {
elaboure 0:07a3605167e8 116 lcd.Clear(LCD_COLOR_BLUE);
elaboure 0:07a3605167e8 117 lcd.SetTextColor(LCD_COLOR_ORANGE);
elaboure 0:07a3605167e8 118 lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"Problemes lidar", CENTER_MODE);
elaboure 0:07a3605167e8 119 wait(2);
elaboure 0:07a3605167e8 120
elaboure 0:07a3605167e8 121 return false;
elaboure 0:07a3605167e8 122
elaboure 0:07a3605167e8 123 } else {
elaboure 0:07a3605167e8 124 sprintf(affichage,"%x",etat_sante_lidar);
elaboure 0:07a3605167e8 125 lcd.Clear(LCD_COLOR_YELLOW);
elaboure 0:07a3605167e8 126 lcd.SetTextColor(LCD_COLOR_ORANGE);
elaboure 0:07a3605167e8 127 lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"Lidar OK", CENTER_MODE);
elaboure 0:07a3605167e8 128 lcd.DisplayStringAt(0, LINE(6), (uint8_t *)affichage, CENTER_MODE);
elaboure 0:07a3605167e8 129 wait(2);
elaboure 0:07a3605167e8 130
elaboure 0:07a3605167e8 131 radar();
elaboure 0:07a3605167e8 132
elaboure 0:07a3605167e8 133 return true;
elaboure 0:07a3605167e8 134 }
elaboure 0:07a3605167e8 135
elaboure 0:07a3605167e8 136 } //End Initialisation
elaboure 0:07a3605167e8 137
elaboure 0:07a3605167e8 138 //********************************************
elaboure 0:07a3605167e8 139 //Fonction lecture de l'état de santé du LIDAR
elaboure 0:07a3605167e8 140 //********************************************
elaboure 0:07a3605167e8 141
elaboure 0:07a3605167e8 142 bool get_health(char *pointeur_etat_sante_lidar)
elaboure 0:07a3605167e8 143 {
elaboure 0:07a3605167e8 144 char response_descriptor[7] = {0,0,0,0,0,0,0};
elaboure 0:07a3605167e8 145 char response[3];
elaboure 0:07a3605167e8 146 bool test_time_out;
elaboure 0:07a3605167e8 147 char ii;
elaboure 0:07a3605167e8 148 char affichage[3] = {0};
elaboure 0:07a3605167e8 149 char Vidage_buffer_liaison_serie;
elaboure 0:07a3605167e8 150
elaboure 0:07a3605167e8 151 // Vidage buffer liaison série
elaboure 0:07a3605167e8 152 while (LIDAR_link.readable() == true) {
elaboure 0:07a3605167e8 153 Vidage_buffer_liaison_serie = LIDAR_link.getc();
elaboure 0:07a3605167e8 154 }
elaboure 0:07a3605167e8 155
elaboure 0:07a3605167e8 156 // On attend que la liaison série soit prête en éccriture
elaboure 0:07a3605167e8 157 while (LIDAR_link.writeable() == false) {
elaboure 0:07a3605167e8 158 }
elaboure 0:07a3605167e8 159
elaboure 0:07a3605167e8 160 // Envoi de la commande GET_HEALTH
elaboure 0:07a3605167e8 161 LIDAR_link.putc(START_FLAG);
elaboure 0:07a3605167e8 162 LIDAR_link.putc(GET_HEALTH);
elaboure 0:07a3605167e8 163
elaboure 0:07a3605167e8 164
elaboure 0:07a3605167e8 165 // Attente de la réponse
elaboure 0:07a3605167e8 166 /* temps_debut = horloge.read();
elaboure 0:07a3605167e8 167 test_time_out = true ;
elaboure 0:07a3605167e8 168
elaboure 0:07a3605167e8 169 while ((LIDAR_link.readable() == false) & (test_time_out == true)) {
elaboure 0:07a3605167e8 170 temps_fin = horloge.read();
elaboure 0:07a3605167e8 171 test_time_out = (temps_fin - temps_debut) < 2;
elaboure 0:07a3605167e8 172 }
elaboure 0:07a3605167e8 173
elaboure 0:07a3605167e8 174 if (!test_time_out) {
elaboure 0:07a3605167e8 175 return false ;
elaboure 0:07a3605167e8 176 }
elaboure 0:07a3605167e8 177 */
elaboure 0:07a3605167e8 178
elaboure 0:07a3605167e8 179 // si le délai de réponse n'est pas dépassé
elaboure 0:07a3605167e8 180
elaboure 0:07a3605167e8 181 // acquisition des données
elaboure 0:07a3605167e8 182 // tout d'abord le descripteur de réponse : 7 octets
elaboure 0:07a3605167e8 183 for (ii = 0 ; ii <=6 ; ii++) {
elaboure 0:07a3605167e8 184
elaboure 0:07a3605167e8 185 response_descriptor[ii] = LIDAR_link.getc();
elaboure 0:07a3605167e8 186 }
elaboure 0:07a3605167e8 187
elaboure 0:07a3605167e8 188 // Et on lit 3 octets pour cette requête
elaboure 0:07a3605167e8 189 for (ii = 0 ; ii <= 2 ; ii++) {
elaboure 0:07a3605167e8 190 response[ii] = LIDAR_link.getc();
elaboure 0:07a3605167e8 191 }
elaboure 0:07a3605167e8 192
elaboure 0:07a3605167e8 193 /* lcd.Clear(LCD_COLOR_WHITE);
elaboure 0:07a3605167e8 194 lcd.SetTextColor(LCD_COLOR_ORANGE);
elaboure 0:07a3605167e8 195 for (ii = 0 ; ii <= 6 ; ii++) {
elaboure 0:07a3605167e8 196 sprintf(affichage,"%x",response_descriptor[ii]);
elaboure 0:07a3605167e8 197 lcd.DisplayStringAt(0, LINE(ii),(uint8_t*) affichage, LEFT_MODE);
elaboure 0:07a3605167e8 198 }
elaboure 0:07a3605167e8 199
elaboure 0:07a3605167e8 200 for (ii = 0 ; ii <= 2 ; ii++) {
elaboure 0:07a3605167e8 201 sprintf(affichage,"%x",response[ii]);
elaboure 0:07a3605167e8 202 lcd.DisplayStringAt(20, LINE(ii),(uint8_t*) affichage, LEFT_MODE);
elaboure 0:07a3605167e8 203 }
elaboure 0:07a3605167e8 204 */
elaboure 0:07a3605167e8 205 wait_ms(2);
elaboure 0:07a3605167e8 206
elaboure 0:07a3605167e8 207 // l'état de santé du Lidar est donné par le premier octet de la réponse : si 0 pas de problème
elaboure 0:07a3605167e8 208 *pointeur_etat_sante_lidar = response[0] ;
elaboure 0:07a3605167e8 209
elaboure 0:07a3605167e8 210 return true ;
elaboure 0:07a3605167e8 211 }
elaboure 0:07a3605167e8 212
elaboure 0:07a3605167e8 213 //********************************************
elaboure 0:07a3605167e8 214 // Fonction Tracé radar sur l'écran
elaboure 0:07a3605167e8 215 //********************************************
elaboure 0:07a3605167e8 216 void radar (void)
elaboure 0:07a3605167e8 217 {
elaboure 0:07a3605167e8 218 float angle[13] = {0,30, 60, 90,120,150, 180, 210,240,270, 300, 330, 360};
elaboure 0:07a3605167e8 219 float distance[13] = {0,1, 2, 3,4,5, 6, 7,8,9, 10, 11, 12};
elaboure 0:07a3605167e8 220 int8_t jj;
elaboure 0:07a3605167e8 221 float x, y;
elaboure 0:07a3605167e8 222
elaboure 0:07a3605167e8 223 lcd.Clear(LCD_COLOR_WHITE);
elaboure 0:07a3605167e8 224 lcd.SetTextColor(LCD_COLOR_BLACK);
elaboure 0:07a3605167e8 225 lcd.DrawCircle(240,136,136);
elaboure 0:07a3605167e8 226 lcd.SetTextColor(LCD_COLOR_BLACK);
elaboure 0:07a3605167e8 227 lcd.DrawCircle(240,136,102);
elaboure 0:07a3605167e8 228 lcd.SetTextColor(LCD_COLOR_BLACK);
elaboure 0:07a3605167e8 229 lcd.DrawCircle(240,136,68);
elaboure 0:07a3605167e8 230 lcd.SetTextColor(LCD_COLOR_BLACK);
elaboure 0:07a3605167e8 231 lcd.DrawCircle(240,136,34);
elaboure 0:07a3605167e8 232 lcd.SetTextColor(LCD_COLOR_BLACK);
elaboure 0:07a3605167e8 233 lcd.DrawLine(104,136,376,136);
elaboure 0:07a3605167e8 234 lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
elaboure 0:07a3605167e8 235 lcd.DrawLine(240,0,240,136);
elaboure 0:07a3605167e8 236 lcd.SetTextColor(LCD_COLOR_BLACK);
elaboure 0:07a3605167e8 237 lcd.DrawLine(240,136,240,272);
elaboure 0:07a3605167e8 238 lcd.SetTextColor(LCD_COLOR_BLACK);
elaboure 0:07a3605167e8 239 lcd.DrawLine(240,136,358,68);
elaboure 0:07a3605167e8 240 lcd.SetTextColor(LCD_COLOR_BLACK);
elaboure 0:07a3605167e8 241 lcd.DrawLine(240,136,308,18);
elaboure 0:07a3605167e8 242 lcd.SetTextColor(LCD_COLOR_BLACK);
elaboure 0:07a3605167e8 243 lcd.DrawLine(240,136,172,18);
elaboure 0:07a3605167e8 244 lcd.SetTextColor(LCD_COLOR_BLACK);
elaboure 0:07a3605167e8 245 lcd.DrawLine(240,136,122,68);
elaboure 0:07a3605167e8 246 lcd.SetTextColor(LCD_COLOR_BLACK);
elaboure 0:07a3605167e8 247 lcd.DrawLine(240,136,172,254);
elaboure 0:07a3605167e8 248 lcd.SetTextColor(LCD_COLOR_BLACK);
elaboure 0:07a3605167e8 249 lcd.DrawLine(240,136,122,204);
elaboure 0:07a3605167e8 250 lcd.SetTextColor(LCD_COLOR_BLACK);
elaboure 0:07a3605167e8 251 lcd.DrawLine(240,136,308,254);
elaboure 0:07a3605167e8 252 lcd.SetTextColor(LCD_COLOR_BLACK);
elaboure 0:07a3605167e8 253 lcd.DrawLine(240,136,358,204);
elaboure 0:07a3605167e8 254 /**********************************************************************************************************************/
elaboure 0:07a3605167e8 255 //****************
elaboure 0:07a3605167e8 256 // test affichage
elaboure 0:07a3605167e8 257 //****************
elaboure 0:07a3605167e8 258
elaboure 0:07a3605167e8 259 /* for (jj = 0 ; jj <= 12 ; jj++) {
elaboure 0:07a3605167e8 260 angle[jj] = angle[jj] * (3.141592 / 180);
elaboure 0:07a3605167e8 261 x= 240 + distance[jj] * 9 * sin(angle[jj]);
elaboure 0:07a3605167e8 262 y= 136 - distance[jj] * 9 * cos(angle[jj]);
elaboure 0:07a3605167e8 263 lcd.SetTextColor(LCD_COLOR_RED);
elaboure 0:07a3605167e8 264 lcd.FillCircle(x,y,3);
elaboure 0:07a3605167e8 265 }
elaboure 0:07a3605167e8 266
elaboure 0:07a3605167e8 267 wait(10);
elaboure 0:07a3605167e8 268 */
elaboure 0:07a3605167e8 269 return;
elaboure 0:07a3605167e8 270 }
elaboure 0:07a3605167e8 271
elaboure 0:07a3605167e8 272 //********************************************
elaboure 0:07a3605167e8 273 // Fonction Scan du LIDAR
elaboure 0:07a3605167e8 274 //********************************************
elaboure 0:07a3605167e8 275
elaboure 0:07a3605167e8 276 bool scan(int8_t *ptr_buffer_donnees_lidar, int16_t Nbre_echantillons)
elaboure 0:07a3605167e8 277 {
elaboure 0:07a3605167e8 278 char response_descriptor[7] = {0};
elaboure 0:07a3605167e8 279 char response[5];
elaboure 0:07a3605167e8 280
elaboure 0:07a3605167e8 281 char Vidage_buffer_liaison_serie;
elaboure 0:07a3605167e8 282 char ii, jj;
elaboure 0:07a3605167e8 283 char data, data1, data2;
elaboure 0:07a3605167e8 284
elaboure 0:07a3605167e8 285 char affichage[8];
elaboure 0:07a3605167e8 286
elaboure 0:07a3605167e8 287 float angle, distance, x, y;
elaboure 0:07a3605167e8 288 uint16_t decodage_angle ;
elaboure 0:07a3605167e8 289 uint16_t decodage_distance;
elaboure 0:07a3605167e8 290 uint8_t decodage_qualite, S_S;
elaboure 0:07a3605167e8 291
elaboure 0:07a3605167e8 292 bool test_time_out;
elaboure 0:07a3605167e8 293
elaboure 0:07a3605167e8 294
elaboure 0:07a3605167e8 295 // On attend que la liaison série soit prête en écriture
elaboure 0:07a3605167e8 296 while (LIDAR_link.writeable() == false) {
elaboure 0:07a3605167e8 297 }
elaboure 0:07a3605167e8 298
elaboure 0:07a3605167e8 299 // Envoi de la commande SCAN
elaboure 0:07a3605167e8 300 LIDAR_link.putc(START_FLAG);
elaboure 0:07a3605167e8 301 LIDAR_link.putc(LIDAR_SCAN);
elaboure 0:07a3605167e8 302
elaboure 0:07a3605167e8 303 for (ii = 0 ; ii <=6 ; ii++) {
elaboure 0:07a3605167e8 304 // attente de la réponse
elaboure 0:07a3605167e8 305 temps_debut = horloge.read();
elaboure 0:07a3605167e8 306 test_time_out = true ;
elaboure 0:07a3605167e8 307 while ((LIDAR_link.readable() == false) & (test_time_out == true)) {
elaboure 0:07a3605167e8 308 temps_fin = horloge.read();
elaboure 0:07a3605167e8 309 test_time_out = (temps_fin - temps_debut) < 2e-3;
elaboure 0:07a3605167e8 310 }
elaboure 0:07a3605167e8 311
elaboure 0:07a3605167e8 312 if (test_time_out == false) {
elaboure 0:07a3605167e8 313 return false;
elaboure 0:07a3605167e8 314 lcd.Clear(LCD_COLOR_GREEN);
elaboure 0:07a3605167e8 315 }
elaboure 0:07a3605167e8 316 response_descriptor[ii] = LIDAR_link.getc();
elaboure 0:07a3605167e8 317 }
elaboure 0:07a3605167e8 318
elaboure 0:07a3605167e8 319 for (jj = 0 ; jj <= (Nbre_echantillons - 1) ; jj++) {
elaboure 0:07a3605167e8 320 // Et on lit des paquets de 5 octets pour cette requête
elaboure 0:07a3605167e8 321 for (ii = 0 ; ii <= 4 ; ii++) {
elaboure 0:07a3605167e8 322 // attente de la réponse
elaboure 0:07a3605167e8 323 temps_debut = horloge.read();
elaboure 0:07a3605167e8 324 test_time_out = true ;
elaboure 0:07a3605167e8 325 while ((LIDAR_link.readable() == false) & (test_time_out == true)) {
elaboure 0:07a3605167e8 326 temps_fin = horloge.read();
elaboure 0:07a3605167e8 327 test_time_out = (temps_fin - temps_debut) < 2e-3;
elaboure 0:07a3605167e8 328 }
elaboure 0:07a3605167e8 329
elaboure 0:07a3605167e8 330 if (test_time_out == false) {
elaboure 0:07a3605167e8 331 return false;
elaboure 0:07a3605167e8 332 lcd.Clear(LCD_COLOR_GREEN);
elaboure 0:07a3605167e8 333 }
elaboure 0:07a3605167e8 334 *(ptr_buffer_donnees_lidar+ ii + 5 * jj) = LIDAR_link.getc();
elaboure 0:07a3605167e8 335 }
elaboure 0:07a3605167e8 336 }
elaboure 0:07a3605167e8 337
elaboure 0:07a3605167e8 338 //On arrête l'acquisition une fois le nombre d'échantillons demandés atteints
elaboure 0:07a3605167e8 339 LIDAR_link.putc(LIDAR_STOP);
elaboure 0:07a3605167e8 340 wait_ms(2);
elaboure 0:07a3605167e8 341
elaboure 0:07a3605167e8 342 // Vidage buffer liaison série
elaboure 0:07a3605167e8 343 while (LIDAR_link.readable() == true) {
elaboure 0:07a3605167e8 344 Vidage_buffer_liaison_serie = LIDAR_link.getc();
elaboure 0:07a3605167e8 345 }
elaboure 0:07a3605167e8 346
elaboure 0:07a3605167e8 347 /* lcd.Clear(LCD_COLOR_WHITE);
elaboure 0:07a3605167e8 348 lcd.SetTextColor(LCD_COLOR_BLACK);
elaboure 0:07a3605167e8 349
elaboure 0:07a3605167e8 350 for (ii = 0 ; ii <= 6 ; ii++) {
elaboure 0:07a3605167e8 351 sprintf(affichage,"%x",response_descriptor[ii]);
elaboure 0:07a3605167e8 352 lcd.DisplayStringAt(0, LINE(ii),(uint8_t*) affichage, LEFT_MODE);
elaboure 0:07a3605167e8 353 }
elaboure 0:07a3605167e8 354
elaboure 0:07a3605167e8 355 for (jj = 0 ; jj <= (Nbre_echantillons - 1) ; jj++) {
elaboure 0:07a3605167e8 356 for (ii = 0 ; ii <= 4 ; ii++) {
elaboure 0:07a3605167e8 357 data = (*(ptr_buffer_donnees_lidar + ii + 5*jj));
elaboure 0:07a3605167e8 358 sprintf(affichage,"%x",data);
elaboure 0:07a3605167e8 359 lcd.DisplayStringAt(50+80*jj, LINE(ii),(uint8_t*) affichage, LEFT_MODE);
elaboure 0:07a3605167e8 360 }
elaboure 0:07a3605167e8 361 }
elaboure 0:07a3605167e8 362 */
elaboure 0:07a3605167e8 363
elaboure 0:07a3605167e8 364 radar();
elaboure 0:07a3605167e8 365 if ((response_descriptor[0] == 0xA5)&(Nbre_echantillons >=2)) {
elaboure 0:07a3605167e8 366
elaboure 0:07a3605167e8 367 for (jj = 1 ; jj <= (Nbre_echantillons - 1) ; jj++) {
elaboure 0:07a3605167e8 368
elaboure 0:07a3605167e8 369 // test erreur transmission S et _S
elaboure 0:07a3605167e8 370 data = (*(ptr_buffer_donnees_lidar + 5 * jj));
elaboure 0:07a3605167e8 371 S_S = data & 0x03;
elaboure 0:07a3605167e8 372
elaboure 0:07a3605167e8 373 // decodage qualité
elaboure 0:07a3605167e8 374
elaboure 0:07a3605167e8 375 decodage_qualite = data & 0xFD ;
elaboure 0:07a3605167e8 376 decodage_qualite = (decodage_qualite >> 2);
elaboure 0:07a3605167e8 377 // sprintf(affichage,"%x",decodage_qualite);
elaboure 0:07a3605167e8 378 // lcd.DisplayStringAt(50+80*jj, LINE(6),(uint8_t*) affichage, LEFT_MODE);
elaboure 0:07a3605167e8 379
elaboure 0:07a3605167e8 380 if ((decodage_qualite >= 0x08) && ((S_S == 0x01)|(S_S == 0x02))) {
elaboure 0:07a3605167e8 381
elaboure 0:07a3605167e8 382
elaboure 0:07a3605167e8 383 //decodage angle
elaboure 0:07a3605167e8 384 decodage_angle = 0x0000;
elaboure 0:07a3605167e8 385 data2 = (*(ptr_buffer_donnees_lidar + 2 + 5 * jj));
elaboure 0:07a3605167e8 386 data1 = (*(ptr_buffer_donnees_lidar + 1 + 5 * jj)) ;
elaboure 0:07a3605167e8 387 decodage_angle = data2;
elaboure 0:07a3605167e8 388 decodage_angle = (((decodage_angle << 8) | data1) >> 1);
elaboure 0:07a3605167e8 389
elaboure 0:07a3605167e8 390 angle = (decodage_angle/64.0) ; // en degré
elaboure 0:07a3605167e8 391 // sprintf(affichage,"%4.0f",angle);
elaboure 0:07a3605167e8 392 // lcd.DisplayStringAt(50+80*jj, LINE(7),(uint8_t*) affichage, LEFT_MODE);
elaboure 0:07a3605167e8 393
elaboure 0:07a3605167e8 394
elaboure 0:07a3605167e8 395 //decodage distance
elaboure 0:07a3605167e8 396 decodage_distance = 0x0000;
elaboure 0:07a3605167e8 397 data2 = (*(ptr_buffer_donnees_lidar + 4 + 5 * jj));
elaboure 0:07a3605167e8 398 data1 = (*(ptr_buffer_donnees_lidar + 3 + 5 * jj)) ;
elaboure 0:07a3605167e8 399 decodage_distance = data2;
elaboure 0:07a3605167e8 400 decodage_distance = ((decodage_distance << 8) | data1);
elaboure 0:07a3605167e8 401
elaboure 0:07a3605167e8 402
elaboure 0:07a3605167e8 403 distance = (decodage_distance/4000.0) ; // en mètres
elaboure 0:07a3605167e8 404 // sprintf(affichage,"%4.0f",distance);
elaboure 0:07a3605167e8 405 // lcd.DisplayStringAt(50+80*jj, LINE(8),(uint8_t*) affichage, LEFT_MODE);
elaboure 0:07a3605167e8 406
elaboure 0:07a3605167e8 407 // affichage du point
elaboure 0:07a3605167e8 408 angle = angle * (3.141592 / 180);
elaboure 0:07a3605167e8 409 x= 240 + distance * 45 * sin(angle);
elaboure 0:07a3605167e8 410 y= 136 - distance * 45 * cos(angle);
elaboure 0:07a3605167e8 411 lcd.SetTextColor(LCD_COLOR_RED);
elaboure 0:07a3605167e8 412 lcd.FillCircle(x,y,1);
elaboure 0:07a3605167e8 413
elaboure 0:07a3605167e8 414
elaboure 0:07a3605167e8 415 }// fin if
elaboure 0:07a3605167e8 416
elaboure 0:07a3605167e8 417 } // for jj
elaboure 0:07a3605167e8 418 } // fin if
elaboure 0:07a3605167e8 419
elaboure 0:07a3605167e8 420 return 1 ;
elaboure 0:07a3605167e8 421 }