exe_lidar.cpp

00001 /**
00002  * @file exe_lidar.cpp
00003  * @author Felícito Manzano (felicito.manzano@detektor.com.sv)
00004  * @brief 
00005  * @version 0.1
00006  * @date 2021-05-24
00007  * 
00008  * @copyright Copyright (c) 2021
00009  * 
00010  */
00011 
00012 #include "BufferedSerial.h"
00013 #include "constantes.hpp "
00014 #include "lidar_tfminiplus.hpp "
00015 #include "f_basicas.hpp "
00016 
00017 extern BufferedSerial   lidar;
00018 extern DigitalOut       display_DP;
00019 extern BusOut           torreLuz;
00020 extern BusOut           display_H;
00021 extern BusOut           display_dM;
00022 extern BusOut           display_uM;
00023 extern Serial           pcusb;
00024 extern Serial           gv300;
00025 extern Timer            t_apagado;
00026 extern bool             lidar_respuesta;
00027 extern bool             lidar_valido;
00028 extern bool             mostrar_tiempo;
00029 extern char             skytrack_frame[];
00030 extern char             lidar_rx_frame[];
00031 extern char             actual_trailerID_HEX[];
00032 extern int              lidar_contador_no_ack;
00033 extern int              distancia;
00034 extern int              lidar_vacio;
00035 extern int              contador_diferentes;
00036 extern int              contador_tramas;
00037 extern int              w;
00038 
00039 void exe_LIDAR() {
00040     lidar.write(TFMINIPLUS_TRIGGER_EXT,4);
00041     wait_us(50000);
00042     lidar_respuesta = leer_lidar(&lidar, lidar_rx_frame);
00043     if (lidar_respuesta > 0) {
00044         lidar_valido = parsear_lidar(lidar_rx_frame, &distancia);
00045         if (mostrar_tiempo) {
00046         //lidar.write(LIDAR_EXTERNAL_TRIGGER, 8);
00047             lidar_contador_no_ack = 0;   
00048             if (lidar_valido) {
00049                 pcusb.printf("\r\nDistancia: %5d cm", distancia);
00050                 if (distancia>=DISTANCIA_VACIO) {
00051                     lidar_vacio++;
00052                 } else {
00053                     lidar_vacio = 0;
00054                 }
00055             } else { // Respuesta invalida
00056                 pcusb.printf("\r\nLiDAR invalida:\r\n");
00057                 for (w=0;w<lidar_respuesta;w++) {
00058                     printf("0x%02X ", lidar_rx_frame[w]);
00059                 }
00060                 pcusb.printf("\r\n");
00061                 lidar_vacio++;
00062             }
00063         } else { // Sin respuesta de LiDAR. Notificar daño
00064             lidar_vacio++;
00065             lidar_contador_no_ack++;
00066             //pcusb.printf("\r\nLiDAR no responde %d/%d", lidar_contador_no_ack, LIDAR_ERROR);
00067 
00068             // Notificar a plataforma que el LiDar lleva mucho tiempo sin responder.
00069             if (lidar_contador_no_ack  >= LIDAR_ERROR) {
00070                 pcusb.printf("\r\n%s",BAY_LIDAR);  // Notificación
00071                 sprintf(skytrack_frame, "%s%s%s%s%04X%s",
00072                         CABECERA_TX,CODIGO_INT,BAY_LIDAR,FIN_CABECERA_TX,contador_tramas,ULTIMO_CARACTER);
00073                 gv300.printf("%s\r\n", skytrack_frame);
00074                 incrementar_trama(&contador_tramas);
00075                 lidar_contador_no_ack = 0;
00076             }
00077         }
00078 
00079         if (lidar_vacio == DECLARAR_LIDAR_VACIO) {
00080             pcusb.printf("\r\nBahia vacia...");  // Notificación
00081             contador_diferentes = 0;             // Se reinicia el contador de diferentes
00082             mostrar_tiempo = false;
00083 
00084             pcusb.printf("\r\nSemaforo APAGADO...");
00085             presentar_torre(&APAGADO_TL, &torreLuz); // Apagar torre de luz
00086             display_DP = 0;
00087 
00088             pcusb.printf("\r\nCronometro a APAGADO...");
00089             display_uM.write(DIGITOS[APAGADO_7S]);          // Apagar display de unidades de minuto
00090             display_dM.write(DIGITOS[APAGADO_7S]);          // Apagar display de decenas de minuto
00091             display_H.write(DIGITOS[APAGADO_7S]);           // Apagar display de horas
00092 
00093             sprintf(skytrack_frame, "%s%s%s%s%04X%s",
00094                     CABECERA_TX,CODIGO_INT,BAY_EMPTY,FIN_CABECERA_TX,contador_tramas,ULTIMO_CARACTER);
00095             gv300.printf("%s\r\n", skytrack_frame);
00096             incrementar_trama(&contador_tramas);
00097             t_apagado.reset(); t_apagado.start();           // Se inicia un contador para saber cuanto tiempo ha estado apagado.
00098             pcusb.printf("\r\nInicia margen de %d segundos para descartar TAG previo...", TIEMPO_RECARGA);
00099             pcusb.printf("\r\nPrevio: %s\r\n\r\n", actual_trailerID_HEX);
00100         }
00101     } else {
00102         flush_uart_rx(&lidar);
00103     }
00104 }