Luis Ruiz / Mbed 2 deprecated RangignMaster

A base program for ranging, open to improvement

Dependencies:   SX1280Lib mbed

main.cpp@0:1ca9a50646e6, 2018-04-23 (annotated)

Committer:
LuisRuiz
Date:
Mon Apr 23 18:42:08 2018 +0000
Revision:
0:1ca9a50646e6
Ranging Master Role for SX1280

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LuisRuiz 0:1ca9a50646e6 1 #include "mbed.h"
LuisRuiz 0:1ca9a50646e6 2 #include "radio.h"
LuisRuiz 0:1ca9a50646e6 3 #include "sx1280-hal.h"
LuisRuiz 0:1ca9a50646e6 4 #include <math.h>
LuisRuiz 0:1ca9a50646e6 5 #include "FreqLUT.h"
LuisRuiz 0:1ca9a50646e6 6 #include "Timers.h"
LuisRuiz 0:1ca9a50646e6 7
LuisRuiz 0:1ca9a50646e6 8 double t0 = -0.016432807883697; // X0
LuisRuiz 0:1ca9a50646e6 9 double t1 = 0.323147003165358; // X1
LuisRuiz 0:1ca9a50646e6 10 double t2 = 0.014922061351196; // X1^2
LuisRuiz 0:1ca9a50646e6 11 double t3 = 0.000137832006285; // X1^3
LuisRuiz 0:1ca9a50646e6 12 double t4 = 0.536873856625399; // X2
LuisRuiz 0:1ca9a50646e6 13 double t5 = 0.040890089178579; // X2^2
LuisRuiz 0:1ca9a50646e6 14 double t6 = -0.001074801048732; // X2^3
LuisRuiz 0:1ca9a50646e6 15 double t7 = 0.000009240142234; // X2^4
LuisRuiz 0:1ca9a50646e6 16
LuisRuiz 0:1ca9a50646e6 17 /*!
LuisRuiz 0:1ca9a50646e6 18 * \brief Used to display firmware version on RS232
LuisRuiz 0:1ca9a50646e6 19 */
LuisRuiz 0:1ca9a50646e6 20 #define FIRMWARE_VERSION ( ( char* )"Firmware Version: 15.0.3" )
LuisRuiz 0:1ca9a50646e6 21
LuisRuiz 0:1ca9a50646e6 22 /*!
LuisRuiz 0:1ca9a50646e6 23 * Mode of operation
LuisRuiz 0:1ca9a50646e6 24 */
LuisRuiz 0:1ca9a50646e6 25 #define MODE_LORA
LuisRuiz 0:1ca9a50646e6 26
LuisRuiz 0:1ca9a50646e6 27 /*!
LuisRuiz 0:1ca9a50646e6 28 * \brief Ranging raw factors
LuisRuiz 0:1ca9a50646e6 29 * SF5 SF6 SF7 SF8 SF9 SF10
LuisRuiz 0:1ca9a50646e6 30 */
LuisRuiz 0:1ca9a50646e6 31 //const uint16_t RNG_CALIB_0400[] = { 10299, 10271, 10244, 10242, 10230, 10246 };
LuisRuiz 0:1ca9a50646e6 32 //const uint16_t RNG_CALIB_0800[] = { 11486, 11474, 11453, 11426, 11417, 11401 };
LuisRuiz 0:1ca9a50646e6 33 const uint16_t RNG_CALIB_1600[] = { 13308, 13493, 13528, 13515, 13430, 13376 };
LuisRuiz 0:1ca9a50646e6 34 //const double RNG_FGRAD_0400[] = { -0.148, -0.214, -0.419, -0.853, -1.686, -3.423 };
LuisRuiz 0:1ca9a50646e6 35 //const double RNG_FGRAD_0800[] = { -0.041, -0.811, -0.218, -0.429, -0.853, -1.737 };
LuisRuiz 0:1ca9a50646e6 36 const double RNG_FGRAD_1600[] = { 0.103, -0.041, -0.101, -0.211, -0.424, -0.87 };
LuisRuiz 0:1ca9a50646e6 37
LuisRuiz 0:1ca9a50646e6 38 /*!
LuisRuiz 0:1ca9a50646e6 39 * \brief Defines the nominal frequency
LuisRuiz 0:1ca9a50646e6 40 */
LuisRuiz 0:1ca9a50646e6 41 #define RF_FREQUENCY 2402000000UL // Hz
LuisRuiz 0:1ca9a50646e6 42
LuisRuiz 0:1ca9a50646e6 43 /*!
LuisRuiz 0:1ca9a50646e6 44 * \brief Defines the output power in dBm
LuisRuiz 0:1ca9a50646e6 45 *
LuisRuiz 0:1ca9a50646e6 46 * \remark The range of the output power is [-18..+13] dBm
LuisRuiz 0:1ca9a50646e6 47 */
LuisRuiz 0:1ca9a50646e6 48 #define TX_OUTPUT_POWER 13
LuisRuiz 0:1ca9a50646e6 49
LuisRuiz 0:1ca9a50646e6 50 uint32_t RngAddress = 0x10000000;
LuisRuiz 0:1ca9a50646e6 51 uint32_t RngAddress1 = 0x32100000;
LuisRuiz 0:1ca9a50646e6 52 uint32_t CntPacketTx = 0;
LuisRuiz 0:1ca9a50646e6 53 uint32_t CntPacketRxOK = 0;
LuisRuiz 0:1ca9a50646e6 54 uint32_t CntPacketRxOKSlave = 0;
LuisRuiz 0:1ca9a50646e6 55 uint32_t CntPacketRxKOSlave = 0;
LuisRuiz 0:1ca9a50646e6 56 uint32_t MeasuredChannels = 0;
LuisRuiz 0:1ca9a50646e6 57 uint32_t CurrentChannel = 0;
LuisRuiz 0:1ca9a50646e6 58 uint16_t RxTimeOutCount = 0;
LuisRuiz 0:1ca9a50646e6 59 uint16_t RngCalib;
LuisRuiz 0:1ca9a50646e6 60 uint16_t RngReqDelay;
LuisRuiz 0:1ca9a50646e6 61 uint16_t RngRequestCount = 10;
LuisRuiz 0:1ca9a50646e6 62 uint8_t RngStatus;
LuisRuiz 0:1ca9a50646e6 63 int8_t RssiValue = 0;
LuisRuiz 0:1ca9a50646e6 64 int8_t SnrValue = 0;
LuisRuiz 0:1ca9a50646e6 65 double RngDistance = 0.0;
LuisRuiz 0:1ca9a50646e6 66 double RngFeiFactor;
LuisRuiz 0:1ca9a50646e6 67 double RngFei = 0.0;
LuisRuiz 0:1ca9a50646e6 68 double RawRngResults[255];
LuisRuiz 0:1ca9a50646e6 69 double RssiRng[255];
LuisRuiz 0:1ca9a50646e6 70 int RngResultIndex;
LuisRuiz 0:1ca9a50646e6 71 int Address = 0;
LuisRuiz 0:1ca9a50646e6 72
LuisRuiz 0:1ca9a50646e6 73 /*!
LuisRuiz 0:1ca9a50646e6 74 * \brief Status of ranging distance
LuisRuiz 0:1ca9a50646e6 75 */
LuisRuiz 0:1ca9a50646e6 76 enum RangingStatus
LuisRuiz 0:1ca9a50646e6 77 {
LuisRuiz 0:1ca9a50646e6 78 RNG_INIT = 0,
LuisRuiz 0:1ca9a50646e6 79 RNG_PROCESS,
LuisRuiz 0:1ca9a50646e6 80 RNG_VALID,
LuisRuiz 0:1ca9a50646e6 81 RNG_TIMEOUT,
LuisRuiz 0:1ca9a50646e6 82 RNG_PER_ERROR
LuisRuiz 0:1ca9a50646e6 83 };
LuisRuiz 0:1ca9a50646e6 84
LuisRuiz 0:1ca9a50646e6 85 /*!
LuisRuiz 0:1ca9a50646e6 86 * \brief Defines the states of the application
LuisRuiz 0:1ca9a50646e6 87 */
LuisRuiz 0:1ca9a50646e6 88 typedef enum
LuisRuiz 0:1ca9a50646e6 89 {
LuisRuiz 0:1ca9a50646e6 90 APP_IDLE,
LuisRuiz 0:1ca9a50646e6 91 APP_RANGING_DONE,
LuisRuiz 0:1ca9a50646e6 92 APP_RANGING_TIMEOUT,
LuisRuiz 0:1ca9a50646e6 93 APP_RANGING_CONFIG,
LuisRuiz 0:1ca9a50646e6 94 APP_RNG,
LuisRuiz 0:1ca9a50646e6 95 APP_RX,
LuisRuiz 0:1ca9a50646e6 96 APP_RX_TIMEOUT,
LuisRuiz 0:1ca9a50646e6 97 APP_RX_ERROR,
LuisRuiz 0:1ca9a50646e6 98 APP_TX,
LuisRuiz 0:1ca9a50646e6 99 APP_TX_TIMEOUT,
LuisRuiz 0:1ca9a50646e6 100 }AppStates_t;
LuisRuiz 0:1ca9a50646e6 101
LuisRuiz 0:1ca9a50646e6 102 /*!
LuisRuiz 0:1ca9a50646e6 103 * \brief Defines the buffer size, i.e. the payload size
LuisRuiz 0:1ca9a50646e6 104 */
LuisRuiz 0:1ca9a50646e6 105 #define BUFFER_SIZE 255
LuisRuiz 0:1ca9a50646e6 106
LuisRuiz 0:1ca9a50646e6 107 /*!
LuisRuiz 0:1ca9a50646e6 108 * \brief The size of the buffer
LuisRuiz 0:1ca9a50646e6 109 */
LuisRuiz 0:1ca9a50646e6 110 uint8_t BufferSize = BUFFER_SIZE;
LuisRuiz 0:1ca9a50646e6 111
LuisRuiz 0:1ca9a50646e6 112 /*!
LuisRuiz 0:1ca9a50646e6 113 * \brief The buffer
LuisRuiz 0:1ca9a50646e6 114 */
LuisRuiz 0:1ca9a50646e6 115 uint8_t Buffer[BUFFER_SIZE];
LuisRuiz 0:1ca9a50646e6 116
LuisRuiz 0:1ca9a50646e6 117 /*!
LuisRuiz 0:1ca9a50646e6 118 * \brief The State of the application
LuisRuiz 0:1ca9a50646e6 119 */
LuisRuiz 0:1ca9a50646e6 120 AppStates_t AppState = APP_IDLE;
LuisRuiz 0:1ca9a50646e6 121
LuisRuiz 0:1ca9a50646e6 122 void SendNextPacketEvent( void );
LuisRuiz 0:1ca9a50646e6 123 uint8_t CheckDistance( void );
LuisRuiz 0:1ca9a50646e6 124
LuisRuiz 0:1ca9a50646e6 125 /*!
LuisRuiz 0:1ca9a50646e6 126 * \brief Function to be executed on Radio Tx Done event
LuisRuiz 0:1ca9a50646e6 127 */
LuisRuiz 0:1ca9a50646e6 128 void OnTxDone( void );
LuisRuiz 0:1ca9a50646e6 129
LuisRuiz 0:1ca9a50646e6 130 /*!
LuisRuiz 0:1ca9a50646e6 131 * \brief Function to be executed on Radio Rx Done event
LuisRuiz 0:1ca9a50646e6 132 */
LuisRuiz 0:1ca9a50646e6 133 void OnRxDone( void );
LuisRuiz 0:1ca9a50646e6 134
LuisRuiz 0:1ca9a50646e6 135 /*!
LuisRuiz 0:1ca9a50646e6 136 * \brief Function executed on Radio Tx Timeout event
LuisRuiz 0:1ca9a50646e6 137 */
LuisRuiz 0:1ca9a50646e6 138 void OnTxTimeout( void );
LuisRuiz 0:1ca9a50646e6 139
LuisRuiz 0:1ca9a50646e6 140 /*!
LuisRuiz 0:1ca9a50646e6 141 * \brief Function executed on Radio Rx Timeout event
LuisRuiz 0:1ca9a50646e6 142 */
LuisRuiz 0:1ca9a50646e6 143 void OnRxTimeout( void );
LuisRuiz 0:1ca9a50646e6 144
LuisRuiz 0:1ca9a50646e6 145 /*!
LuisRuiz 0:1ca9a50646e6 146 * \brief Function executed on Radio Rx Error event
LuisRuiz 0:1ca9a50646e6 147 */
LuisRuiz 0:1ca9a50646e6 148 void OnRxError( IrqErrorCode_t );
LuisRuiz 0:1ca9a50646e6 149
LuisRuiz 0:1ca9a50646e6 150 /*!
LuisRuiz 0:1ca9a50646e6 151 * \brief Function executed on Ranging Done
LuisRuiz 0:1ca9a50646e6 152 */
LuisRuiz 0:1ca9a50646e6 153 void OnRangingDone( IrqRangingCode_t val );
LuisRuiz 0:1ca9a50646e6 154
LuisRuiz 0:1ca9a50646e6 155 /*!
LuisRuiz 0:1ca9a50646e6 156 * \brief Function for Interrupt
LuisRuiz 0:1ca9a50646e6 157 */
LuisRuiz 0:1ca9a50646e6 158 void pressed( void );
LuisRuiz 0:1ca9a50646e6 159
LuisRuiz 0:1ca9a50646e6 160 /*!
LuisRuiz 0:1ca9a50646e6 161 * \brief All the callbacks are stored in a structure
LuisRuiz 0:1ca9a50646e6 162 */
LuisRuiz 0:1ca9a50646e6 163 RadioCallbacks_t callbacks =
LuisRuiz 0:1ca9a50646e6 164 {
LuisRuiz 0:1ca9a50646e6 165 &OnTxDone, // txDone
LuisRuiz 0:1ca9a50646e6 166 &OnRxDone, // rxDone
LuisRuiz 0:1ca9a50646e6 167 NULL, // syncWordDone
LuisRuiz 0:1ca9a50646e6 168 NULL, // headerDone
LuisRuiz 0:1ca9a50646e6 169 &OnTxTimeout, // txTimeout
LuisRuiz 0:1ca9a50646e6 170 &OnRxTimeout, // rxTimeout
LuisRuiz 0:1ca9a50646e6 171 &OnRxError, // rxError
LuisRuiz 0:1ca9a50646e6 172 &OnRangingDone, // rangingDone
LuisRuiz 0:1ca9a50646e6 173 NULL, // cadDone
LuisRuiz 0:1ca9a50646e6 174 };
LuisRuiz 0:1ca9a50646e6 175
LuisRuiz 0:1ca9a50646e6 176 // mosi, miso, sclk, nss, busy, dio1, dio2, dio3, rst, callbacks...
LuisRuiz 0:1ca9a50646e6 177 SX1280Hal Radio( D11, D12, D13, D7, D3, D5, NC, NC, A0, &callbacks );
LuisRuiz 0:1ca9a50646e6 178
LuisRuiz 0:1ca9a50646e6 179 //DigitalOut ANT_SW( A3 );
LuisRuiz 0:1ca9a50646e6 180 InterruptIn Button( USER_BUTTON ); // Interrupt Declaration
LuisRuiz 0:1ca9a50646e6 181 DigitalOut TxLed( A4 );
LuisRuiz 0:1ca9a50646e6 182 DigitalOut RxLed( A5 );
LuisRuiz 0:1ca9a50646e6 183
LuisRuiz 0:1ca9a50646e6 184
LuisRuiz 0:1ca9a50646e6 185 /*!
LuisRuiz 0:1ca9a50646e6 186 * \brief Define IO for Unused Pin
LuisRuiz 0:1ca9a50646e6 187 */
LuisRuiz 0:1ca9a50646e6 188 DigitalOut F_CS( D6 ); // MBED description of pin RX_EN
LuisRuiz 0:1ca9a50646e6 189 DigitalOut SD_CS( D8 ); // MBED description of pin TX_EN
LuisRuiz 0:1ca9a50646e6 190
LuisRuiz 0:1ca9a50646e6 191 /*!
LuisRuiz 0:1ca9a50646e6 192 * \brief Number of tick size steps for tx timeout
LuisRuiz 0:1ca9a50646e6 193 */
LuisRuiz 0:1ca9a50646e6 194 #define TX_TIMEOUT_VALUE 100 // ms
LuisRuiz 0:1ca9a50646e6 195
LuisRuiz 0:1ca9a50646e6 196 /*!
LuisRuiz 0:1ca9a50646e6 197 * \brief Number of tick size steps for rx timeout
LuisRuiz 0:1ca9a50646e6 198 */
LuisRuiz 0:1ca9a50646e6 199 #define RX_TIMEOUT_VALUE 100 // ms
LuisRuiz 0:1ca9a50646e6 200
LuisRuiz 0:1ca9a50646e6 201 /*!
LuisRuiz 0:1ca9a50646e6 202 * \brief Size of ticks (used for Tx and Rx timeout)
LuisRuiz 0:1ca9a50646e6 203 */
LuisRuiz 0:1ca9a50646e6 204 #define RX_TIMEOUT_TICK_SIZE RADIO_TICK_SIZE_1000_US
LuisRuiz 0:1ca9a50646e6 205
LuisRuiz 0:1ca9a50646e6 206 #define RNG_TIMER_MS 384 // ms
LuisRuiz 0:1ca9a50646e6 207 #define RNG_COM_TIMEOUT 100 // ms
LuisRuiz 0:1ca9a50646e6 208
LuisRuiz 0:1ca9a50646e6 209 /*!
LuisRuiz 0:1ca9a50646e6 210 * \brief Mask of IRQs to listen to in rx mode
LuisRuiz 0:1ca9a50646e6 211 */
LuisRuiz 0:1ca9a50646e6 212 uint16_t RxIrqMask = IRQ_RX_DONE | IRQ_RX_TX_TIMEOUT;
LuisRuiz 0:1ca9a50646e6 213
LuisRuiz 0:1ca9a50646e6 214 /*!
LuisRuiz 0:1ca9a50646e6 215 * \brief Mask of IRQs to listen to in tx mode
LuisRuiz 0:1ca9a50646e6 216 */
LuisRuiz 0:1ca9a50646e6 217 uint16_t TxIrqMask = IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT;
LuisRuiz 0:1ca9a50646e6 218
LuisRuiz 0:1ca9a50646e6 219 /*!
LuisRuiz 0:1ca9a50646e6 220 * \brief Locals parameters and status for radio API
LuisRuiz 0:1ca9a50646e6 221 * NEED TO BE OPTIMIZED, COPY OF STUCTURE ALREADY EXISTING
LuisRuiz 0:1ca9a50646e6 222 */
LuisRuiz 0:1ca9a50646e6 223 PacketParams_t PacketParams;
LuisRuiz 0:1ca9a50646e6 224 PacketStatus_t PacketStatus;
LuisRuiz 0:1ca9a50646e6 225 ModulationParams_t ModulationParams;
LuisRuiz 0:1ca9a50646e6 226
LuisRuiz 0:1ca9a50646e6 227 /*!
LuisRuiz 0:1ca9a50646e6 228 * \brief Ticker for master to synch Tx frames. Flags for PER and PingPong demo
LuisRuiz 0:1ca9a50646e6 229 * for Synch TX in cycle.
LuisRuiz 0:1ca9a50646e6 230 */
LuisRuiz 0:1ca9a50646e6 231 Ticker SendNextPacket;
LuisRuiz 0:1ca9a50646e6 232 static bool SendNext = false;
LuisRuiz 0:1ca9a50646e6 233
LuisRuiz 0:1ca9a50646e6 234 /*!
LuisRuiz 0:1ca9a50646e6 235 * \brief Specify serial datarate for UART debug output
LuisRuiz 0:1ca9a50646e6 236 */
LuisRuiz 0:1ca9a50646e6 237 void baud( int baudrate )
LuisRuiz 0:1ca9a50646e6 238 {
LuisRuiz 0:1ca9a50646e6 239 Serial s( USBTX, USBRX );
LuisRuiz 0:1ca9a50646e6 240
LuisRuiz 0:1ca9a50646e6 241 s.baud( baudrate );
LuisRuiz 0:1ca9a50646e6 242 }
LuisRuiz 0:1ca9a50646e6 243
LuisRuiz 0:1ca9a50646e6 244 int main( )
LuisRuiz 0:1ca9a50646e6 245 {
LuisRuiz 0:1ca9a50646e6 246 baud( 115200 );
LuisRuiz 0:1ca9a50646e6 247
LuisRuiz 0:1ca9a50646e6 248 F_CS = 1;
LuisRuiz 0:1ca9a50646e6 249 SD_CS = 1;
LuisRuiz 0:1ca9a50646e6 250 RxLed = 1;
LuisRuiz 0:1ca9a50646e6 251 TxLed = 1;
LuisRuiz 0:1ca9a50646e6 252
LuisRuiz 0:1ca9a50646e6 253 wait_ms( 500 ); // wait for on board DC/DC start-up time
LuisRuiz 0:1ca9a50646e6 254
LuisRuiz 0:1ca9a50646e6 255 Radio.Init( );
LuisRuiz 0:1ca9a50646e6 256 Radio.SetRegulatorMode( USE_DCDC ); // Can also be set in LDO mode but consume more power
LuisRuiz 0:1ca9a50646e6 257
LuisRuiz 0:1ca9a50646e6 258 memset( &Buffer, 0x00, BufferSize );
LuisRuiz 0:1ca9a50646e6 259
LuisRuiz 0:1ca9a50646e6 260 printf( "\n\n\r SX1280 Ranging Demo Application (%s)\n\n\r", FIRMWARE_VERSION );
LuisRuiz 0:1ca9a50646e6 261 printf( "\nRunning in LORA mode\n\r" );
LuisRuiz 0:1ca9a50646e6 262 printf( "\nMaster\n\r" );
LuisRuiz 0:1ca9a50646e6 263
LuisRuiz 0:1ca9a50646e6 264 TimersInit( );
LuisRuiz 0:1ca9a50646e6 265
LuisRuiz 0:1ca9a50646e6 266 RngStatus = RNG_INIT;
LuisRuiz 0:1ca9a50646e6 267 Radio.SetDioIrqParams( IRQ_RX_DONE | IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT | IRQ_RANGING_MASTER_RESULT_VALID | IRQ_RANGING_MASTER_TIMEOUT,
LuisRuiz 0:1ca9a50646e6 268 IRQ_RX_DONE | IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT | IRQ_RANGING_MASTER_RESULT_VALID | IRQ_RANGING_MASTER_TIMEOUT,
LuisRuiz 0:1ca9a50646e6 269 IRQ_RADIO_NONE, IRQ_RADIO_NONE );
LuisRuiz 0:1ca9a50646e6 270
LuisRuiz 0:1ca9a50646e6 271 RxLed = 0;
LuisRuiz 0:1ca9a50646e6 272 TxLed = 0;
LuisRuiz 0:1ca9a50646e6 273
LuisRuiz 0:1ca9a50646e6 274 AppState = APP_IDLE;//APP_RANGING_CONFIG;
LuisRuiz 0:1ca9a50646e6 275 Button.fall(&pressed);
LuisRuiz 0:1ca9a50646e6 276
LuisRuiz 0:1ca9a50646e6 277 while( 1 ){
LuisRuiz 0:1ca9a50646e6 278 switch( AppState )
LuisRuiz 0:1ca9a50646e6 279 {
LuisRuiz 0:1ca9a50646e6 280 case APP_RANGING_CONFIG:
LuisRuiz 0:1ca9a50646e6 281 //printf("App_Ranging_Config\r\n");
LuisRuiz 0:1ca9a50646e6 282 /*RngStatus = RNG_INIT;
LuisRuiz 0:1ca9a50646e6 283 CntPacketTx++;
LuisRuiz 0:1ca9a50646e6 284
LuisRuiz 0:1ca9a50646e6 285 ModulationParams.PacketType = PACKET_TYPE_LORA;
LuisRuiz 0:1ca9a50646e6 286 PacketParams.PacketType = PACKET_TYPE_LORA;
LuisRuiz 0:1ca9a50646e6 287
LuisRuiz 0:1ca9a50646e6 288 ModulationParams.Params.LoRa.SpreadingFactor = LORA_SF7;
LuisRuiz 0:1ca9a50646e6 289 ModulationParams.Params.LoRa.Bandwidth = LORA_BW_1600;
LuisRuiz 0:1ca9a50646e6 290 ModulationParams.Params.LoRa.CodingRate = LORA_CR_4_5;
LuisRuiz 0:1ca9a50646e6 291
LuisRuiz 0:1ca9a50646e6 292 PacketParams.Params.LoRa.PreambleLength = 0x08;
LuisRuiz 0:1ca9a50646e6 293 PacketParams.Params.LoRa.HeaderType = LORA_PACKET_VARIABLE_LENGTH;
LuisRuiz 0:1ca9a50646e6 294 PacketParams.Params.LoRa.PayloadLength = 7;
LuisRuiz 0:1ca9a50646e6 295 PacketParams.Params.LoRa.Crc = LORA_CRC_ON;
LuisRuiz 0:1ca9a50646e6 296 PacketParams.Params.LoRa.InvertIQ = LORA_IQ_INVERTED;
LuisRuiz 0:1ca9a50646e6 297
LuisRuiz 0:1ca9a50646e6 298 Radio.SetPacketType( ModulationParams.PacketType );
LuisRuiz 0:1ca9a50646e6 299 Radio.SetModulationParams( &ModulationParams );
LuisRuiz 0:1ca9a50646e6 300 Radio.SetPacketParams( &PacketParams );
LuisRuiz 0:1ca9a50646e6 301 Radio.SetRfFrequency( RF_FREQUENCY );
LuisRuiz 0:1ca9a50646e6 302
LuisRuiz 0:1ca9a50646e6 303 RngCalib = RNG_CALIB_1600[ ( ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) - 5 ];
LuisRuiz 0:1ca9a50646e6 304 RngFeiFactor = ( double )RNG_FGRAD_1600[ ( ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) - 5 ];
LuisRuiz 0:1ca9a50646e6 305 RngReqDelay = RNG_TIMER_MS >> ( 2 + 10 - ( ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) );
LuisRuiz 0:1ca9a50646e6 306
LuisRuiz 0:1ca9a50646e6 307 CntPacketRxOK = 0;
LuisRuiz 0:1ca9a50646e6 308 CntPacketRxOKSlave = 0;
LuisRuiz 0:1ca9a50646e6 309 MeasuredChannels = 0;
LuisRuiz 0:1ca9a50646e6 310 CurrentChannel = 0;
LuisRuiz 0:1ca9a50646e6 311
LuisRuiz 0:1ca9a50646e6 312 Buffer[0] = ( RngAddress >> 24 ) & 0xFF;
LuisRuiz 0:1ca9a50646e6 313 Buffer[1] = ( RngAddress >> 16 ) & 0xFF;
LuisRuiz 0:1ca9a50646e6 314 Buffer[2] = ( RngAddress >> 8 ) & 0xFF;
LuisRuiz 0:1ca9a50646e6 315 Buffer[3] = ( RngAddress & 0xFF );
LuisRuiz 0:1ca9a50646e6 316 Buffer[4] = CurrentChannel; // set the first channel to use
LuisRuiz 0:1ca9a50646e6 317 Buffer[5] = 0x00;
LuisRuiz 0:1ca9a50646e6 318 Buffer[6] = RngRequestCount; // set the number of hops
LuisRuiz 0:1ca9a50646e6 319
LuisRuiz 0:1ca9a50646e6 320 TxLed = 1;
LuisRuiz 0:1ca9a50646e6 321
LuisRuiz 0:1ca9a50646e6 322 if( button == 0 ){
LuisRuiz 0:1ca9a50646e6 323 Radio.SendPayload( Buffer, PacketParams.Params.LoRa.PayloadLength, ( TickTime_t ){ RX_TIMEOUT_TICK_SIZE, RNG_COM_TIMEOUT } );
LuisRuiz 0:1ca9a50646e6 324 }
LuisRuiz 0:1ca9a50646e6 325 */
LuisRuiz 0:1ca9a50646e6 326
LuisRuiz 0:1ca9a50646e6 327 AppState = APP_IDLE;
LuisRuiz 0:1ca9a50646e6 328 pressed();
LuisRuiz 0:1ca9a50646e6 329 //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]);
LuisRuiz 0:1ca9a50646e6 330 break;
LuisRuiz 0:1ca9a50646e6 331
LuisRuiz 0:1ca9a50646e6 332 case APP_RNG:
LuisRuiz 0:1ca9a50646e6 333 printf("App_Rng ");
LuisRuiz 0:1ca9a50646e6 334 if( SendNext == true )
LuisRuiz 0:1ca9a50646e6 335 {
LuisRuiz 0:1ca9a50646e6 336 SendNext = false;
LuisRuiz 0:1ca9a50646e6 337 MeasuredChannels++;
LuisRuiz 0:1ca9a50646e6 338 if( MeasuredChannels <= RngRequestCount )
LuisRuiz 0:1ca9a50646e6 339 {
LuisRuiz 0:1ca9a50646e6 340 Radio.SetRfFrequency( Channels[CurrentChannel] );
LuisRuiz 0:1ca9a50646e6 341 TxLed = 1;
LuisRuiz 0:1ca9a50646e6 342 CurrentChannel++;
LuisRuiz 0:1ca9a50646e6 343 if( CurrentChannel >= CHANNELS )
LuisRuiz 0:1ca9a50646e6 344 {
LuisRuiz 0:1ca9a50646e6 345 CurrentChannel -= CHANNELS;
LuisRuiz 0:1ca9a50646e6 346 }
LuisRuiz 0:1ca9a50646e6 347 AppState = APP_IDLE;
LuisRuiz 0:1ca9a50646e6 348 Radio.SetTx( ( TickTime_t ){ RADIO_TICK_SIZE_1000_US, 0xFFFF } );
LuisRuiz 0:1ca9a50646e6 349 }
LuisRuiz 0:1ca9a50646e6 350 else
LuisRuiz 0:1ca9a50646e6 351 {
LuisRuiz 0:1ca9a50646e6 352 CntPacketRxOKSlave = CheckDistance( );
LuisRuiz 0:1ca9a50646e6 353 SendNextPacket.detach( );
LuisRuiz 0:1ca9a50646e6 354 SendNext = false;
LuisRuiz 0:1ca9a50646e6 355
LuisRuiz 0:1ca9a50646e6 356 if( Address == 0 )
LuisRuiz 0:1ca9a50646e6 357 {
LuisRuiz 0:1ca9a50646e6 358 Address = 1;
LuisRuiz 0:1ca9a50646e6 359 wait_ms( 100 );
LuisRuiz 0:1ca9a50646e6 360 pressed( );
LuisRuiz 0:1ca9a50646e6 361 }
LuisRuiz 0:1ca9a50646e6 362 else
LuisRuiz 0:1ca9a50646e6 363 {
LuisRuiz 0:1ca9a50646e6 364 Address = 0;
LuisRuiz 0:1ca9a50646e6 365 AppState = APP_IDLE;//APP_RANGING_CONFIG;
LuisRuiz 0:1ca9a50646e6 366 }
LuisRuiz 0:1ca9a50646e6 367 }
LuisRuiz 0:1ca9a50646e6 368 }
LuisRuiz 0:1ca9a50646e6 369 break;
LuisRuiz 0:1ca9a50646e6 370
LuisRuiz 0:1ca9a50646e6 371 case APP_RANGING_DONE:
LuisRuiz 0:1ca9a50646e6 372 printf("App_Ranging_Done ");
LuisRuiz 0:1ca9a50646e6 373 TxLed = 0;
LuisRuiz 0:1ca9a50646e6 374 RawRngResults[RngResultIndex++] = Radio.GetRangingResult( RANGING_RESULT_RAW );
LuisRuiz 0:1ca9a50646e6 375 //printf( "\r\nResult : %f ", RawRngResults[RngResultIndex] );
LuisRuiz 0:1ca9a50646e6 376 CntPacketRxOK++;
LuisRuiz 0:1ca9a50646e6 377 AppState = APP_RNG;
LuisRuiz 0:1ca9a50646e6 378 break;
LuisRuiz 0:1ca9a50646e6 379
LuisRuiz 0:1ca9a50646e6 380 case APP_RANGING_TIMEOUT:
LuisRuiz 0:1ca9a50646e6 381 //printf("App_Ranging_Timeout ");
LuisRuiz 0:1ca9a50646e6 382 TxLed = 0;
LuisRuiz 0:1ca9a50646e6 383 AppState = APP_RNG;
LuisRuiz 0:1ca9a50646e6 384 break;
LuisRuiz 0:1ca9a50646e6 385
LuisRuiz 0:1ca9a50646e6 386 case APP_RX:
LuisRuiz 0:1ca9a50646e6 387 printf("App_RX ");
LuisRuiz 0:1ca9a50646e6 388 RxLed = 0;
LuisRuiz 0:1ca9a50646e6 389 if( RngStatus == RNG_INIT )
LuisRuiz 0:1ca9a50646e6 390 {
LuisRuiz 0:1ca9a50646e6 391 Radio.GetPayload( Buffer, &BufferSize, BUFFER_SIZE );
LuisRuiz 0:1ca9a50646e6 392 if( BufferSize > 0 )
LuisRuiz 0:1ca9a50646e6 393 {
LuisRuiz 0:1ca9a50646e6 394 RxTimeOutCount = 0;
LuisRuiz 0:1ca9a50646e6 395 RngStatus = RNG_PROCESS;
LuisRuiz 0:1ca9a50646e6 396 RngFei = ( double )( ( ( int32_t )Buffer[4] << 24 ) | \
LuisRuiz 0:1ca9a50646e6 397 ( ( int32_t )Buffer[5] << 16 ) | \
LuisRuiz 0:1ca9a50646e6 398 ( ( int32_t )Buffer[6] << 8 ) | \
LuisRuiz 0:1ca9a50646e6 399 Buffer[7] );
LuisRuiz 0:1ca9a50646e6 400
LuisRuiz 0:1ca9a50646e6 401 RssiValue = Buffer[8]; // for ranging post-traitment (since V3 only)
LuisRuiz 0:1ca9a50646e6 402
LuisRuiz 0:1ca9a50646e6 403 ModulationParams.PacketType = PACKET_TYPE_RANGING;
LuisRuiz 0:1ca9a50646e6 404 PacketParams.PacketType = PACKET_TYPE_RANGING;
LuisRuiz 0:1ca9a50646e6 405
LuisRuiz 0:1ca9a50646e6 406 ModulationParams.Params.LoRa.SpreadingFactor = LORA_SF7;
LuisRuiz 0:1ca9a50646e6 407 ModulationParams.Params.LoRa.Bandwidth = LORA_BW_1600;
LuisRuiz 0:1ca9a50646e6 408 ModulationParams.Params.LoRa.CodingRate = LORA_CR_4_5;
LuisRuiz 0:1ca9a50646e6 409
LuisRuiz 0:1ca9a50646e6 410 PacketParams.Params.LoRa.PreambleLength = 0x08;
LuisRuiz 0:1ca9a50646e6 411 PacketParams.Params.LoRa.HeaderType = LORA_PACKET_VARIABLE_LENGTH;
LuisRuiz 0:1ca9a50646e6 412 PacketParams.Params.LoRa.PayloadLength = 10;
LuisRuiz 0:1ca9a50646e6 413 PacketParams.Params.LoRa.Crc = LORA_CRC_ON;
LuisRuiz 0:1ca9a50646e6 414 PacketParams.Params.LoRa.InvertIQ = LORA_IQ_INVERTED;
LuisRuiz 0:1ca9a50646e6 415
LuisRuiz 0:1ca9a50646e6 416 RngCalib = RNG_CALIB_1600[ ( ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) - 5 ];
LuisRuiz 0:1ca9a50646e6 417 RngFeiFactor = ( double )RNG_FGRAD_1600[ ( ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) - 5 ];
LuisRuiz 0:1ca9a50646e6 418 RngReqDelay = RNG_TIMER_MS >> ( 2 + 10 - ( ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) );
LuisRuiz 0:1ca9a50646e6 419
LuisRuiz 0:1ca9a50646e6 420 Radio.SetPacketType( ModulationParams.PacketType );
LuisRuiz 0:1ca9a50646e6 421 Radio.SetModulationParams( &ModulationParams );
LuisRuiz 0:1ca9a50646e6 422 Radio.SetPacketParams( &PacketParams );
LuisRuiz 0:1ca9a50646e6 423 Radio.SetRangingRequestAddress( RngAddress );
LuisRuiz 0:1ca9a50646e6 424 Radio.SetRangingCalibration( RngCalib );
LuisRuiz 0:1ca9a50646e6 425 Radio.SetTxParams( TX_OUTPUT_POWER, RADIO_RAMP_20_US );
LuisRuiz 0:1ca9a50646e6 426
LuisRuiz 0:1ca9a50646e6 427 MeasuredChannels = 0;
LuisRuiz 0:1ca9a50646e6 428 RngResultIndex = 0;
LuisRuiz 0:1ca9a50646e6 429 SendNextPacket.attach_us( &SendNextPacketEvent, RngReqDelay * 1000 );
LuisRuiz 0:1ca9a50646e6 430 AppState = APP_RNG;
LuisRuiz 0:1ca9a50646e6 431 }
LuisRuiz 0:1ca9a50646e6 432 else
LuisRuiz 0:1ca9a50646e6 433 {
LuisRuiz 0:1ca9a50646e6 434 AppState = APP_IDLE;//APP_RANGING_CONFIG;
LuisRuiz 0:1ca9a50646e6 435 }
LuisRuiz 0:1ca9a50646e6 436 }
LuisRuiz 0:1ca9a50646e6 437 else
LuisRuiz 0:1ca9a50646e6 438 {
LuisRuiz 0:1ca9a50646e6 439 AppState = APP_IDLE;//APP_RANGING_CONFIG;
LuisRuiz 0:1ca9a50646e6 440 }
LuisRuiz 0:1ca9a50646e6 441 break;
LuisRuiz 0:1ca9a50646e6 442
LuisRuiz 0:1ca9a50646e6 443 case APP_TX:
LuisRuiz 0:1ca9a50646e6 444 printf("App_TX ");
LuisRuiz 0:1ca9a50646e6 445 TxLed = 0;
LuisRuiz 0:1ca9a50646e6 446 if( RngStatus == RNG_INIT )
LuisRuiz 0:1ca9a50646e6 447 {
LuisRuiz 0:1ca9a50646e6 448 RxLed = 1;
LuisRuiz 0:1ca9a50646e6 449 Radio.SetRx( ( TickTime_t ) { RX_TIMEOUT_TICK_SIZE, RNG_COM_TIMEOUT } );
LuisRuiz 0:1ca9a50646e6 450 AppState = APP_IDLE;
LuisRuiz 0:1ca9a50646e6 451 }
LuisRuiz 0:1ca9a50646e6 452 else
LuisRuiz 0:1ca9a50646e6 453 {
LuisRuiz 0:1ca9a50646e6 454 AppState = APP_IDLE;//APP_RANGING_CONFIG;
LuisRuiz 0:1ca9a50646e6 455 }
LuisRuiz 0:1ca9a50646e6 456 break;
LuisRuiz 0:1ca9a50646e6 457
LuisRuiz 0:1ca9a50646e6 458 case APP_RX_TIMEOUT:
LuisRuiz 0:1ca9a50646e6 459 //printf("App_RX_Timeout ");
LuisRuiz 0:1ca9a50646e6 460 RxLed = 0;
LuisRuiz 0:1ca9a50646e6 461 RngStatus = RNG_TIMEOUT;
LuisRuiz 0:1ca9a50646e6 462 AppState = APP_IDLE;//APP_RANGING_CONFIG;
LuisRuiz 0:1ca9a50646e6 463 break;
LuisRuiz 0:1ca9a50646e6 464
LuisRuiz 0:1ca9a50646e6 465 case APP_RX_ERROR:
LuisRuiz 0:1ca9a50646e6 466 //printf("App_RX_Error ");
LuisRuiz 0:1ca9a50646e6 467 RxLed = 0;
LuisRuiz 0:1ca9a50646e6 468 AppState = APP_RANGING_CONFIG;
LuisRuiz 0:1ca9a50646e6 469 break;
LuisRuiz 0:1ca9a50646e6 470
LuisRuiz 0:1ca9a50646e6 471 case APP_TX_TIMEOUT:
LuisRuiz 0:1ca9a50646e6 472 //printf("App_TX_Timeout ");
LuisRuiz 0:1ca9a50646e6 473 TxLed = 0;
LuisRuiz 0:1ca9a50646e6 474 AppState = APP_RANGING_CONFIG;
LuisRuiz 0:1ca9a50646e6 475 break;
LuisRuiz 0:1ca9a50646e6 476
LuisRuiz 0:1ca9a50646e6 477 case APP_IDLE: // do nothing
LuisRuiz 0:1ca9a50646e6 478 break;
LuisRuiz 0:1ca9a50646e6 479
LuisRuiz 0:1ca9a50646e6 480 default:
LuisRuiz 0:1ca9a50646e6 481 AppState = APP_RANGING_CONFIG;
LuisRuiz 0:1ca9a50646e6 482 break;
LuisRuiz 0:1ca9a50646e6 483 }
LuisRuiz 0:1ca9a50646e6 484 }
LuisRuiz 0:1ca9a50646e6 485 }
LuisRuiz 0:1ca9a50646e6 486
LuisRuiz 0:1ca9a50646e6 487 // ************************ Interrupt Action ****************************
LuisRuiz 0:1ca9a50646e6 488 void pressed( void ){ // Send the next PING frame
LuisRuiz 0:1ca9a50646e6 489 printf("App_Ranging_Config (Pressed)\r\n");
LuisRuiz 0:1ca9a50646e6 490 RngStatus = RNG_INIT;
LuisRuiz 0:1ca9a50646e6 491 CntPacketTx++;
LuisRuiz 0:1ca9a50646e6 492
LuisRuiz 0:1ca9a50646e6 493 ModulationParams.PacketType = PACKET_TYPE_LORA;
LuisRuiz 0:1ca9a50646e6 494 PacketParams.PacketType = PACKET_TYPE_LORA;
LuisRuiz 0:1ca9a50646e6 495
LuisRuiz 0:1ca9a50646e6 496 ModulationParams.Params.LoRa.SpreadingFactor = LORA_SF7;
LuisRuiz 0:1ca9a50646e6 497 ModulationParams.Params.LoRa.Bandwidth = LORA_BW_1600;
LuisRuiz 0:1ca9a50646e6 498 ModulationParams.Params.LoRa.CodingRate = LORA_CR_4_5;
LuisRuiz 0:1ca9a50646e6 499
LuisRuiz 0:1ca9a50646e6 500 PacketParams.Params.LoRa.PreambleLength = 0x08;
LuisRuiz 0:1ca9a50646e6 501 PacketParams.Params.LoRa.HeaderType = LORA_PACKET_VARIABLE_LENGTH;
LuisRuiz 0:1ca9a50646e6 502 PacketParams.Params.LoRa.PayloadLength = 7;
LuisRuiz 0:1ca9a50646e6 503 PacketParams.Params.LoRa.Crc = LORA_CRC_ON;
LuisRuiz 0:1ca9a50646e6 504 PacketParams.Params.LoRa.InvertIQ = LORA_IQ_INVERTED;
LuisRuiz 0:1ca9a50646e6 505
LuisRuiz 0:1ca9a50646e6 506 Radio.SetPacketType( ModulationParams.PacketType );
LuisRuiz 0:1ca9a50646e6 507 Radio.SetModulationParams( &ModulationParams );
LuisRuiz 0:1ca9a50646e6 508 Radio.SetPacketParams( &PacketParams );
LuisRuiz 0:1ca9a50646e6 509 Radio.SetRfFrequency( RF_FREQUENCY );
LuisRuiz 0:1ca9a50646e6 510
LuisRuiz 0:1ca9a50646e6 511 CntPacketRxOK = 0;
LuisRuiz 0:1ca9a50646e6 512 CntPacketRxOKSlave = 0;
LuisRuiz 0:1ca9a50646e6 513 MeasuredChannels = 0;
LuisRuiz 0:1ca9a50646e6 514 CurrentChannel = 0;
LuisRuiz 0:1ca9a50646e6 515
LuisRuiz 0:1ca9a50646e6 516 if( Address == 0 )
LuisRuiz 0:1ca9a50646e6 517 {
LuisRuiz 0:1ca9a50646e6 518 Buffer[0] = ( RngAddress >> 24 ) & 0xFF;
LuisRuiz 0:1ca9a50646e6 519 Buffer[1] = ( RngAddress >> 16 ) & 0xFF;
LuisRuiz 0:1ca9a50646e6 520 Buffer[2] = ( RngAddress >> 8 ) & 0xFF;
LuisRuiz 0:1ca9a50646e6 521 Buffer[3] = ( RngAddress & 0xFF );
LuisRuiz 0:1ca9a50646e6 522 }
LuisRuiz 0:1ca9a50646e6 523 else if ( Address == 1 )
LuisRuiz 0:1ca9a50646e6 524 {
LuisRuiz 0:1ca9a50646e6 525 Buffer[0] = ( RngAddress1 >> 24 ) & 0xFF;
LuisRuiz 0:1ca9a50646e6 526 Buffer[1] = ( RngAddress1 >> 16 ) & 0xFF;
LuisRuiz 0:1ca9a50646e6 527 Buffer[2] = ( RngAddress1 >> 8 ) & 0xFF;
LuisRuiz 0:1ca9a50646e6 528 Buffer[3] = ( RngAddress1 & 0xFF );
LuisRuiz 0:1ca9a50646e6 529 }
LuisRuiz 0:1ca9a50646e6 530
LuisRuiz 0:1ca9a50646e6 531 Buffer[4] = CurrentChannel; // set the first channel to use
LuisRuiz 0:1ca9a50646e6 532 Buffer[5] = 0x00;
LuisRuiz 0:1ca9a50646e6 533 Buffer[6] = RngRequestCount; // set the number of hops
LuisRuiz 0:1ca9a50646e6 534
LuisRuiz 0:1ca9a50646e6 535 TxLed = 1;
LuisRuiz 0:1ca9a50646e6 536
LuisRuiz 0:1ca9a50646e6 537 Radio.SendPayload( Buffer, PacketParams.Params.LoRa.PayloadLength, ( TickTime_t ){ RX_TIMEOUT_TICK_SIZE, RNG_COM_TIMEOUT } );
LuisRuiz 0:1ca9a50646e6 538 AppState = APP_IDLE;
LuisRuiz 0:1ca9a50646e6 539 }
LuisRuiz 0:1ca9a50646e6 540
LuisRuiz 0:1ca9a50646e6 541 /*!
LuisRuiz 0:1ca9a50646e6 542 * \brief Callback of ticker PerSendNextPacket
LuisRuiz 0:1ca9a50646e6 543 */
LuisRuiz 0:1ca9a50646e6 544 void SendNextPacketEvent( void )
LuisRuiz 0:1ca9a50646e6 545 {
LuisRuiz 0:1ca9a50646e6 546 SendNext = true;
LuisRuiz 0:1ca9a50646e6 547 if( RngStatus == RNG_PROCESS )
LuisRuiz 0:1ca9a50646e6 548 {
LuisRuiz 0:1ca9a50646e6 549 CntPacketRxKOSlave++;
LuisRuiz 0:1ca9a50646e6 550 }
LuisRuiz 0:1ca9a50646e6 551 }
LuisRuiz 0:1ca9a50646e6 552
LuisRuiz 0:1ca9a50646e6 553 /*!
LuisRuiz 0:1ca9a50646e6 554 * CheckDistance
LuisRuiz 0:1ca9a50646e6 555 */
LuisRuiz 0:1ca9a50646e6 556 uint8_t CheckDistance( void ){
LuisRuiz 0:1ca9a50646e6 557 uint16_t j = 0;
LuisRuiz 0:1ca9a50646e6 558 uint16_t i;
LuisRuiz 0:1ca9a50646e6 559 double rssi = RssiValue;
LuisRuiz 0:1ca9a50646e6 560 double median;
LuisRuiz 0:1ca9a50646e6 561
LuisRuiz 0:1ca9a50646e6 562 if( RngResultIndex > 0 ){
LuisRuiz 0:1ca9a50646e6 563 for( i = 0; i < RngResultIndex; ++i ){
LuisRuiz 0:1ca9a50646e6 564 RawRngResults[i] = RawRngResults[i] - ( RngFeiFactor * RngFei / 1000 );
LuisRuiz 0:1ca9a50646e6 565 //printf("\r\nRawFei: %f ",RawRngResults[i]);
LuisRuiz 0:1ca9a50646e6 566 }
LuisRuiz 0:1ca9a50646e6 567
LuisRuiz 0:1ca9a50646e6 568 for (int i = RngResultIndex - 1; i > 0; --i){
LuisRuiz 0:1ca9a50646e6 569 for (int j = 0; j < i; ++j){
LuisRuiz 0:1ca9a50646e6 570 if (RawRngResults[j] > RawRngResults[j+1]){
LuisRuiz 0:1ca9a50646e6 571 int temp = RawRngResults[j];
LuisRuiz 0:1ca9a50646e6 572 RawRngResults[j] = RawRngResults[j+1];
LuisRuiz 0:1ca9a50646e6 573 RawRngResults[j+1] = temp;
LuisRuiz 0:1ca9a50646e6 574 }
LuisRuiz 0:1ca9a50646e6 575 }
LuisRuiz 0:1ca9a50646e6 576 }
LuisRuiz 0:1ca9a50646e6 577
LuisRuiz 0:1ca9a50646e6 578 if ((RngResultIndex % 2) == 0){
LuisRuiz 0:1ca9a50646e6 579 median = (RawRngResults[RngResultIndex/2] + RawRngResults[(RngResultIndex/2) - 1])/2.0;
LuisRuiz 0:1ca9a50646e6 580 }
LuisRuiz 0:1ca9a50646e6 581 else{
LuisRuiz 0:1ca9a50646e6 582 median = RawRngResults[RngResultIndex/2];
LuisRuiz 0:1ca9a50646e6 583 }
LuisRuiz 0:1ca9a50646e6 584
LuisRuiz 0:1ca9a50646e6 585 if( median < 50 ){
LuisRuiz 0:1ca9a50646e6 586 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) ;
LuisRuiz 0:1ca9a50646e6 587 for( int i = 0; i < RngResultIndex; ++i ){
LuisRuiz 0:1ca9a50646e6 588 // Apply the short range correction and RSSI short range improvement below 50 m
LuisRuiz 0:1ca9a50646e6 589 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) ;
LuisRuiz 0:1ca9a50646e6 590 printf( "\r\n%5.3f", RawRngResults[i] );
LuisRuiz 0:1ca9a50646e6 591 }
LuisRuiz 0:1ca9a50646e6 592 }
LuisRuiz 0:1ca9a50646e6 593
LuisRuiz 0:1ca9a50646e6 594 if( j < RngRequestCount ){
LuisRuiz 0:1ca9a50646e6 595 RngStatus = RNG_PER_ERROR;
LuisRuiz 0:1ca9a50646e6 596 }
LuisRuiz 0:1ca9a50646e6 597 else{
LuisRuiz 0:1ca9a50646e6 598 RngStatus = RNG_VALID;
LuisRuiz 0:1ca9a50646e6 599 }
LuisRuiz 0:1ca9a50646e6 600 }
LuisRuiz 0:1ca9a50646e6 601 /*for( int i = 0; i < RngResultIndex; ++i ){
LuisRuiz 0:1ca9a50646e6 602 printf("%5.3f\r\n", RawRngResults[i]);
LuisRuiz 0:1ca9a50646e6 603 }*/
LuisRuiz 0:1ca9a50646e6 604 printf( "\r\n#id: %d, Rssi: %4.0f, Median: %5.1f, FEI: %f\r\n", CntPacketTx, rssi, median, RngFei );
LuisRuiz 0:1ca9a50646e6 605 return j;
LuisRuiz 0:1ca9a50646e6 606 }
LuisRuiz 0:1ca9a50646e6 607
LuisRuiz 0:1ca9a50646e6 608 // ************************ Radio Callbacks ****************************
LuisRuiz 0:1ca9a50646e6 609 // * *
LuisRuiz 0:1ca9a50646e6 610 // * These functions are called through function pointer by the Radio low *
LuisRuiz 0:1ca9a50646e6 611 // * level drivers *
LuisRuiz 0:1ca9a50646e6 612 // * *
LuisRuiz 0:1ca9a50646e6 613 // *****************************************************************************
LuisRuiz 0:1ca9a50646e6 614
LuisRuiz 0:1ca9a50646e6 615 void OnTxDone( void )
LuisRuiz 0:1ca9a50646e6 616 {
LuisRuiz 0:1ca9a50646e6 617 AppState = APP_TX;
LuisRuiz 0:1ca9a50646e6 618 }
LuisRuiz 0:1ca9a50646e6 619
LuisRuiz 0:1ca9a50646e6 620 void OnRxDone( void )
LuisRuiz 0:1ca9a50646e6 621 {
LuisRuiz 0:1ca9a50646e6 622 AppState = APP_RX;
LuisRuiz 0:1ca9a50646e6 623 }
LuisRuiz 0:1ca9a50646e6 624
LuisRuiz 0:1ca9a50646e6 625 void OnTxTimeout( void )
LuisRuiz 0:1ca9a50646e6 626 {
LuisRuiz 0:1ca9a50646e6 627 AppState = APP_TX_TIMEOUT;
LuisRuiz 0:1ca9a50646e6 628 printf( "<>>>>>>>>TXE\r\n" );
LuisRuiz 0:1ca9a50646e6 629 }
LuisRuiz 0:1ca9a50646e6 630
LuisRuiz 0:1ca9a50646e6 631 void OnRxTimeout( void )
LuisRuiz 0:1ca9a50646e6 632 {
LuisRuiz 0:1ca9a50646e6 633 AppState = APP_RX_TIMEOUT;
LuisRuiz 0:1ca9a50646e6 634 }
LuisRuiz 0:1ca9a50646e6 635
LuisRuiz 0:1ca9a50646e6 636 void OnRxError( IrqErrorCode_t errorCode )
LuisRuiz 0:1ca9a50646e6 637 {
LuisRuiz 0:1ca9a50646e6 638 AppState = APP_RX_ERROR;
LuisRuiz 0:1ca9a50646e6 639 printf( "RXE<>>>>>>>>\r\n" );
LuisRuiz 0:1ca9a50646e6 640 }
LuisRuiz 0:1ca9a50646e6 641
LuisRuiz 0:1ca9a50646e6 642 void OnRangingDone( IrqRangingCode_t val )
LuisRuiz 0:1ca9a50646e6 643 {
LuisRuiz 0:1ca9a50646e6 644 if( val == IRQ_RANGING_MASTER_VALID_CODE || val == IRQ_RANGING_SLAVE_VALID_CODE )
LuisRuiz 0:1ca9a50646e6 645 {
LuisRuiz 0:1ca9a50646e6 646 AppState = APP_RANGING_DONE;
LuisRuiz 0:1ca9a50646e6 647 }
LuisRuiz 0:1ca9a50646e6 648 else if( val == IRQ_RANGING_MASTER_ERROR_CODE || val == IRQ_RANGING_SLAVE_ERROR_CODE )
LuisRuiz 0:1ca9a50646e6 649 {
LuisRuiz 0:1ca9a50646e6 650 AppState = APP_RANGING_TIMEOUT;
LuisRuiz 0:1ca9a50646e6 651 }
LuisRuiz 0:1ca9a50646e6 652 else
LuisRuiz 0:1ca9a50646e6 653 {
LuisRuiz 0:1ca9a50646e6 654 AppState = APP_RANGING_TIMEOUT;
LuisRuiz 0:1ca9a50646e6 655 }
LuisRuiz 0:1ca9a50646e6 656 }
LuisRuiz 0:1ca9a50646e6 657
LuisRuiz 0:1ca9a50646e6 658 void OnCadDone( bool channelActivityDetected )
LuisRuiz 0:1ca9a50646e6 659 {
LuisRuiz 0:1ca9a50646e6 660 }