Astek

Dependencies:   mbed GPS_I2C_astek Sensors LSM9DS1 DigitDisplay Chainable_RGB_LED LoRaWAN-lib SX1272Lib

Revision:
10:58fa3853e803
Parent:
9:0083afd69815
--- a/app/main.cpp	Mon Apr 24 13:31:49 2017 +0000
+++ b/app/main.cpp	Fri Dec 27 16:05:09 2019 +0000
@@ -22,6 +22,10 @@
 #include "DigitDisplay.h"
 #include "ChainableLED.h"
 
+#include "GPS_I2C.h"
+#include "LSM9DS1.h"
+#include "Sensors.h"
+
 /*!
  * Defines the application data transmission duty cycle. 5s, value in [ms].
  */
@@ -89,7 +93,7 @@
 #define LORAWAN_APP_DATA_SIZE                       6
 
 #else
-#define LORAWAN_APP_DATA_SIZE                       5
+#define LORAWAN_APP_DATA_SIZE                       8
 
 #endif
 
@@ -122,7 +126,7 @@
 /*!
  * User application data buffer size
  */
-#define LORAWAN_APP_DATA_MAX_SIZE                           64
+#define LORAWAN_APP_DATA_MAX_SIZE                          64
 
 /*!
  * User application data
@@ -167,62 +171,23 @@
  */
 static bool NextTx = true;
 
-/*!
- * Hold the value returned from the Light Sensor
- */
-static float LightValue = 0.0;
-
-/*!
- * Control the 3-color LED 
- * 0: automatic (LED goes brigther as the light decrease, 
- * 1: manual (The LED is controlled by the user)
- */
-static uint8_t LightMode = 0;  // 0: automatic, 1: manual
-
-/*!
- * Ticker to create a PWM for the buzzer
- */
-Ticker BuzzerTimer;
-
-
-/*!
- * Constructor for Buzzer
- */
-DigitalOut Buzzer( A3 );
-
-/*!
- * Constructor for the 3-color LED
- */
-#define NUM_LED 3
-ChainableLED ColorLed( D6, D7, NUM_LED );
-
-/*!
- * Constructor for Light Sensor
- */
-AnalogIn LightSens( A1 ); 
-
-/*!
- * Constructor for 4 Digit 7 semgent display
- */
-DigitDisplay Display( D8, D9 );
-
+GPS_I2C myGPS_I2C(I2C_SDA, I2C_SCL, I2C_SPEED_FAST);
+LSM9DS1 myIMU(I2C_SDA, I2C_SCL, 0xD6, 0x3C);
 /*!
  * Device states
  */
-static enum eDeviceState
-{
+static enum eDeviceState {
     DEVICE_STATE_INIT,
     DEVICE_STATE_JOIN,
     DEVICE_STATE_SEND,
     DEVICE_STATE_CYCLE,
     DEVICE_STATE_SLEEP
-}DeviceState;
+} DeviceState;
 
 /*!
  * LoRaWAN compliance tests support data
  */
-struct ComplianceTest_s
-{
+struct ComplianceTest_s {
     bool Running;
     uint8_t State;
     bool IsTxConfirmed;
@@ -233,7 +198,7 @@
     bool LinkCheck;
     uint8_t DemodMargin;
     uint8_t NbGateways;
-}ComplianceTest;
+} ComplianceTest;
 
 /*
  * SerialDisplay managment variables
@@ -247,22 +212,20 @@
 /*!
  * Strucure containing the Uplink status
  */
-struct sLoRaMacUplinkStatus
-{
+struct sLoRaMacUplinkStatus {
     uint8_t Acked;
     int8_t Datarate;
     uint16_t UplinkCounter;
     uint8_t Port;
     uint8_t *Buffer;
     uint8_t BufferSize;
-}LoRaMacUplinkStatus;
+} LoRaMacUplinkStatus;
 volatile bool UplinkStatusUpdated = false;
 
 /*!
  * Strucure containing the Downlink status
  */
-struct sLoRaMacDownlinkStatus
-{
+struct sLoRaMacDownlinkStatus {
     int16_t Rssi;
     int8_t Snr;
     uint16_t DownlinkCounter;
@@ -270,7 +233,7 @@
     uint8_t Port;
     uint8_t *Buffer;
     uint8_t BufferSize;
-}LoRaMacDownlinkStatus;
+} LoRaMacDownlinkStatus;
 volatile bool DownlinkStatusUpdated = false;
 
 void SerialDisplayRefresh( void )
@@ -301,16 +264,14 @@
     SerialDisplayUpdateDutyCycle( false );
 #endif
     SerialDisplayUpdatePublicNetwork( LORAWAN_PUBLIC_NETWORK );
-    
+
     SerialDisplayUpdateLedState( 3, AppLedStateOn );
 }
 
 void SerialRxProcess( void )
 {
-    if( SerialDisplayReadable( ) == true )
-    {
-        switch( SerialDisplayGetChar( ) )
-        {
+    if( SerialDisplayReadable( ) == true ) {
+        switch( SerialDisplayGetChar( ) ) {
             case 'R':
             case 'r':
                 // Refresh Serial screen
@@ -327,23 +288,15 @@
  */
 static void PrepareTxFrame( uint8_t port )
 {
-    switch( port )
-    {
-    case 10:
-        {  
-            uint32_t tempValue = ( uint32_t )( LightValue * 1000000.0 );
-            AppData[0] = LightMode;
-            AppData[1] = ( ( tempValue & 0xFF000000 ) >> 24 ) & 0xFF;
-            AppData[2] = ( ( tempValue & 0x00FF0000 ) >> 16 ) & 0xFF;
-            AppData[3] = ( ( tempValue & 0x0000FF00 ) >> 8 ) & 0xFF;
-            AppData[4] = ( tempValue & 0x000000FF );
+    switch( port ) {
+        case 10: {
+            string nmea_sentence = "$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A";
+            strcpy((char*)AppData, nmea_sentence.c_str());
         }
         break;
-    case 15:
-        {
+        case 15: {
             AppData[0] = AppLedStateOn;
-            if( IsTxConfirmed == true )
-            {
+            if( IsTxConfirmed == true ) {
                 AppData[1] = LoRaMacDownlinkStatus.DownlinkCounter >> 8;
                 AppData[2] = LoRaMacDownlinkStatus.DownlinkCounter;
                 AppData[3] = LoRaMacDownlinkStatus.Rssi >> 8;
@@ -352,33 +305,29 @@
             }
         }
         break;
-    case 224:
-        if( ComplianceTest.LinkCheck == true )
-        {
-            ComplianceTest.LinkCheck = false;
-            AppDataSize = 3;
-            AppData[0] = 5;
-            AppData[1] = ComplianceTest.DemodMargin;
-            AppData[2] = ComplianceTest.NbGateways;
-            ComplianceTest.State = 1;
-        }
-        else
-        {
-            switch( ComplianceTest.State )
-            {
-            case 4:
+        case 224:
+            if( ComplianceTest.LinkCheck == true ) {
+                ComplianceTest.LinkCheck = false;
+                AppDataSize = 3;
+                AppData[0] = 5;
+                AppData[1] = ComplianceTest.DemodMargin;
+                AppData[2] = ComplianceTest.NbGateways;
                 ComplianceTest.State = 1;
-                break;
-            case 1:
-                AppDataSize = 2;
-                AppData[0] = ComplianceTest.DownLinkCounter >> 8;
-                AppData[1] = ComplianceTest.DownLinkCounter;
-                break;
+            } else {
+                switch( ComplianceTest.State ) {
+                    case 4:
+                        ComplianceTest.State = 1;
+                        break;
+                    case 1:
+                        AppDataSize = 2;
+                        AppData[0] = ComplianceTest.DownLinkCounter >> 8;
+                        AppData[1] = ComplianceTest.DownLinkCounter;
+                        break;
+                }
             }
-        }
-        break;
-    default:
-        break;
+            break;
+        default:
+            break;
     }
 }
 
@@ -392,8 +341,7 @@
     McpsReq_t mcpsReq;
     LoRaMacTxInfo_t txInfo;
 
-    if( LoRaMacQueryTxPossible( AppDataSize, &txInfo ) != LORAMAC_STATUS_OK )
-    {
+    if( LoRaMacQueryTxPossible( AppDataSize, &txInfo ) != LORAMAC_STATUS_OK ) {
         // Send empty frame in order to flush MAC commands
         mcpsReq.Type = MCPS_UNCONFIRMED;
         mcpsReq.Req.Unconfirmed.fBuffer = NULL;
@@ -405,25 +353,20 @@
         LoRaMacUplinkStatus.Buffer = NULL;
         LoRaMacUplinkStatus.BufferSize = 0;
         SerialDisplayUpdateFrameType( false );
-    }
-    else
-    {
+    } else {
         LoRaMacUplinkStatus.Acked = false;
         LoRaMacUplinkStatus.Port = AppPort;
         LoRaMacUplinkStatus.Buffer = AppData;
         LoRaMacUplinkStatus.BufferSize = AppDataSize;
         SerialDisplayUpdateFrameType( IsTxConfirmed );
 
-        if( IsTxConfirmed == false )
-        {
+        if( IsTxConfirmed == false ) {
             mcpsReq.Type = MCPS_UNCONFIRMED;
             mcpsReq.Req.Unconfirmed.fPort = AppPort;
             mcpsReq.Req.Unconfirmed.fBuffer = AppData;
             mcpsReq.Req.Unconfirmed.fBufferSize = AppDataSize;
             mcpsReq.Req.Unconfirmed.Datarate = LORAWAN_DEFAULT_DATARATE;
-        }
-        else
-        {
+        } else {
             mcpsReq.Type = MCPS_CONFIRMED;
             mcpsReq.Req.Confirmed.fPort = AppPort;
             mcpsReq.Req.Confirmed.fBuffer = AppData;
@@ -433,8 +376,7 @@
         }
     }
 
-    if( LoRaMacMcpsRequest( &mcpsReq ) == LORAMAC_STATUS_OK )
-    {
+    if( LoRaMacMcpsRequest( &mcpsReq ) == LORAMAC_STATUS_OK ) {
         return false;
     }
     return true;
@@ -453,15 +395,11 @@
     mibReq.Type = MIB_NETWORK_JOINED;
     status = LoRaMacMibGetRequestConfirm( &mibReq );
 
-    if( status == LORAMAC_STATUS_OK )
-    {
-        if( mibReq.Param.IsNetworkJoined == true )
-        {
+    if( status == LORAMAC_STATUS_OK ) {
+        if( mibReq.Param.IsNetworkJoined == true ) {
             DeviceState = DEVICE_STATE_SEND;
             NextTx = true;
-        }
-        else
-        {
+        } else {
             DeviceState = DEVICE_STATE_JOIN;
         }
     }
@@ -489,14 +427,6 @@
     Led2StateChanged = true;
 }
 
-/*!
- * \brief Function executed on Buzzer Timeout event
- */
-static void OnBuzzerTimerEvent( void )
-{
-    Buzzer = 0;
-    BuzzerTimer.detach( );
-}
 
 /*!
  * \brief   MCPS-Confirm event function
@@ -506,18 +436,14 @@
  */
 static void McpsConfirm( McpsConfirm_t *mcpsConfirm )
 {
-    if( mcpsConfirm->Status == LORAMAC_EVENT_INFO_STATUS_OK )
-    {
-        switch( mcpsConfirm->McpsRequest )
-        {
-            case MCPS_UNCONFIRMED:
-            {
+    if( mcpsConfirm->Status == LORAMAC_EVENT_INFO_STATUS_OK ) {
+        switch( mcpsConfirm->McpsRequest ) {
+            case MCPS_UNCONFIRMED: {
                 // Check Datarate
                 // Check TxPower
                 break;
             }
-            case MCPS_CONFIRMED:
-            {
+            case MCPS_CONFIRMED: {
                 // Check Datarate
                 // Check TxPower
                 // Check AckReceived
@@ -525,8 +451,7 @@
                 LoRaMacUplinkStatus.Acked = mcpsConfirm->AckReceived;
                 break;
             }
-            case MCPS_PROPRIETARY:
-            {
+            case MCPS_PROPRIETARY: {
                 break;
             }
             default:
@@ -553,27 +478,21 @@
  */
 static void McpsIndication( McpsIndication_t *mcpsIndication )
 {
-    if( mcpsIndication->Status != LORAMAC_EVENT_INFO_STATUS_OK )
-    {
+    if( mcpsIndication->Status != LORAMAC_EVENT_INFO_STATUS_OK ) {
         return;
     }
 
-    switch( mcpsIndication->McpsIndication )
-    {
-        case MCPS_UNCONFIRMED:
-        {
+    switch( mcpsIndication->McpsIndication ) {
+        case MCPS_UNCONFIRMED: {
             break;
         }
-        case MCPS_CONFIRMED:
-        {
+        case MCPS_CONFIRMED: {
             break;
         }
-        case MCPS_PROPRIETARY:
-        {
+        case MCPS_PROPRIETARY: {
             break;
         }
-        case MCPS_MULTICAST:
-        {
+        case MCPS_MULTICAST: {
             break;
         }
         default:
@@ -590,14 +509,11 @@
     // Check Snr
     // Check RxSlot
     LoRaMacDownlinkStatus.Rssi = mcpsIndication->Rssi;
-    if( mcpsIndication->Snr & 0x80 ) // The SNR sign bit is 1
-    {
+    if( mcpsIndication->Snr & 0x80 ) { // The SNR sign bit is 1
         // Invert and divide by 4
         LoRaMacDownlinkStatus.Snr = ( ( ~mcpsIndication->Snr + 1 ) & 0xFF ) >> 2;
         LoRaMacDownlinkStatus.Snr = -LoRaMacDownlinkStatus.Snr;
-    }
-    else
-    {
+    } else {
         // Divide by 4
         LoRaMacDownlinkStatus.Snr = ( mcpsIndication->Snr & 0xFF ) >> 2;
     }
@@ -607,175 +523,148 @@
     LoRaMacDownlinkStatus.Buffer = mcpsIndication->Buffer;
     LoRaMacDownlinkStatus.BufferSize = mcpsIndication->BufferSize;
 
-    if( ComplianceTest.Running == true )
-    {
+    if( ComplianceTest.Running == true ) {
         ComplianceTest.DownLinkCounter++;
     }
 
-    if( mcpsIndication->RxData == true )
-    {
-        switch( mcpsIndication->Port )
-        {
-        case 1: // The application LED can be controlled on port 1 or 2
-        case 2:
-            if( mcpsIndication->BufferSize == 1 )
-            {
-                AppLedStateOn = mcpsIndication->Buffer[0] & 0x01;
-                Led3StateChanged = true;
-            }
-            break;
-        case 10: 
-            Display.write( 0, mcpsIndication->Buffer[0] );
-            Display.write( 1, mcpsIndication->Buffer[1] );
-            Display.write( 2, mcpsIndication->Buffer[2] );
-            Display.write( 3, mcpsIndication->Buffer[3] ); 
-            break;
-        case 20:
-            LightMode = mcpsIndication->Buffer[0];
-            if( LightMode )
-            {
-                ColorLed.setColorRGB( 0, mcpsIndication->Buffer[1], mcpsIndication->Buffer[2], mcpsIndication->Buffer[3] );
-            }
-            break;
-        case 30:
-            BuzzerTimer.attach_us( &OnBuzzerTimerEvent, 200000 );
-            Buzzer = 1;
-            break;
-        case 224:
-            if( ComplianceTest.Running == false )
-            {
-                // Check compliance test enable command (i)
-                if( ( mcpsIndication->BufferSize == 4 ) &&
-                    ( mcpsIndication->Buffer[0] == 0x01 ) &&
-                    ( mcpsIndication->Buffer[1] == 0x01 ) &&
-                    ( mcpsIndication->Buffer[2] == 0x01 ) &&
-                    ( mcpsIndication->Buffer[3] == 0x01 ) )
-                {
-                    IsTxConfirmed = false;
-                    AppPort = 224;
-                    AppDataSize = 2;
-                    ComplianceTest.DownLinkCounter = 0;
-                    ComplianceTest.LinkCheck = false;
-                    ComplianceTest.DemodMargin = 0;
-                    ComplianceTest.NbGateways = 0;
-                    ComplianceTest.Running = true;
-                    ComplianceTest.State = 1;
-
-                    MibRequestConfirm_t mibReq;
-                    mibReq.Type = MIB_ADR;
-                    mibReq.Param.AdrEnable = true;
-                    LoRaMacMibSetRequestConfirm( &mibReq );
-
-#if defined( USE_BAND_868 )
-                    LoRaMacTestSetDutyCycleOn( false );
-#endif
+    if( mcpsIndication->RxData == true ) {
+        switch( mcpsIndication->Port ) {
+            case 1: // The application LED can be controlled on port 1 or 2
+            case 2:
+                if( mcpsIndication->BufferSize == 1 ) {
+                    AppLedStateOn = mcpsIndication->Buffer[0] & 0x01;
+                    Led3StateChanged = true;
                 }
-            }
-            else
-            {
-                ComplianceTest.State = mcpsIndication->Buffer[0];
-                switch( ComplianceTest.State )
-                {
-                case 0: // Check compliance test disable command (ii)
-                    IsTxConfirmed = LORAWAN_CONFIRMED_MSG_ON;
-                    AppPort = LORAWAN_APP_PORT;
-                    AppDataSize = LORAWAN_APP_DATA_SIZE;
-                    ComplianceTest.DownLinkCounter = 0;
-                    ComplianceTest.Running = false;
-
-                    MibRequestConfirm_t mibReq;
-                    mibReq.Type = MIB_ADR;
-                    mibReq.Param.AdrEnable = LORAWAN_ADR_ON;
-                    LoRaMacMibSetRequestConfirm( &mibReq );
-#if defined( USE_BAND_868 )
-                    LoRaMacTestSetDutyCycleOn( LORAWAN_DUTYCYCLE_ON );
-#endif
-                    break;
-                case 1: // (iii, iv)
-                    AppDataSize = 2;
-                    break;
-                case 2: // Enable confirmed messages (v)
-                    IsTxConfirmed = true;
-                    ComplianceTest.State = 1;
-                    break;
-                case 3:  // Disable confirmed messages (vi)
-                    IsTxConfirmed = false;
-                    ComplianceTest.State = 1;
-                    break;
-                case 4: // (vii)
-                    AppDataSize = mcpsIndication->BufferSize;
-
-                    AppData[0] = 4;
-                    for( uint8_t i = 1; i < AppDataSize; i++ )
-                    {
-                        AppData[i] = mcpsIndication->Buffer[i] + 1;
-                    }
-                    break;
-                case 5: // (viii)
-                    {
-                        MlmeReq_t mlmeReq;
-                        mlmeReq.Type = MLME_LINK_CHECK;
-                        LoRaMacMlmeRequest( &mlmeReq );
-                    }
-                    break;
-                case 6: // (ix)
-                    {
-                        MlmeReq_t mlmeReq;
-
-                        // Disable TestMode and revert back to normal operation
-                        IsTxConfirmed = LORAWAN_CONFIRMED_MSG_ON;
-                        AppPort = LORAWAN_APP_PORT;
-                        AppDataSize = LORAWAN_APP_DATA_SIZE;
+                break;
+            case 10:
+                break;
+            case 20:
+                break;
+            case 30:
+                break;
+            case 224:
+                if( ComplianceTest.Running == false ) {
+                    // Check compliance test enable command (i)
+                    if( ( mcpsIndication->BufferSize == 4 ) &&
+                            ( mcpsIndication->Buffer[0] == 0x01 ) &&
+                            ( mcpsIndication->Buffer[1] == 0x01 ) &&
+                            ( mcpsIndication->Buffer[2] == 0x01 ) &&
+                            ( mcpsIndication->Buffer[3] == 0x01 ) ) {
+                        IsTxConfirmed = false;
+                        AppPort = 224;
+                        AppDataSize = 2;
                         ComplianceTest.DownLinkCounter = 0;
-                        ComplianceTest.Running = false;
+                        ComplianceTest.LinkCheck = false;
+                        ComplianceTest.DemodMargin = 0;
+                        ComplianceTest.NbGateways = 0;
+                        ComplianceTest.Running = true;
+                        ComplianceTest.State = 1;
 
                         MibRequestConfirm_t mibReq;
                         mibReq.Type = MIB_ADR;
-                        mibReq.Param.AdrEnable = LORAWAN_ADR_ON;
+                        mibReq.Param.AdrEnable = true;
                         LoRaMacMibSetRequestConfirm( &mibReq );
+
 #if defined( USE_BAND_868 )
-                        LoRaMacTestSetDutyCycleOn( LORAWAN_DUTYCYCLE_ON );
+                        LoRaMacTestSetDutyCycleOn( false );
+#endif
+                    }
+                } else {
+                    ComplianceTest.State = mcpsIndication->Buffer[0];
+                    switch( ComplianceTest.State ) {
+                        case 0: // Check compliance test disable command (ii)
+                            IsTxConfirmed = LORAWAN_CONFIRMED_MSG_ON;
+                            AppPort = LORAWAN_APP_PORT;
+                            AppDataSize = LORAWAN_APP_DATA_SIZE;
+                            ComplianceTest.DownLinkCounter = 0;
+                            ComplianceTest.Running = false;
+
+                            MibRequestConfirm_t mibReq;
+                            mibReq.Type = MIB_ADR;
+                            mibReq.Param.AdrEnable = LORAWAN_ADR_ON;
+                            LoRaMacMibSetRequestConfirm( &mibReq );
+#if defined( USE_BAND_868 )
+                            LoRaMacTestSetDutyCycleOn( LORAWAN_DUTYCYCLE_ON );
+#endif
+                            break;
+                        case 1: // (iii, iv)
+                            AppDataSize = 2;
+                            break;
+                        case 2: // Enable confirmed messages (v)
+                            IsTxConfirmed = true;
+                            ComplianceTest.State = 1;
+                            break;
+                        case 3:  // Disable confirmed messages (vi)
+                            IsTxConfirmed = false;
+                            ComplianceTest.State = 1;
+                            break;
+                        case 4: // (vii)
+                            AppDataSize = mcpsIndication->BufferSize;
+
+                            AppData[0] = 4;
+                            for( uint8_t i = 1; i < AppDataSize; i++ ) {
+                                AppData[i] = mcpsIndication->Buffer[i] + 1;
+                            }
+                            break;
+                        case 5: { // (viii)
+                            MlmeReq_t mlmeReq;
+                            mlmeReq.Type = MLME_LINK_CHECK;
+                            LoRaMacMlmeRequest( &mlmeReq );
+                        }
+                        break;
+                        case 6: { // (ix)
+                            MlmeReq_t mlmeReq;
+
+                            // Disable TestMode and revert back to normal operation
+                            IsTxConfirmed = LORAWAN_CONFIRMED_MSG_ON;
+                            AppPort = LORAWAN_APP_PORT;
+                            AppDataSize = LORAWAN_APP_DATA_SIZE;
+                            ComplianceTest.DownLinkCounter = 0;
+                            ComplianceTest.Running = false;
+
+                            MibRequestConfirm_t mibReq;
+                            mibReq.Type = MIB_ADR;
+                            mibReq.Param.AdrEnable = LORAWAN_ADR_ON;
+                            LoRaMacMibSetRequestConfirm( &mibReq );
+#if defined( USE_BAND_868 )
+                            LoRaMacTestSetDutyCycleOn( LORAWAN_DUTYCYCLE_ON );
 #endif
 
-                        mlmeReq.Type = MLME_JOIN;
-
-                        mlmeReq.Req.Join.DevEui = DevEui;
-                        mlmeReq.Req.Join.AppEui = AppEui;
-                        mlmeReq.Req.Join.AppKey = AppKey;
-                        mlmeReq.Req.Join.NbTrials = 3;
+                            mlmeReq.Type = MLME_JOIN;
 
-                        LoRaMacMlmeRequest( &mlmeReq );
-                        DeviceState = DEVICE_STATE_SLEEP;
-                    }
-                    break;
-                case 7: // (x)
-                    {
-                        if( mcpsIndication->BufferSize == 3 )
-                        {
-                            MlmeReq_t mlmeReq;
-                            mlmeReq.Type = MLME_TXCW;
-                            mlmeReq.Req.TxCw.Timeout = ( uint16_t )( ( mcpsIndication->Buffer[1] << 8 ) | mcpsIndication->Buffer[2] );
+                            mlmeReq.Req.Join.DevEui = DevEui;
+                            mlmeReq.Req.Join.AppEui = AppEui;
+                            mlmeReq.Req.Join.AppKey = AppKey;
+                            mlmeReq.Req.Join.NbTrials = 3;
+
                             LoRaMacMlmeRequest( &mlmeReq );
+                            DeviceState = DEVICE_STATE_SLEEP;
                         }
-                        else if( mcpsIndication->BufferSize == 7 )
-                        {
-                            MlmeReq_t mlmeReq;
-                            mlmeReq.Type = MLME_TXCW_1;
-                            mlmeReq.Req.TxCw.Timeout = ( uint16_t )( ( mcpsIndication->Buffer[1] << 8 ) | mcpsIndication->Buffer[2] );
-                            mlmeReq.Req.TxCw.Frequency = ( uint32_t )( ( mcpsIndication->Buffer[3] << 16 ) | ( mcpsIndication->Buffer[4] << 8 ) | mcpsIndication->Buffer[5] ) * 100;
-                            mlmeReq.Req.TxCw.Power = mcpsIndication->Buffer[6];
-                            LoRaMacMlmeRequest( &mlmeReq );
+                        break;
+                        case 7: { // (x)
+                            if( mcpsIndication->BufferSize == 3 ) {
+                                MlmeReq_t mlmeReq;
+                                mlmeReq.Type = MLME_TXCW;
+                                mlmeReq.Req.TxCw.Timeout = ( uint16_t )( ( mcpsIndication->Buffer[1] << 8 ) | mcpsIndication->Buffer[2] );
+                                LoRaMacMlmeRequest( &mlmeReq );
+                            } else if( mcpsIndication->BufferSize == 7 ) {
+                                MlmeReq_t mlmeReq;
+                                mlmeReq.Type = MLME_TXCW_1;
+                                mlmeReq.Req.TxCw.Timeout = ( uint16_t )( ( mcpsIndication->Buffer[1] << 8 ) | mcpsIndication->Buffer[2] );
+                                mlmeReq.Req.TxCw.Frequency = ( uint32_t )( ( mcpsIndication->Buffer[3] << 16 ) | ( mcpsIndication->Buffer[4] << 8 ) | mcpsIndication->Buffer[5] ) * 100;
+                                mlmeReq.Req.TxCw.Power = mcpsIndication->Buffer[6];
+                                LoRaMacMlmeRequest( &mlmeReq );
+                            }
+                            ComplianceTest.State = 1;
                         }
-                        ComplianceTest.State = 1;
+                        break;
+                        default:
+                            break;
                     }
-                    break;
-                default:
-                    break;
                 }
-            }
-            break;
-        default:
-            break;
+                break;
+            default:
+                break;
         }
     }
 
@@ -794,31 +683,23 @@
  */
 static void MlmeConfirm( MlmeConfirm_t *mlmeConfirm )
 {
-    switch( mlmeConfirm->MlmeRequest )
-    {
-        case MLME_JOIN:
-        {
-            if( mlmeConfirm->Status == LORAMAC_EVENT_INFO_STATUS_OK )
-            {
+    switch( mlmeConfirm->MlmeRequest ) {
+        case MLME_JOIN: {
+            if( mlmeConfirm->Status == LORAMAC_EVENT_INFO_STATUS_OK ) {
                 // Status is OK, node has joined the network
                 IsNetworkJoinedStatusUpdate = true;
                 DeviceState = DEVICE_STATE_SEND;
-            }
-            else
-            {
+            } else {
                 // Join was not successful. Try to join again
                 DeviceState = DEVICE_STATE_JOIN;
             }
             break;
         }
-        case MLME_LINK_CHECK:
-        {
-            if( mlmeConfirm->Status == LORAMAC_EVENT_INFO_STATUS_OK )
-            {
+        case MLME_LINK_CHECK: {
+            if( mlmeConfirm->Status == LORAMAC_EVENT_INFO_STATUS_OK ) {
                 // Check DemodMargin
                 // Check NbGateways
-                if( ComplianceTest.Running == true )
-                {
+                if( ComplianceTest.Running == true ) {
                     ComplianceTest.LinkCheck = true;
                     ComplianceTest.DemodMargin = mlmeConfirm->DemodMargin;
                     ComplianceTest.NbGateways = mlmeConfirm->NbGateways;
@@ -845,9 +726,6 @@
     BoardInit( );
     SerialDisplayInit( );
 
-    LightMode = 0;      // 0: manual,   1: automatic
-    Buzzer = 0;         // 0: OFF,      1: ON
-
     SerialDisplayUpdateEui( 5, DevEui );
     SerialDisplayUpdateEui( 6, AppEui );
     SerialDisplayUpdateKey( 7, AppKey );
@@ -860,48 +738,40 @@
 #endif
 
     DeviceState = DEVICE_STATE_INIT;
+    init_sensors(myGPS_I2C, myIMU);
 
-    while( 1 )
-    {
+    while( 1 ) {
         SerialRxProcess( );
-        if( IsNetworkJoinedStatusUpdate == true )
-        {
+        if( IsNetworkJoinedStatusUpdate == true ) {
             IsNetworkJoinedStatusUpdate = false;
             mibReq.Type = MIB_NETWORK_JOINED;
             LoRaMacMibGetRequestConfirm( &mibReq );
             SerialDisplayUpdateNetworkIsJoined( mibReq.Param.IsNetworkJoined );
         }
-        if( Led1StateChanged == true )
-        {
+        if( Led1StateChanged == true ) {
             Led1StateChanged = false;
             SerialDisplayUpdateLedState( 1, Led1State );
         }
-        if( Led2StateChanged == true )
-        {
+        if( Led2StateChanged == true ) {
             Led2StateChanged = false;
             SerialDisplayUpdateLedState( 2, Led2State );
         }
-        if( Led3StateChanged == true )
-        {
+        if( Led3StateChanged == true ) {
             Led3StateChanged = false;
             SerialDisplayUpdateLedState( 3, AppLedStateOn );
         }
-        if( UplinkStatusUpdated == true )
-        {
+        if( UplinkStatusUpdated == true ) {
             UplinkStatusUpdated = false;
             SerialDisplayUpdateUplink( LoRaMacUplinkStatus.Acked, LoRaMacUplinkStatus.Datarate, LoRaMacUplinkStatus.UplinkCounter, LoRaMacUplinkStatus.Port, LoRaMacUplinkStatus.Buffer, LoRaMacUplinkStatus.BufferSize );
         }
-        if( DownlinkStatusUpdated == true )
-        {
+        if( DownlinkStatusUpdated == true ) {
             DownlinkStatusUpdated = false;
             SerialDisplayUpdateLedState( 2, Led2State );
             SerialDisplayUpdateDownlink( LoRaMacDownlinkStatus.RxData, LoRaMacDownlinkStatus.Rssi, LoRaMacDownlinkStatus.Snr, LoRaMacDownlinkStatus.DownlinkCounter, LoRaMacDownlinkStatus.Port, LoRaMacDownlinkStatus.Buffer, LoRaMacDownlinkStatus.BufferSize );
         }
-        
-        switch( DeviceState )
-        {
-            case DEVICE_STATE_INIT:
-            {
+
+        switch( DeviceState ) {
+            case DEVICE_STATE_INIT: {
                 LoRaMacPrimitives.MacMcpsConfirm = McpsConfirm;
                 LoRaMacPrimitives.MacMcpsIndication = McpsIndication;
                 LoRaMacPrimitives.MacMlmeConfirm = MlmeConfirm;
@@ -938,11 +808,15 @@
                 LoRaMacChannelAdd( 9, ( ChannelParams_t )LC10 );
 
                 mibReq.Type = MIB_RX2_DEFAULT_CHANNEL;
-                mibReq.Param.Rx2DefaultChannel = ( Rx2ChannelParams_t ){ 869525000, DR_3 };
+                mibReq.Param.Rx2DefaultChannel = ( Rx2ChannelParams_t ) {
+                    869525000, DR_3
+                };
                 LoRaMacMibSetRequestConfirm( &mibReq );
 
                 mibReq.Type = MIB_RX2_CHANNEL;
-                mibReq.Param.Rx2Channel = ( Rx2ChannelParams_t ){ 869525000, DR_3 };
+                mibReq.Param.Rx2Channel = ( Rx2ChannelParams_t ) {
+                    869525000, DR_3
+                };
                 LoRaMacMibSetRequestConfirm( &mibReq );
 #endif
 
@@ -956,8 +830,7 @@
                 DeviceState = DEVICE_STATE_JOIN;
                 break;
             }
-            case DEVICE_STATE_JOIN:
-            {
+            case DEVICE_STATE_JOIN: {
 #if( OVER_THE_AIR_ACTIVATION != 0 )
                 MlmeReq_t mlmeReq;
 
@@ -967,8 +840,7 @@
                 mlmeReq.Req.Join.AppEui = AppEui;
                 mlmeReq.Req.Join.AppKey = AppKey;
 
-                if( NextTx == true )
-                {
+                if( NextTx == true ) {
                     LoRaMacMlmeRequest( &mlmeReq );
                 }
                 DeviceState = DEVICE_STATE_SLEEP;
@@ -998,31 +870,25 @@
                 IsNetworkJoinedStatusUpdate = true;
                 break;
             }
-            case DEVICE_STATE_SEND:
-            {
-                if( NextTx == true )
-                {
+            case DEVICE_STATE_SEND: {
+                if( NextTx == true ) {
                     SerialDisplayUpdateUplinkAcked( false );
                     SerialDisplayUpdateDonwlinkRxData( false );
                     PrepareTxFrame( AppPort );
 
                     NextTx = SendFrame( );
                 }
-                if( ComplianceTest.Running == true )
-                {
+                if( ComplianceTest.Running == true ) {
                     // Schedule next packet transmission
                     TxDutyCycleTime = 5000; // 5000 ms
-                }
-                else
-                {
+                } else {
                     // Schedule next packet transmission
                     TxDutyCycleTime = APP_TX_DUTYCYCLE + randr( -APP_TX_DUTYCYCLE_RND, APP_TX_DUTYCYCLE_RND );
                 }
                 DeviceState = DEVICE_STATE_CYCLE;
                 break;
             }
-            case DEVICE_STATE_CYCLE:
-            {
+            case DEVICE_STATE_CYCLE: {
                 DeviceState = DEVICE_STATE_SLEEP;
 
                 // Schedule next packet transmission
@@ -1030,26 +896,21 @@
                 TimerStart( &TxNextPacketTimer );
                 break;
             }
-            case DEVICE_STATE_SLEEP:
-            {
+            case DEVICE_STATE_SLEEP: {
                 // Wake up through events
                 break;
             }
-            default:
-            {
+            default: {
                 DeviceState = DEVICE_STATE_INIT;
                 break;
             }
-            
+
         }
 
-        // Read light sensor
-        LightValue = ( 1 - ( LightSens.read( ) * 1.65 ) );
+        // Read GPS
+        //read_gps(myGPS_I2C);
 
         // Set automatic RGB from light sensor
-        if( LightMode == 0 )
-        {
-            ColorLed.setColorRGB( 0, ( uint8_t )( 255 * LightValue ), ( uint8_t )( 255 * LightValue ), ( uint8_t )( 255 * LightValue ) );
-        }
+
     }
 }