File content as of revision 0:a5908bca4740:

/***************************************
 VIRTUALMECH 
 Autor: Juan Manuel Amador Olivares
 Fecha: 16/9/2015
 ***************************************/
 
/***************************************
    Adquisición de datos IMU y Láser
 ***************************************/
#include "mbed.h"
#include "Buffering.h"
#include "BufferBig.h"
#include "Bufferinguint.h"

#define TAMPAQUETEIMU 34    // Tamaño del paquete recibido de la IMU
#define TAMENVIOIMU 30      // Tamaño del paquete enviado al PC con los datos de la IMU
#define TAMENVIOLASER 8     // Tamaño del paquete enviado al PC con los datos del sensor de distancia

/********************
    Funciones
 ********************/
void startStreamingIMU();   // Manda los bytes a la IMU necesarios para que comience a enviar datos por streaming
void envioPaquete(unsigned char paquete[], int nElementos); // Envía por el puerto serie un paquete de datos
void envioDatos();          // Envio al PC los bytes de datos cuando es posible
void flushSerialBuffer1(void);
void flushSerialBuffer2(void);
void flushSerialBuffer3(void);
/********************/

DigitalOut led(LED1);   // Led indicador. Se enciende cuando se están adquiriendo datos

// Puertos UART a utilizar
Serial pLaser(p9, p10);
Serial pIMU(p28, p27);
Serial pIMU2(p13, p14);
Serial pc(USBTX, USBRX);

// Temporizador para el control del tiempo
Timer t;
unsigned int auxtime = 0;   // Variable auxiliar para la lectura del tiempo
unsigned char byteIN, byteINanterior, byteINbuff, byteINanteriorbuff;   // Variables auxiliares para guardar bytes recibidos de la IMU
unsigned char byteIN2, byteINanterior2, byteINbuff2, byteINanteriorbuff2;   // Variables auxiliares para guardar bytes recibidos de la IMU2
unsigned char byteINL, byteINbuffL;  // Variable auxiliar para guardar bytes recibidos del laser

// Variables para la reconstrucción de los datos de la IMU
unsigned int nBytes = TAMPAQUETEIMU;
unsigned char paqueteEnvio[TAMENVIOIMU]; // 12 bytes para las aceleraciones (bytes cada eje) y otros 4 para la marca de tiempo y otros 12 para las velocidades angulares más un byte 'i' que indica que es el paquete de una IMU

// Variables para la reconstrucción de los datos de la IMU2
unsigned int nBytes2 = TAMPAQUETEIMU;
unsigned char paqueteEnvio2[TAMENVIOIMU]; // 12 bytes para las aceleraciones (bytes cada eje) y otros 4 para la marca de tiempo y otros 12 para las velocidades angulares más un byte 'i' que indica que es el paquete de una IMU2
/*union datoCompuesto{
    char B[4];
    float unidos;
} dato;*/

// Variables para la reconstrucción datos del láser
unsigned int distancia; // Sólo se usarán los 2 bytes menos significativos
unsigned char paqueteEnvioL[TAMENVIOLASER];
char fbyte = 0; // Indica si el byte leído es el primero delos dos enviados por el láser (tomando valor true) o el segundo (tomando valor false)

// Buffers dónde se guardarán los bytes provenientes de los sensores
Buffering IMUbuff;
Bufferinguint IMUtime;
Buffering IMUbuff2;
Bufferinguint IMUtime2;
Buffering LASERbuff;
Bufferinguint LASERtime;

// Buffer dónde se guardarán los datos antes de ser enviados
BufferBig envioBuff;
char continuaEnviando;

// Variables semáforo para que no se mezclen los bytes de distintos paquetes
// Estas variables indican de que sensor se pueden leer y procesar los bytes de los paquetes recibidos.
// Independientemente de estas variables se recibirán los bytes de los distintos sensores, que se irán guardando en sus respectivos buffer,
// y se irá guardando la marca de tiempo cuando se detecte la llegada de un nuevo paquete.
bool enviandoIMU = false;
bool enviandoIMU2 = false;
bool enviandoLASER = false;
bool adquiriendo = false;

// Variable de control de bytes enviados
unsigned int bytesEnviados;
unsigned int selIMUleer;

int main() {    
    // Inicialización de puertos UART
    pc.baud(921600);    // Configuración del puerto conectado al PC
    pIMU.baud(460800);  // Configuración del puerto conectado a la IMU
    pIMU2.baud(460800);  // Configuración del puerto conectado a la IMU2
    pLaser.baud(115200);    // Configuración del puerto conectado al sensor de distancia
    
    //pc.printf("Listo para recibir datos.\n\r");
        
    while(1) {  // Bucle infinito
        // Se comprueba la llegada de nuevos bytes procedentes del PC
        if(pc.readable()){
            char c = pc.getc(); // Se lee el byte
            if(c == 'a'){   // Si es el carácter 'a', indica el inicio de la adquisición
                adquiriendo = true;
                t.reset();
                t.start();
                startStreamingIMU();
                bytesEnviados = 0;
                led = 1;
            }else if(c == 's'){ // Si es el carácter 'a', indica el final de la adquisición
                adquiriendo = false;
                led = 0;
                t.stop();
                t.reset();
                
                // Puertos serie
                flushSerialBuffer1();
                flushSerialBuffer2();
                flushSerialBuffer3();
                
                // Buffer en RAM de mbed
                while(!IMUbuff.isEmpty()){
                    IMUbuff.get();
                }
                while(!IMUtime.isEmpty()){
                    IMUtime.get();
                }
                while(!IMUbuff2.isEmpty()){
                    IMUbuff2.get();
                }
                while(!IMUtime2.isEmpty()){
                    IMUtime2.get();
                }
                while(!LASERbuff.isEmpty()){
                    LASERbuff.get();
                }
                while(!LASERtime.isEmpty()){
                    LASERtime.get();
                }
                while(!envioBuff.isEmpty()){
                    envioBuff.get();
                }
                /*pc.printf("\n\nBytes enviados: %u\n\n", bytesEnviados);
                pc.printf("\n\nDatos en el buffer de envio: %u, %u, %u, %u\n\n", envioBuff.getDif(),IMUbuff.getDif(), IMUbuff2.getDif(),LASERbuff.getDif());*/
            }
        }
        if(adquiriendo){    // Sólo se reconstruye y envían datos cuando se ha iniciado la adquisición
        
            if(pIMU.readable()){
                byteINanterior = byteIN;
                byteIN = pIMU.getc();
                IMUbuff.put(byteIN);        // Se guarda el byte en el buffer
                if(byteIN == 0x65 && byteINanterior == 0x75){// Si el byte recibido es un 75 en hex, este byte marca la llegada de una nueva medida
                    // Se guarda la marca de tiempo
                    IMUtime.put(t.read_us());
                }            
            }
            
            // Se reciben los bytes de la IMU2, se guardan en un buffer y se guarda una marca de tiempo si corresponde
            if(pIMU2.readable()){
                byteINanterior2 = byteIN2;
                byteIN2 = pIMU2.getc();
                IMUbuff2.put(byteIN2);        // Se guarda el byte en el buffer
                if(byteIN2 == 0x65 && byteINanterior2 == 0x75){// Si el byte recibido es un 75 en hex, este byte marca la llegada de una nueva medida
                    // Se guarda la marca de tiempo
                    IMUtime2.put(t.read_us());
                }           
            }
            
            // Se reciben los bytes del LASER, se guardan en un buffer y se guarda una marca de tiempo si corresponde
            if(pLaser.readable()){
                byteINL = pLaser.getc();
                LASERbuff.put(byteINL); // Se guarda el byte en el buffer
                if(byteINL > 127){    // Los bytes con el bit más significativo a 1 son los primeros bytes d elos paquetes
                    // por lo que se guarda la marca de tiempo correspondiente
                    LASERtime.put(t.read_us());
                }
            }        
         
            if(!IMUbuff.isEmpty() && enviandoIMU2 == false && enviandoLASER == false){  // Si no se está enviando un paquete del láser y existen bytes por leer
                byteINanteriorbuff = byteINbuff;
                byteINbuff = IMUbuff.get();
                // Reconstrucción del dato
                if(byteINbuff == 0x65 && byteINanteriorbuff == 0x75 && nBytes >= (TAMPAQUETEIMU-1)){    // Si el byte recibido es un 75 en hex, este byte marca la llegada de una nueva medida
                    // Se pone el contador de bytes de un paquete a 0
                    //pc.putc('I');   // Se envían los dos bytes que indican el comienzo del paquete
                    //pc.putc('I');   //antes de enviar el primer bytes de datos
                    envioBuff.put('I');
                    envioBuff.put('I');
                    nBytes = 1;
                    enviandoIMU = true;     // Se está enviando un paquete de IMU

                }else{  // Todo lo que no sea 0x75 es parte del resto del dato
                    // Se suma un byte mas
                    nBytes++;
                    // Para reconstruir el dato se mandan primero los bytes más significativos (que también es el orden en el que llegan)
                    switch(nBytes){
                    case 6:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 7:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 8:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 9:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 10:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 11:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 12:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 13:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 14:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 15:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 16:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 17:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 20:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 21:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 22:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 23:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 24:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 25:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 26:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 27:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 28:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 29:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 30:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        break;
                    case 31:
                        //pc.putc(byteINbuff);
                        envioBuff.put(byteINbuff);
                        
                        auxtime = IMUtime.get();     // Se recupera la marca de tiempo del paquete y se envía dividido en 4 bytes
                        paqueteEnvio[3] = auxtime;
                        auxtime >>= 8;
                        paqueteEnvio[2] = auxtime;    
                        auxtime >>= 8;
                        paqueteEnvio[1] = auxtime;
                        auxtime >>= 8;    
                        paqueteEnvio[0] = auxtime;
                        /*pc.putc(paqueteEnvio[0]);
                        pc.putc(paqueteEnvio[1]);
                        pc.putc(paqueteEnvio[2]);
                        pc.putc(paqueteEnvio[3]);*/
                        envioBuff.put(paqueteEnvio[0]);
                        envioBuff.put(paqueteEnvio[1]);
                        envioBuff.put(paqueteEnvio[2]);
                        envioBuff.put(paqueteEnvio[3]);
                        enviandoIMU = false;        // Se ha terminado de enviar el paquete de la IMU
                        bytesEnviados += TAMENVIOIMU;
                        break;
                    }
                }            
            }
            
            if(!IMUbuff2.isEmpty() && enviandoIMU == false && enviandoLASER == false){  // Si no se está enviando un paquete del láser y existen bytes por leer
                byteINanteriorbuff2 = byteINbuff2;
                byteINbuff2 = IMUbuff2.get();
                // Reconstrucción del dato
                if(byteINbuff2 == 0x65 && byteINanteriorbuff2 == 0x75 && nBytes2 >= (TAMPAQUETEIMU-1)){    // Si el byte recibido es un 75 en hex, este byte marca la llegada de una nueva medida
                    // Se pone el contador de bytes de un paquete a 0
                    //pc.putc('I');   // Se envían los dos bytes que indican el comienzo del paquete
                    //pc.putc('I');   //antes de enviar el primer bytes de datos
                    envioBuff.put('H');
                    envioBuff.put('H');
                    nBytes2 = 1;
                    enviandoIMU2 = true;     // Se está enviando un paquete de IMU

                }else{  // Todo lo que no sea 0x75 es parte del resto del dato
                    // Se suma un byte mas
                    nBytes2++;
                    // Para reconstruir el dato se mandan primero los bytes más significativos (que también es el orden en el que llegan)
                    switch(nBytes2){
                    case 6:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 7:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 8:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 9:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 10:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 11:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 12:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 13:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 14:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 15:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 16:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 17:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 20:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 21:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 22:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 23:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 24:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 25:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 26:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 27:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 28:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 29:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 30:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        break;
                    case 31:
                        //pc.putc(byteINbuff2);
                        envioBuff.put(byteINbuff2);
                        
                        auxtime = IMUtime2.get();     // Se recupera la marca de tiempo del paquete y se envía dividido en 4 bytes
                        paqueteEnvio2[3] = auxtime;
                        auxtime >>= 8;
                        paqueteEnvio2[2] = auxtime;    
                        auxtime >>= 8;
                        paqueteEnvio2[1] = auxtime;
                        auxtime >>= 8;
                        paqueteEnvio2[0] = auxtime;
                        envioBuff.put(paqueteEnvio2[0]);
                        envioBuff.put(paqueteEnvio2[1]);
                        envioBuff.put(paqueteEnvio2[2]);
                        envioBuff.put(paqueteEnvio2[3]);
                        enviandoIMU2 = false;        // Se ha terminado de enviar el paquete de la IMU
                        bytesEnviados += TAMENVIOIMU;
                        break;
                    }
                }            
            }
            
            if(!LASERbuff.isEmpty() && enviandoIMU == false && enviandoIMU2 == false){
                byteINbuffL = LASERbuff.get();       
                // Si el byte recibido tiene el bit más significativo a 1 es el byte más significativo
                if(byteINbuffL > 127){
                    fbyte = 1;
                    distancia = byteINbuffL; 
                    distancia &=~0x80;    // El bit mas significativo hay que ponerlo a 0
                    enviandoLASER = true;   // Se comienza a enviar el paquete del láser
                }else{  // Recibido byte menos significativo
                    if (fbyte == 1){    // Si se recibió un byte más significativo puede reconstruirse el valor medido por el laser
                        auxtime = LASERtime.get();  // Se lee el tiempo cuando en el instante de llegada del primer byte del paquete
                        fbyte = 0;
                        distancia <<= 7;
                        distancia += byteINbuffL;    // Unidades de ingenieria
                        //distancia = distancia*0.0498;     // (mm) La conversión a mm se hace en el PC para no tener que enviar un tipo float 
                                                            // que guardara los decimales de la operación
                        // Los dos primeros bytes byte de los paquetes del laser seran una 'L'
                        paqueteEnvioL[0] = 'L';
                        paqueteEnvioL[1] = 'L';
                        paqueteEnvioL[3] = distancia;
                        distancia >>= 8;
                        paqueteEnvioL[2] = distancia;
                        // Se añaden también los 4 bytes del tiempo y se envía el paquete (siempre el byte más significativo se envía primero)
                        paqueteEnvioL[7] = auxtime;
                        auxtime >>= 8;
                        paqueteEnvioL[6] = auxtime;    
                        auxtime >>= 8;
                        paqueteEnvioL[5] = auxtime;
                        auxtime >>= 8;    
                        paqueteEnvioL[4] = auxtime;
                        envioPaquete(paqueteEnvioL, TAMENVIOLASER);  
                        enviandoLASER = false;    // Se ha terminado de enviar el paquete del láser
                        bytesEnviados += TAMENVIOLASER;             
                    }                
                }
                       
            }
            envioDatos();
        }     // if(adquiriendo)
    }   // while(1)
}       // main

// Envia todos los datos que pueda hasta que ya no se puede escribir en el puerto o no haya más datos que enviar
void envioDatos(){
    continuaEnviando = 1;
    while(continuaEnviando){
        if (!envioBuff.isEmpty()){
            if(pc.writeable()){
                pc.putc(envioBuff.get());
                /*envioBuff.get();
                pc.putc('2');*/
            }else{
                continuaEnviando = 0;
            }
        }else{
           continuaEnviando = 0;
        }
    }
}

void envioPaquete(unsigned char paquete[], int nElementos){ // Envía por el puerto serie un paquete de datos
    for(int i = 0; i < nElementos; i++){
        //pc.putc(paquete[i]);
        envioBuff.put(paquete[i]);
    }
}

void startStreamingIMU(){
    // Se envia un paquete de datos a la IMU que indica el comienzo de lectura de medidas
    pIMU.putc(0x75); // K
    pIMU.putc(0x65); // A
    pIMU.putc(0x0C); // alt + 12 ♀
    pIMU.putc(0x05); // alt + 5 ♣
    pIMU.putc(0x05); // alt + 5 ♣
    pIMU.putc(0x11); // alt + 11 ♂
    pIMU.putc(0x01); // alt + 1 ☺
    pIMU.putc(0x01); // alt + 1 ☺
    pIMU.putc(0x01); // alt + 1 ☺    
    pIMU.putc(0x04); // alt + 4 ♦   
    pIMU.putc(0x1A); // alt + 26 →
    
    pIMU2.putc(0x75); // K
    pIMU2.putc(0x65); // A
    pIMU2.putc(0x0C); // alt + 12 ♀
    pIMU2.putc(0x05); // alt + 5 ♣
    pIMU2.putc(0x05); // alt + 5 ♣
    pIMU2.putc(0x11); // alt + 11 ♂
    pIMU2.putc(0x01); // alt + 1 ☺
    pIMU2.putc(0x01); // alt + 1 ☺
    pIMU2.putc(0x01); // alt + 1 ☺    
    pIMU2.putc(0x04); // alt + 4 ♦   
    pIMU2.putc(0x1A); // alt + 26 →
    //pc.printf("Paquete de inicio enviado.\n\r");
}

void flushSerialBuffer1(void) { char char1 = 0; while (pIMU.readable()) { char1 = pIMU.getc(); } return; }
void flushSerialBuffer2(void) { char char1 = 0; while (pIMU2.readable()) { char1 = pIMU2.getc(); } return; }
void flushSerialBuffer3(void) { char char1 = 0; while (pLaser.readable()) { char1 = pLaser.getc(); } return; }