File content as of revision 1:b72e95cee6fd:

#include "mbed.h"
#include "SDFileSystem.h"
#include "QEI.h"


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
//////   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;


//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() {
    //Aqui se realiza el estado A
    EstadoA();
    
    //Aqui se realiza el estado B
    EstadoB();
    
    while(true){
        GuardarEstado();
        ConstruirPaquete();
        if( Bandera_Recepcion ){
            GuardarPaquete();
            EnviarPaquete();
            NumeroPaquetes++;
        }
        
        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 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;
}