File content as of revision 0:1ca9a50646e6:

#include "mbed.h"
#include "radio.h"
#include "sx1280-hal.h"
#include <math.h>
#include "FreqLUT.h"
#include "Timers.h"

double t0 =       -0.016432807883697;                         // X0
double t1 =       0.323147003165358;                          // X1
double t2 =       0.014922061351196;                          // X1^2
double t3 =       0.000137832006285;                          // X1^3
double t4 =       0.536873856625399;                          // X2
double t5 =       0.040890089178579;                          // X2^2
double t6 =       -0.001074801048732;                         // X2^3
double t7 =       0.000009240142234;                          // X2^4

/*!
 * \brief Used to display firmware version on RS232
 */
#define FIRMWARE_VERSION ( ( char* )"Firmware Version: 15.0.3" )

/*!
 * Mode of operation
 */
#define MODE_LORA

/*!
 * \brief Ranging raw factors
 *                                  SF5     SF6     SF7     SF8     SF9     SF10
 */
//const uint16_t RNG_CALIB_0400[] = { 10299,  10271,  10244,  10242,  10230,  10246  };
//const uint16_t RNG_CALIB_0800[] = { 11486,  11474,  11453,  11426,  11417,  11401  };
const uint16_t RNG_CALIB_1600[] = { 13308,  13493,  13528,  13515,  13430,  13376  };
//const double   RNG_FGRAD_0400[] = { -0.148, -0.214, -0.419, -0.853, -1.686, -3.423 };
//const double   RNG_FGRAD_0800[] = { -0.041, -0.811, -0.218, -0.429, -0.853, -1.737 };
const double   RNG_FGRAD_1600[] = { 0.103,  -0.041, -0.101, -0.211, -0.424, -0.87  };

/*!
 * \brief Defines the nominal frequency
 */
#define RF_FREQUENCY                                2402000000UL // Hz

/*!
 * \brief Defines the output power in dBm
 *
 * \remark The range of the output power is [-18..+13] dBm
 */
#define TX_OUTPUT_POWER                             13

uint32_t RngAddress  = 0x10000000;
uint32_t RngAddress1 = 0x32100000;
uint32_t CntPacketTx = 0;
uint32_t CntPacketRxOK = 0;
uint32_t CntPacketRxOKSlave = 0;
uint32_t CntPacketRxKOSlave = 0;
uint32_t MeasuredChannels = 0;
uint32_t CurrentChannel = 0;
uint16_t RxTimeOutCount = 0;
uint16_t RngCalib;
uint16_t RngReqDelay;
uint16_t RngRequestCount = 10;
uint8_t  RngStatus;
int8_t RssiValue = 0;
int8_t SnrValue = 0;
double RngDistance = 0.0;
double RngFeiFactor;
double RngFei = 0.0;
double RawRngResults[255];
double RssiRng[255];
int    RngResultIndex;
int    Address = 0;

/*!
 * \brief Status of ranging distance
 */
enum RangingStatus
{
    RNG_INIT = 0,
    RNG_PROCESS,
    RNG_VALID,
    RNG_TIMEOUT,
    RNG_PER_ERROR
};

/*!
 * \brief Defines the states of the application
 */
typedef enum
{
    APP_IDLE,
    APP_RANGING_DONE,
    APP_RANGING_TIMEOUT,
    APP_RANGING_CONFIG,
    APP_RNG,
    APP_RX,
    APP_RX_TIMEOUT,
    APP_RX_ERROR,
    APP_TX,
    APP_TX_TIMEOUT,
}AppStates_t;

/*!
 * \brief Defines the buffer size, i.e. the payload size
 */
#define BUFFER_SIZE                                 255

/*!
 * \brief The size of the buffer
 */
uint8_t BufferSize = BUFFER_SIZE;

/*!
 * \brief The buffer
 */
uint8_t Buffer[BUFFER_SIZE];

/*!
 * \brief The State of the application
 */
AppStates_t AppState = APP_IDLE;

void SendNextPacketEvent( void );
uint8_t CheckDistance( void );

/*!
 * \brief Function to be executed on Radio Tx Done event
 */
void OnTxDone( void );

/*!
 * \brief Function to be executed on Radio Rx Done event
 */
void OnRxDone( void );

/*!
 * \brief Function executed on Radio Tx Timeout event
 */
void OnTxTimeout( void );

/*!
 * \brief Function executed on Radio Rx Timeout event
 */
void OnRxTimeout( void );

/*!
 * \brief Function executed on Radio Rx Error event
 */
void OnRxError( IrqErrorCode_t );

/*!
 * \brief Function executed on Ranging Done
 */
void OnRangingDone( IrqRangingCode_t val );

/*!
 * \brief Function for Interrupt
 */
void pressed( void );

/*!
 * \brief All the callbacks are stored in a structure
 */
RadioCallbacks_t callbacks =
{
    &OnTxDone,        // txDone
    &OnRxDone,        // rxDone
    NULL,             // syncWordDone
    NULL,             // headerDone
    &OnTxTimeout,     // txTimeout
    &OnRxTimeout,     // rxTimeout
    &OnRxError,       // rxError
    &OnRangingDone,   // rangingDone
    NULL,             // cadDone
};

// mosi, miso, sclk, nss, busy, dio1, dio2, dio3, rst, callbacks...
SX1280Hal Radio( D11, D12, D13, D7, D3, D5, NC, NC, A0, &callbacks );

//DigitalOut ANT_SW( A3 );
InterruptIn Button( USER_BUTTON );         // Interrupt Declaration
DigitalOut TxLed( A4 );
DigitalOut RxLed( A5 );


/*!
 * \brief Define IO for Unused Pin
 */
DigitalOut F_CS( D6 );      // MBED description of pin  RX_EN
DigitalOut SD_CS( D8 );     // MBED description of pin  TX_EN

/*!
 * \brief Number of tick size steps for tx timeout
 */
#define TX_TIMEOUT_VALUE                            100 // ms

/*!
 * \brief Number of tick size steps for rx timeout
 */
#define RX_TIMEOUT_VALUE                            100 // ms

/*!
 * \brief Size of ticks (used for Tx and Rx timeout)
 */
#define RX_TIMEOUT_TICK_SIZE                        RADIO_TICK_SIZE_1000_US

#define RNG_TIMER_MS                    384 // ms
#define RNG_COM_TIMEOUT                 100 // ms

/*!
 * \brief Mask of IRQs to listen to in rx mode
 */
uint16_t RxIrqMask = IRQ_RX_DONE | IRQ_RX_TX_TIMEOUT;

/*!
 * \brief Mask of IRQs to listen to in tx mode
 */
uint16_t TxIrqMask = IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT;

/*!
 * \brief Locals parameters and status for radio API
 * NEED TO BE OPTIMIZED, COPY OF STUCTURE ALREADY EXISTING
 */
PacketParams_t PacketParams;
PacketStatus_t PacketStatus;
ModulationParams_t ModulationParams;

/*!
 * \brief Ticker for master to synch Tx frames. Flags for PER and PingPong demo
 * for Synch TX in cycle.
 */ 
Ticker SendNextPacket;
static bool SendNext = false;

/*!
 * \brief Specify serial datarate for UART debug output
 */
void baud( int baudrate )
{
    Serial s( USBTX, USBRX );

    s.baud( baudrate );
}

int main( )
{
    baud( 115200 );

    F_CS   = 1;
    SD_CS  = 1;
    RxLed  = 1;
    TxLed  = 1;

    wait_ms( 500 ); // wait for on board DC/DC start-up time

    Radio.Init( );
    Radio.SetRegulatorMode( USE_DCDC ); // Can also be set in LDO mode but consume more power

    memset( &Buffer, 0x00, BufferSize );

    printf( "\n\n\r     SX1280 Ranging Demo Application (%s)\n\n\r", FIRMWARE_VERSION );
    printf( "\nRunning in LORA mode\n\r" );
    printf( "\nMaster\n\r" );
    
    TimersInit( );

    RngStatus = RNG_INIT;
    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,
                           IRQ_RADIO_NONE, IRQ_RADIO_NONE );

    RxLed = 0;
    TxLed = 0;

    AppState = APP_IDLE;//APP_RANGING_CONFIG;
    Button.fall(&pressed);
    
    while( 1 ){
        switch( AppState )
        {
            case APP_RANGING_CONFIG:
            //printf("App_Ranging_Config\r\n");
                    /*RngStatus = RNG_INIT;
                    CntPacketTx++;
                    
                    ModulationParams.PacketType = PACKET_TYPE_LORA;
                    PacketParams.PacketType     = PACKET_TYPE_LORA;
                    
                    ModulationParams.Params.LoRa.SpreadingFactor = LORA_SF7;
                    ModulationParams.Params.LoRa.Bandwidth       = LORA_BW_1600;
                    ModulationParams.Params.LoRa.CodingRate      = LORA_CR_4_5;
                    
                    PacketParams.Params.LoRa.PreambleLength      = 0x08;
                    PacketParams.Params.LoRa.HeaderType          = LORA_PACKET_VARIABLE_LENGTH;
                    PacketParams.Params.LoRa.PayloadLength       = 7;
                    PacketParams.Params.LoRa.Crc                 = LORA_CRC_ON;
                    PacketParams.Params.LoRa.InvertIQ            = LORA_IQ_INVERTED;
                    
                    Radio.SetPacketType( ModulationParams.PacketType );
                    Radio.SetModulationParams( &ModulationParams );
                    Radio.SetPacketParams( &PacketParams );
                    Radio.SetRfFrequency( RF_FREQUENCY );
                    
                    RngCalib     = RNG_CALIB_1600[ ( ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) - 5 ];
                    RngFeiFactor = ( double )RNG_FGRAD_1600[ ( ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) - 5 ];
                    RngReqDelay  = RNG_TIMER_MS >> ( 2 + 10 - ( ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) );
                    
                    CntPacketRxOK = 0;
                    CntPacketRxOKSlave = 0;
                    MeasuredChannels  = 0;
                    CurrentChannel    = 0;
                    
                    Buffer[0] = ( RngAddress >> 24 ) & 0xFF;
                    Buffer[1] = ( RngAddress >> 16 ) & 0xFF;
                    Buffer[2] = ( RngAddress >>  8 ) & 0xFF;
                    Buffer[3] = ( RngAddress & 0xFF );
                    Buffer[4] = CurrentChannel;    // set the first channel to use
                    Buffer[5] = 0x00;
                    Buffer[6] = RngRequestCount; // set the number of hops
                    
                    TxLed = 1;
                    
                    if( button == 0 ){
                    Radio.SendPayload( Buffer, PacketParams.Params.LoRa.PayloadLength, ( TickTime_t ){ RX_TIMEOUT_TICK_SIZE, RNG_COM_TIMEOUT } );
                    }
                    */
                    
                    AppState = APP_IDLE;
                    pressed(); 
                //printf("B0= %d,B1= %d,B2= %d,B3= %d,B4= %d,B5= %d,B6= %d",Buffer[0],Buffer[1],Buffer[2],Buffer[3],Buffer[4],Buffer[5],Buffer[6]);
                break;

            case APP_RNG:
            printf("App_Rng ");
                if( SendNext == true )
                {
                    SendNext = false;
                    MeasuredChannels++;
                    if( MeasuredChannels <= RngRequestCount )
                    {
                        Radio.SetRfFrequency( Channels[CurrentChannel] );
                        TxLed = 1;
                        CurrentChannel++;
                        if( CurrentChannel >= CHANNELS )
                        {
                            CurrentChannel -= CHANNELS;
                        }
                        AppState = APP_IDLE;
                        Radio.SetTx( ( TickTime_t ){ RADIO_TICK_SIZE_1000_US, 0xFFFF } );
                    }
                    else
                    {
                        CntPacketRxOKSlave = CheckDistance( );
                        SendNextPacket.detach( );
                        SendNext = false;
                        
                        if( Address == 0 )
                        {
                            Address = 1;
                            wait_ms( 100 );
                            pressed( );
                        }
                        else
                        {
                            Address = 0;
                            AppState = APP_IDLE;//APP_RANGING_CONFIG;
                        }
                    }
                }
                break;

            case APP_RANGING_DONE:
            printf("App_Ranging_Done ");
                TxLed = 0;
                RawRngResults[RngResultIndex++] = Radio.GetRangingResult( RANGING_RESULT_RAW );
                //printf( "\r\nResult : %f ", RawRngResults[RngResultIndex] );
                CntPacketRxOK++;
                AppState = APP_RNG;
                break;

            case APP_RANGING_TIMEOUT:
            //printf("App_Ranging_Timeout ");
                TxLed = 0;
                AppState = APP_RNG;
                break;

            case APP_RX:
            printf("App_RX ");
                RxLed = 0;
                if( RngStatus == RNG_INIT )
                {
                    Radio.GetPayload( Buffer, &BufferSize, BUFFER_SIZE );
                    if( BufferSize > 0 )
                    {
                        RxTimeOutCount = 0;
                        RngStatus = RNG_PROCESS;
                        RngFei = ( double )( ( ( int32_t )Buffer[4] << 24 ) | \
                                             ( ( int32_t )Buffer[5] << 16 ) | \
                                             ( ( int32_t )Buffer[6] <<  8 ) | \
                                                          Buffer[7] );
                                                          
                        RssiValue = Buffer[8]; // for ranging post-traitment (since V3 only)
                        
                        ModulationParams.PacketType = PACKET_TYPE_RANGING;
                        PacketParams.PacketType     = PACKET_TYPE_RANGING;

                        ModulationParams.Params.LoRa.SpreadingFactor = LORA_SF7;
                        ModulationParams.Params.LoRa.Bandwidth       = LORA_BW_1600;
                        ModulationParams.Params.LoRa.CodingRate      = LORA_CR_4_5;
                        
                        PacketParams.Params.LoRa.PreambleLength      = 0x08;
                        PacketParams.Params.LoRa.HeaderType          = LORA_PACKET_VARIABLE_LENGTH;
                        PacketParams.Params.LoRa.PayloadLength       = 10;
                        PacketParams.Params.LoRa.Crc                 = LORA_CRC_ON;
                        PacketParams.Params.LoRa.InvertIQ            = LORA_IQ_INVERTED;

                        RngCalib     = RNG_CALIB_1600[ ( ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) - 5 ];
                        RngFeiFactor = ( double )RNG_FGRAD_1600[ ( ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) - 5 ];
                        RngReqDelay  = RNG_TIMER_MS >> ( 2 + 10 - ( ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) );

                        Radio.SetPacketType( ModulationParams.PacketType );
                        Radio.SetModulationParams( &ModulationParams );
                        Radio.SetPacketParams( &PacketParams );
                        Radio.SetRangingRequestAddress( RngAddress );
                        Radio.SetRangingCalibration( RngCalib );
                        Radio.SetTxParams( TX_OUTPUT_POWER, RADIO_RAMP_20_US );

                        MeasuredChannels = 0;
                        RngResultIndex   = 0;
                        SendNextPacket.attach_us( &SendNextPacketEvent, RngReqDelay * 1000 );
                        AppState = APP_RNG;
                    }
                    else
                    {
                        AppState = APP_IDLE;//APP_RANGING_CONFIG;
                    }
                }
                else
                {
                    AppState = APP_IDLE;//APP_RANGING_CONFIG;
                }
                break;

            case APP_TX:
            printf("App_TX ");
                TxLed = 0;
                if( RngStatus == RNG_INIT )
                {
                    RxLed = 1;
                    Radio.SetRx( ( TickTime_t ) { RX_TIMEOUT_TICK_SIZE, RNG_COM_TIMEOUT } );
                    AppState = APP_IDLE;
                }
                else
                {
                    AppState = APP_IDLE;//APP_RANGING_CONFIG;
                }
                break;

            case APP_RX_TIMEOUT:
            //printf("App_RX_Timeout ");
                RxLed = 0;
                RngStatus = RNG_TIMEOUT;
                AppState = APP_IDLE;//APP_RANGING_CONFIG;
                break;

            case APP_RX_ERROR:
            //printf("App_RX_Error ");
                RxLed = 0;
                AppState = APP_RANGING_CONFIG;
                break;

            case APP_TX_TIMEOUT:
            //printf("App_TX_Timeout ");
                TxLed = 0;
                AppState = APP_RANGING_CONFIG;
                break;

            case APP_IDLE: // do nothing
                break;

            default:
                AppState = APP_RANGING_CONFIG;
                break;
        }
    }
}

// ************************     Interrupt Action    ****************************
void pressed( void ){                               // Send the next PING frame
   printf("App_Ranging_Config (Pressed)\r\n");
                    RngStatus = RNG_INIT;
                    CntPacketTx++;
                    
                    ModulationParams.PacketType = PACKET_TYPE_LORA;
                    PacketParams.PacketType     = PACKET_TYPE_LORA;
                    
                    ModulationParams.Params.LoRa.SpreadingFactor = LORA_SF7;
                    ModulationParams.Params.LoRa.Bandwidth       = LORA_BW_1600;
                    ModulationParams.Params.LoRa.CodingRate      = LORA_CR_4_5;
                    
                    PacketParams.Params.LoRa.PreambleLength      = 0x08;
                    PacketParams.Params.LoRa.HeaderType          = LORA_PACKET_VARIABLE_LENGTH;
                    PacketParams.Params.LoRa.PayloadLength       = 7;
                    PacketParams.Params.LoRa.Crc                 = LORA_CRC_ON;
                    PacketParams.Params.LoRa.InvertIQ            = LORA_IQ_INVERTED;
                    
                    Radio.SetPacketType( ModulationParams.PacketType );
                    Radio.SetModulationParams( &ModulationParams );
                    Radio.SetPacketParams( &PacketParams );
                    Radio.SetRfFrequency( RF_FREQUENCY );
                    
                    CntPacketRxOK = 0;
                    CntPacketRxOKSlave = 0;
                    MeasuredChannels  = 0;
                    CurrentChannel    = 0;
                    
                    if( Address == 0 )
                    {
                        Buffer[0] = ( RngAddress >> 24 ) & 0xFF;
                        Buffer[1] = ( RngAddress >> 16 ) & 0xFF;
                        Buffer[2] = ( RngAddress >>  8 ) & 0xFF;
                        Buffer[3] = ( RngAddress & 0xFF );
                    }
                    else if ( Address == 1 )
                    {
                        Buffer[0] = ( RngAddress1 >> 24 ) & 0xFF;
                        Buffer[1] = ( RngAddress1 >> 16 ) & 0xFF;
                        Buffer[2] = ( RngAddress1 >>  8 ) & 0xFF;
                        Buffer[3] = ( RngAddress1 & 0xFF );
                    }
                    
                    Buffer[4] = CurrentChannel;    // set the first channel to use
                    Buffer[5] = 0x00;
                    Buffer[6] = RngRequestCount; // set the number of hops
                    
                    TxLed = 1;
                    
                    Radio.SendPayload( Buffer, PacketParams.Params.LoRa.PayloadLength, ( TickTime_t ){ RX_TIMEOUT_TICK_SIZE, RNG_COM_TIMEOUT } );
                    AppState = APP_IDLE;     
}

/*!
 * \brief Callback of ticker PerSendNextPacket
 */
void SendNextPacketEvent( void )
{
    SendNext = true;
    if( RngStatus == RNG_PROCESS )
    {
        CntPacketRxKOSlave++;
    }
}

/*!
 * CheckDistance
 */
uint8_t CheckDistance( void ){
    uint16_t j = 0;
    uint16_t i;
    double rssi = RssiValue;
    double median;
    
    if( RngResultIndex > 0 ){
        for( i = 0; i < RngResultIndex; ++i ){
            RawRngResults[i] = RawRngResults[i] - ( RngFeiFactor * RngFei / 1000 );
            //printf("\r\nRawFei: %f ",RawRngResults[i]);
        }

        for (int i = RngResultIndex - 1; i > 0; --i){
            for (int j = 0; j < i; ++j){
                if (RawRngResults[j] > RawRngResults[j+1]){
                    int temp = RawRngResults[j];
                    RawRngResults[j] = RawRngResults[j+1];
                    RawRngResults[j+1] = temp;
                }
            }
        }
        
        if ((RngResultIndex % 2) == 0){
            median = (RawRngResults[RngResultIndex/2] + RawRngResults[(RngResultIndex/2) - 1])/2.0;
        } 
        else{
            median = RawRngResults[RngResultIndex/2];
        }
        
        if( median < 50 ){
            median = t0 + t1 * rssi + t2 * pow(rssi,2) + t3 * pow(rssi, 3) +t4 * median + t5 * pow(median,2) + t6 * pow(median, 3) + t7 * pow(median, 4) ;
            for( int i = 0; i < RngResultIndex; ++i ){
            // Apply the short range correction and RSSI short range improvement below 50 m
            RawRngResults[i] = t0 + t1 * rssi + t2 * pow(rssi,2) + t3 * pow(rssi, 3) +t4 * RawRngResults[i] + t5 * pow(RawRngResults[i],2) + t6 * pow(RawRngResults[i], 3) + t7 * pow(RawRngResults[i], 4) ;    
            printf( "\r\n%5.3f", RawRngResults[i] );
            }
        }
        
        if( j < RngRequestCount ){
            RngStatus = RNG_PER_ERROR;
        }
        else{
            RngStatus = RNG_VALID;
        }
    }
    /*for( int i = 0; i < RngResultIndex; ++i ){
        printf("%5.3f\r\n", RawRngResults[i]);
    }*/
    printf( "\r\n#id: %d, Rssi: %4.0f, Median: %5.1f, FEI: %f\r\n", CntPacketTx, rssi, median, RngFei );
    return j;
}  

// ************************     Radio Callbacks     ****************************
// *                                                                           *
// * These functions are called through function pointer by the Radio low      *
// * level drivers                                                             *
// *                                                                           *
// *****************************************************************************

void OnTxDone( void )
{
    AppState = APP_TX;
}

void OnRxDone( void )
{
    AppState = APP_RX;
}

void OnTxTimeout( void )
{
    AppState = APP_TX_TIMEOUT;
    printf( "<>>>>>>>>TXE\r\n" );
}

void OnRxTimeout( void )
{
    AppState = APP_RX_TIMEOUT;
}

void OnRxError( IrqErrorCode_t errorCode )
{
    AppState = APP_RX_ERROR;
    printf( "RXE<>>>>>>>>\r\n" );
}

void OnRangingDone( IrqRangingCode_t val )
{
    if( val == IRQ_RANGING_MASTER_VALID_CODE || val == IRQ_RANGING_SLAVE_VALID_CODE )
    {
        AppState = APP_RANGING_DONE;
    }
    else if( val == IRQ_RANGING_MASTER_ERROR_CODE || val == IRQ_RANGING_SLAVE_ERROR_CODE )
    {
        AppState = APP_RANGING_TIMEOUT;
    }
    else
    {
        AppState = APP_RANGING_TIMEOUT;
    }
}

void OnCadDone( bool channelActivityDetected )
{
}