Diff: Demo/DemoApplication.cpp

Revision:

17:a0bbfc228415

Parent:

14:ebd89dacc807

Child:

18:22796c3be774

--- a/Demo/DemoApplication.cpp	Tue Aug 22 13:26:29 2017 +0000
+++ b/Demo/DemoApplication.cpp	Wed Jul 18 08:51:53 2018 +0000
@@ -20,6 +20,11 @@
 #include "DemoApplication.h"
 #include "FreqLUT.h"
 
+#define max(a,b) \
+   ({ __typeof__ (a) _a = (a); \
+       __typeof__ (b) _b = (b); \
+     _a > _b ? _a : _b; })
+   
 double t0 =       -0.016432807883697;                         // X0
 double t1 =       0.323147003165358;                          // X1
 double t2 =       0.014922061351196;                          // X1^2
@@ -45,10 +50,10 @@
 
 /*!
  * \brief Define time used in PingPong demo to synch with cycle
- * RX_TIMEOUT_MARGIN is the free time between each cycle (time reserve)
+ * RX_TX_INTER_PACKET_DELAY is the free time between each cycle (time reserve)
  */
-#define RX_TIMEOUT_MARGIN               50  // ms
-#define RX_TX_TRANSITION_WAIT           5   // ms
+#define RX_TX_INTER_PACKET_DELAY        150  // ms
+#define RX_TX_TRANSITION_WAIT           15   // ms
 
 /*!
  * \brief Size of ticks (used for Tx and Rx timeout)
@@ -76,8 +81,6 @@
 const uint8_t PongMsg[] = "PONG";
 const uint8_t PerMsg[]  = "PER";
 
-const uint32_t RefreshScreenDelayMs = 300;   // in ms
-
 /*!
  * \brief Buffer and its size
  */
@@ -189,6 +192,13 @@
 static bool SendNext = false;
 
 /*!
+ * \brief Ticker for slave to synch Tx frames. Flags for PER and PingPong demo
+ * for Synch RX in cycle.
+ */
+Ticker ReceiveNextPacket;
+static bool ReceiveNext = false;
+
+/*!
  * \brief Hold last Rx packet number to compute PER in PER and PingPong demo
  */
 static uint32_t PacketRxSequence = 0;
@@ -200,6 +210,7 @@
 void InitializeDemoParameters( uint8_t modulation );
 uint16_t GetTimeOnAir( uint8_t modulation );
 void SendNextPacketEvent( void );
+void ReceiveNextPacketEvent( void );
 uint8_t CheckDistance( void );
 
 // **************************     RF Test Demo    ******************************
@@ -332,13 +343,10 @@
     if( DemoRunning == false )
     {
         DemoRunning = true;
-
+        InitializeDemoParameters( Eeprom.EepromData.DemoSettings.ModulationType );
         TX_LED = 0;
         RX_LED = 0;
 
-        InitializeDemoParameters( Eeprom.EepromData.DemoSettings.ModulationType );
-
-        // Send the next PING frame
         IrqMask = IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT;
         Radio.SetDioIrqParams( IrqMask, IrqMask, IRQ_RADIO_NONE, IRQ_RADIO_NONE );
         for( i = 0; i < localPayloadSize; i++ )
@@ -401,6 +409,7 @@
 {
     uint8_t i = 0;
     uint8_t refreshDisplay = 0;
+    static uint8_t CurrentChannel;
 
     if( Eeprom.EepromData.DemoSettings.HoldDemo == true )
     {
@@ -410,8 +419,11 @@
     if( DemoRunning == false )
     {
         DemoRunning = true;
+        CurrentChannel = 0;
 
+#ifdef PRINT_DEBUG
         printf( "Start RunDemoApplicationPer\n\r" );
+#endif
 
         TX_LED = 0;
         RX_LED = 0;
@@ -423,11 +435,14 @@
 
         InitializeDemoParameters( Eeprom.EepromData.DemoSettings.ModulationType );
 
-        Eeprom.EepromData.DemoSettings.InterPacketDelay = GetTimeOnAir( Eeprom.EepromData.DemoSettings.ModulationType ) + RefreshScreenDelayMs;
+        UpdateRadioFrequency( Channels[CurrentChannel] );
+        Radio.SetRfFrequency( Channels[CurrentChannel] );
+
+        Eeprom.EepromData.DemoSettings.TimeOnAir = GetTimeOnAir( Eeprom.EepromData.DemoSettings.ModulationType );
 
         if( Eeprom.EepromData.DemoSettings.Entity == MASTER )
         {
-            SendNextPacket.attach_us( &SendNextPacketEvent, Eeprom.EepromData.DemoSettings.InterPacketDelay * 1000 );
+            SendNextPacket.attach_us( &SendNextPacketEvent, ( ( uint32_t )( Eeprom.EepromData.DemoSettings.TimeOnAir + RX_TX_INTER_PACKET_DELAY ) ) * 1000 );
             DemoInternalState = APP_TX;
         }
         else
@@ -454,6 +469,7 @@
             DemoInternalState = APP_IDLE;
             Radio.SetStandby( STDBY_RC );
             SendNextPacket.detach( );
+            ReceiveNextPacket.detach( );
             Eeprom.EepromData.DemoSettings.HoldDemo = true;
             refreshDisplay = 1;
         }
@@ -478,17 +494,29 @@
             }
             TX_LED = !TX_LED;
             IrqMask = IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT;
+
+            UpdateRadioFrequency( Channels[CurrentChannel] );
+            Radio.SetRfFrequency( Channels[CurrentChannel] );
+
             Radio.SetDioIrqParams( IrqMask, IrqMask, IRQ_RADIO_NONE, IRQ_RADIO_NONE );
-            Radio.SendPayload( Buffer, Eeprom.EepromData.DemoSettings.PayloadLength, \
-                               ( TickTime_t ){ RX_TIMEOUT_TICK_SIZE, \
-                               Eeprom.EepromData.DemoSettings.InterPacketDelay } );
+            Radio.SendPayload( Buffer, Eeprom.EepromData.DemoSettings.PayloadLength, 
+                               ( TickTime_t ){ RX_TIMEOUT_TICK_SIZE, Eeprom.EepromData.DemoSettings.TimeOnAir << 1 } );
+            if( CurrentChannel < ( CHANNELS - 1 ) )
+            {
+                CurrentChannel++;
+            }
+            else
+            {
+                CurrentChannel = 0;
+            }
             break;
 
         case PER_RX_START:
+            UpdateRadioFrequency( Channels[CurrentChannel] );
+            Radio.SetRfFrequency( Channels[CurrentChannel] );
             IrqMask = IRQ_RX_DONE | IRQ_CRC_ERROR | IRQ_RX_TX_TIMEOUT;
             Radio.SetDioIrqParams( IrqMask, IrqMask, IRQ_RADIO_NONE, IRQ_RADIO_NONE );
-            Radio.SetRx( ( TickTime_t ) { RX_TIMEOUT_TICK_SIZE, \
-                         Eeprom.EepromData.DemoSettings.InterPacketDelay + RX_TIMEOUT_MARGIN } );
+            Radio.SetRx( ( TickTime_t ) { RX_TIMEOUT_TICK_SIZE, 0xFFFF } );
             DemoInternalState = APP_IDLE;
             break;
 
@@ -496,15 +524,10 @@
             if( SendNext == true )
             {
                 SendNext = false;
-                if( Eeprom.EepromData.DemoSettings.MaxNumPacket == 0 )
+                if( ( Eeprom.EepromData.DemoSettings.MaxNumPacket == 0 ) || 
+                    ( Eeprom.EepromData.DemoSettings.CntPacketTx < Eeprom.EepromData.DemoSettings.MaxNumPacket ) )
                 {
-                    DemoInternalState = PER_TX_START;  // Infinite -> send next
-                    refreshDisplay = 1;
-                }
-                else if( Eeprom.EepromData.DemoSettings.CntPacketTx < \
-                         Eeprom.EepromData.DemoSettings.MaxNumPacket )
-                {
-                    DemoInternalState = PER_TX_START;  // MaxNumPacket not sent
+                    DemoInternalState = PER_TX_START;  // send next
                     refreshDisplay = 1;
                 }
                 else    // MaxNumPacket sent -> end of demo
@@ -522,6 +545,18 @@
 
         case APP_RX:
             RX_LED = !RX_LED;
+            Radio.SetStandby( STDBY_RC );
+            ReceiveNext = false;
+            ReceiveNextPacket.detach( );
+            ReceiveNextPacket.attach_us( &ReceiveNextPacketEvent, ( ( uint32_t )( Eeprom.EepromData.DemoSettings.TimeOnAir + RX_TX_INTER_PACKET_DELAY + RX_TX_TRANSITION_WAIT ) ) * 1000 );
+            if( CurrentChannel < ( CHANNELS - 1 ) )
+            {
+                CurrentChannel++;
+            }
+            else
+            {
+                CurrentChannel = 0;
+            }
             Radio.GetPayload( Buffer, &BufferSize, BUFFER_SIZE );
             Radio.GetPacketStatus( &PacketStatus );
             if( Eeprom.EepromData.DemoSettings.ModulationType == PACKET_TYPE_LORA )
@@ -552,20 +587,42 @@
             break;
 
         case APP_RX_ERROR:
+            printf("crc\n\r");
+            Radio.SetStandby( STDBY_RC );
+            DemoInternalState = APP_IDLE;
+            break;
+
         case APP_RX_TIMEOUT:
+            printf("tmt\n\r");
+            Radio.SetStandby( STDBY_RC );
+            if( CurrentChannel < ( CHANNELS - 1 ) )
+            {
+                CurrentChannel++;
+            }
+            else
+            {
+                CurrentChannel = 0;
+            }
             Eeprom.EepromData.DemoSettings.RxTimeOutCount++;
             DemoInternalState = PER_RX_START;
             refreshDisplay = 1;
             break;
 
         case APP_TX_TIMEOUT:
+#ifdef PRINT_DEBUG
             printf( "Failure: timeout in Tx is shorter than the packet time on air\n\r" );
+#endif
             DemoInternalState = APP_IDLE;
             Eeprom.EepromData.DemoSettings.HoldDemo = true;
             refreshDisplay = 1;
             break;
 
         case APP_IDLE: // do nothing
+            if( ReceiveNext == true )
+            {
+                ReceiveNext = false;
+                DemoInternalState = APP_RX_TIMEOUT;
+            }
             break;
 
         default:
@@ -585,7 +642,7 @@
                                      buffer[3];
 
     if( ( PacketRxSequence <= PacketRxSequencePrev ) || \
-        ( PacketRxSequencePrev == 0 ) )
+        ( PacketRxSequencePrev == 0xFFFFFFFF ) )
     {
         // Sequence went back => resynchronization
         // Don't count missed packets this time
@@ -624,6 +681,8 @@
         TX_LED = 0;
         RX_LED = 0;
         SetAntennaSwitch( );
+        ReceiveNext = false;
+        SendNext = false;
         Eeprom.EepromData.DemoSettings.CntPacketTx        = 0;
         Eeprom.EepromData.DemoSettings.CntPacketRxOK      = 0;
         Eeprom.EepromData.DemoSettings.CntPacketRxOKSlave = 0;
@@ -633,16 +692,16 @@
 
         InitializeDemoParameters( Eeprom.EepromData.DemoSettings.ModulationType );
 
-        Eeprom.EepromData.DemoSettings.InterPacketDelay = ( 2 * \
-            GetTimeOnAir( Eeprom.EepromData.DemoSettings.ModulationType ) ) + \
-            RX_TIMEOUT_MARGIN + RefreshScreenDelayMs;
+        Eeprom.EepromData.DemoSettings.TimeOnAir = GetTimeOnAir( Eeprom.EepromData.DemoSettings.ModulationType );
 
+#ifdef PRINT_DEBUG
         printf( "Start RunDemoApplicationPingPong.\n\r" );
+#endif
+
         if( Eeprom.EepromData.DemoSettings.Entity == MASTER )
         {
             DemoInternalState = SEND_PING_MSG;
-            SendNextPacket.attach_us( &SendNextPacketEvent, \
-                ( Eeprom.EepromData.DemoSettings.InterPacketDelay * 1000 ) );
+            SendNextPacket.attach_us( &SendNextPacketEvent, ( ( uint32_t )( ( Eeprom.EepromData.DemoSettings.TimeOnAir * 2 ) + RX_TX_INTER_PACKET_DELAY * 2 ) * 1000 ) );
         }
         else
         {
@@ -666,11 +725,13 @@
                     && ( Eeprom.EepromData.DemoSettings.CntPacketTx >= Eeprom.EepromData.DemoSettings.MaxNumPacket ) )
                 {
                     SendNextPacket.detach( );
+                    ReceiveNextPacket.detach( );
+                    ReceiveNext = false;
                     SendNext = false;
                     RX_LED = 0;
                     TX_LED = 0;
                     DemoInternalState = APP_IDLE;
-                    Radio.SetStandby( STDBY_RC );
+                    
                     Eeprom.EepromData.DemoSettings.HoldDemo = true;
                     refreshDisplay = 1;
                 }
@@ -699,22 +760,19 @@
                         IrqMask = IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT;
                         Radio.SetDioIrqParams( IrqMask, IrqMask, IRQ_RADIO_NONE, IRQ_RADIO_NONE );
                         Radio.SendPayload( Buffer, Eeprom.EepromData.DemoSettings.PayloadLength, \
-                                           ( TickTime_t ){ RX_TIMEOUT_TICK_SIZE, \
-                                           Eeprom.EepromData.DemoSettings.InterPacketDelay - \
-                                           ( RX_TIMEOUT_MARGIN / 2 ) } );
+                                                   ( TickTime_t ){ RX_TIMEOUT_TICK_SIZE, \
+                                                   Eeprom.EepromData.DemoSettings.TimeOnAir + RX_TX_TRANSITION_WAIT } );
                     }
                 }
                 break;
 
             case APP_TX:
-                DemoInternalState = APP_IDLE;
+                DemoInternalState = SEND_PING_MSG;
                 TX_LED = !TX_LED;
                 RX_LED = !RX_LED;
                 IrqMask = IRQ_RX_DONE | IRQ_CRC_ERROR | IRQ_RX_TX_TIMEOUT;
                 Radio.SetDioIrqParams( IrqMask, IrqMask, IRQ_RADIO_NONE, IRQ_RADIO_NONE );
-                Radio.SetRx( ( TickTime_t ) { RX_TIMEOUT_TICK_SIZE, \
-                             Eeprom.EepromData.DemoSettings.InterPacketDelay - \
-                             ( RX_TIMEOUT_MARGIN / 2 ) } );
+                Radio.SetRx( ( TickTime_t ) { RX_TIMEOUT_TICK_SIZE, Eeprom.EepromData.DemoSettings.TimeOnAir + RX_TX_TRANSITION_WAIT } );
                 break;
 
             case APP_RX:
@@ -750,6 +808,7 @@
 
             case APP_RX_TIMEOUT:
             case APP_RX_ERROR:
+                Radio.SetStandby( STDBY_RC );
                 RX_LED = !RX_LED;
                 Eeprom.EepromData.DemoSettings.CntPacketRxKO++;
                 DemoInternalState = SEND_PING_MSG;
@@ -757,7 +816,9 @@
                 break;
 
             case APP_TX_TIMEOUT:
+#ifdef PRINT_DEBUG
                 printf( "Failure: timeout in Tx is shorter than the packet time on air\n\r" );
+#endif
                 DemoInternalState = APP_IDLE;
                 Eeprom.EepromData.DemoSettings.HoldDemo = true;
                 refreshDisplay = 1;
@@ -775,7 +836,7 @@
         switch( DemoInternalState )
         {
             case SEND_PONG_MSG:
-                wait_ms( RX_TX_TRANSITION_WAIT );
+                wait_ms( 2 );
 
                 DemoInternalState = APP_IDLE;
                 // Send the next PING frame
@@ -804,7 +865,7 @@
                 Radio.SetDioIrqParams( IrqMask, IrqMask, IRQ_RADIO_NONE, IRQ_RADIO_NONE );
                 Radio.SendPayload( Buffer, Eeprom.EepromData.DemoSettings.PayloadLength, \
                                    ( TickTime_t ){ RX_TIMEOUT_TICK_SIZE, \
-                                   Eeprom.EepromData.DemoSettings.InterPacketDelay } );
+                                   Eeprom.EepromData.DemoSettings.TimeOnAir + RX_TX_TRANSITION_WAIT } );
                 break;
 
             case APP_TX:
@@ -828,8 +889,7 @@
                     RX_LED = !RX_LED;
                     IrqMask = IRQ_RX_DONE | IRQ_CRC_ERROR | IRQ_RX_TX_TIMEOUT;
                     Radio.SetDioIrqParams( IrqMask, IrqMask, IRQ_RADIO_NONE, IRQ_RADIO_NONE );
-                    Radio.SetRx( ( TickTime_t ){ RX_TIMEOUT_TICK_SIZE, \
-                                 Eeprom.EepromData.DemoSettings.InterPacketDelay } );
+                    Radio.SetRx( ( TickTime_t ){ RX_TIMEOUT_TICK_SIZE, 0xFFFF } );
                     refreshDisplay = 1;
                 }
                 break;
@@ -837,6 +897,12 @@
             case APP_RX:
                 DemoInternalState = APP_IDLE;
                 RX_LED = !RX_LED;
+
+                Radio.SetStandby( STDBY_RC );
+                ReceiveNext = false;
+                ReceiveNextPacket.detach( );
+                ReceiveNextPacket.attach_us( &ReceiveNextPacketEvent, ( ( uint32_t )( ( Eeprom.EepromData.DemoSettings.TimeOnAir << 1 ) + RX_TX_INTER_PACKET_DELAY * 2 + RX_TX_TRANSITION_WAIT ) ) * 1000 );
+
                 Radio.GetPayload( Buffer, &BufferSize, BUFFER_SIZE );
                 Radio.GetPacketStatus( &PacketStatus );
                 if( Eeprom.EepromData.ModulationParams.PacketType == PACKET_TYPE_LORA )
@@ -854,42 +920,39 @@
                     Eeprom.EepromData.DemoSettings.RssiValue = PacketStatus.Gfsk.RssiSync;
                     Eeprom.EepromData.DemoSettings.SnrValue = 0;
                 }
-                if( ( BufferSize >= PINGPONG_SIZE ) && ( strncmp( ( const char* )( Buffer + 4 ), ( const char* )PingMsg, PINGPONG_SIZE ) == 0 ) )
-                {
-                    ComputePingPongPayload( Buffer, BufferSize );
-                    DemoInternalState = SEND_PONG_MSG;
-                }
-                else
-                {
-                    Eeprom.EepromData.DemoSettings.CntPacketRxKO++;
-                    RX_LED = !RX_LED;
-                    IrqMask = IRQ_RX_DONE | IRQ_CRC_ERROR | IRQ_RX_TX_TIMEOUT;
-                    Radio.SetDioIrqParams( IrqMask, IrqMask, IRQ_RADIO_NONE, IRQ_RADIO_NONE );
-                    Radio.SetRx( ( TickTime_t ) { RX_TIMEOUT_TICK_SIZE, \
-                        Eeprom.EepromData.DemoSettings.InterPacketDelay } );
-                    refreshDisplay = 1;
-                }
+                ComputePingPongPayload( Buffer, BufferSize );
+                DemoInternalState = SEND_PONG_MSG;
                 break;
 
             case APP_RX_TIMEOUT:
-            case APP_RX_ERROR:
-                DemoInternalState = APP_IDLE;
+                printf("tmt\n\r");
+                Radio.SetStandby( STDBY_RC );
                 Eeprom.EepromData.DemoSettings.RxTimeOutCount++;
-                IrqMask = IRQ_RX_DONE | IRQ_CRC_ERROR | IRQ_RX_TX_TIMEOUT;
-                Radio.SetDioIrqParams( IrqMask, IrqMask, IRQ_RADIO_NONE, IRQ_RADIO_NONE );
-                Radio.SetRx( ( TickTime_t ) { RX_TIMEOUT_TICK_SIZE, \
-                        Eeprom.EepromData.DemoSettings.InterPacketDelay } );
+                DemoInternalState = SEND_PONG_MSG;
                 refreshDisplay = 1;
                 break;
 
+            case APP_RX_ERROR:
+                printf("crc\n\r");
+                Radio.SetStandby( STDBY_RC );
+                DemoInternalState = APP_IDLE;
+                break;
+
             case APP_TX_TIMEOUT:
+#ifdef PRINT_DEBUG
                 printf( "Failure: timeout in Tx is shorter than the packet time on air\n\r" );
+#endif
                 DemoInternalState = APP_IDLE;
                 Eeprom.EepromData.DemoSettings.HoldDemo = true;
                 refreshDisplay = 1;
                 break;
 
             case APP_IDLE: // do nothing
+                if( ReceiveNext == true )
+                {
+                    ReceiveNext = false;
+                    DemoInternalState = APP_RX_TIMEOUT;
+                }
                 break;
 
             default:
@@ -979,7 +1042,9 @@
         RX_LED = 0;
         ANT_SW = 1;
 
+#ifdef PRINT_DEBUG
         printf( "Start RunDemoApplicationRanging\r\n" );
+#endif
 
         Eeprom.EepromData.DemoSettings.CntPacketTx = 0;
         Eeprom.EepromData.DemoSettings.RngFei      = 0.0;
@@ -988,9 +1053,9 @@
 
         if( Eeprom.EepromData.DemoSettings.Entity == MASTER )
         {
-            Eeprom.EepromData.DemoSettings.InterPacketDelay = RefreshScreenDelayMs;
-            Radio.SetDioIrqParams( IRQ_RX_DONE | IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT | IRQ_RANGING_MASTER_RESULT_VALID | IRQ_RANGING_MASTER_TIMEOUT,
-                                   IRQ_RX_DONE | IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT | IRQ_RANGING_MASTER_RESULT_VALID | IRQ_RANGING_MASTER_TIMEOUT,
+            Eeprom.EepromData.DemoSettings.TimeOnAir = RX_TX_INTER_PACKET_DELAY;
+            Radio.SetDioIrqParams( IRQ_RX_DONE | IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT | IRQ_RANGING_MASTER_RESULT_VALID | IRQ_RANGING_MASTER_ERROR_CODE,
+                                   IRQ_RX_DONE | IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT | IRQ_RANGING_MASTER_RESULT_VALID | IRQ_RANGING_MASTER_ERROR_CODE,
                                    IRQ_RADIO_NONE, IRQ_RADIO_NONE );
             Eeprom.EepromData.DemoSettings.RngDistance = 0.0;
             DemoInternalState = APP_RANGING_CONFIG;
@@ -1002,6 +1067,8 @@
         }
     }
 
+    Radio.ProcessIrqs( );
+
     if( Eeprom.EepromData.DemoSettings.Entity == MASTER )
     {
         switch( DemoInternalState )
@@ -1461,7 +1528,10 @@
     {
         __disable_irq( );    // Disable Interrupts
 
+#ifdef PRINT_DEBUG
         printf( "StopDemoApplication\n\r" );
+#endif
+
         if( Radio.GetOpMode( ) == MODE_SLEEP )
         {
             Radio.Wakeup( );
@@ -1471,6 +1541,7 @@
         TX_LED = 0;
         DemoRunning = false;
         SendNext = false;
+        ReceiveNext = false;
         PacketRxSequence = 0;
         PacketRxSequencePrev = 0;
         Eeprom.EepromData.DemoSettings.CntPacketTx    = 0;
@@ -1482,8 +1553,9 @@
         DemoInternalState = APP_IDLE;
         Radio.SetStandby( STDBY_RC );
         Radio.ClearIrqStatus( IRQ_RADIO_ALL );
-        SendNextPacket.detach( ); 
-
+        SendNextPacket.detach( );
+        ReceiveNextPacket.detach( );
+        
         __enable_irq( );     // Enable Interrupts
     }
 }
@@ -1496,239 +1568,197 @@
 uint16_t GetTimeOnAir( uint8_t modulation )
 {
     uint16_t result = 2000;
-
+    double tPayload = 0.0;
+    
     if( modulation == PACKET_TYPE_LORA )
     {
-        double bw = 0.0;
+        uint16_t bw = 0.0;
+        double nPayload = 0.0;
+        double ts = 0.0;
+
+        uint8_t SF = Eeprom.EepromData.ModulationParams.Params.LoRa.SpreadingFactor >> 4;
+        uint8_t crc = ( Eeprom.EepromData.PacketParams.Params.LoRa.Crc == LORA_CRC_ON ) ? 16 : 0; // 16 bit if present else 0
+        uint8_t header = ( Eeprom.EepromData.PacketParams.Params.LoRa.HeaderType == LORA_PACKET_VARIABLE_LENGTH ) ? 20 : 0; // 20 if present else 0
+        uint16_t payload = 8 * Eeprom.EepromData.PacketParams.Params.LoRa.PayloadLength;
+        uint8_t CR = Eeprom.EepromData.ModulationParams.Params.LoRa.CodingRate;
 
         switch( Eeprom.EepromData.ModulationParams.Params.LoRa.Bandwidth )
         {
             case LORA_BW_0200:
-                bw = 203e3;
+                bw = 203;
                 break;
 
             case LORA_BW_0400:
-                bw = 406e3;
+                bw = 406;
                 break;
 
             case LORA_BW_0800:
-                bw = 812e3;
+                bw = 812;
                 break;
 
             case LORA_BW_1600:
-                bw = 1625e3;
+                bw = 1625;
                 break;
 
             default:
-                bw = 100e3;
                 break;
         }
-        double rs = bw / ( 1 << ( Eeprom.EepromData.ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) );        // Symbol rate : time for one symbol (secs)
-        double ts = 1 / rs;
-        double tPreamble = ( Eeprom.EepromData.PacketParams.Params.LoRa.PreambleLength + 4.25 ) * ts; // time of preamble
-        uint8_t de = 1;    // always 1 on SX1280. "low data rate optimization" condition.
-        double tmp = ceil( ( 8 * Eeprom.EepromData.PacketParams.Params.LoRa.PayloadLength - 4 * ( Eeprom.EepromData.ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) +
-                             28 + 16 * ( ( Eeprom.EepromData.PacketParams.Params.LoRa.Crc == 0x00 ) ? 0 : 1 ) - \
-                             ( ( Eeprom.EepromData.PacketParams.Params.LoRa.HeaderType >> 7 ) ? 20 : 0 ) ) / \
-                           ( double )( 4 * ( ( Eeprom.EepromData.ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) - ( de * 2 ) ) ) * \
-                           ( ( Eeprom.EepromData.ModulationParams.Params.LoRa.CodingRate % 4 ) + 4 ) );                     // Symbol length of payload and time
-        double nPayload = 8 + ( ( tmp > 0 ) ? tmp : 0 );
-        double tPayload = nPayload * ts;
 
-        // Time on air [ms]
-        result = floor( ( ( tPreamble + tPayload ) * 1000 * 1.2 ) + 0.999 ); // Set some margin
-        result *= 1.8;   // Set some margin
+        if( SF < 7 )
+        {
+            nPayload = max( ( ( double )( payload + crc -(4 * SF) + header ) ), 0.0 );
+            nPayload = nPayload / ( double )( 4 * SF );
+            nPayload = ceil( nPayload );
+            nPayload = nPayload * ( CR + 4 );
+            nPayload = nPayload + Eeprom.EepromData.PacketParams.Params.LoRa.PreambleLength + 6.25 + 8;
+        }
+        else if( SF > 10 )
+        {
+            nPayload = max( ( ( double )( payload + crc -(4 * SF) + 8 + header ) ), 0.0 );
+            nPayload = nPayload / ( double )( 4 * ( SF - 2 ) );
+            nPayload = ceil( nPayload );
+            nPayload = nPayload * ( CR + 4 );
+            nPayload = nPayload + Eeprom.EepromData.PacketParams.Params.LoRa.PreambleLength + 4.25 + 8;
+        }
+        else
+        {
+            nPayload = max( ( ( double )( payload + crc -(4 * SF) + 8 + header ) ), 0.0 );
+            nPayload = nPayload / ( double )( 4 * SF );
+            nPayload = ceil( nPayload );
+            nPayload = nPayload * ( CR + 4 );
+            nPayload = nPayload + Eeprom.EepromData.PacketParams.Params.LoRa.PreambleLength + 4.25 + 8;
+        }
+        ts = ( double )( 1 << SF ) / ( double )( bw );
+        tPayload = nPayload * ts;
+#ifdef PRINT_DEBUG
+        printf( "ToA LoRa: %f \n\r", tPayload );
+#endif
+        result = ceil( tPayload );   
     }
     else if( modulation == PACKET_TYPE_FLRC )
     {
-        uint16_t packetBitCount;
-
-        switch( Eeprom.EepromData.PacketParams.Params.Flrc.PreambleLength )
-        {
-            case PREAMBLE_LENGTH_04_BITS:     // Preamble length: 04 bits
-            case PREAMBLE_LENGTH_08_BITS:     // Preamble length: 08 bits
-                packetBitCount = 1;
-                break;
-
-            case PREAMBLE_LENGTH_12_BITS:     // Preamble length: 12 bits
-            case PREAMBLE_LENGTH_16_BITS:     // Preamble length: 16 bits
-                packetBitCount = 2;
-                break;
-
-            case PREAMBLE_LENGTH_20_BITS:     // Preamble length: 20 bits
-            case PREAMBLE_LENGTH_24_BITS:     // Preamble length: 24 bits
-                packetBitCount = 3;
-                break;
+        uint16_t BitCount = 0;
+        uint16_t BitCountCoded = 0;
 
-            case PREAMBLE_LENGTH_28_BITS:     // Preamble length: 28 bits
-            case PREAMBLE_LENGTH_32_BITS:     // Preamble length: 32 bits
-                packetBitCount = 4;
-                break;
+        BitCount = 4 + ( Eeprom.EepromData.PacketParams.Params.Flrc.PreambleLength >> 4 ) * 4;              // AGC preamble 
+        BitCount = BitCount + 32;                                                                           // Sync Word
+        BitCount = BitCount + 21;                                                                           // Preamble
+        BitCount = BitCount + ( ( Eeprom.EepromData.PacketParams.Params.Flrc.HeaderType == RADIO_PACKET_VARIABLE_LENGTH ) ? 16 : 0 );
 
-            default:
-                packetBitCount = 4;
-                break;
-        }
-        packetBitCount += 3;                  // Preamble 21 bits
-        switch( Eeprom.EepromData.PacketParams.Params.Flrc.SyncWordLength )
-        {
-            case FLRC_SYNCWORD_LENGTH_4_BYTE:
-                packetBitCount += 4;
-                break;
-
-            case FLRC_NO_SYNCWORD:
-            default:
-                break;
-        }
         switch( Eeprom.EepromData.ModulationParams.Params.Flrc.CodingRate )
         {
-            // += 7;  // CRC length is maximum 4 bytes (short-cut) + 2 Header + 6 bits Tail
             case FLRC_CR_3_4:
-                packetBitCount += ( uint16_t )( ceil( ( ( float )( Eeprom.EepromData.PacketParams.Params.Flrc.PayloadLength + 7 ) * 4 ) / 3 ) );
+                BitCountCoded =  6 + ( Eeprom.EepromData.PacketParams.Params.Flrc.CrcLength >> 4 ) * 8;
+                BitCountCoded = BitCountCoded + Eeprom.EepromData.PacketParams.Params.Flrc.PayloadLength * 8;
+                BitCountCoded = ( uint16_t )( ( ( double )BitCountCoded * 4.0 ) / 3.0 );
                 break;
 
             case FLRC_CR_1_0:
-                packetBitCount += Eeprom.EepromData.PacketParams.Params.Flrc.PayloadLength + 7;
+                BitCountCoded =  ( Eeprom.EepromData.PacketParams.Params.Flrc.CrcLength >> 4 ) * 8;
+                BitCountCoded = BitCountCoded + Eeprom.EepromData.PacketParams.Params.Flrc.PayloadLength * 8;
                 break;
 
             default:
             case FLRC_CR_1_2:
-                packetBitCount += ( Eeprom.EepromData.PacketParams.Params.Flrc.PayloadLength + 7 ) * 2;
+                BitCountCoded =  6 + ( Eeprom.EepromData.PacketParams.Params.Flrc.CrcLength >> 4 ) * 8;
+                BitCountCoded = BitCountCoded + Eeprom.EepromData.PacketParams.Params.Flrc.PayloadLength * 8;
+                BitCountCoded = BitCountCoded << 1;
                 break;
         }
-        packetBitCount *= 8;
+        BitCount = BitCount + BitCountCoded;
+
         switch( Eeprom.EepromData.ModulationParams.Params.Flrc.BitrateBandwidth )
         {
             case FLRC_BR_1_300_BW_1_2:
-                result = ( uint16_t )( ceil( ( float )packetBitCount / 1300 ) );
+                tPayload = ( double )BitCount / 1300.0;
                 break;
 
             case FLRC_BR_1_040_BW_1_2:
-                result = ( uint16_t )( ceil( ( float )packetBitCount / 1040 ) );
+                tPayload = ( double )BitCount / 1040.0;
                 break;
 
             case FLRC_BR_0_650_BW_0_6:
-                result = ( uint16_t )( ceil( ( float )packetBitCount / 650 ) );
+                tPayload = ( double )BitCount / 650.0;
                 break;
 
             case FLRC_BR_0_520_BW_0_6:
-                result = ( uint16_t )( ceil( ( float )packetBitCount / 520 ) );
+                tPayload = ( double )BitCount / 520.0;
                 break;
 
             case FLRC_BR_0_325_BW_0_3:
-                result = ( uint16_t )( ceil( ( float )packetBitCount / 325 ) );
+                tPayload = ( double )BitCount / 325.0;
                 break;
 
+            case FLRC_BR_0_260_BW_0_3:
+                tPayload = ( double )BitCount / 260.0;
+                break;
+            
             default:
-            case FLRC_BR_0_260_BW_0_3:
-                result = ( uint16_t )( ceil( ( float )packetBitCount / 260 ) );
                 break;
         }
-        result *= 2; // Set some margin
+
+        printf( "ToA FLRC: %f \n\r", tPayload );
+
+        result = ceil( tPayload );
     }
     else if( modulation == PACKET_TYPE_GFSK )
     {
-        uint16_t packetBitCount;
-
-        switch( Eeprom.EepromData.PacketParams.Params.Gfsk.PreambleLength )
-        {
-            case PREAMBLE_LENGTH_04_BITS:     // Preamble length: 04 bits
-            case PREAMBLE_LENGTH_08_BITS:     // Preamble length: 08 bits
-                packetBitCount = 1;
-                break;
-
-            case PREAMBLE_LENGTH_12_BITS:     // Preamble length: 12 bits
-            case PREAMBLE_LENGTH_16_BITS:     // Preamble length: 16 bits
-                packetBitCount = 2;
-                break;
-
-            case PREAMBLE_LENGTH_20_BITS:     // Preamble length: 20 bits
-            case PREAMBLE_LENGTH_24_BITS:     // Preamble length: 24 bits
-                packetBitCount = 3;
-                break;
-
-            case PREAMBLE_LENGTH_28_BITS:     // Preamble length: 28 bits
-            case PREAMBLE_LENGTH_32_BITS:     // Preamble length: 32 bits
-                packetBitCount = 4;
-                break;
-
-            default:
-                packetBitCount = 4;
-                break;
-        }
-        switch( Eeprom.EepromData.PacketParams.Params.Gfsk.SyncWordLength )
-        {
-            case GFSK_SYNCWORD_LENGTH_1_BYTE:      // Sync word length: 1 byte
-                packetBitCount += 1;
-                break;
-
-            case GFSK_SYNCWORD_LENGTH_2_BYTE:      // Sync word length: 2 bytes
-                packetBitCount += 2;
-                break;
-
-            case GFSK_SYNCWORD_LENGTH_3_BYTE:      // Sync word length: 3 bytes
-                packetBitCount += 3;
-                break;
-
-            case GFSK_SYNCWORD_LENGTH_4_BYTE:      // Sync word length: 4 bytes
-                packetBitCount += 4;
-                break;
-
-            case GFSK_SYNCWORD_LENGTH_5_BYTE:      // Sync word length: 5 bytes
-                packetBitCount += 5;
-                break;
-
-            default:
-                packetBitCount += 5;
-                break;
-        }
-        packetBitCount += Eeprom.EepromData.PacketParams.Params.Gfsk.PayloadLength + 3;
-        packetBitCount *= 8;
+        uint16_t BitCount = 0;
+        
+        BitCount = 4 + ( Eeprom.EepromData.PacketParams.Params.Gfsk.PreambleLength >> 4 ) * 4;              // preamble
+        BitCount = BitCount + 8 + ( Eeprom.EepromData.PacketParams.Params.Gfsk.SyncWordLength >> 1 ) * 8;   // sync word
+        BitCount = BitCount + ( ( Eeprom.EepromData.PacketParams.Params.Gfsk.HeaderType == RADIO_PACKET_VARIABLE_LENGTH ) ? 8 : 0 );
+        BitCount = BitCount + Eeprom.EepromData.PacketParams.Params.Gfsk.PayloadLength * 8;
+        BitCount = BitCount + ( Eeprom.EepromData.PacketParams.Params.Gfsk.CrcLength >> 4 ) * 8;
+        
         switch( Eeprom.EepromData.ModulationParams.Params.Gfsk.BitrateBandwidth )
         {
             case GFSK_BLE_BR_2_000_BW_2_4:
-                result = ( uint16_t )( ceil( ( float )packetBitCount / 2000 ) );
+                tPayload = ( double )BitCount / 2000.0 ;
                 break;
 
             case GFSK_BLE_BR_1_600_BW_2_4:
-                result = ( uint16_t )( ceil( ( float )packetBitCount / 1600 ) );
+                tPayload = ( double )BitCount / 1600.0 ;
                 break;
 
             case GFSK_BLE_BR_1_000_BW_2_4:
             case GFSK_BLE_BR_1_000_BW_1_2:
-                result = ( uint16_t )( ceil( ( float )packetBitCount / 1000 ) );
+                tPayload = ( double )BitCount / 1000.0;
                 break;
 
             case GFSK_BLE_BR_0_800_BW_2_4:
             case GFSK_BLE_BR_0_800_BW_1_2:
-                result = ( uint16_t )( ceil( ( float )packetBitCount / 800 ) );
+                tPayload = ( double )BitCount / 800.0;
                 break;
 
             case GFSK_BLE_BR_0_500_BW_1_2:
             case GFSK_BLE_BR_0_500_BW_0_6:
-                result = ( uint16_t )( ceil( ( float )packetBitCount / 500 ) );
+                tPayload = ( double )BitCount / 500.0;
                 break;
 
             case GFSK_BLE_BR_0_400_BW_1_2:
             case GFSK_BLE_BR_0_400_BW_0_6:
-                result = ( uint16_t )( ceil( ( float )packetBitCount / 400 ) );
+                tPayload = ( double )BitCount / 400.0;
                 break;
 
             case GFSK_BLE_BR_0_250_BW_0_6:
             case GFSK_BLE_BR_0_250_BW_0_3:
-                result = ( uint16_t )( ceil( ( float )packetBitCount / 250 ) );
+                tPayload = ( double )BitCount / 250.0;
                 break;
 
             case GFSK_BLE_BR_0_125_BW_0_3:
-                result = ( uint16_t )( ceil( ( float )packetBitCount / 125 ) );
+                tPayload = ( double )BitCount / 125.0;
                 break;
 
             default:
-                result = 100;
                 break;
         }
-        result *= 1.5; // Set 50% margin
+#ifdef PRINT_DEBUG
+        printf( "ToA GFSK: %f \n\r", tPayload );
+#endif
+        result = ceil( tPayload );
     }
+    
     return result;
 }
 
@@ -1738,10 +1768,14 @@
 
     Radio.SetRegulatorMode( ( RadioRegulatorModes_t )Eeprom.EepromData.DemoSettings.RadioPowerMode );
 
+#ifdef PRINT_DEBUG
     printf("> InitializeDemoParameters\n\r");
+#endif
     if( modulation == PACKET_TYPE_LORA )
     {
+#ifdef PRINT_DEBUG
         printf("set param LORA for demo\n\r");
+#endif
         ModulationParams.PacketType = PACKET_TYPE_LORA;
         PacketParams.PacketType     = PACKET_TYPE_LORA;
 
@@ -1755,10 +1789,14 @@
         PacketParams.Params.LoRa.InvertIQ            = ( RadioLoRaIQModes_t )           Eeprom.EepromData.DemoSettings.PacketParam5;
 
         Eeprom.EepromData.DemoSettings.PayloadLength = PacketParams.Params.LoRa.PayloadLength;
+
+        Radio.WriteRegister( REG_HIGH_SENSITIVITY, Radio.ReadRegister( REG_HIGH_SENSITIVITY ) & ~LNA_HIGH_SENSITIVITY_MASK );
     }
     else if( modulation == PACKET_TYPE_FLRC )
     {
+#ifdef PRINT_DEBUG
         printf("set param FLRC for demo\n\r");
+#endif
         ModulationParams.PacketType = PACKET_TYPE_FLRC;
         PacketParams.PacketType     = PACKET_TYPE_FLRC;
 
@@ -1774,10 +1812,14 @@
         PacketParams.Params.Flrc.Whitening             = ( RadioWhiteningModes_t )     Eeprom.EepromData.DemoSettings.PacketParam7;
 
         Eeprom.EepromData.DemoSettings.PayloadLength = PacketParams.Params.Flrc.PayloadLength;
+
+        Radio.WriteRegister( REG_HIGH_SENSITIVITY, Radio.ReadRegister( REG_HIGH_SENSITIVITY ) | LNA_HIGH_SENSITIVITY_MASK );
     }
     else if( modulation == PACKET_TYPE_GFSK )
     {
+#ifdef PRINT_DEBUG
         printf("set param GFSK for demo\n\r");
+#endif
         ModulationParams.PacketType = PACKET_TYPE_GFSK;
         PacketParams.PacketType     = PACKET_TYPE_GFSK;
 
@@ -1793,6 +1835,8 @@
         PacketParams.Params.Gfsk.Whitening             = ( RadioWhiteningModes_t )   Eeprom.EepromData.DemoSettings.PacketParam7;
 
         Eeprom.EepromData.DemoSettings.PayloadLength = PacketParams.Params.Gfsk.PayloadLength;
+        
+        Radio.WriteRegister( REG_HIGH_SENSITIVITY, Radio.ReadRegister( REG_HIGH_SENSITIVITY ) | LNA_HIGH_SENSITIVITY_MASK );
     }
     if( modulation == PACKET_TYPE_RANGING )
     {
@@ -1820,7 +1864,9 @@
                 Eeprom.EepromData.DemoSettings.RngReqDelay  = RNG_TIMER_MS >> ( 2 + 10 - ( ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) );
                 break;
         }
-        Radio.SetInterruptMode( );
+
+        Radio.SetPollingMode( );
+        Radio.WriteRegister( REG_HIGH_SENSITIVITY, Radio.ReadRegister( REG_HIGH_SENSITIVITY ) & ~LNA_HIGH_SENSITIVITY_MASK );
     }
     else
     {
@@ -1833,7 +1879,7 @@
         // only used in GFSK, FLRC (4 bytes max) and BLE mode
         Radio.SetSyncWord( 1, ( uint8_t[] ){ 0xDD, 0xA0, 0x96, 0x69, 0xDD } );
         // only used in GFSK, FLRC
-        uint8_t crcSeedLocal[3] = { 0x00, 0x45, 0x67 };
+        uint8_t crcSeedLocal[2] = {0x45, 0x67};
         Radio.SetCrcSeed( crcSeedLocal );
         Radio.SetCrcPolynomial( 0x0123 );
         Radio.SetTxParams( Eeprom.EepromData.DemoSettings.TxPower, RADIO_RAMP_20_US );
@@ -1853,6 +1899,14 @@
     }
 }
 
+/*!
+ * \brief Callback of ticker ReceiveNextPacket
+ */
+void ReceiveNextPacketEvent( void )
+{
+    ReceiveNext = true;
+}
+
 uint8_t CheckDistance( void )
 {
     double displayRange = 0.0;
@@ -1861,7 +1915,9 @@
     uint16_t j = 0;
     uint16_t i;
 
+#ifdef PRINT_DEBUG
     printf( "#id: %d", Eeprom.EepromData.DemoSettings.CntPacketTx );
+#endif
     if( RngResultIndex > 0 )
     {
         for( i = 0; i < RngResultIndex; ++i )