Felícito Manzano / Mbed OS CCN_Control_Bahia_TFMiniPlus

Actualizacion General del codigo para CCN con el objetivo de proveer mantenimiento estable.

Dependencies:   BufferedSerial

lidar_tfminiplus.cpp@1:c7bcbf5eac28, 2021-05-24 (annotated)

Committer:
fmanzano_dtk
Date:
Mon May 24 01:42:28 2021 -0600
Revision:
1:c7bcbf5eac28
Child:
2:ec0ddc911bd1
Actualizacion general de codigo. Version no compilable.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fmanzano_dtk 1:c7bcbf5eac28 1 /**
fmanzano_dtk 1:c7bcbf5eac28 2 * @file lidar_tfminiplus.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-23
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 "lidar_tfminiplus.hpp"
fmanzano_dtk 1:c7bcbf5eac28 13 #include "BufferedSerial.h"
fmanzano_dtk 1:c7bcbf5eac28 14 #include "constantes.hpp"
fmanzano_dtk 1:c7bcbf5eac28 15
fmanzano_dtk 1:c7bcbf5eac28 16
fmanzano_dtk 1:c7bcbf5eac28 17 // CONSTANTES LIDAR
fmanzano_dtk 1:c7bcbf5eac28 18 const char LIDAR_EXTERNAL_TRIGGER[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x00, 0x41 };
fmanzano_dtk 1:c7bcbf5eac28 19 const char LIDAR_GI_CONFIGURATION[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x02 };
fmanzano_dtk 1:c7bcbf5eac28 20 const char LIDAR_GO_CONFIGURATION[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x00, 0x02 };
fmanzano_dtk 1:c7bcbf5eac28 21 const char LIDAR_CHANGE_OUTPUT_1S[] = { 0x42, 0x57, 0x02, 0x00, 0xE8, 0x03, 0x00, 0x07 };
fmanzano_dtk 1:c7bcbf5eac28 22 const char LIDAR_SET_DISTANCE_500[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x03, 0x11 };
fmanzano_dtk 1:c7bcbf5eac28 23 const char LIDAR_SET_EXTERNAL_TRI[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x00, 0x40 };
fmanzano_dtk 1:c7bcbf5eac28 24 const char TFMINIPLUS_UPDATE_RATE[] = { 0x5A, 0x06, 0x03, 0x00, 0x00, 0x63};
fmanzano_dtk 1:c7bcbf5eac28 25 const char TFMINIPLUS_TRIGGER_EXT[] = { 0x5A, 0x04, 0x04, 0x62};
fmanzano_dtk 1:c7bcbf5eac28 26 const int LIDAR_BYTE0 = 0x59;
fmanzano_dtk 1:c7bcbf5eac28 27 const int DISTANCIA_VACIO = 450;
fmanzano_dtk 1:c7bcbf5eac28 28 const int DECLARAR_LIDAR_VACIO = 270;
fmanzano_dtk 1:c7bcbf5eac28 29 const int LIDAR_ERROR = 900;
fmanzano_dtk 1:c7bcbf5eac28 30
fmanzano_dtk 1:c7bcbf5eac28 31
fmanzano_dtk 1:c7bcbf5eac28 32 /**
fmanzano_dtk 1:c7bcbf5eac28 33 * @brief
fmanzano_dtk 1:c7bcbf5eac28 34 * Esta función recibe un puerto Buffered Serial de Lidar
fmanzano_dtk 1:c7bcbf5eac28 35 también un char array para almacenar los datos recibidos
fmanzano_dtk 1:c7bcbf5eac28 36 * @param puerto_lidar
fmanzano_dtk 1:c7bcbf5eac28 37 * @param mybuffer
fmanzano_dtk 1:c7bcbf5eac28 38 * @return true
fmanzano_dtk 1:c7bcbf5eac28 39 * @return false
fmanzano_dtk 1:c7bcbf5eac28 40 */
fmanzano_dtk 1:c7bcbf5eac28 41 int leer_lidar(BufferedSerial *puerto_lidar, char mybuffer[18]) {
fmanzano_dtk 1:c7bcbf5eac28 42 int contador = 0;
fmanzano_dtk 1:c7bcbf5eac28 43 while (puerto_lidar->readable()) {
fmanzano_dtk 1:c7bcbf5eac28 44 char incoming_char = puerto_lidar->getc();
fmanzano_dtk 1:c7bcbf5eac28 45 mybuffer[contador] = incoming_char;
fmanzano_dtk 1:c7bcbf5eac28 46 contador++;
fmanzano_dtk 1:c7bcbf5eac28 47 }
fmanzano_dtk 1:c7bcbf5eac28 48
fmanzano_dtk 1:c7bcbf5eac28 49 return(contador);
fmanzano_dtk 1:c7bcbf5eac28 50 }
fmanzano_dtk 1:c7bcbf5eac28 51
fmanzano_dtk 1:c7bcbf5eac28 52
fmanzano_dtk 1:c7bcbf5eac28 53
fmanzano_dtk 1:c7bcbf5eac28 54 /**
fmanzano_dtk 1:c7bcbf5eac28 55 * @brief
fmanzano_dtk 1:c7bcbf5eac28 56 * Esta función recibe un CHAR ARRAY y evalua si cumple
fmanzano_dtk 1:c7bcbf5eac28 57 con el protocolo de comunicación de LIDAR
fmanzano_dtk 1:c7bcbf5eac28 58 retorna el valor de distancia expresado en centímetros
fmanzano_dtk 1:c7bcbf5eac28 59 *
fmanzano_dtk 1:c7bcbf5eac28 60 * @param mybuffer
fmanzano_dtk 1:c7bcbf5eac28 61 * @param lidar_dist
fmanzano_dtk 1:c7bcbf5eac28 62 * @return true
fmanzano_dtk 1:c7bcbf5eac28 63 * @return false
fmanzano_dtk 1:c7bcbf5eac28 64 */
fmanzano_dtk 1:c7bcbf5eac28 65 bool parsear_lidar(char mybuffer[18], int *lidar_dist) {
fmanzano_dtk 1:c7bcbf5eac28 66 bool x = false;
fmanzano_dtk 1:c7bcbf5eac28 67
fmanzano_dtk 1:c7bcbf5eac28 68 if ((mybuffer[0] == LIDAR_BYTE0) and (mybuffer[1] == LIDAR_BYTE0)) {
fmanzano_dtk 1:c7bcbf5eac28 69 x= true;
fmanzano_dtk 1:c7bcbf5eac28 70 unsigned int t1 = mybuffer[2]; //Byte3
fmanzano_dtk 1:c7bcbf5eac28 71 unsigned int t2 = mybuffer[3]; //Byte4
fmanzano_dtk 1:c7bcbf5eac28 72 t2 <<= 8;
fmanzano_dtk 1:c7bcbf5eac28 73 t2 += t1;
fmanzano_dtk 1:c7bcbf5eac28 74 *lidar_dist = t2;
fmanzano_dtk 1:c7bcbf5eac28 75 }
fmanzano_dtk 1:c7bcbf5eac28 76
fmanzano_dtk 1:c7bcbf5eac28 77 return(x);
fmanzano_dtk 1:c7bcbf5eac28 78 }