Felícito Manzano
/
CCN_Control_Bahia_TFMiniPlus
Actualizacion General del codigo para CCN con el objetivo de proveer mantenimiento estable.
exe_lidar.cpp@8:3fc41e5029f7, 2021-05-24 (annotated)
- Committer:
- fmanzano_dtk
- Date:
- Mon May 24 12:13:06 2021 -0600
- Revision:
- 8:3fc41e5029f7
- Parent:
- 6:5608afb92c51
Cambio de puertos RFID y LIDAR
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fmanzano_dtk | 1:c7bcbf5eac28 | 1 | /** |
fmanzano_dtk | 1:c7bcbf5eac28 | 2 | * @file exe_lidar.cpp |
fmanzano_dtk | 1:c7bcbf5eac28 | 3 | * @author Felícito Manzano (felicito.manzano@detektor.com.sv) |
fmanzano_dtk | 1:c7bcbf5eac28 | 4 | * @brief |
fmanzano_dtk | 1:c7bcbf5eac28 | 5 | * @version 0.1 |
fmanzano_dtk | 1:c7bcbf5eac28 | 6 | * @date 2021-05-24 |
fmanzano_dtk | 1:c7bcbf5eac28 | 7 | * |
fmanzano_dtk | 1:c7bcbf5eac28 | 8 | * @copyright Copyright (c) 2021 |
fmanzano_dtk | 1:c7bcbf5eac28 | 9 | * |
fmanzano_dtk | 1:c7bcbf5eac28 | 10 | */ |
fmanzano_dtk | 1:c7bcbf5eac28 | 11 | |
fmanzano_dtk | 1:c7bcbf5eac28 | 12 | #include "BufferedSerial.h" |
fmanzano_dtk | 1:c7bcbf5eac28 | 13 | #include "constantes.hpp" |
fmanzano_dtk | 4:e0b1d06c5cc5 | 14 | #include "lidar_tfminiplus.hpp" |
fmanzano_dtk | 6:5608afb92c51 | 15 | #include "f_basicas.hpp" |
fmanzano_dtk | 1:c7bcbf5eac28 | 16 | |
fmanzano_dtk | 1:c7bcbf5eac28 | 17 | extern BufferedSerial lidar; |
fmanzano_dtk | 1:c7bcbf5eac28 | 18 | extern DigitalOut display_DP; |
fmanzano_dtk | 6:5608afb92c51 | 19 | extern BusOut torreLuz; |
fmanzano_dtk | 6:5608afb92c51 | 20 | extern BusOut display_H; |
fmanzano_dtk | 6:5608afb92c51 | 21 | extern BusOut display_dM; |
fmanzano_dtk | 6:5608afb92c51 | 22 | extern BusOut display_uM; |
fmanzano_dtk | 6:5608afb92c51 | 23 | extern Serial pcusb; |
fmanzano_dtk | 6:5608afb92c51 | 24 | extern Serial gv300; |
fmanzano_dtk | 6:5608afb92c51 | 25 | extern Timer t_apagado; |
fmanzano_dtk | 1:c7bcbf5eac28 | 26 | extern bool lidar_respuesta; |
fmanzano_dtk | 1:c7bcbf5eac28 | 27 | extern bool lidar_valido; |
fmanzano_dtk | 1:c7bcbf5eac28 | 28 | extern bool mostrar_tiempo; |
fmanzano_dtk | 1:c7bcbf5eac28 | 29 | extern char skytrack_frame[]; |
fmanzano_dtk | 6:5608afb92c51 | 30 | extern char lidar_rx_frame[]; |
fmanzano_dtk | 6:5608afb92c51 | 31 | extern char actual_trailerID_HEX[]; |
fmanzano_dtk | 1:c7bcbf5eac28 | 32 | extern int lidar_contador_no_ack; |
fmanzano_dtk | 1:c7bcbf5eac28 | 33 | extern int distancia; |
fmanzano_dtk | 1:c7bcbf5eac28 | 34 | extern int lidar_vacio; |
fmanzano_dtk | 1:c7bcbf5eac28 | 35 | extern int contador_diferentes; |
fmanzano_dtk | 1:c7bcbf5eac28 | 36 | extern int contador_tramas; |
fmanzano_dtk | 6:5608afb92c51 | 37 | extern int w; |
fmanzano_dtk | 1:c7bcbf5eac28 | 38 | |
fmanzano_dtk | 3:bf6747585064 | 39 | void exe_LIDAR() { |
fmanzano_dtk | 1:c7bcbf5eac28 | 40 | lidar.write(TFMINIPLUS_TRIGGER_EXT,4); |
fmanzano_dtk | 4:e0b1d06c5cc5 | 41 | wait_us(50000); |
fmanzano_dtk | 1:c7bcbf5eac28 | 42 | lidar_respuesta = leer_lidar(&lidar, lidar_rx_frame); |
fmanzano_dtk | 1:c7bcbf5eac28 | 43 | if (lidar_respuesta > 0) { |
fmanzano_dtk | 1:c7bcbf5eac28 | 44 | lidar_valido = parsear_lidar(lidar_rx_frame, &distancia); |
fmanzano_dtk | 1:c7bcbf5eac28 | 45 | if (mostrar_tiempo) { |
fmanzano_dtk | 1:c7bcbf5eac28 | 46 | //lidar.write(LIDAR_EXTERNAL_TRIGGER, 8); |
fmanzano_dtk | 1:c7bcbf5eac28 | 47 | lidar_contador_no_ack = 0; |
fmanzano_dtk | 1:c7bcbf5eac28 | 48 | if (lidar_valido) { |
fmanzano_dtk | 1:c7bcbf5eac28 | 49 | pcusb.printf("\r\nDistancia: %5d cm", distancia); |
fmanzano_dtk | 1:c7bcbf5eac28 | 50 | if (distancia>=DISTANCIA_VACIO) { |
fmanzano_dtk | 1:c7bcbf5eac28 | 51 | lidar_vacio++; |
fmanzano_dtk | 1:c7bcbf5eac28 | 52 | } else { |
fmanzano_dtk | 1:c7bcbf5eac28 | 53 | lidar_vacio = 0; |
fmanzano_dtk | 1:c7bcbf5eac28 | 54 | } |
fmanzano_dtk | 1:c7bcbf5eac28 | 55 | } else { // Respuesta invalida |
fmanzano_dtk | 1:c7bcbf5eac28 | 56 | pcusb.printf("\r\nLiDAR invalida:\r\n"); |
fmanzano_dtk | 1:c7bcbf5eac28 | 57 | for (w=0;w<lidar_respuesta;w++) { |
fmanzano_dtk | 1:c7bcbf5eac28 | 58 | printf("0x%02X ", lidar_rx_frame[w]); |
fmanzano_dtk | 1:c7bcbf5eac28 | 59 | } |
fmanzano_dtk | 1:c7bcbf5eac28 | 60 | pcusb.printf("\r\n"); |
fmanzano_dtk | 1:c7bcbf5eac28 | 61 | lidar_vacio++; |
fmanzano_dtk | 1:c7bcbf5eac28 | 62 | } |
fmanzano_dtk | 1:c7bcbf5eac28 | 63 | } else { // Sin respuesta de LiDAR. Notificar daño |
fmanzano_dtk | 1:c7bcbf5eac28 | 64 | lidar_vacio++; |
fmanzano_dtk | 1:c7bcbf5eac28 | 65 | lidar_contador_no_ack++; |
fmanzano_dtk | 1:c7bcbf5eac28 | 66 | //pcusb.printf("\r\nLiDAR no responde %d/%d", lidar_contador_no_ack, LIDAR_ERROR); |
fmanzano_dtk | 1:c7bcbf5eac28 | 67 | |
fmanzano_dtk | 1:c7bcbf5eac28 | 68 | // Notificar a plataforma que el LiDar lleva mucho tiempo sin responder. |
fmanzano_dtk | 1:c7bcbf5eac28 | 69 | if (lidar_contador_no_ack >= LIDAR_ERROR) { |
fmanzano_dtk | 1:c7bcbf5eac28 | 70 | pcusb.printf("\r\n%s",BAY_LIDAR); // Notificación |
fmanzano_dtk | 1:c7bcbf5eac28 | 71 | sprintf(skytrack_frame, "%s%s%s%s%04X%s", |
fmanzano_dtk | 1:c7bcbf5eac28 | 72 | CABECERA_TX,CODIGO_INT,BAY_LIDAR,FIN_CABECERA_TX,contador_tramas,ULTIMO_CARACTER); |
fmanzano_dtk | 1:c7bcbf5eac28 | 73 | gv300.printf("%s\r\n", skytrack_frame); |
fmanzano_dtk | 1:c7bcbf5eac28 | 74 | incrementar_trama(&contador_tramas); |
fmanzano_dtk | 1:c7bcbf5eac28 | 75 | lidar_contador_no_ack = 0; |
fmanzano_dtk | 1:c7bcbf5eac28 | 76 | } |
fmanzano_dtk | 1:c7bcbf5eac28 | 77 | } |
fmanzano_dtk | 1:c7bcbf5eac28 | 78 | |
fmanzano_dtk | 1:c7bcbf5eac28 | 79 | if (lidar_vacio == DECLARAR_LIDAR_VACIO) { |
fmanzano_dtk | 1:c7bcbf5eac28 | 80 | pcusb.printf("\r\nBahia vacia..."); // Notificación |
fmanzano_dtk | 1:c7bcbf5eac28 | 81 | contador_diferentes = 0; // Se reinicia el contador de diferentes |
fmanzano_dtk | 1:c7bcbf5eac28 | 82 | mostrar_tiempo = false; |
fmanzano_dtk | 1:c7bcbf5eac28 | 83 | |
fmanzano_dtk | 1:c7bcbf5eac28 | 84 | pcusb.printf("\r\nSemaforo APAGADO..."); |
fmanzano_dtk | 1:c7bcbf5eac28 | 85 | presentar_torre(&APAGADO_TL, &torreLuz); // Apagar torre de luz |
fmanzano_dtk | 1:c7bcbf5eac28 | 86 | display_DP = 0; |
fmanzano_dtk | 1:c7bcbf5eac28 | 87 | |
fmanzano_dtk | 1:c7bcbf5eac28 | 88 | pcusb.printf("\r\nCronometro a APAGADO..."); |
fmanzano_dtk | 1:c7bcbf5eac28 | 89 | display_uM.write(DIGITOS[APAGADO_7S]); // Apagar display de unidades de minuto |
fmanzano_dtk | 1:c7bcbf5eac28 | 90 | display_dM.write(DIGITOS[APAGADO_7S]); // Apagar display de decenas de minuto |
fmanzano_dtk | 1:c7bcbf5eac28 | 91 | display_H.write(DIGITOS[APAGADO_7S]); // Apagar display de horas |
fmanzano_dtk | 1:c7bcbf5eac28 | 92 | |
fmanzano_dtk | 1:c7bcbf5eac28 | 93 | sprintf(skytrack_frame, "%s%s%s%s%04X%s", |
fmanzano_dtk | 1:c7bcbf5eac28 | 94 | CABECERA_TX,CODIGO_INT,BAY_EMPTY,FIN_CABECERA_TX,contador_tramas,ULTIMO_CARACTER); |
fmanzano_dtk | 1:c7bcbf5eac28 | 95 | gv300.printf("%s\r\n", skytrack_frame); |
fmanzano_dtk | 1:c7bcbf5eac28 | 96 | incrementar_trama(&contador_tramas); |
fmanzano_dtk | 1:c7bcbf5eac28 | 97 | t_apagado.reset(); t_apagado.start(); // Se inicia un contador para saber cuanto tiempo ha estado apagado. |
fmanzano_dtk | 1:c7bcbf5eac28 | 98 | pcusb.printf("\r\nInicia margen de %d segundos para descartar TAG previo...", TIEMPO_RECARGA); |
fmanzano_dtk | 1:c7bcbf5eac28 | 99 | pcusb.printf("\r\nPrevio: %s\r\n\r\n", actual_trailerID_HEX); |
fmanzano_dtk | 1:c7bcbf5eac28 | 100 | } |
fmanzano_dtk | 1:c7bcbf5eac28 | 101 | } else { |
fmanzano_dtk | 1:c7bcbf5eac28 | 102 | flush_uart_rx(&lidar); |
fmanzano_dtk | 1:c7bcbf5eac28 | 103 | } |
fmanzano_dtk | 1:c7bcbf5eac28 | 104 | } |