Luis Ruiz
/
RangignMaster
A base program for ranging, open to improvement
main.cpp
- Committer:
- LuisRuiz
- Date:
- 2018-04-23
- Revision:
- 0:1ca9a50646e6
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 ) { }