Diff: main.cpp

Revision:

3:92160b13f3c3

Parent:

0:a73914f20498

--- a/main.cpp	Thu Apr 18 13:56:50 2019 +0000
+++ b/main.cpp	Wed Jun 05 00:20:00 2019 +0000
@@ -7,16 +7,42 @@
 #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 '1' to display debug messages on the console */
-#define DEBUG_MESSAGE   1
+/* Set this flag to display debug messages on the console */
+#define DEBUG_MESSAGE
 
-/* Set this flag to '1' to use the LoRa modulation or to '0' to use FSK modulation */
-#define USE_MODEM_LORA  1
-#define USE_MODEM_FSK   !USE_MODEM_LORA
+/* 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                  // 14 dBm
+#define TX_OUTPUT_POWER         14                  // 20 dBm
 
 #if USE_MODEM_LORA == 1
 
@@ -35,13 +61,12 @@
 #endif 
 
 
-#define RX_TIMEOUT_VALUE    3500    // in ms
-
-//#define BUFFER_SIZE       32        // Define the payload size here
-#define BUFFER_SIZE         512       // Define the payload size here
+#define RX_TIMEOUT_VALUE    0       // In ms
+#define TX_TIMEOUT_VALUE    1000000 // In ms
 
 /* Sensors instances */
 
+
 /* Instantiate the expansion board */
 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
 
@@ -52,180 +77,422 @@
 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
 static LSM303AGRAccSensor *accelerometer = mems_expansion_board->accelerometer;
 
-char buffer1[32], buffer2[32]; // buffers to help theprinting of doubles
+typedef struct {
+    /* Header for identification of updated informations */
+    bool header [8];
+    int time; // Time between transmissions
+    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
+    float p;  // Pressure of LPS22HB
+    float temperatureLPS22HB; // Temperature from LPS22HB
+    float humidity; // Humidity of HTS221 
+    float temperatureHTS221; // Temperature from HTS221
+    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
+    bool drogueStatus; // Drogue parachute status provided by Avionics
+    bool mainStatus; //Main parachute status provided by Avionics
+    float pressureBar; // Pressure by COTS Altimeter
+    float temperature; // Temperature by COTS Altimeter
+    bool mainStatusCOTS; // Main parachute status provided by COTS Altimeter
+    bool drogueStatusCOTS; // Drogue status provided 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
+}Data; // Data struct
 
-uint32_t dados[16]; //data vector
+Data data;
+
 
 /* LoRa modem instances and configurations */
 
 static RadioEvents_t RadioEvents; // Calback functions struct
 
-SX1276Generic *Radio; //Defenition of a Radio object
+SX1276Generic *Radio; // Definition of a Radio object
 
-/*Configuration function*/
+/* Configuration function */
 void SystemClock_Config(void);
 
- bool transmited = true;
+bool transmited = true;// Flag to indicate the end of transmission
 
 /* 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);
 
-void OnCadDone(void *radio, void *userThisPtr, void *userData);
-
+#ifdef DEBUG_MESSAGE
 /* Serial communication to debug program */
+BufferedSerial *ser;
+#endif
 
-Serial pc(USBTX,USBRX);
+/* Buzzer definition */
+DigitalOut buzzer (PA_0);
+
+#ifdef KEY_ENABLE
+/* Key  digital input port */
+DigitalIn key (PA_8);
+#endif
 
 int main() {
-    /* General Header*/
+    #ifdef KEY_ENABLE
+    while (key == 0);
+    #endif
+    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 */
+    data.header [0] = 0;
+    data.header [1] = 0; 
+    data.header [2] = 0; 
+    data.header [3] = 0; 
+    data.header [4] = 0; 
+    data.header [5] = 0; 
+    data.header [6] = 0; 
+    data.header [7] = 0;  
+    data.time = 0; 
+    data.ag[0] = 0;
+    data.ag[1] = 0;
+    data.ag[2] = 0;
+    data.w[0] = 0;
+    data.w[1] = 0;
+    data.w[2] = 0;
+    data.a[0] = 0;
+    data.a[1] = 0;
+    data.a[2] = 0;
+    data.m [0] = 0;
+    data.m [1] = 0;
+    data.m [2] = 0;
+    data.p = 0;  
+    data.temperatureLPS22HB = 0; 
+    data.humidity = 0; 
+    data.temperatureHTS221 = 0; 
+    data.timeOfWeek = 0; 
+    data.timeOfWeekFracPart = 0;
+    data.gpsFix = 0; 
+    data.ecefx = 0; 
+    data.ecefy = 0; 
+    data.ecefz = 0; 
+    data.positionAcc3D = 0; 
+    data.ecefvx = 0; 
+    data.ecefvy = 0; 
+    data.ecefvz = 0; 
+    data.speedAcc = 0; 
+    data.numbSat = 0; 
+    data.drogueStatus = 0; 
+    data.mainStatus = 0; 
+    data.pressureBar = 0; 
+    data.temperature = 0; 
+    data.mainStatusCOTS = 0; 
+    data.drogueStatusCOTS = 0; 
+    data.timeStamp = 0; 
+    data.aglAlt = 0; 
+    data.battery = 0;
+         
+    #ifdef GPS_EN
+    //Serial connections (GPS)(TX,RX,baud)
+    UbxGpsNavSol gps(PA_9, PA_10, GPS_BAUDRATE);
+    #endif
     
-    pc.printf("Telemetry Tx inicial version program\r\n\r\n");
+    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 */
-    hum_temp->enable();
-    press_temp->enable();
-    magnetometer->enable();
-    accelerometer->enable();
-    acc_gyro->enable_x();
-    acc_gyro->enable_g();
-      
-    pc.printf("\r\n--- Starting the sensors ---\r\n");
+    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);
-    pc.printf("HTS221  humidity & temperature    = 0x%X\r\n", id);
+    ser->printf("HTS221  humidity & temperature    = 0x%X\r\n", id);
     press_temp->read_id(&id);
-    pc.printf("LPS22HB  pressure & temperature   = 0x%X\r\n", id);
+    ser->printf("LPS22HB  pressure & temperature   = 0x%X\r\n", id);
     magnetometer->read_id(&id);
-    pc.printf("LSM303AGR magnetometer            = 0x%X\r\n", id);
+    ser->printf("LSM303AGR magnetometer            = 0x%X\r\n", id);
     accelerometer->read_id(&id);
-    pc.printf("LSM303AGR accelerometer           = 0x%X\r\n", id);
+    ser->printf("LSM303AGR accelerometer           = 0x%X\r\n", id);
     acc_gyro->read_id(&id);
-    pc.printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
-    
-    pc.printf("\r\n");
+    ser->printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
+        
+    ser->printf("\r\n");
+    #endif
     
-        /* Radio setup */
-     pc.printf("\r\n--- Starting the modem LoRa ---\r\n");
-    
+    /* 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);
-    pc.printf("SX1276 Simple transmission aplication\r\n" );
-    pc.printf("Frequency: %.1f\r\n", (double)RF_FREQUENCY/1000000.0);
-    pc.printf("TXPower: %d dBm\r\n",  TX_OUTPUT_POWER);
-    pc.printf("Bandwidth: %d Hz\r\n", LORA_BANDWIDTH);
-    pc.printf("Spreading factor: SF%d\r\n", LORA_SPREADING_FACTOR);
+    #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;    
-    while (Radio->Init( &RadioEvents ) == false) {
-        pc.printf("Radio could not be detected!\r\n");
-        wait( 1 );
+    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:
-            if (DEBUG_MESSAGE)
-                pc.printf(" > Board Type: SX1276MB1LAS <\r\n");
+            ser->printf(" > Board Type: SX1276MB1LAS <\r\n");
             break;
         case SX1276MB1MAS:
-            if (DEBUG_MESSAGE)
-                pc.printf(" > Board Type: SX1276MB1LAS <\r\n");
+            ser->printf(" > Board Type: SX1276MB1LAS <\r\n");
+            break;
         case MURATA_SX1276:
-            if (DEBUG_MESSAGE)
-                pc.printf(" > Board Type: MURATA_SX1276_STM32L0 <\r\n");
+            ser->printf(" > Board Type: MURATA_SX1276_STM32L0 <\r\n");
             break;
         case RFM95_SX1276:
-            if (DEBUG_MESSAGE)
-                pc.printf(" > HopeRF RFM95xx <\r\n");
+            ser->printf(" > HopeRF RFM95xx <\r\n");
             break;
         default:
-            pc.printf(" > Board Type: unknown <\r\n");
+            ser->printf(" > Board Type: unknown <\r\n");
     }
-    
-    Radio->SetChannel(RF_FREQUENCY );
+    #endif
+    Radio->SetChannel(RF_FREQUENCY ); // Sets the frequency of the communication
     
-    if (LORA_FHSS_ENABLED)
-        pc.printf("             > LORA FHSS Mode <\r\n");
-    if (!LORA_FHSS_ENABLED)
-        pc.printf("             > LORA Mode <\r\n");
-        
-    pc.printf("\r\n");
-        
+    // 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 );
       
-    Radio->Tx(1000000);
+    
+    #ifdef SD_EN
+    SPI spi (PA_7, PA_6, PA_5);
+    spi.frequency (SPI_FREQUENCY);
+    SDCard sdCard (&spi, PB_10);
+    uint8_t sdData[sizeof(data)];
+    int block = 0;
+    #endif
+    
+    
+    Radio->Tx(TX_TIMEOUT_VALUE); // Puts the device in transmission mode for a long period
+    
+    Timer timer; // Timer
+    timer.start(); // Starting timer
     
     while(1) {   
-        float p; //pressure
-        float temperatureHTS221; //temperature from HTS221
-        float humidity; //humidity
-        float temperatureLPS22HB; //temperature from LPS22HB
-        int32_t w[3]; //angular velocity
-        int32_t a[3]; //acceleration of the accelerometer LSM303AGR
-        int32_t ag[3]; //acceleration of the accelerometer and gyroscope LSM6DSL 
-        int32_t m [3]; //heading 
-        
-        press_temp->get_pressure(&p); //get the pressure
-        press_temp->get_temperature(&temperatureLPS22HB); //get temperature from LPS22HB
-        accelerometer->get_x_axes(a);//get the acceleration
-        acc_gyro->get_x_axes(ag);//get the acceleration
-        acc_gyro->get_g_axes(w);//get the angular velocity
-        magnetometer->get_m_axes(m); //get the magnetometer heading
-        hum_temp->get_temperature(&temperatureHTS221); //get temperature from HTS221
-        hum_temp->get_humidity(&humidity); //get humidity
-        
         
-        //sensors data
+        if (press_temp->get_pressure(&data.p) == 0) { // Get the pressure
+            data.header[3] = 1; // LPS22HB updated
+            #ifdef DEBUG_MESSAGE
+            //ser->printf("The pressure data from LPS22HB was read\r\n");
+            #endif
+        } else {
+            data.header[3] = 0; // LPS22HB was not updated
+        }
+        if (press_temp->get_temperature(&data.temperatureLPS22HB) == 0) { // Get temperature from LPS22HB
+            data.header [3] = 1; // LPS22HB updated
+            #ifdef DEBUG_MESSAGE
+            //ser->printf("The temperature data from LPS22HB was read\r\n");
+            #endif
+        } else {
+            data.header[3] = 0; // LPS22HB not updated
+        }
+        if (accelerometer->get_x_axes(data.a) == 0) {// Get the acceleration
+            data.header [2] = 1; // LSM303AGR updated
+            #ifdef DEBUG_MESSAGE
+            //ser->printf("The acceleration data from LSM303AGR was read\r\n");
+            #endif
+        } else {
+            data.header [2] = 0; // LSM303AGR not updated
+        }
+        if (acc_gyro->get_x_axes(data.ag) == 0) {// Get the acceleration
+            data.header [1] = 1; // LSM6DSL updated 
+            #ifdef DEBUG_MESSAGE
+            //ser->printf("The acceleration data from LSM6DSL was read\r\n");
+            #endif
+        } else {
+            data.header [1] = 0; // LSM6DSL not updated
+        }
+        if (acc_gyro->get_g_axes(data.w) == 0) {// Get the angular velocity
+            data.header [1] = 1; // LSM6DSL updated 
+            #ifdef DEBUG_MESSAGE
+            //ser->printf("The angular velocity data from LSM6DSL was read\r\n");
+            #endif
+        } else {
+            data.header [1] = 0; // LSM6DSL not updated    
+        }
+        if (magnetometer->get_m_axes(data.m) == 0) { // Get the magnetometer heading
+            data.header [2] = 1; // LSM303AGR updated
+            #ifdef DEBUG_MESSAGE
+            //ser->printf("The heading data from LSM6DSL was read\r\n");
+            #endif
+        } else {
+            data.header [2] = 0; // LSM303AGR not updated
+        }
+        if (timer.read_ms() >= 10) {
+            if (hum_temp->get_humidity(&data.humidity)) { // Get humidity
+                data.header [4] = 1; // HTS221 updated
+                #ifdef DEBUG_MESSAGE
+                //ser->printf("The humidity data from HTS221 was read\r\n");
+                #endif
+            } else {
+                data.header [4] = 0; // HTS221 not updated
+            }
+            if (hum_temp->get_temperature(&data.temperatureHTS221)== 0) { // Get temperature from HTS221
+                data.header [4] = 1; // HTS221 updated
+                #ifdef DEBUG_MESSAGE
+                //ser->printf("The temperature data from HTS221 was read\r\n");
+                #endif
+            } else {
+                data.header [4] = 0; // HTS221 not updated
+            }
+            #ifdef GPS_EN
+            if (gps.readable()) {
+                if (gps.ready()) {
+                    data.ecefx = gps.ecefX;
+                    data.ecefy = gps.ecefY;
+                    data.ecefz = gps.ecefZ;
+                    data.ecefvx = gps.ecefVX;
+                    data.ecefvy = gps.ecefVY;
+                    data.ecefvz = gps.ecefVZ;
+                    data.timeOfWeek = gps.iTOW;
+                    data.timeOfWeekFracPart = gps.fTOW;
+                    data.positionAcc3D = gps.pAcc;
+                    data.speedAcc = gps.sAcc;
+                    data.numbSat = gps.numSV;
+                    data.gpsFix = gps.gpsFix;
+                    data.header [5] = 1; // GPS updated
+                    #ifdef DEBUG_MESSAGE
+                    ser->printf("The GPS data was read\r\n");
+                    #endif  
+                } else {
+                    data.header [5] = 1; // GPS not updated
+                }
+            }
+            #endif
+            timer.reset();
+        }    
+        /* Check internal comunication for avionicas data */  
+        // Implement this part here
+        //...
+        //end
+         
+        #ifdef SD_EN    
+        // Saving the data on SD Card
+        memcpy (sdData, &data, sizeof(data));
+        while(sdCard.write(&sdData[0],block) == 0);
+        block ++;
+        while (sdCard.write (&sdData[64],block) == 0);
+        block++;
+        while (sdCard.write(&sdData[128],block) == 0);
+        block++;
+        #endif
         
-        dados[0] = a[0];
-        dados[1] = a[1];
-        dados[2] = a[2];
-        dados[3] = ag[0];
-        dados[4] = ag[1];
-        dados[5] = ag[2];
-        dados[6] = w[0];
-        dados[7] = w[1];
-        dados[8] = w[2];
-        dados[9] = m[0];
-        dados[10] = m[1];
-        dados[11] = m[2];
-        dados[12] = humidity;
-        dados[13] = temperatureHTS221;
-        dados[14] = temperatureLPS22HB;
-        dados[15] = p;
-        
+        // Only sends a new packet when the device already have transmited the previous one 
         if (transmited==true) {
             transmited = false;
             wait_ms(10);
-            Radio->Send( dados, sizeof(dados) );
+            Radio->Send( &data, sizeof(data) );
+            ser->printf("%d\r\n", sizeof(data));
         }
     }
 }
@@ -274,77 +541,56 @@
 #endif
 }
 
-/* Helper function for printing floats & doubles */
-static char *print_double(char* str, double v, int decimalDigits=2)
-{
-  int i = 1;
-  int intPart, fractPart;
-  int len;
-  char *ptr;
 
-  /* prepare decimal digits multiplicator */
-  for (;decimalDigits!=0; i*=10, decimalDigits--);
-
-  /* calculate integer & fractinal parts */
-  intPart = (int)v;
-  fractPart = (int)((v-(double)(int)v)*i);
-
-  /* fill in integer part */
-  sprintf(str, "%i.", intPart);
-
-  /* prepare fill in of fractional part */
-  len = strlen(str);
-  ptr = &str[len];
-
-  /* fill in leading fractional zeros */
-  for (i/=10;i>1; i/=10, ptr++) {
-    if (fractPart >= i) {
-      break;
-    }
-    *ptr = '0';
-  }
-
-  /* fill in (rest of) fractional part */
-  sprintf(ptr, "%i", fractPart);
-
-  return str;
-}
 
 void OnTxDone(void *radio, void *userThisPtr, void *userData)
 {   
     Radio->Sleep( );
     transmited = true;
-    if (DEBUG_MESSAGE) {
-        pc.printf("> OnTxDone\r\n");
-        pc.printf("I transmited %6ld, %6ld, %6ld, %6ld, %6ld, %6ld, %6ld, %6ld, %6ld\r\n", dados[0], dados[1], dados[2], dados[3], dados[4], dados[5], dados[6], dados[7], dados[8]);
-        pc.printf("and %6ld, %6ld, %6ld, %s, %7s, %7s %s\r\n", dados[9], dados[10], dados[11], print_double(buffer2, dados[12]), print_double(buffer1, dados[13]), print_double(buffer1, dados[14]), print_double(buffer2, dados[15]));
-    }    
+    #ifdef DEBUG_MESSAGE
+    ser->printf("> OnTxDone\r\n");
+    ser->printf("I transmited %d mg, %d mg, %d mg, %d mg, %d mg, %d mg, %d mdps, %d mdps, %d mdps\r\n", data.a[0], data.a[1], data.a[2], data.ag[0], data.ag[1], data.ag[2], data.w[0], data.w[1], data.w[2]);
+    ser->printf("and %d mG, %d mG, %d mG, %g %%, %g C, %g C, %g mBar\r\n", data.m[0], data.m[1], data.m[2], data.humidity, data.temperatureHTS221, data.temperatureLPS22HB, data.p);
+    ser->printf("and time - %d ms, time of the week - %d ms, time of week frac part - %d ns\r\n",data.time, data.timeOfWeek, data.timeOfWeekFracPart);
+    ser->printf("and GPS fix %c, ECEFX %d cm, ECEFY %d cm, ECEFZ %d cm\r\n", data.gpsFix, data.ecefx, data.ecefy, data.ecefz);
+    ser->printf("and 3D Position accuracy %d cm, ECEFVX %d cm/s, ECEFVY %d cm/s, ECEFVZ %d cm/s, Speed accuracy %d cm/s\r\n", data.positionAcc3D, data.ecefvx, data.ecefvy, data.ecefvz, data.speedAcc);    
+    ser->printf("and Number of satelites %x, Drogue Status %x, Main status %x, BMP280 %f bar, temperature %f C\r\n", data.numbSat, data.drogueStatus, data.mainStatus, data.pressureBar, data.temperature);
+    ser->printf("Main Status COTS %x, Drogue Status COTS %x, Time Stamp %d s, AGL altitude %d m, Battery voltage %d V\r\n",data.mainStatusCOTS, data.drogueStatusCOTS, data.timeStamp, data.aglAlt, data.battery);
+    ser->printf("Header: %d, %d, %d, %d, %d, %d, %d, %d\r\n",data.header[0], data.header[1], data.header[2], data.header[3], data.header[4], data.header[5], data.header[6], data.header[7]);
+    uint8_t payload[sizeof(data)];
+    memcpy(payload, &data, sizeof(data));
+    ser->printf("%x\r\n", *payload);
+    #endif
 }
 
 void OnRxDone(void *radio, void *userThisPtr, void *userData, uint8_t *payload, uint16_t size, int16_t rssi, int8_t snr)
 {
     Radio->Sleep( );
-    if (DEBUG_MESSAGE)
-        pc.printf("> OnRxDone: RssiValue=%d dBm, SnrValue=%d\r\n", rssi, snr);
+    #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( );
-    if(DEBUG_MESSAGE)
-        pc.printf("> OnTxTimeout\r\n");
+    #ifdef DEBUG_MESSAGE
+    ser->printf("> OnTxTimeout\r\n");
+    #endif
 }
 
 void OnRxTimeout(void *radio, void *userThisPtr, void *userData)
 {
     Radio->Sleep( );
-    if (DEBUG_MESSAGE)
-        pc.printf("> OnRxTimeout\r\n");
+    #ifdef DEBUG_MESSAGE
+    ser->printf("> OnRxTimeout\r\n");
+    #endif
 }
 
 void OnRxError(void *radio, void *userThisPtr, void *userData)
 {
     Radio->Sleep( );
-    if (DEBUG_MESSAGE)
-        pc.printf("> OnRxError\r\n");
+    #ifdef DEBUG_MESSAGE
+    ser->printf("> OnRxError\r\n");
+    #endif
 }