Luis Ruiz
Slave program for ranging, open for improvements
Diff: main.cpp
- Revision:
- 0:871af6563674
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Apr 23 22:08:14 2018 +0000
@@ -0,0 +1,515 @@
+#include "mbed.h"
+#include "radio.h"
+#include "sx1280-hal.h"
+#include <math.h>
+#include "FreqLUT.h"
+#include "Timers.h"
+
+/*!
+ * \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 = 0x32100000;//0x10000000;
+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;
+double RawRngResults[255];
+double RssiRng[255];
+int RngResultIndex;
+
+/*!
+ * \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 );
+
+/*!
+ * \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 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 );
+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( "\nSlave\n\r" );
+
+ TimersInit( );
+
+ RngStatus = RNG_INIT;
+ Radio.SetDioIrqParams( IRQ_RADIO_ALL, IRQ_RADIO_ALL, IRQ_RADIO_NONE, IRQ_RADIO_NONE );
+
+ RxLed = 0;
+ TxLed = 0;
+
+ AppState = APP_RANGING_CONFIG;
+
+ while( 1 ){
+ switch( AppState )
+ {
+ case APP_RANGING_CONFIG:
+ //printf("App_Ranging_Config\r\n");
+ RngStatus = RNG_INIT;
+
+ 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 = 9;
+ 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 );
+
+ RxLed = 1;
+
+ // use listen mode here instead of rx continuous
+ Radio.SetRx( ( TickTime_t ) { RADIO_TICK_SIZE_1000_US, 0xFFFF } );
+ AppState = APP_IDLE;
+ break;
+
+ case APP_RNG:
+ //printf("App_Rng ");
+ if( SendNext == true )
+ {
+ SendNext = false;
+ MeasuredChannels++;
+ if( MeasuredChannels <= RngRequestCount )
+ {
+ Radio.SetRfFrequency( Channels[CurrentChannel] );
+ RxLed = 1;
+ CurrentChannel++;
+ if( CurrentChannel >= CHANNELS )
+ {
+ CurrentChannel -= CHANNELS;
+ }
+ AppState = APP_IDLE;
+ Radio.SetRx( ( TickTime_t ){ RADIO_TICK_SIZE_1000_US, RngReqDelay } );
+ }
+ else
+ {
+ Radio.SetStandby( STDBY_RC );
+ SendNextPacket.detach( );
+ RngStatus = RNG_VALID;
+ AppState = APP_RANGING_CONFIG;
+ }
+ }
+ break;
+
+ case APP_RANGING_DONE:
+ //printf("App_Ranging_Done ");
+ RxLed = 0;
+ CntPacketRxOK++;
+ AppState = APP_RNG;
+ break;
+
+ case APP_RANGING_TIMEOUT:
+ //printf("App_Ranging_Timeout ");
+ RxLed = 0;
+ AppState = APP_RNG;
+ break;
+
+ case APP_RX:
+ //printf("App_Rx ");
+ RxLed = 0;
+ if( RngStatus == RNG_INIT )
+ {
+ Radio.GetPayload( Buffer, &BufferSize, BUFFER_SIZE );
+ Radio.GetPacketStatus( &PacketStatus );
+ if( ( BufferSize > 0 ) && \
+ ( Buffer[0] == ( ( RngAddress >> 24 ) & 0xFF ) ) && \
+ ( Buffer[1] == ( ( RngAddress >> 16 ) & 0xFF ) ) && \
+ ( Buffer[2] == ( ( RngAddress >> 8 ) & 0xFF ) ) && \
+ ( Buffer[3] == ( RngAddress & 0xFF ) ) )
+ {
+ RngFei = Radio.GetFrequencyError( );
+ RssiValue = PacketStatus.LoRa.RssiPkt;
+ CntPacketTx++;
+
+ CurrentChannel = Buffer[4];
+ //RngAntenna = Buffer[5];
+ RngRequestCount = Buffer[6];
+ wait_us( 10 );
+
+ Buffer[4] = ( ( ( int32_t )RngFei ) >> 24 ) & 0xFF ;
+ Buffer[5] = ( ( ( int32_t )RngFei ) >> 16 ) & 0xFF ;
+ Buffer[6] = ( ( ( int32_t )RngFei ) >> 8 ) & 0xFF ;
+ Buffer[7] = ( ( ( int32_t )RngFei ) & 0xFF );
+ Buffer[8] = RssiValue;
+
+ TxLed = 1;
+ Radio.SendPayload( Buffer, 9, ( TickTime_t ){ RADIO_TICK_SIZE_1000_US, RNG_COM_TIMEOUT } );
+ AppState = APP_IDLE;
+ }
+ else
+ {
+ AppState = APP_RANGING_CONFIG;
+ }
+ }
+ else
+ {
+ AppState = APP_RANGING_CONFIG;
+ }
+ break;
+
+ case APP_TX:
+ //printf("App_Tx ");
+ TxLed = 0;
+ if( RngStatus == RNG_INIT )
+ {
+ RngStatus = RNG_PROCESS;
+
+ 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.SetDeviceRangingAddress( RngAddress );
+ Radio.SetRangingCalibration( RngCalib );
+ Radio.SetTxParams( TX_OUTPUT_POWER, RADIO_RAMP_20_US );
+
+ CntPacketRxOK = 0;
+ MeasuredChannels = 0;
+ CntPacketRxKOSlave = 0;
+
+ SendNextPacket.attach_us( &SendNextPacketEvent, RngReqDelay * 1000 );
+ AppState = APP_RNG;
+ }
+ else
+ {
+ AppState = APP_RANGING_CONFIG;
+ }
+ break;
+
+ case APP_RX_TIMEOUT:
+ //printf("App_Rx ");
+ RxLed = 0;
+ AppState = 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
+ if( CntPacketRxKOSlave > 255 )
+ {
+ CntPacketRxKOSlave = 0;
+ RxLed = 0;
+ AppState = APP_RANGING_CONFIG;
+ SendNextPacket.detach( );
+ }
+ break;
+
+ default:
+ AppState = APP_RANGING_CONFIG;
+ SendNextPacket.detach( );
+ break;
+ }
+ }
+}
+
+/*!
+ * \brief Callback of ticker PerSendNextPacket
+ */
+void SendNextPacketEvent( void )
+{
+ SendNext = true;
+ if( RngStatus == RNG_PROCESS )
+ {
+ CntPacketRxKOSlave++;
+ }
+}
+
+// ************************ 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