File content as of revision 2:91a80cc7d87d:

/* Includes */
#include "mbed.h" /* Mbed include */

#include "XNucleoIKS01A2.h" /* Sensors include*/

/* LoRa includes */
#include "PinMap.h" 
#include "sx1276-mbed-hal.h" 

/* Serial communication include */
#include "BufferedSerial.h"

/* SD card includes */
#include "SDCard_Y.hh"

/* GPS include */
#include "UbxGpsNavSol.hh"


/* GPS definitions */
#define GPS_BAUDRATE 115200
//#define GPS_EN

/* Definition of buzzer time in ms */
#define BUZZER_TIME 500

/* Definition of Disable enable flag */
//#define KEY_ENABLE

/* Definition of the SD card */
#define SPI_FREQUENCY 1000000

/* Definition of the SD card */
//#define SD_EN

/* LoRa definitions */

/* Set this flag to display debug messages on the console */
#define DEBUG_MESSAGE

/* Set this flag to '1' to use the LoRa modulation */
#define USE_MODEM_LORA          1
#define USE_MODEM_FSK           !USE_MODEM_LORA
#define RF_FREQUENCY            RF_FREQUENCY_915_0  // Hz
#define TX_OUTPUT_POWER         14                  // 20 dBm

#if USE_MODEM_LORA == 1

#define LORA_BANDWIDTH          125000  // LoRa default, details in SX1276::BandwidthMap
#define LORA_SPREADING_FACTOR   LORA_SF7
#define LORA_CODINGRATE         LORA_ERROR_CODING_RATE_4_5

#define LORA_PREAMBLE_LENGTH    8       // Same for Tx and Rx
#define LORA_SYMBOL_TIMEOUT     5       // Symbols
#define LORA_FIX_LENGTH_PAYLOAD_ON  false
#define LORA_FHSS_ENABLED       false  
#define LORA_NB_SYMB_HOP        4     
#define LORA_IQ_INVERSION_ON    false
#define LORA_CRC_ENABLED        true
    
#endif 


#define RX_TIMEOUT_VALUE    0       // In ms
#define TX_TIMEOUT_VALUE    1000000 // In ms
#define BUFFER_SIZE         64      // In bytes

/* Sensors instances */


/* Instantiate the expansion board */
static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);

/* Retrieve the composing elements of the expansion board */
static LSM303AGRMagSensor *magnetometer = mems_expansion_board->magnetometer;
static HTS221Sensor *hum_temp = mems_expansion_board->ht_sensor;
static LPS22HBSensor *press_temp = mems_expansion_board->pt_sensor;
static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
static LSM303AGRAccSensor *accelerometer = mems_expansion_board->accelerometer;

typedef struct {
        uint8_t header; // Header for identification of updated informations - 0 0 p temp LSM6DSL LSM303AGR
        int time; // Time between transmissions
        float p;  // Pressure of LPS22HB
        float temperatureLPS22HB; // Temperature from LPS22HB
        int32_t ag[3]; // Acceleration of the accelerometer and gyroscope LSM6DSL 
        int32_t w[3]; // Angular velocity of LSM6DSL
        int32_t a[3]; // Acceleration of the accelerometer LSM303AGR
        int32_t m[3]; // Heading of LSM303AGR
}Pkg1;

typedef struct {
        uint8_t header; // Header for identification of updated informations - 0 1 InternalCommunication HTS221
        int time; // Time between transmissions
        bool drogueStatus; // Drogue parachute status provided by Avionics
        bool mainStatus; //Main parachute status provided by Avionics
        bool mainStatusCOTS; // Main parachute status provided by COTS Altimeter
        bool drogueStatusCOTS; // Drogue status provided by COTS Altimeter
        float pressureBar; // Pressure by COTS Altimeter
        float temperature; // Temperature by COTS Altimeter
        int16_t timeStamp; // Timestamp from COTS Altimeter
        int16_t aglAlt; // AGL Altitude from COTS Altimeter
        int8_t battery; // Battery voltage reading from COTS Altimeter
        float humidity; // Humidity of HTS221
        float temperatureHTS221; // Temperature from HTS221
        //uint8_t filler[25];
}Pkg2;

typedef struct {
        uint8_t header; // Header for identification of updated informations - 1 0 GPS 
        int time; // Time between transmissions
        unsigned long timeOfWeek; //GPS time of week
        long timeOfWeekFracPart; // GPS time of week fractional part
        unsigned char gpsFix; // GPS fix
        long ecefx; // GPS X posiition
        long ecefy; // GPS Y posistion
        long ecefz; // GPS Z postion
        unsigned long positionAcc3D; // GPS 3D position accuracy
        long ecefvx; // GPS X velocity
        long ecefvy; // GPS Y velocity
        long ecefvz; // GPS Z velocity
        unsigned long speedAcc; // GPS speed accuracy
        unsigned char numbSat; // GPS number of satellites conected
        //uint8_t filler[8];
}Pkg3;


union Data {
    Pkg1 pkg1;
    Pkg2 pkg2;
    Pkg3 pkg3;
};

Data data;


/* LoRa modem instances and configurations */

static RadioEvents_t RadioEvents; // Calback functions struct

SX1276Generic *Radio; // Definition of a Radio object

/* Configuration function */
void SystemClock_Config(void);

bool transmited = true;// Flag to indicate the end of transmission
uint8_t state = 0; // Flag to indicate state of transmission, i.e. which package is to be transmited

/* Callback functions prototypes */

// Brief Function to be executed on Radio Tx Done event
void OnTxDone(void *radio, void *userThisPtr, void *userData);

// Brief Function to be executed on Radio Rx Done event
void OnRxDone(void *radio, void *userThisPtr, void *userData, uint8_t *payload, uint16_t size, int16_t rssi, int8_t snr );

// Brief Function executed on Radio Tx Timeout event
void OnTxTimeout(void *radio, void *userThisPtr, void *userData);

// Brief Function executed on Radio Rx Timeout event
void OnRxTimeout(void *radio, void *userThisPtr, void *userData);

// Brief Function executed on Radio Rx Error event
void OnRxError(void *radio, void *userThisPtr, void *userData);

// Brief Function executed on Radio Fhss Change Channel event
void OnFhssChangeChannel(void *radio, void *userThisPtr, void *userData, uint8_t channelIndex);

#ifdef DEBUG_MESSAGE
/* Serial communication to debug program */
BufferedSerial *ser;
#endif

/* Buzzer definition */
DigitalOut buzzer (PA_0);

Timer timer2;

int main() {
    #ifdef SD_EN
    Timer timerSD;
    uint8_t bufferSD[512]; // Buffer to store data to save in SD card
    uint8_t bufferSDCounter = 0; // Counter to access correct position of bufferSD 
    for(int i = 0; i < 512; i++){
        bufferSD[i] = 0;   
    }
    timerSD.start();
    #endif
    timer2.start();
    buzzer = 0;
    /* Power on*/
    buzzer = 1;
    wait_ms (BUZZER_TIME);
    buzzer = 0;
    wait_ms (BUZZER_TIME);
    /* Set to zero all parameters of the data struct */
    /*
    pkg1.header = 0x00;
    pkg2.header = 0x40;
    pkg3.header = 0x80;
    pkg1.time = 0;
    pkg2.time = 0;
    pkg3.time = 0;
    pkg1.ag[0] = 0;
    pkg1.ag[1] = 0;
    pkg1.ag[2] = 0;
    pkg1.w[0] = 0;
    pkg1.w[1] = 0;
    pkg1.w[2] = 0;
    pkg1.a[0] = 0;
    pkg1.a[1] = 0;
    pkg1.a[2] = 0;
    pkg1.m [0] = 0;
    pkg1.m [1] = 0;
    pkg1.m [2] = 0;
    pkg1.p = 0;  
    pkg1.temperatureLPS22HB = 0; 
    pkg2.humidity = 0; 
    pkg2.temperatureHTS221 = 0; 
    pkg3.timeOfWeek = 0; 
    pkg3.timeOfWeekFracPart = 0;
    pkg3.gpsFix = 0; 
    pkg3.ecefx = 0; 
    pkg3.ecefy = 0; 
    pkg3.ecefz = 0; 
    pkg3.positionAcc3D = 0; 
    pkg3.ecefvx = 0; 
    pkg3.ecefvy = 0; 
    pkg3.ecefvz = 0; 
    pkg3.speedAcc = 0; 
    pkg3.numbSat = 0; 
    pkg2.drogueStatus = 0; 
    pkg2.mainStatus = 0; 
    pkg2.pressureBar = 0; 
    pkg2.temperature = 0; 
    pkg2.mainStatusCOTS = 0; 
    pkg2.drogueStatusCOTS = 0; 
    pkg2.timeStamp = 0; 
    pkg2.aglAlt = 0; 
    pkg2.battery = 0;
    
    for(int i = 0; i < sizeof(pkg2.filler); i++){
         pkg2.filler[i] = 0;     
    }
    for(int i = 0; i < sizeof(pkg3.filler); i++){
        pkg3.filler[i] = 0;
    }
    */
         
    #ifdef GPS_EN
    //Serial connections (GPS)(TX,RX,baud)
    UbxGpsNavSol gps(PA_9, PA_10, GPS_BAUDRATE);
    #endif
    
    SystemClock_Config(); /* Synchronize clock for TX and RX boards */
    
    /* Serial configuration */
    #ifdef DEBUG_MESSAGE
    ser = new BufferedSerial(USBTX, USBRX);
    ser->baud(115200);
    ser->format(8);
    #endif
        
    /* General Header*/
    #ifdef DEBUG_MESSAGE
    ser->printf ("Telemetry Tx inicial version program\r\n\r\n");
    uint8_t id; //Sensor id parameter for debug purpose
    #endif
    
    /* Enable all sensors */
    if (hum_temp->enable() != 0) {
        #ifdef DEBUG_MESSAGE
        ser->printf ("Humidity sensor not enabled\r\n");
        #endif
    }
    if (press_temp->enable() != 0) {
        #ifdef DEBUG_MESSAGE
        ser->printf ("Temperature sensor not enabled\r\n");
        #endif
    }    
    if (magnetometer->enable() != 0) {
        #ifdef DEBUG_MESSAGE
        ser->printf ("Magnetometer sensor not enabled\r\n");
        #endif
    }
    if (accelerometer->enable() != 0) {
        #ifdef DEBUG_MESSAGE
        ser->printf ("Accelerometer1 sensor not enabled\r\n");
        #endif
    }
    if (acc_gyro->enable_x() != 0) {
        #ifdef DEBUG_MESSAGE
        ser->printf ("Gyroscope sensor not enabled\r\n");
        #endif
    }    
    if (acc_gyro->enable_g() != 0) {
        #ifdef DEBUG_MESSAGE
        ser->printf ("Accelerometer2 sensor not enabled\r\n");
        #endif
    }
    
    #ifdef  DEBUG_MESSAGE
    ser->printf("\r\n--- Starting the sensors ---\r\n");
        
    hum_temp->read_id(&id);
    ser->printf("HTS221  humidity & temperature    = 0x%X\r\n", id);
    press_temp->read_id(&id);
    ser->printf("LPS22HB  pressure & temperature   = 0x%X\r\n", id);
    magnetometer->read_id(&id);
    ser->printf("LSM303AGR magnetometer            = 0x%X\r\n", id);
    accelerometer->read_id(&id);
    ser->printf("LSM303AGR accelerometer           = 0x%X\r\n", id);
    acc_gyro->read_id(&id);
    ser->printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
        
    ser->printf("\r\n");
    #endif
    
    /* Radio setup */
    #ifdef DEBUG_MESSAGE
    ser->printf("\r\n--- Starting the modem LoRa ---\r\n");
    #endif
    Radio = new SX1276Generic(NULL, MURATA_SX1276,
            LORA_SPI_MOSI, LORA_SPI_MISO, LORA_SPI_SCLK, LORA_CS, LORA_RESET,
            LORA_DIO0, LORA_DIO1, LORA_DIO2, LORA_DIO3, LORA_DIO4, LORA_DIO5,
            LORA_ANT_RX, LORA_ANT_TX, LORA_ANT_BOOST, LORA_TCXO);
    #ifdef DEBUG_MESSAGE
    ser->printf("SX1276 Simple transmission aplication\r\n" );
    ser->printf("Frequency: %.1f\r\n", (double)RF_FREQUENCY/1000000.0);
    ser->printf("TXPower: %d dBm\r\n",  TX_OUTPUT_POWER);
    ser->printf("Bandwidth: %d Hz\r\n", LORA_BANDWIDTH);
    ser->printf("Spreading factor: SF%d\r\n", LORA_SPREADING_FACTOR);
    #endif
    
    // Initialize Radio driver
    RadioEvents.TxDone = OnTxDone;
    RadioEvents.RxDone = OnRxDone;
    RadioEvents.RxError = OnRxError;
    RadioEvents.TxTimeout = OnTxTimeout;
    RadioEvents.RxTimeout = OnRxTimeout; 
    
    for (int i = 0; Radio->Init( &RadioEvents ) != true && i < 40; i++) {
        #ifdef DEBUG_MESSAGE
        ser->printf("Radio could not be detected!\r\n");
        #endif
        buzzer = 1;
        wait_ms (BUZZER_TIME);
        buzzer = 0;
        wait_ms (BUZZER_TIME);
    }
    
    // Display the board type
    #ifdef DEBUG_MESSAGE
    switch(Radio->DetectBoardType()) {
        case SX1276MB1LAS:
            ser->printf(" > Board Type: SX1276MB1LAS <\r\n");
            break;
        case SX1276MB1MAS:
            ser->printf(" > Board Type: SX1276MB1LAS <\r\n");
            break;
        case MURATA_SX1276:
            ser->printf(" > Board Type: MURATA_SX1276_STM32L0 <\r\n");
            break;
        case RFM95_SX1276:
            ser->printf(" > HopeRF RFM95xx <\r\n");
            break;
        default:
            ser->printf(" > Board Type: unknown <\r\n");
    }
    #endif
    Radio->SetChannel(RF_FREQUENCY ); // Sets the frequency of the communication
    
    // Debug message of the state of fhss
    #ifdef DEBUG_MESSAGE
    if (LORA_FHSS_ENABLED) {
        ser->printf("             > LORA FHSS Mode <\r\n");
    }    
    if (!LORA_FHSS_ENABLED) {
        ser->printf("             > LORA Mode <\r\n");
    }
    #endif
    // Sets the configuration of the transmission    
    Radio->SetTxConfig( MODEM_LORA, TX_OUTPUT_POWER, 0, LORA_BANDWIDTH,
                         LORA_SPREADING_FACTOR, LORA_CODINGRATE,
                         LORA_PREAMBLE_LENGTH, LORA_FIX_LENGTH_PAYLOAD_ON,
                         LORA_CRC_ENABLED, LORA_FHSS_ENABLED, LORA_NB_SYMB_HOP, 
                         LORA_IQ_INVERSION_ON, 2000 );
    
    // Sets the configuration of the reception
    Radio->SetRxConfig( MODEM_LORA, LORA_BANDWIDTH, LORA_SPREADING_FACTOR,
                         LORA_CODINGRATE, 0, LORA_PREAMBLE_LENGTH,
                         LORA_SYMBOL_TIMEOUT, LORA_FIX_LENGTH_PAYLOAD_ON, 0,
                         LORA_CRC_ENABLED, LORA_FHSS_ENABLED, LORA_NB_SYMB_HOP, 
                         LORA_IQ_INVERSION_ON, true );
      
    
    #ifdef SD_EN
    SPI spi (PA_7, PA_6, PA_5);
    spi.frequency (SPI_FREQUENCY);
    SDCard sdCard (&spi, PB_10);
    uint8_t sdData[BUFFER_SIZE];
    int block = 0;
    #endif
    
    
    Radio->Tx(TX_TIMEOUT_VALUE); // Puts the device in transmission mode for a long period
    
    Timer timerHTS221; // Timer for HTS221
    Timer timerGPS; // Timer for GPS
    timerHTS221.start(); // Starting timer for humidity sensor
    timerGPS.start();
    
    //----------------> Starting the main loop <----------------
    while(1) { // 1
        
        if(state == 0){ // Update and send pkg1 // 2
            #ifdef DEBUG_MESSAGE
            //ser->printf("    Here\r\n");
            #endif 
            // Initialization of pkg1
            data.pkg1.header = 0xC0;
            data.pkg1.time = 0;
            data.pkg1.ag[0] = 0;
            data.pkg1.ag[1] = 0;
            data.pkg1.ag[2] = 0;
            data.pkg1.w[0] = 0;
            data.pkg1.w[1] = 0;
            data.pkg1.w[2] = 0;
            data.pkg1.a[0] = 0;
            data.pkg1.a[1] = 0;
            data.pkg1.a[2] = 0;
            data.pkg1.m [0] = 0;
            data.pkg1.m [1] = 0;
            data.pkg1.m [2] = 0;
            data.pkg1.p = 0;  
            data.pkg1.temperatureLPS22HB = 0;
            
            if (press_temp->get_pressure(&data.pkg1.p) == 0) { // Get the pressure // 3
                data.pkg1.header |= 0xE0; // LPS22HB updated
                #ifdef DEBUG_MESSAGE
                //ser->printf("The pressure data from LPS22HB was read\r\n");
                #endif
            } else { // 3 4
                data.pkg1.header &= 0xDC; // LPS22HB was not updated
            } // 4
            
            if (press_temp->get_temperature(&data.pkg1.temperatureLPS22HB) == 0) { // Get temperature from LPS22HB // 5
                data.pkg1.header  |= 0xD0; // LPS22HB updated
                #ifdef DEBUG_MESSAGE
                //ser->printf("The temperature data from LPS22HB was read\r\n");
                #endif
            } else { // 5 6
                data.pkg1.header &= 0xEC; // LPS22HB not updated
            } // 6
            
            if (accelerometer->get_x_axes(data.pkg1.a) == 0) {// Get the acceleration // 7
                data.pkg1.header  |= 0xC4; // LSM303AGR updated
                #ifdef DEBUG_MESSAGE
                //ser->printf("The acceleration data from LSM303AGR was read\r\n");
                #endif
            } else { // 7 8
                data.pkg1.header  &= 0xF8; // LSM303AGR not updated
            } // 8
            
            if (acc_gyro->get_x_axes(data.pkg1.ag) == 0) {// Get the acceleration // 9
                data.pkg1.header  |= 0xC8; // LSM6DSL updated 
                #ifdef DEBUG_MESSAGE
                //ser->printf("The acceleration data from LSM6DSL was read\r\n");
                #endif
            } else { // 9 10
                data.pkg1.header  &= 0xF4; // LSM6DSL not updated
            } // 10
            
            if (acc_gyro->get_g_axes(data.pkg1.w) == 0) {// Get the angular velocity // 11
                data.pkg1.header  |= 0xC8; // LSM6DSL updated  
                #ifdef DEBUG_MESSAGE
                //ser->printf("The angular velocity data from LSM6DSL was read\r\n");
                #endif
            } else { //11 12
                data.pkg1.header  &= 0xF4; // LSM6DSL not updated 
            } // 12
            
            if (magnetometer->get_m_axes(data.pkg1.m) == 0) { // Get the magnetometer heading // 13
                data.pkg1.header  |= 0xC4; // LSM303AGR updated
                #ifdef DEBUG_MESSAGE
                //ser->printf("The heading data from LSM6DSL was read\r\n");
                #endif
            } else { // 13 14
                data.pkg1.header  &= 0xF8; // LSM303AGR not updated
            } // 14
            
            #ifdef SD_EN
            data.pkg1.time = timerSD.read_ms();
            memcpy(&bufferSD[bufferSDCounter], &data, BUFFER_SIZE);
            bufferSDCounter += BUFFER_SIZE;
            timerSD.reset();
            #endif
            data.pkg1.time = timer2.read_ms();
            if(transmited){ // Only sends a new packet when the device already have transmited the previous one //15
                transmited = false;
                Radio->Send( &data, BUFFER_SIZE );
                #ifdef DEBUG_MESSAGE
                //ser->printf("    Transmited = true, State = %d\r\n", state);
                ser->printf("%x,%d,%f,%f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n", data.pkg1.header, data.pkg1.time, data.pkg1.p, data.pkg1.temperatureLPS22HB, data.pkg1.ag[0], data.pkg1.ag[1], data.pkg1.ag[2], data.pkg1.w[0], data.pkg1.w[1], data.pkg1.w[2], data.pkg1.a[0], data.pkg1.a[1], data.pkg1.a[2], data.pkg1.m[0], data.pkg1.m[1], data.pkg1.m[2]);
                #endif
                state = 1;
            } // 15
        } // 2
        
        else if(state == 1){ // Update and send pkg2 // 16
            #ifdef DEBUG_MESSAGE
            //ser->printf("    Here1\r\n");
            #endif
            // Initialization of pkg2
            data.pkg2.header = 0x40;
            data.pkg2.time = 0;
            data.pkg2.humidity = 0;
            data.pkg2.temperatureHTS221 = 0;
            data.pkg2.drogueStatus = 0;
            data.pkg2.mainStatus = 0;
            data.pkg2.pressureBar = 0; 
            data.pkg2.temperature = 0; 
            data.pkg2.mainStatusCOTS = 0; 
            data.pkg2.drogueStatusCOTS = 0; 
            data.pkg2.timeStamp = 0;
            data.pkg2.aglAlt = 0; 
            data.pkg2.battery = 0;
            
            // Check internal comunication for avionicas data
            // Implement this part here
            // ...
            // end
            
            if (timerHTS221.read_ms() >= 10) { //17
                if (hum_temp->get_humidity(&data.pkg2.humidity)) { // Get humidity //18
                    data.pkg2.header |= 0x50; // HTS221 updated
                    #ifdef DEBUG_MESSAGE
                    //ser->printf("The humidity data from HTS221 was read\r\n");
                    #endif
                } else {
                    data.pkg2.header &= 0x60; // HTS221 not updated
                } // 18
                
                if (hum_temp->get_temperature(&data.pkg2.temperatureHTS221)== 0) { // Get temperature from HTS221 //19
                    data.pkg2.header |= 0x50; // HTS221 updated
                    #ifdef DEBUG_MESSAGE
                    //ser->printf("The temperature data from HTS221 was read\r\n");
                    #endif
                } else {
                    data.pkg2.header &= 0x60; // HTS221 not updated
                } //19
                
                timerHTS221.reset();
            }//17
            
            #ifdef SD_EN
            data.pkg2.time = timerSD.read_ms();
            memcpy(&bufferSD[bufferSDCounter], &data, BUFFER_SIZE);
            bufferSDCounter += BUFFER_SIZE;
            timerSD.reset();
            #endif
            data.pkg2.time = timer2.read_ms();
            #ifdef DEBUG_MESSAGE
            //ser->printf("    Transmited = %d\r\n, State = %d", transmited, state);
            #endif
            if(transmited){ // Only sends a new packet when the device already have transmited the previous one //20
                transmited = false;
                Radio->Send( &data, BUFFER_SIZE );
                #ifdef DEBUG_MESSAGE
                //ser->printf("    Transmited = true, State = %d\r\n", state);
                ser->printf("%x,%d,%d%d%d%d,%f,%f,%d,%d,%d,%f,%f\r\n", data.pkg2.header, data.pkg2.time, data.pkg2.drogueStatus, data.pkg2.mainStatus, data.pkg2.mainStatusCOTS, data.pkg2.drogueStatusCOTS, data.pkg2.pressureBar, data.pkg2.temperature, data.pkg2.timeStamp, data.pkg2.aglAlt, data.pkg2.battery, data.pkg2.humidity, data.pkg2.temperatureHTS221);
                #endif
                state = 2;
            }//20
        }//16
        
        else if(state == 2){//21
            data.pkg3.header = 0x80;
            data.pkg3.time = 0;
            data.pkg3.timeOfWeek = 0; 
            data.pkg3.timeOfWeekFracPart = 0;
            data.pkg3.gpsFix = 0; 
            data.pkg3.ecefx = 0; 
            data.pkg3.ecefy = 0; 
            data.pkg3.ecefz = 0; 
            data.pkg3.positionAcc3D = 0; 
            data.pkg3.ecefvx = 0; 
            data.pkg3.ecefvy = 0; 
            data.pkg3.ecefvz = 0; 
            data.pkg3.speedAcc = 0; 
            data.pkg3.numbSat = 0;
            
            if (timerGPS.read_ms() >= 10) {//22
                #ifdef GPS_EN
                if (gps.readable()) {//23
                    if (gps.ready()) {//24
                        data.pkg3.ecefx = gps.ecefX;
                        data.pkg3.ecefy = gps.ecefY;
                        data.pkg3.ecefz = gps.ecefZ;
                        data.pkg3.ecefvx = gps.ecefVX;
                        data.pkg3.ecefvy = gps.ecefVY;
                        data.pkg3.ecefvz = gps.ecefVZ;
                        data.pkg3.timeOfWeek = gps.iTOW;
                        data.pkg3.timeOfWeekFracPart = gps.fTOW;
                        data.pkg3.positionAcc3D = gps.pAcc;
                        data.pkg3.speedAcc = gps.sAcc;
                        data.pkg3.numbSat = gps.numSV;
                        data.pkg3.gpsFix = gps.gpsFix;
                        data.pkg3.header |= 0xA0; // GPS updated
                        #ifdef DEBUG_MESSAGE
                        //ser->printf("The GPS data was read\r\n");
                        #endif  
                    } else {
                        data.pkg3.header  &= 0x80; // GPS not updated
                    }//24
                }//23
                #endif
            
                timerGPS.reset();
            }//22
            #ifdef SD_EN
            data.pkg3.time = timerSD.read_ms();
            memcpy(&bufferSD[bufferSDCounter], &data, BUFFER_SIZE);
            bufferSDCounter += BUFFER_SIZE;
            timerSD.reset();
            #endif
            data.pkg3.time = timer2.read_ms();
            if(transmited){ // Only sends a new packet when the device already have transmited the previous one //25
                transmited = false;
                Radio->Send( &data, BUFFER_SIZE );
                #ifdef DEBUG_MESSAGE
                //ser->printf("    Transmited = true, State = %d\r\n", state);
                ser->printf("%x,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n", data.pkg3.header, data.pkg3.time, data.pkg3.timeOfWeek, data.pkg3.timeOfWeekFracPart, data.pkg3.gpsFix, data.pkg3.ecefx, data.pkg3.ecefy, data.pkg3.ecefz, data.pkg3.positionAcc3D, data.pkg3.ecefvx, data.pkg3.ecefvy, data.pkg3.ecefvz, data.pkg3.speedAcc, data.pkg3.numbSat);
                #endif
                state = 0;
            }//25
        }//21
        
        #ifdef SD_EN
        if(bufferSDCounter >= 511){ // When the bufferSD is full, save the informations //26
            while(sdCard.write(&sdData[0],block) == 0);
            block ++;
        }//26
        #endif
        
        #ifdef DEBUG_MESSAGE
        //ser->printf("    Fim do loop\r\n");
        #endif
        
    
    }//1
    //----------------> Ending the main loop <----------------
}

void SystemClock_Config(void)
{
#ifdef B_L072Z_LRWAN1_LORA
    /* 
     * The L072Z_LRWAN1_LORA clock setup is somewhat differnt from the Nucleo board.
     * It has no LSE.
     */
    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
    RCC_OscInitTypeDef RCC_OscInitStruct = {0};

    /* Enable HSE Oscillator and Activate PLL with HSE as source */
    RCC_OscInitStruct.OscillatorType      = RCC_OSCILLATORTYPE_HSI;
    RCC_OscInitStruct.HSEState            = RCC_HSE_OFF;
    RCC_OscInitStruct.HSIState            = RCC_HSI_ON;
    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
    RCC_OscInitStruct.PLL.PLLState        = RCC_PLL_ON;
    RCC_OscInitStruct.PLL.PLLSource       = RCC_PLLSOURCE_HSI;
    RCC_OscInitStruct.PLL.PLLMUL          = RCC_PLLMUL_6;
    RCC_OscInitStruct.PLL.PLLDIV          = RCC_PLLDIV_3;

    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
        // Error_Handler();
    }

    /* Set Voltage scale1 as MCU will run at 32MHz */
    __HAL_RCC_PWR_CLK_ENABLE();
    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

    /* Poll VOSF bit of in PWR_CSR. Wait until it is reset to 0 */
    while (__HAL_PWR_GET_FLAG(PWR_FLAG_VOS) != RESET) {};

    /* Select PLL as system clock source and configure the HCLK, PCLK1 and PCLK2
    clocks dividers */
    RCC_ClkInitStruct.ClockType = (RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2);
    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != HAL_OK) {
        // Error_Handler();
    }
#endif
}



void OnTxDone(void *radio, void *userThisPtr, void *userData)
{   
    Radio->Sleep( );
    transmited = true;
    #ifdef DEBUG_MESSAGE
    ser->printf("> OnTxDone\r\n");
    //if(state == 0){
        //ser->printf("Package transmited: %d\r\n", pkg3.header >> 6);
        //ser->printf("Header: %x, time: %d, timeOfWeek: %d, frac: %d, gpsFix: %d\r\n", pkg3.header, pkg3.time, pkg3.timeOfWeek, pkg3.timeOfWeekFracPart, pkg3.gpsFix);
        //ser->printf("eceFx: %d, eceFy: %d, eceFz: %d, posAcc3D: %d, eceFvx: %d, eceFvy: %d, eceFvz: %d\r\n", pkg3.ecefx, pkg3.ecefy, pkg3.ecefz, pkg3.positionAcc3D, pkg3.ecefvx, pkg3.ecefvy, pkg3.ecefvz);
        //ser->printf("speedAcc: %d, numbSat: %d\r\n", pkg3.speedAcc, pkg3.numbSat);
        //ser->printf("%x,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n", pkg3.header, pkg3.time, pkg3.timeOfWeek, pkg3.timeOfWeekFracPart, pkg3.gpsFix, pkg3.ecefx, pkg3.ecefy, pkg3.ecefz, pkg3.positionAcc3D, pkg3.ecefvx, pkg3.ecefvy, pkg3.ecefvz, pkg3.speedAcc, pkg3.numbSat);
    //}
    //else if(state == 1){
        //ser->printf("Package transmited: %d\r\n", pkg1.header >> 6);
        //ser->printf("Header: %x, time: %d, p: %f, tempLPS22HB: %f, ag: %d; %d; %d, w: %d; %d; %d, a: %d; %d; %d, m: %d; %d; %d\r\n", pkg1.header, pkg1.time, pkg1.p, pkg1.temperatureLPS22HB, pkg1.ag[0], pkg1.ag[1], pkg1.ag[2], pkg1.w[0], pkg1.w[1], pkg1.w[2], pkg1.a[0], pkg1.a[1], pkg1.a[2], pkg1.m[0], pkg1.m[1], pkg1.m[2]);   
        //ser->printf("%x,%d,%f,%f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n", pkg1.header, pkg1.time, pkg1.p, pkg1.temperatureLPS22HB, pkg1.ag[0], pkg1.ag[1], pkg1.ag[2], pkg1.w[0], pkg1.w[1], pkg1.w[2], pkg1.a[0], pkg1.a[1], pkg1.a[2], pkg1.m[0], pkg1.m[1], pkg1.m[2]);
    //}
    //else if(state == 2){
        //ser->printf("Package transmited: %d\r\n", pkg2.header >> 6);
        //ser->printf("Header: %x, time: %d, parachuteStatus: %d%d%d%d, pressureBar: %f, temperature: %f, timeStamp: %d, aglAlt: %d, battery: %d, humidity: %f, tempHTS221: %f\r\n", pkg2.header, pkg2.time, pkg2.drogueStatus, pkg2.mainStatus, pkg2.mainStatusCOTS, pkg2.drogueStatusCOTS, pkg2.pressureBar, pkg2.temperature, pkg2.timeStamp, pkg2.aglAlt, pkg2.battery, pkg2.humidity, pkg2.temperatureHTS221);
        //ser->printf("%x,%d,%d%d%d%d,%f,%f,%d,%d,%d,%f,%f\r\n", pkg2.header, pkg2.time, pkg2.drogueStatus, pkg2.mainStatus, pkg2.mainStatusCOTS, pkg2.drogueStatusCOTS, pkg2.pressureBar, pkg2.temperature, pkg2.timeStamp, pkg2.aglAlt, pkg2.battery, pkg2.humidity, pkg2.temperatureHTS221);
    //}
    #endif
    timer2.reset();
}

void OnRxDone(void *radio, void *userThisPtr, void *userData, uint8_t *payload, uint16_t size, int16_t rssi, int8_t snr)
{
    Radio->Sleep( );
    #ifdef DEBUG_MESSAGE
    ser->printf("> OnRxDone: RssiValue=%d dBm, SnrValue=%d\r\n", rssi, snr);
    #endif
}

void OnTxTimeout(void *radio, void *userThisPtr, void *userData)
{
    Radio->Sleep( );
    #ifdef DEBUG_MESSAGE
    ser->printf("> OnTxTimeout\r\n");
    #endif
}

void OnRxTimeout(void *radio, void *userThisPtr, void *userData)
{
    Radio->Sleep( );
    #ifdef DEBUG_MESSAGE
    ser->printf("> OnRxTimeout\r\n");
    #endif
}

void OnRxError(void *radio, void *userThisPtr, void *userData)
{
    Radio->Sleep( );
    #ifdef DEBUG_MESSAGE
    ser->printf("> OnRxError\r\n");
    #endif
}