Luis Ruiz
/
RangignMaster
A base program for ranging, open to improvement
Revision 0:1ca9a50646e6, committed 2018-04-23
- Comitter:
- LuisRuiz
- Date:
- Mon Apr 23 18:42:08 2018 +0000
- Commit message:
- Ranging Master Role for SX1280
Changed in this revision
diff -r 000000000000 -r 1ca9a50646e6 FreqLUT.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FreqLUT.h Mon Apr 23 18:42:08 2018 +0000 @@ -0,0 +1,54 @@ +#define CHANNELS 40 + +/* + * Frequency look up table : + * To avoid Wifi channels, 40 Bluetooth channels are defined below (they already + * avoid Wifi common channels) : from 2402 MHz to 2480 MHz, step 2 MHz. + * User can define channel count for Ranging run, and it is optimized to have + * several frequencies in the largest band as possible. Also the 40 frequencies + * are generated by random sorting to preferate the 10 first in the largest band + * as possible (10 is the shortest channel count the user can choose). + */ +const uint32_t Channels[] = +{ + 2450000000, + 2402000000, + 2476000000, + 2436000000, + 2430000000, + 2468000000, + 2458000000, + 2416000000, + 2424000000, + 2478000000, + 2456000000, + 2448000000, + 2462000000, + 2472000000, + 2432000000, + 2446000000, + 2422000000, + 2442000000, + 2460000000, + 2474000000, + 2414000000, + 2464000000, + 2454000000, + 2444000000, + 2404000000, + 2434000000, + 2410000000, + 2408000000, + 2440000000, + 2452000000, + 2480000000, + 2426000000, + 2428000000, + 2466000000, + 2418000000, + 2412000000, + 2406000000, + 2470000000, + 2438000000, + 2420000000, +};
diff -r 000000000000 -r 1ca9a50646e6 SX1280Lib.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SX1280Lib.lib Mon Apr 23 18:42:08 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Semtech/code/SX1280Lib/#c1b368a5052f
diff -r 000000000000 -r 1ca9a50646e6 Timers/Timers.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Timers/Timers.cpp Mon Apr 23 18:42:08 2018 +0000 @@ -0,0 +1,56 @@ +/* + ______ _ + / _____) _ | | +( (____ _____ ____ _| |_ _____ ____| |__ + \____ \| ___ | (_ _) ___ |/ ___) _ \ + _____) ) ____| | | || |_| ____( (___| | | | +(______/|_____)_|_|_| \__)_____)\____)_| |_| + (C)2016 Semtech + +Description: Timers + +Maintainer: Gregory Cristian & Gilbert Menth +*/ + +#include "mbed.h" +#include "Timers.h" + + +Ticker TickTimer; + +static uint32_t SoftTimer = 0; +static void TimersIncSoftTimer( void ); + + +void TimersInit( void ) +{ + TickTimer.attach_us( &TimersIncSoftTimer, 1000 ); // Ticks every millisecond +} + +static void TimersIncSoftTimer( void ) +{ + SoftTimer++; +} + +void TimersSetTimer( uint32_t *sTimer, uint32_t timeLength ) +{ + if( timeLength > MAX_TIMER_VALUE ) + { + timeLength = MAX_TIMER_VALUE; + } + *sTimer = SoftTimer + timeLength; +} + +uint32_t TimersTimerHasExpired ( const uint32_t * sTimer ) +{ + if( ( SoftTimer - *sTimer ) > 0x7fffffff ) + { + return false; + } + return true; +} + +uint32_t TimersTimerValue ( void ) +{ + return SoftTimer; +}
diff -r 000000000000 -r 1ca9a50646e6 Timers/Timers.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Timers/Timers.h Mon Apr 23 18:42:08 2018 +0000 @@ -0,0 +1,58 @@ +/* + ______ _ + / _____) _ | | +( (____ _____ ____ _| |_ _____ ____| |__ + \____ \| ___ | (_ _) ___ |/ ___) _ \ + _____) ) ____| | | || |_| ____( (___| | | | +(______/|_____)_|_|_| \__)_____)\____)_| |_| + (C)2016 Semtech + +Description: Timers header + +Maintainer: Gregory Cristian & Gilbert Menth +*/ + +#ifndef TIMERS_H +#define TIMERS_H + + +#define TIM_MSEC ( uint32_t )1 +#define TIM_SEC ( uint32_t )1000 +#define TIM_MIN ( uint32_t )60000 +#define TIM_HOUR ( uint32_t )3600000 +#define MAX_TIMER_VALUE ( TIM_MIN * 150 ) // maximum time for timer + + + /*! + * \brief Initialses the hardware and variables associated with the timers. + */ +void TimersInit( void ); + + /*! + * \brief Sets a timer to a specific value + * + * \param [in] *STimer Pointer to the timer value to be set. + * \param [in] TimeLength Value to set the timer to in milliseconds. + */ +void TimersSetTimer( uint32_t *sTimer, uint32_t timeLength ); + + /*! + * \brief Checks if a timer has expired. + * + * \param [in] *STimer Pointer to the timer value to be read. + * + * \retval Status Non zero if the timer has not expired and is still + * running. + */ +uint32_t TimersTimerHasExpired ( const uint32_t * sTimer ); + + /*! + * \brief Returns the value of the current time in milliseconds + * + * \param [in] refresh Flag indicates refresh display required (touch) + * + * \retval Value value of current time in milliseconds + */ +uint32_t TimersTimerValue ( void ); + +#endif //TIMERS_H
diff -r 000000000000 -r 1ca9a50646e6 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Apr 23 18:42:08 2018 +0000 @@ -0,0 +1,660 @@ +#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 ) +{ +} \ No newline at end of file
diff -r 000000000000 -r 1ca9a50646e6 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Apr 23 18:42:08 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/994bdf8177cb \ No newline at end of file