Diff: main.cpp

Revision:

1:b72e95cee6fd

Parent:

0:5852409c8a40

--- a/main.cpp	Mon Jun 10 16:47:31 2019 +0000
+++ b/main.cpp	Tue Jun 11 20:01:09 2019 +0000
@@ -1,33 +1,510 @@
 #include "mbed.h"
-#include "QEI.h"
 #include "SDFileSystem.h"
+#include "QEI.h"
 
-void ClearMessage(char *Message, unsigned int len);
+
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+//////   Sección para la definición de Variables Estaticas   ///////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+
+// Variable que define la velocidad de muestreo para el calculo de la velocidad
+// de las helices
+#define Velocidad_Muestreo_Rotor    0.25
+
+// Esta variable sirve para convertir el numero de pulsos en radianes. Debido a 
+// que el calculo es (pulsos * Factor)/tiempo, las unidades de esta variable
+// deben de ser de radianes/pulsos
+#define FactorRotor                 0.25 
+
+// Esta variable define la frecuencia de trabajo de todos los PWMs
+#define FrecuenciaPWM               10000.0
+
+
+
+/*
+Puerto serial para la computadora, solo es para debug
+*/
+Serial PC(USBTX, USBRX, 115200);
+
+/*
+Puerto serial para el modulo de sensores, en estos pines
+se conecta el modulo fisicamente
+*/
+Serial Sensor(p9, p10, 115200);
+
+/*
+Puerto serial para el modulo Xbee, en estos pines
+se conecta el modulo fisicamente
+*/
+Serial XBee(p13, p14, 115200);
+
+
+/*
+Creacion de timers para las operaciones dependientes del tiempo
+*/
+Ticker      BaseTiempo; // Timer dedicado al calculo de velocidad de las helices
+Timeout     Expiracion; // Timer dedicado a checar si un paquete expira en tiempo de respuesta
+
+/*
+Creacion del objeto de gestion del encoder para el rotor
+Los parametros de creacion son: CanalA, CanalB, Index, Numero de Pulsos por Revolucion, (opcional)Tipo de decodificacion
+*/
+QEI Rotor(p15, p16, NC, 12);
+QEI EncCamara(p17, p18, NC, 12);
+
+/*
+Configuracion del PWM para el control de los motores y el buzzer
+*/
+PwmOut MotorCamara(p26);
+PwmOut MotorDesacople(p25);
+PwmOut Buzzer(p24);
+
+/*
+Definicion de los LEDS de proposito general
+*/
+DigitalOut ledA(LED1);
+DigitalOut ledB(LED2);
+DigitalOut ledC(LED3);
+DigitalOut ledD(LED4);
+
+/*
+Definicion de las variables para la maquina de estados
+*/
+#define A       0   //Estado dedicado a la inicializacion de los perifericos y recuperacion de estado
+#define B       1   //Estado dedicado a la recuperacion del estado anterior
+#define C       2   //Estado dedicado a realizar un diagnostico del sistema
+#define D       3   //Estado de espera del despegue
+#define E       4   //Estado de ascenso
+#define F       5   //Estado de descenso con paracaídas
+#define G       6   //Estado de Decenso con las helices
+#define H       7   //Estado aterrizaje
+
+LocalFileSystem local("local");
+bool Bandera_Empaquetar     =   false;
+bool Bandera_Enviar         =   false;
+bool Bandera_Almacenar      =   false;
+bool Bandera_Recepcion      =   false;
+
+float AlturaReferencia      =   0;
+
+//////////////////////////////////////////////////////
+//////////////////////////////////////////////////////
+///////         PAQUETE A TRANSMITIR        //////////
+//////////////////////////////////////////////////////
+                                                    //
+const unsigned int  Numero_Equipo       =   4784;   //
+unsigned long int   Tiempo              =   0;      //
+unsigned long int   NumeroPaquetes      =   0;      //
+float               Altura_metros       =   0;      //
+float               Presion_hPascales   =   0;      //
+float               Temperatura_Celsius =   0;      //
+float               Voltaje_Volts       =   0;      //
+float               Tiempo_hhmmss       =   0;      //
+float               GPS_Latitud         =   0;      //
+float               GPS_Longitud        =   0;      //
+float               GPS_Altitud         =   0;      //
+int                 GPS_Satelites       =   0;      //
+float               Pitch_grad          =   0;      //
+float               Roll_grad           =   0;      //
+float               Vel_Aspas_grad_s    =   0;      //
+int                 Estado              =   0;      //
+float               Desviacion_Camara   =   0;      //
+                                                    //
+//////////////////////////////////////////////////////
+
+/*
+Variables de referencia
+*/
+unsigned int EstadoGPS = 0;
+
+/*
+Definicion de las variables para el almacenamiento de datos en la SD Card
+*/
+SDFileSystem sd(p5,p6,p7,p8,"sd");
+
+/*
+Variables para la creacion y manipulacion del paquete a guardar y transmitir
+*/
+char Paquete[400] = {0};
+unsigned int len;
 
 
-DigitalOut myled(LED1);
-Serial Sensor_Module(p9, p10,115200);
+//Funcion que solo limpia una cadena
+void cleanBuffer(char *Buffer, unsigned int Len);
+//Funcion para el calculo de la velocidad, funciona con un timer de velocidad definida
+void CalculoVelocidad(void);
+//Funcion especial para la configuracion del rotor
+void ConfiguracionEncoderRotor(void);
+//Funcion especial para la configuracion del PWM
+void ConfiguracionGeneralPWM(void);
+//Funcion que gestiona la informacion proviniente del modulo de sensores
+void SensorInterrupt(void);
+//Funcion que gestiona la informacion proviniente del modulo de sensores
+void XbeeInterrupt(void);
+//Funcion dedicada a la construccion del paquete
+void ConstruirPaquete(void);
+//Funcion dedicada a guardar el paquete en la memoria SD
+void GuardarPaquete(void);
+void GuardarEstado(void);
+//Funcion dedicada a enviar el paquete a traves del Xbee
+void EnviarPaquete(void);
+//Funcion del estado A
+void EstadoA(void);
+//Funcion del estado B
+void EstadoB(void);
+//Funcion del estado C
+void EstadoC(void);
+//Funcion del estado D
+void EstadoD(void);
+//Funcion del estado E
+void EstadoE(void);
+//Funcion del estado F
+void EstadoF(void);
+//Funcion del estado G
+void EstadoG(void);
+//Funcion del estado H
+void EstadoH(void);
 
+/*
+Funciones requeridas para las comunicaciones
+*/
+void CheckSumInt(char *String, unsigned int *len, unsigned long Data);
+void CheckSumFloat(char *String, unsigned int *len, float Data);
+void PaqueteExpirado(void);
+ 
 int main() {
-    while(1) {
-        
-        char c = 0;
-        char Message[255] = {0};
-        unsigned int Index = 0;
-        
-        while ( c != 13 ){
-            c = Sensor_Module.getc();
-            Message[Index] = c;
-            Index++;
+    //Aqui se realiza el estado A
+    EstadoA();
+    
+    //Aqui se realiza el estado B
+    EstadoB();
+    
+    while(true){
+        GuardarEstado();
+        ConstruirPaquete();
+        if( Bandera_Recepcion ){
+            GuardarPaquete();
+            EnviarPaquete();
+            NumeroPaquetes++;
         }
         
-        Sensor_Module.printf(Message);
-        ClearMessage(Message,Index);
+        switch( Estado ){
+            case C:
+                EstadoC();
+                break;
+            case D:
+                EstadoD();
+                break;
+            case E:
+                EstadoE();
+                break;
+            case F:
+                EstadoF();
+                break;
+            case G:
+                EstadoG();
+                break;
+            case H:
+                EstadoH();
+                break;
+        }
+    }
+}
+ 
+ void cleanBuffer(char *Buffer, unsigned int Len){
+    for(int i = 0; i <= Len; i++ )Buffer[i] = 0;
+}
+ 
+ 
+/*
+Esta funcion tiene los calculos para la velocidad de rotacion del rotor
+*/
+void CalculoVelocidad(void) {
+  ledA = !ledA;
+  float Velocidad = ( (float)Rotor.getPulses() * FactorRotor )/ Velocidad_Muestreo_Rotor;
+}
+
+void ConfiguracionEncoderRotor(void){
+}
+
+/*
+Configuracion del PWM para los diferentes elementos
+El metodo write permite escribir el ciclo de trabajo del componente en cuestion
+*/
+void ConfiguracionGeneralPWM(void){
+    
+    MotorCamara.period(1.0/FrecuenciaPWM);
+    MotorCamara.write(0.25);
+    MotorDesacople.write(0.5);
+    Buzzer.write(1.0);
+    
+}
+
+//////////////////////////////////////////////////////
+//////////////////////////////////////////////////////
+///////         PAQUETE A TRANSMITIR        //////////
+///////////////////////////////////////////////////////
+//                                                   //
+//const unsigned int  Numero_Equipo       =   4784;  //
+//unsigned long int   Tiempo              =   0;      //
+//unsigned long int   NumeroPaquetes      =   0;      //
+//float               Altura_metros       =   0;      //
+//float               Presion_hPascales   =   0;      //
+//float               Temperatura_Celsius =   0;      //
+//float               Voltaje_Volts       =   0;      //
+//float               Tiempo_hhmmss       =   0;      ////
+//float               GPS_Latitud         =   0;      //
+//float               GPS_Longitud        =   0;      //
+//float               GPS_Altitud         =   0;      //
+//float               GPS_Satelites       =   0;      //
+//float               Pitch_grad          =   0;      //
+//float               Roll_grad           =   0;      //
+//float               Vel_Aspas_grad_s    =   0;      //
+//int                 Estado              =   0;      //
+//float               Desviacion_Camara   =   0;      //
+//                                                    //
+////////////////////////////////////////////////////////
+
+void SensorInterrupt(void){
+    static char Buffer[255] = {0};
+    static unsigned int Index = 0;
+    char c = Sensor.getc();
+    char Lat_Dir;
+    char Lon_Dir;
+    char Alt_Unidades;
+    
+    
+    if( Index == 0 ) cleanBuffer(Buffer,255);
+    
+    if( c != 13 ){
+        Buffer[Index] = c;
+        Index++;
+    }else{
+        sscanf(Buffer,"%f,%f,%f,%f,%f,%c,%f,%c,%i,%i,%f,%c",
+            &Roll_grad,
+            &Pitch_grad,
+            &Desviacion_Camara,
+            &Tiempo_hhmmss,
+            &GPS_Latitud,
+            &Lat_Dir,
+            &GPS_Longitud,
+            &Lon_Dir,
+            &EstadoGPS,
+            &GPS_Satelites,
+            &GPS_Altitud,
+            &Alt_Unidades);
         Index = 0;
-        
     }
 }
 
-void ClearMessage(char *Message, unsigned int len){
-    for(int i = 0; i <= len; i++) Message[i] = 0;
+void XbeeInterrupt(void){
+    static char Buffer[255] = {0};
+    static unsigned int Index = 0;
+    char c = XBee.getc();
+    
+    if( Index == 0 ) cleanBuffer(Buffer,255);
+    
+    if( c != 13 ){
+        Buffer[Index] = c;
+        Index++;
+    }else{
+        XBee.printf(Buffer);
+        Index = 0;
+    }
+}
+
+void ConstruirPaquete(void){
+    if (Bandera_Empaquetar){
+        cleanBuffer(Paquete,350);
+        
+        //Creacion de la cadena para el Numero de Equipo
+        char S_Num_Eq[50] = {0};
+        CheckSumInt(S_Num_Eq, &len, Numero_Equipo);
+    
+        char S_Tempo_s[50] = {0};
+        CheckSumInt(S_Tempo_s, &len, (unsigned long int)Tiempo);
+    
+        char S_Num_Paq[50] = {0};
+        CheckSumInt(S_Num_Paq, &len, (unsigned long int)NumeroPaquetes);
+        
+        srand(Tiempo);
+        Altura_metros       = (rand()%10000)*0.1;
+        char                S_Altura_m[50] = {0};
+        CheckSumFloat(S_Altura_m, &len, Altura_metros);
+    
+        Presion_hPascales   = (7000 + rand()%(10000-7000))*0.1;
+        char                S_Presion_hPa[50] = {0};
+        CheckSumFloat(S_Presion_hPa, &len, Presion_hPascales);
+    
+        Temperatura_Celsius = (150 + rand()%(400 - 150))*0.1;
+        char                S_Temp_C[50] = {0};
+        CheckSumFloat(S_Temp_C, &len, Temperatura_Celsius);
+    
+        Voltaje_Volts       = (33 + rand()%(90 - 33))*0.1;
+        char                S_Volts[50] = {0};
+        CheckSumFloat(S_Volts, &len, Voltaje_Volts);
+    
+        Tiempo_hhmmss       = 120000 + rand()%(240000 - 120000);
+        char                S_Tiempo[50] = {0};
+        CheckSumInt(S_Tiempo, &len, (unsigned long int)Tiempo_hhmmss);
+    
+        GPS_Latitud         = (2000000 + rand()%(12000000 - 2000000))*0.001;
+        char                S_Lat[50] = {0};
+        CheckSumFloat(S_Lat, &len,GPS_Latitud);
+    
+        GPS_Longitud        = (2000000 + rand()%(12000000 - 2000000))*0.001;
+        char                S_Lon[50] = {0};
+        CheckSumFloat(S_Lon, &len,GPS_Longitud);
+    
+        GPS_Altitud         = Altura_metros + (rand()%(10 + 10) - 10);
+        char                S_Alt[50] = {0};
+        CheckSumFloat(S_Alt, &len,GPS_Altitud);
+    
+        GPS_Satelites       = 3 + rand()%(7-3);
+        char                S_Sat[50] = {0};
+        CheckSumInt(S_Sat, &len,(int)GPS_Satelites);
+    
+        Pitch_grad          = (rand()%(300 + 300)- 300)*0.1;
+        char                S_Pitch[50] = {0};
+        CheckSumFloat(S_Pitch, &len,Pitch_grad);
+    
+        Roll_grad           = (rand()%(300 + 300)- 300)*0.1;
+        char                S_Roll[50] = {0};
+        CheckSumFloat(S_Roll, &len,Roll_grad);
+    
+        Vel_Aspas_grad_s    = 10 + rand()%(50 -10);
+        char                S_VelA[50] = {0};
+        CheckSumFloat(S_VelA, &len,Vel_Aspas_grad_s);
+    
+        Estado              = 1 + rand()%(9-1);
+        char                S_Estado[50] = {0};
+        CheckSumFloat(S_Estado, &len,Estado);
+    
+        Desviacion_Camara   = (rand()%(300 + 300)- 300)*0.1;
+        char                S_Desv[50] = {0};
+        CheckSumFloat(S_Desv, &len,Desviacion_Camara);
+        
+        len = sprintf(Paquete,"<%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s,>",
+                S_Num_Eq,
+                S_Tempo_s,
+                S_Num_Paq,
+                S_Altura_m,
+                S_Presion_hPa,
+                S_Temp_C,
+                S_Volts,
+                S_Tiempo,
+                S_Lat,
+                S_Lon,
+                S_Alt,
+                S_Sat,
+                S_Pitch,
+                S_Roll,
+                S_VelA,
+                S_Estado,
+                S_Desv);
+    }
+}
+
+void GuardarPaquete(void){
+    if(Bandera_Almacenar){
+        FILE *fp = fopen("/sd/Telemetria.txt","a");
+        fprintf(fp,"%s\r\n",Paquete);
+        fclose(fp);
+    }
+}
+
+void EnviarPaquete(void){
+    if(Bandera_Enviar){
+        PC.printf(Paquete);
+    }
+}
+
+void GuardarEstado(){
+    FILE *fp = fopen("local/Estado.txt","w");
+    fprintf(fp,"%i,%lu,%lu,%f",Estado,NumeroPaquetes,Tiempo,AlturaReferencia);
+    fclose(fp);
+}
+
+void EstadoA(void){
+    // Funcion dedicada a la configuracion del enconder del Rotor
+    ConfiguracionEncoderRotor();
+    
+    // Funcion dedicada para la configuracion del PWM
+    // NOTA IMPORTANTE: Para el LPC1768 todos los PWMs trabajan a la misma velocidad
+    // intentar cambiar la velocidad de un solo pin implica que todos los PWM 
+    // cambiaran a dicha velocidad
+    ConfiguracionGeneralPWM();
+    
+    /*
+    Este timer se usar para determinar la velocidad de
+    giro del rotor
+    */
+    BaseTiempo.attach(&CalculoVelocidad,2);
+    
+    /*
+    Aqui se establecen las interrupciones por ingreso de datos en el UART
+    */
+    Sensor.attach(&SensorInterrupt);
+    XBee.attach(&XbeeInterrupt);
+    
+}
+
+//Funcion del estado B
+void EstadoB(void){
+    char String[255] = {0};
+    FILE *fp = fopen("local/Estado.txt","r");
+    fgets(String,255,fp);
+    fclose(fp);
+    sscanf(String,"%i,%lu,%lu,%f",&Estado,&NumeroPaquetes,&Tiempo,&AlturaReferencia);
+}
+
+//Funcion del estado C
+void EstadoC(void);
+//Funcion del estado D
+void EstadoD(void);
+//Funcion del estado E
+void EstadoE(void);
+//Funcion del estado F
+void EstadoF(void);
+//Funcion del estado G
+void EstadoG(void);
+//Funcion del estado H
+void EstadoH(void);
+
+
+
+
+void CheckSumInt(char *String, unsigned int *len,unsigned long Data){
+    char Temp[50] = {0};
+    uint8_t Xor;
+
+    *len = sprintf(Temp,",%lu",Data);
+
+    Xor = Temp[0]^Temp[1];
+
+    for(int i = 2; i <= *len; i++){
+        Xor ^= Temp[i];
+    }
+
+    *len = sprintf(String,"%s*%02X",Temp,(int)Xor&0xFF);
+    return;
+}
+
+
+void CheckSumFloat(char *String, unsigned int *len, float Data){
+    char Temp[50] = {0};
+    uint8_t Xor;
+
+    *len = sprintf(Temp,",%f",Data);
+
+    Xor = Temp[0]^Temp[1];
+
+    for(int i = 2; i <= *len; i++){
+        Xor ^= Temp[i];
+    }
+
+    *len = sprintf(String,"%s*%02X",Temp,(int)Xor&0xFF);
+    return;
 }
\ No newline at end of file