amiro-lld / source / deca_instance_common.c @ fce9feec
History | View | Annotate | Download (34.084 KB)
| 1 |
/*! ----------------------------------------------------------------------------
|
|---|---|
| 2 |
* @file instance_common.c
|
| 3 |
* @brief DecaWave application level common instance functions
|
| 4 |
*
|
| 5 |
* @attention
|
| 6 |
*
|
| 7 |
* Copyright 2015 (c) DecaWave Ltd, Dublin, Ireland.
|
| 8 |
*
|
| 9 |
* All rights reserved.
|
| 10 |
*
|
| 11 |
* @author DecaWave
|
| 12 |
*/
|
| 13 |
|
| 14 |
|
| 15 |
|
| 16 |
#include <deca_instance.h> |
| 17 |
#if defined(AMIROLLD_CFG_USE_DW1000) || defined(__DOXYGEN__)
|
| 18 |
|
| 19 |
|
| 20 |
#include <alld_dw1000.h> |
| 21 |
#include <string.h> |
| 22 |
#include <math.h> |
| 23 |
|
| 24 |
|
| 25 |
|
| 26 |
extern double dwt_getrangebias(uint8_t chan, float range, uint8_t prf); |
| 27 |
|
| 28 |
extern const uint16_t rfDelays[2]; |
| 29 |
extern const uint16_t rfDelaysTREK[2]; |
| 30 |
extern const tx_struct txSpectrumConfig[8]; |
| 31 |
|
| 32 |
|
| 33 |
|
| 34 |
// -------------------------------------------------------------------------------------------------------------------
|
| 35 |
// Deca Calibration Values
|
| 36 |
// -------------------------------------------------------------------------------------------------------------------
|
| 37 |
|
| 38 |
#define DWT_PRF_64M_RFDLY (514.462f) |
| 39 |
#define DWT_PRF_16M_RFDLY (513.9067f) |
| 40 |
|
| 41 |
// -------------------------------------------------------------------------------------------------------------------
|
| 42 |
|
| 43 |
//The table below specifies the default TX spectrum configuration parameters... this has been tuned for DW EVK hardware units
|
| 44 |
//the table is set for smart power - see below in the instance_config function how this is used when not using smart power
|
| 45 |
const tx_struct txSpectrumConfig[8] = |
| 46 |
{
|
| 47 |
//Channel 0 ----- this is just a place holder so the next array element is channel 1
|
| 48 |
{
|
| 49 |
0x0, //0 |
| 50 |
{
|
| 51 |
0x0, //0 |
| 52 |
0x0 //0 |
| 53 |
} |
| 54 |
}, |
| 55 |
//Channel 1
|
| 56 |
{
|
| 57 |
0xc9, //PG_DELAY |
| 58 |
{
|
| 59 |
0x15355575, //16M prf power |
| 60 |
0x07274767 //64M prf power |
| 61 |
} |
| 62 |
|
| 63 |
}, |
| 64 |
//Channel 2
|
| 65 |
{
|
| 66 |
0xc2, //PG_DELAY |
| 67 |
{
|
| 68 |
0x15355575, //16M prf power |
| 69 |
0x07274767 //64M prf power |
| 70 |
} |
| 71 |
}, |
| 72 |
//Channel 3
|
| 73 |
{
|
| 74 |
0xc5, //PG_DELAY |
| 75 |
{
|
| 76 |
0x0f2f4f6f, //16M prf power |
| 77 |
0x2b4b6b8b //64M prf power |
| 78 |
} |
| 79 |
}, |
| 80 |
//Channel 4
|
| 81 |
{
|
| 82 |
0x95, //PG_DELAY |
| 83 |
{
|
| 84 |
0x1f1f3f5f, //16M prf power |
| 85 |
0x3a5a7a9a //64M prf power |
| 86 |
} |
| 87 |
}, |
| 88 |
//Channel 5
|
| 89 |
{
|
| 90 |
0xc0, //PG_DELAY |
| 91 |
{
|
| 92 |
0x0E082848, //16M prf power |
| 93 |
0x25456585 //64M prf power |
| 94 |
} |
| 95 |
}, |
| 96 |
//Channel 6 ----- this is just a place holder so the next array element is channel 7
|
| 97 |
{
|
| 98 |
0x0, //0 |
| 99 |
{
|
| 100 |
0x0, //0 |
| 101 |
0x0 //0 |
| 102 |
} |
| 103 |
}, |
| 104 |
//Channel 7
|
| 105 |
{
|
| 106 |
0x93, //PG_DELAY |
| 107 |
{
|
| 108 |
0x32527292, //16M prf power |
| 109 |
0x5171B1d1 //64M prf power |
| 110 |
} |
| 111 |
} |
| 112 |
}; |
| 113 |
|
| 114 |
//these are default antenna delays for EVB1000, these can be used if there is no calibration data in the DW1000,
|
| 115 |
//or instead of the calibration data
|
| 116 |
const uint16_t rfDelays[2] = { |
| 117 |
(uint16_t) ((DWT_PRF_16M_RFDLY/ 2.0) * 1e-9 / DWT_TIME_UNITS),//PRF 16 |
| 118 |
(uint16_t) ((DWT_PRF_64M_RFDLY/ 2.0) * 1e-9 / DWT_TIME_UNITS) |
| 119 |
}; |
| 120 |
|
| 121 |
//these are default TREK Tag/Anchor antenna delays
|
| 122 |
const uint16_t rfDelaysTREK[2] = { |
| 123 |
(uint16_t) ((514.83f/ 2.0) * 1e-9 / DWT_TIME_UNITS),//channel 2 |
| 124 |
(uint16_t) ((514.65f/ 2.0) * 1e-9 / DWT_TIME_UNITS) //channel 5 |
| 125 |
}; |
| 126 |
|
| 127 |
//int instance_starttxtest(int framePeriod)
|
| 128 |
//{
|
| 129 |
// //define some test data for the tx buffer
|
| 130 |
// uint8 msg[127] = "The quick brown fox jumps over the lazy dog. The quick brown fox jumps over the lazy dog. The quick brown fox jumps over the l";
|
| 131 |
|
| 132 |
// //NOTE: SPI frequency must be < 3MHz
|
| 133 |
// port_set_dw1000_slowrate(); //max SPI before PLLs configured is ~4M
|
| 134 |
|
| 135 |
// // the value here 0x1000 gives a period of 32.82 µs
|
| 136 |
// //this is setting 0x1000 as frame period (125MHz clock cycles) (time from Tx en - to next - Tx en)
|
| 137 |
// dwt_configcontinuousframemode(framePeriod);
|
| 138 |
|
| 139 |
// dwt_writetxdata(127, (uint8 *) msg, 0) ;
|
| 140 |
// dwt_writetxfctrl(127, 0, 0);
|
| 141 |
|
| 142 |
// //to start the first frame - set TXSTRT
|
| 143 |
// dwt_starttx(DWT_START_TX_IMMEDIATE);
|
| 144 |
|
| 145 |
// //measure the power
|
| 146 |
// //Spectrum Analyser set:
|
| 147 |
// //FREQ to be channel default e.g. 3.9936 GHz for channel 2
|
| 148 |
// //SPAN to 1GHz
|
| 149 |
// //SWEEP TIME 1s
|
| 150 |
// //RBW and VBW 1MHz
|
| 151 |
// //measure channel power
|
| 152 |
|
| 153 |
// return DWT_SUCCESS ;
|
| 154 |
//}
|
| 155 |
|
| 156 |
// -------------------------------------------------------------------------------------------------------------------
|
| 157 |
// Data Definitions
|
| 158 |
// -------------------------------------------------------------------------------------------------------------------
|
| 159 |
|
| 160 |
static instance_data_t instance_data[NUM_INST] ;
|
| 161 |
|
| 162 |
static double inst_tdist[MAX_TAG_LIST_SIZE] ; |
| 163 |
static double inst_idist[MAX_ANCHOR_LIST_SIZE] ; |
| 164 |
static double inst_idistraw[MAX_ANCHOR_LIST_SIZE] ; |
| 165 |
|
| 166 |
// -------------------------------------------------------------------------------------------------------------------
|
| 167 |
// Functions
|
| 168 |
// -------------------------------------------------------------------------------------------------------------------
|
| 169 |
|
| 170 |
|
| 171 |
/* @fn instance_get_local_structure_ptr
|
| 172 |
* @brief function to return the pointer to local instance data structure
|
| 173 |
* */
|
| 174 |
instance_data_t* instance_get_local_structure_ptr(unsigned int x) |
| 175 |
{
|
| 176 |
if (x >= NUM_INST)
|
| 177 |
{
|
| 178 |
return NULL; |
| 179 |
} |
| 180 |
|
| 181 |
return &instance_data[x];
|
| 182 |
} |
| 183 |
|
| 184 |
|
| 185 |
// -------------------------------------------------------------------------------------------------------------------
|
| 186 |
/* @fn instance_convert_usec_to_devtimeu
|
| 187 |
* @brief function to convert microseconds to device time
|
| 188 |
* */
|
| 189 |
uint64_t instance_convert_usec_to_devtimeu (double microsecu)
|
| 190 |
{
|
| 191 |
uint64_t dt; |
| 192 |
long double dtime; |
| 193 |
|
| 194 |
dtime = (microsecu / (double) DWT_TIME_UNITS) / 1e6 ; |
| 195 |
|
| 196 |
dt = (uint64_t) (dtime) ; |
| 197 |
|
| 198 |
return dt;
|
| 199 |
} |
| 200 |
|
| 201 |
/* @fn instance_calculate_rangefromTOF
|
| 202 |
* @brief function to calculate and the range from given Time of Flight
|
| 203 |
* */
|
| 204 |
int instance_calculate_rangefromTOF(int idx, uint32_t tofx) |
| 205 |
{
|
| 206 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 207 |
double distance ;
|
| 208 |
double distance_to_correct;
|
| 209 |
double tof ;
|
| 210 |
int32_t tofi ; |
| 211 |
|
| 212 |
// check for negative results and accept them making them proper negative integers
|
| 213 |
tofi = (int32_t) tofx ; // make it signed
|
| 214 |
if (tofi > 0x7FFFFFFF) // close up TOF may be negative |
| 215 |
{
|
| 216 |
tofi -= 0x80000000 ; // |
| 217 |
} |
| 218 |
|
| 219 |
// convert device time units to seconds (as floating point)
|
| 220 |
tof = tofi * DWT_TIME_UNITS ; |
| 221 |
inst_idistraw[idx] = distance = tof * SPEED_OF_LIGHT; |
| 222 |
|
| 223 |
#if (CORRECT_RANGE_BIAS == 1) |
| 224 |
//for the 6.81Mb data rate we assume gating gain of 6dB is used,
|
| 225 |
//thus a different range bias needs to be applied
|
| 226 |
//if(inst->configData.dataRate == DWT_BR_6M8)
|
| 227 |
if(inst->smartPowerEn)
|
| 228 |
{
|
| 229 |
//1.31 for channel 2 and 1.51 for channel 5
|
| 230 |
if(inst->configData.chan == 5) |
| 231 |
{
|
| 232 |
distance_to_correct = distance/1.51; |
| 233 |
} |
| 234 |
else //channel 2 |
| 235 |
{
|
| 236 |
distance_to_correct = distance/1.31; |
| 237 |
} |
| 238 |
} |
| 239 |
else
|
| 240 |
{
|
| 241 |
distance_to_correct = distance; |
| 242 |
} |
| 243 |
|
| 244 |
distance = distance - dwt_getrangebias(inst->configData.chan, (float) distance_to_correct, inst->configData.prf);
|
| 245 |
#endif
|
| 246 |
|
| 247 |
if ((distance < 0) || (distance > 20000.000)) // discard any results less than <0 cm or >20 km |
| 248 |
return 0; |
| 249 |
|
| 250 |
inst_idist[idx] = distance; |
| 251 |
|
| 252 |
inst->longTermRangeCount++ ; // for computing a long term average
|
| 253 |
|
| 254 |
return 1; |
| 255 |
}// end of calculateRangeFromTOF
|
| 256 |
|
| 257 |
void instance_set_tagdist(int tidx, int aidx) |
| 258 |
{
|
| 259 |
inst_tdist[tidx] = inst_idist[aidx]; |
| 260 |
} |
| 261 |
|
| 262 |
double instance_get_tagdist(int idx) |
| 263 |
{
|
| 264 |
return inst_tdist[idx];
|
| 265 |
} |
| 266 |
|
| 267 |
void instance_cleardisttable(int idx) |
| 268 |
{
|
| 269 |
inst_idistraw[idx] = 0;
|
| 270 |
inst_idist[idx] = 0;
|
| 271 |
} |
| 272 |
|
| 273 |
void instance_cleardisttableall(void) |
| 274 |
{
|
| 275 |
int i;
|
| 276 |
|
| 277 |
for(i=0; i<MAX_ANCHOR_LIST_SIZE; i++) |
| 278 |
{
|
| 279 |
inst_idistraw[i] = 0xffff;
|
| 280 |
inst_idist[i] = 0xffff;
|
| 281 |
} |
| 282 |
} |
| 283 |
|
| 284 |
// -------------------------------------------------------------------------------------------------------------------
|
| 285 |
// Set this instance role as the Tag, Anchor
|
| 286 |
/*void instance_set_role(int inst_mode)
|
| 287 |
{
|
| 288 |
// assume instance 0, for this
|
| 289 |
inst->mode = inst_mode; // set the role
|
| 290 |
}
|
| 291 |
*/
|
| 292 |
int instance_get_role(void) |
| 293 |
{
|
| 294 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 295 |
|
| 296 |
return inst->mode;
|
| 297 |
} |
| 298 |
|
| 299 |
int instance_newrange(void) |
| 300 |
{
|
| 301 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 302 |
int x = inst->newRange;
|
| 303 |
inst->newRange = TOF_REPORT_NUL; |
| 304 |
return x;
|
| 305 |
} |
| 306 |
|
| 307 |
int instance_newrangeancadd(void) |
| 308 |
{
|
| 309 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 310 |
return inst->newRangeAncAddress;
|
| 311 |
} |
| 312 |
|
| 313 |
int instance_newrangetagadd(void) |
| 314 |
{
|
| 315 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 316 |
return inst->newRangeTagAddress;
|
| 317 |
} |
| 318 |
|
| 319 |
int instance_newrangetim(void) |
| 320 |
{
|
| 321 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 322 |
return inst->newRangeTime;
|
| 323 |
} |
| 324 |
|
| 325 |
// -------------------------------------------------------------------------------------------------------------------
|
| 326 |
// function to clear counts/averages/range values
|
| 327 |
//
|
| 328 |
void instance_clearcounts(void) |
| 329 |
{
|
| 330 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 331 |
int i= 0 ; |
| 332 |
|
| 333 |
//inst->rxTimeouts = 0 ;
|
| 334 |
//inst->txMsgCount = 0 ;
|
| 335 |
//inst->rxMsgCount = 0 ;
|
| 336 |
|
| 337 |
dwt_configeventcounters(1); //enable and clear - NOTE: the counters are not preserved when in DEEP SLEEP |
| 338 |
|
| 339 |
inst->frameSN = 0;
|
| 340 |
|
| 341 |
inst->longTermRangeCount = 0;
|
| 342 |
|
| 343 |
|
| 344 |
for(i=0; i<MAX_ANCHOR_LIST_SIZE; i++) |
| 345 |
{
|
| 346 |
inst->tofArray[i] = INVALID_TOF; |
| 347 |
} |
| 348 |
|
| 349 |
for(i=0; i<MAX_TAG_LIST_SIZE; i++) |
| 350 |
{
|
| 351 |
inst->tof[i] = INVALID_TOF; |
| 352 |
} |
| 353 |
|
| 354 |
} // end instanceclearcounts()
|
| 355 |
|
| 356 |
|
| 357 |
// -------------------------------------------------------------------------------------------------------------------
|
| 358 |
// function to initialise instance structures
|
| 359 |
//
|
| 360 |
// Returns 0 on success and -1 on error
|
| 361 |
int instance_init(int inst_mode, DW1000Driver* drv) |
| 362 |
{
|
| 363 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 364 |
int result;
|
| 365 |
|
| 366 |
inst->mode = inst_mode; // assume listener,
|
| 367 |
inst->twrMode = LISTENER; |
| 368 |
inst->testAppState = TA_INIT ; |
| 369 |
inst->instToSleep = FALSE; |
| 370 |
|
| 371 |
|
| 372 |
// Reset the IC (might be needed if not getting here from POWER ON)
|
| 373 |
// ARM code: Remove soft reset here as using hard reset in the inittestapplication() in the main.c file
|
| 374 |
//dwt_softreset();
|
| 375 |
|
| 376 |
//this initialises DW1000 and uses specified configurations from OTP/ROM
|
| 377 |
result = dwt_initialise(DWT_LOADUCODE, drv) ; |
| 378 |
|
| 379 |
//this is platform dependent - only program if DW EVK/EVB
|
| 380 |
dwt_setleds(3) ; //configure the GPIOs which control the leds on EVBs |
| 381 |
|
| 382 |
if (DWT_SUCCESS != result)
|
| 383 |
{
|
| 384 |
return (-1) ; // device initialise has failed |
| 385 |
} |
| 386 |
|
| 387 |
instance_clearcounts() ; |
| 388 |
|
| 389 |
inst->wait4ack = 0;
|
| 390 |
inst->instanceTimerEn = 0;
|
| 391 |
|
| 392 |
instance_clearevents(); |
| 393 |
|
| 394 |
#if (DISCOVERY == 1) |
| 395 |
dwt_geteui(inst->eui64); |
| 396 |
inst->panID = 0xdada ;
|
| 397 |
#else
|
| 398 |
memset(inst->eui64, 0, ADDR_BYTE_SIZE_L);
|
| 399 |
inst->panID = 0xdeca ;
|
| 400 |
#endif
|
| 401 |
inst->tagSleepCorrection_ms = 0;
|
| 402 |
|
| 403 |
dwt_setdblrxbuffmode(0); //disable double RX buffer |
| 404 |
|
| 405 |
// if using auto CRC check (DWT_INT_RFCG and DWT_INT_RFCE) are used instead of DWT_INT_RDFR flag
|
| 406 |
// other errors which need to be checked (as they disable receiver) are
|
| 407 |
//dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG | (DWT_INT_SFDT | DWT_INT_RFTO /*| DWT_INT_RXPTO*/), 1);
|
| 408 |
dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO | DWT_INT_RXPTO), 1);
|
| 409 |
|
| 410 |
if(inst_mode == ANCHOR)
|
| 411 |
{
|
| 412 |
dwt_setcallbacks(tx_conf_cb, rx_ok_cb_anch, rx_to_cb_anch, rx_err_cb_anch); |
| 413 |
} |
| 414 |
else
|
| 415 |
{
|
| 416 |
dwt_setcallbacks(tx_conf_cb, rx_ok_cb_tag, rx_to_cb_tag, rx_err_cb_tag); |
| 417 |
} |
| 418 |
|
| 419 |
inst->monitor = 0;
|
| 420 |
|
| 421 |
//inst->lateTX = 0;
|
| 422 |
//inst->lateRX = 0;
|
| 423 |
|
| 424 |
inst->remainingRespToRx = -1; //initialise |
| 425 |
|
| 426 |
inst->rxResps = 0;
|
| 427 |
|
| 428 |
dwt_setlnapamode(1, 1); //enable TX, RX state on GPIOs 6 and 5 |
| 429 |
|
| 430 |
inst->delayedTRXTime32h = 0;
|
| 431 |
|
| 432 |
#if (READ_EVENT_COUNTERS == 1) |
| 433 |
dwt_configeventcounters(1);
|
| 434 |
#endif
|
| 435 |
return 0 ; |
| 436 |
} |
| 437 |
|
| 438 |
// -------------------------------------------------------------------------------------------------------------------
|
| 439 |
//
|
| 440 |
// Return the Device ID register value, enables higher level validation of physical device presence
|
| 441 |
//
|
| 442 |
|
| 443 |
uint32_t instance_readdeviceid(void)
|
| 444 |
{
|
| 445 |
return dwt_readdevid() ;
|
| 446 |
} |
| 447 |
|
| 448 |
|
| 449 |
//OTP memory addresses for TREK calibration data
|
| 450 |
#define TXCFG_ADDRESS (0x10) |
| 451 |
#define ANTDLY_ADDRESS (0x1C) |
| 452 |
#define TREK_ANTDLY_1 (0xD) |
| 453 |
#define TREK_ANTDLY_2 (0xE) |
| 454 |
#define TREK_ANTDLY_3 (0xF) |
| 455 |
#define TREK_ANTDLY_4 (0x1D) |
| 456 |
|
| 457 |
extern uint8_t chan_idx[];
|
| 458 |
// -------------------------------------------------------------------------------------------------------------------
|
| 459 |
//
|
| 460 |
// function to allow application configuration be passed into instance and affect underlying device operation
|
| 461 |
//
|
| 462 |
void instance_config(instanceConfig_t *config, sfConfig_t *sfConfig)
|
| 463 |
{
|
| 464 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 465 |
uint32_t power = 0;
|
| 466 |
uint8_t otprev ; |
| 467 |
|
| 468 |
inst->configData.chan = config->channelNumber ; |
| 469 |
inst->configData.rxCode = config->preambleCode ; |
| 470 |
inst->configData.txCode = config->preambleCode ; |
| 471 |
inst->configData.prf = config->pulseRepFreq ; |
| 472 |
inst->configData.dataRate = config->dataRate ; |
| 473 |
inst->configData.txPreambLength = config->preambleLen ; |
| 474 |
inst->configData.rxPAC = config->pacSize ; |
| 475 |
inst->configData.nsSFD = config->nsSFD ; |
| 476 |
inst->configData.phrMode = DWT_PHRMODE_STD ; |
| 477 |
inst->configData.sfdTO = config->sfdTO; |
| 478 |
|
| 479 |
//the DW1000 will automatically use gating gain for frames < 1ms duration (i.e. 6.81Mbps data rate)
|
| 480 |
//smartPowerEn should be set based on the frame length, but we can also use dtaa rate.
|
| 481 |
if(inst->configData.dataRate == DWT_BR_6M8)
|
| 482 |
{
|
| 483 |
inst->smartPowerEn = 1;
|
| 484 |
} |
| 485 |
else
|
| 486 |
{
|
| 487 |
inst->smartPowerEn = 0;
|
| 488 |
} |
| 489 |
|
| 490 |
//configure the channel parameters
|
| 491 |
dwt_configure(&inst->configData) ; |
| 492 |
|
| 493 |
port_set_dw1000_slowrate(); //reduce SPI to < 3MHz
|
| 494 |
//load TX values from OTP
|
| 495 |
dwt_otpread(TXCFG_ADDRESS+(config->pulseRepFreq - DWT_PRF_16M) + (chan_idx[inst->configData.chan] * 2), &power, 1); |
| 496 |
port_set_dw1000_fastrate(); //increase SPI
|
| 497 |
//check if there are calibrated TX power value in the DW1000 OTP
|
| 498 |
if((power == 0x0) || (power == 0xFFFFFFFF)) //if there are no calibrated values... need to use defaults |
| 499 |
{
|
| 500 |
power = txSpectrumConfig[config->channelNumber].txPwr[config->pulseRepFreq- DWT_PRF_16M]; |
| 501 |
} |
| 502 |
|
| 503 |
//Configure TX power and PG delay
|
| 504 |
inst->configTX.power = power; |
| 505 |
inst->configTX.PGdly = txSpectrumConfig[config->channelNumber].pgDelay ; |
| 506 |
//configure the tx spectrum parameters (power and PG delay)
|
| 507 |
dwt_configuretxrf(&inst->configTX); |
| 508 |
|
| 509 |
otprev = dwt_otprevision() ; // this revision tells us how OTP is programmed.
|
| 510 |
|
| 511 |
if ((2 == otprev) || (3 == otprev)) // board is calibrated with TREK1000 with antenna delays set for each use case) |
| 512 |
{
|
| 513 |
uint8_t mode = (inst->mode == ANCHOR ? 1 : 0); |
| 514 |
uint8_t chanindex = 0;
|
| 515 |
uint32_t dly = 0;
|
| 516 |
|
| 517 |
port_set_dw1000_slowrate(); //reduce SPI to < 3MHz
|
| 518 |
|
| 519 |
//read 32-bit antenna delay value from OTP, high 16 bits is value for Anchor mode, low 16-bits for Tag mode
|
| 520 |
switch(inst->configData.chan)
|
| 521 |
{
|
| 522 |
case 2: |
| 523 |
if(inst->configData.dataRate == DWT_BR_6M8)
|
| 524 |
dwt_otpread(TREK_ANTDLY_1, &dly, 1);
|
| 525 |
else if(inst->configData.dataRate == DWT_BR_110K) |
| 526 |
dwt_otpread(TREK_ANTDLY_2, &dly, 1);
|
| 527 |
break;
|
| 528 |
case 5: |
| 529 |
if(inst->configData.dataRate == DWT_BR_6M8)
|
| 530 |
dwt_otpread(TREK_ANTDLY_3, &dly, 1);
|
| 531 |
else if(inst->configData.dataRate == DWT_BR_110K) |
| 532 |
dwt_otpread(TREK_ANTDLY_4, &dly, 1);
|
| 533 |
break;
|
| 534 |
default:
|
| 535 |
dly = 0;
|
| 536 |
break;
|
| 537 |
} |
| 538 |
|
| 539 |
port_set_dw1000_fastrate(); //increase SPI to max
|
| 540 |
|
| 541 |
// if nothing was actually programmed then set a reasonable value anyway
|
| 542 |
if ((dly == 0) |
| 543 |
|| (dly == 0xffffffff))
|
| 544 |
{
|
| 545 |
if(inst->configData.chan == 5) |
| 546 |
{
|
| 547 |
chanindex = 1;
|
| 548 |
} |
| 549 |
|
| 550 |
inst->txAntennaDelay = rfDelaysTREK[chanindex]; |
| 551 |
} |
| 552 |
else
|
| 553 |
{
|
| 554 |
inst->txAntennaDelay = (dly >> (16*(mode & 0x1))) & 0xFFFF; |
| 555 |
} |
| 556 |
|
| 557 |
} |
| 558 |
else // assume it is older EVK1000 programming. |
| 559 |
{
|
| 560 |
uint32_t antennaDly; |
| 561 |
port_set_dw1000_slowrate(); //reduce SPI to < 3MHz
|
| 562 |
//read the antenna delay that was programmed in the OTP calibration area
|
| 563 |
dwt_otpread(ANTDLY_ADDRESS, &antennaDly, 1) ;
|
| 564 |
port_set_dw1000_fastrate(); //increase SPI to max
|
| 565 |
|
| 566 |
// if nothing was actually programmed then set a reasonable value anyway
|
| 567 |
if ((antennaDly == 0) |
| 568 |
|| (antennaDly == 0xffffffff))
|
| 569 |
{
|
| 570 |
inst->txAntennaDelay = rfDelays[config->pulseRepFreq - DWT_PRF_16M]; |
| 571 |
} |
| 572 |
else
|
| 573 |
{
|
| 574 |
// 32-bit antenna delay value read from OTP, high 16 bits is value for 64 MHz PRF, low 16-bits for 16 MHz PRF
|
| 575 |
inst->txAntennaDelay = ((antennaDly >> (16*(inst->configData.prf-DWT_PRF_16M))) & 0xFFFF) >> 1; |
| 576 |
} |
| 577 |
} |
| 578 |
|
| 579 |
// -------------------------------------------------------------------------------------------------------------------
|
| 580 |
// set the antenna delay, we assume that the RX is the same as TX.
|
| 581 |
dwt_setrxantennadelay(inst->txAntennaDelay); |
| 582 |
dwt_settxantennadelay(inst->txAntennaDelay); |
| 583 |
|
| 584 |
inst->rxAntennaDelay = inst->txAntennaDelay; |
| 585 |
|
| 586 |
if(config->preambleLen == DWT_PLEN_64) //if preamble length is 64 |
| 587 |
{
|
| 588 |
port_set_dw1000_slowrate(); //reduce SPI to < 3MHz
|
| 589 |
|
| 590 |
dwt_loadopsettabfromotp(0);
|
| 591 |
|
| 592 |
port_set_dw1000_fastrate(); //increase SPI to max
|
| 593 |
} |
| 594 |
|
| 595 |
|
| 596 |
inst->tagPeriod_ms = sfConfig->tagPeriod_ms; //set the Tag sleep time
|
| 597 |
inst->sframePeriod_ms = sfConfig->sfPeriod_ms; |
| 598 |
inst->slotDuration_ms = sfConfig->slotDuration_ms; |
| 599 |
inst->tagSleepRnd_ms = sfConfig->slotDuration_ms; |
| 600 |
inst->numSlots = sfConfig->numSlots; |
| 601 |
|
| 602 |
//last two slots are used for anchor to anchor ranging
|
| 603 |
inst->a0SlotTime_ms = (inst->numSlots-2) * inst->slotDuration_ms;
|
| 604 |
|
| 605 |
//set the default response delays
|
| 606 |
instance_set_replydelay(sfConfig->pollTxToFinalTxDly_us); |
| 607 |
|
| 608 |
} |
| 609 |
|
| 610 |
int instance_get_rnum(void) //get ranging number |
| 611 |
{
|
| 612 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 613 |
return inst->rangeNum;
|
| 614 |
} |
| 615 |
|
| 616 |
int instance_get_rnuma(int idx) //get ranging number |
| 617 |
{
|
| 618 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 619 |
return inst->rangeNumA[idx];
|
| 620 |
} |
| 621 |
|
| 622 |
int instance_get_rnumanc(int idx) //get ranging number |
| 623 |
{
|
| 624 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 625 |
return inst->rangeNumAAnc[idx];
|
| 626 |
} |
| 627 |
|
| 628 |
int instance_get_lcount(void) //get count of ranges used for calculation of lt avg |
| 629 |
{
|
| 630 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 631 |
int x = inst->longTermRangeCount;
|
| 632 |
|
| 633 |
return (x);
|
| 634 |
} |
| 635 |
|
| 636 |
double instance_get_idist(int idx) //get instantaneous range |
| 637 |
{
|
| 638 |
double x ;
|
| 639 |
|
| 640 |
idx &= (MAX_ANCHOR_LIST_SIZE - 1);
|
| 641 |
|
| 642 |
x = inst_idist[idx]; |
| 643 |
|
| 644 |
return (x);
|
| 645 |
} |
| 646 |
|
| 647 |
double instance_get_idistraw(int idx) //get instantaneous range (uncorrected) |
| 648 |
{
|
| 649 |
double x ;
|
| 650 |
|
| 651 |
idx &= (MAX_ANCHOR_LIST_SIZE - 1);
|
| 652 |
|
| 653 |
x = inst_idistraw[idx]; |
| 654 |
|
| 655 |
return (x);
|
| 656 |
} |
| 657 |
|
| 658 |
int instance_get_idist_mm(int idx) //get instantaneous range |
| 659 |
{
|
| 660 |
int x ;
|
| 661 |
|
| 662 |
idx &= (MAX_ANCHOR_LIST_SIZE - 1);
|
| 663 |
|
| 664 |
x = (int)(inst_idist[idx]*1000); |
| 665 |
|
| 666 |
return (x);
|
| 667 |
} |
| 668 |
|
| 669 |
int instance_get_idistraw_mm(int idx) //get instantaneous range (uncorrected) |
| 670 |
{
|
| 671 |
int x ;
|
| 672 |
|
| 673 |
idx &= (MAX_ANCHOR_LIST_SIZE - 1);
|
| 674 |
|
| 675 |
x = (int)(inst_idistraw[idx]*1000); |
| 676 |
|
| 677 |
return (x);
|
| 678 |
} |
| 679 |
|
| 680 |
/* @fn instanceSet16BitAddress
|
| 681 |
* @brief set the 16-bit MAC address
|
| 682 |
*
|
| 683 |
*/
|
| 684 |
void instance_set_16bit_address(uint16_t address)
|
| 685 |
{
|
| 686 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 687 |
inst->instanceAddress16 = address ; // copy configurations
|
| 688 |
} |
| 689 |
|
| 690 |
/**
|
| 691 |
* @brief this function configures the Frame Control and PAN ID bits
|
| 692 |
*/
|
| 693 |
void instance_config_frameheader_16bit(instance_data_t *inst)
|
| 694 |
{
|
| 695 |
//set frame type (0-2), SEC (3), Pending (4), ACK (5), PanIDcomp(6)
|
| 696 |
inst->msg_f.frameCtrl[0] = 0x1 /*frame type 0x1 == data*/ | 0x40 /*PID comp*/; |
| 697 |
|
| 698 |
//source/dest addressing modes and frame version
|
| 699 |
inst->msg_f.frameCtrl[1] = 0x8 /*dest extended address (16bits)*/ | 0x80 /*src extended address (16bits)*/; |
| 700 |
|
| 701 |
inst->msg_f.panID[0] = (inst->panID) & 0xff; |
| 702 |
inst->msg_f.panID[1] = inst->panID >> 8; |
| 703 |
|
| 704 |
inst->msg_f.seqNum = 0;
|
| 705 |
} |
| 706 |
|
| 707 |
/**
|
| 708 |
* @brief this function writes DW TX Frame Control, Delay TX Time and Starts Transmission
|
| 709 |
*/
|
| 710 |
int instance_send_delayed_frame(instance_data_t *inst, int delayedTx) |
| 711 |
{
|
| 712 |
int result = 0; |
| 713 |
|
| 714 |
dwt_writetxfctrl(inst->psduLength, 0, 1); |
| 715 |
if(delayedTx == DWT_START_TX_DELAYED)
|
| 716 |
{
|
| 717 |
dwt_setdelayedtrxtime(inst->delayedTRXTime32h) ; //should be high 32-bits of delayed TX TS
|
| 718 |
} |
| 719 |
|
| 720 |
//begin delayed TX of frame
|
| 721 |
if (dwt_starttx(delayedTx | inst->wait4ack)) // delayed start was too late |
| 722 |
{
|
| 723 |
result = 1; //late/error |
| 724 |
//inst->lateTX++;
|
| 725 |
} |
| 726 |
else
|
| 727 |
{
|
| 728 |
inst->timeofTx = chVTGetSystemTimeX(); |
| 729 |
inst->monitor = 1;
|
| 730 |
} |
| 731 |
return result; // state changes |
| 732 |
} |
| 733 |
|
| 734 |
//
|
| 735 |
// NB: This function is called from the (TX) interrupt handler
|
| 736 |
//
|
| 737 |
void tx_conf_cb(const dwt_cb_data_t *txd) |
| 738 |
{
|
| 739 |
(void) txd;
|
| 740 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 741 |
uint8_t txTimeStamp[5] = {0, 0, 0, 0, 0}; |
| 742 |
event_data_t dw_event; |
| 743 |
|
| 744 |
dw_event.uTimeStamp = chVTGetSystemTimeX(); |
| 745 |
|
| 746 |
if(inst->twrMode == RESPONDER_B) //anchor has responded to a blink - don't report this event |
| 747 |
{
|
| 748 |
inst->twrMode = LISTENER ; |
| 749 |
} |
| 750 |
#if(DISCOVERY == 1) |
| 751 |
else if (inst->twrMode == GREETER) |
| 752 |
{
|
| 753 |
//don't report TX event ...
|
| 754 |
} |
| 755 |
#endif
|
| 756 |
else
|
| 757 |
{
|
| 758 |
//uint64_t txtimestamp = 0;
|
| 759 |
|
| 760 |
//NOTE - we can only get TX good (done) while here
|
| 761 |
//dwt_readtxtimestamp((uint8*) &inst->txu.txTimeStamp);
|
| 762 |
|
| 763 |
dwt_readtxtimestamp(txTimeStamp) ; |
| 764 |
instance_seteventtime(&dw_event, txTimeStamp); |
| 765 |
|
| 766 |
dw_event.rxLength = inst->psduLength; |
| 767 |
dw_event.type = 0;
|
| 768 |
dw_event.typePend = 0;
|
| 769 |
//dw_event.typeSave = DWT_SIG_TX_DONE;
|
| 770 |
|
| 771 |
memcpy((uint8_t *)&dw_event.msgu.frame[0], (uint8_t *)&inst->msg_f, inst->psduLength);
|
| 772 |
|
| 773 |
instance_putevent(dw_event, DWT_SIG_TX_DONE); |
| 774 |
|
| 775 |
//inst->txMsgCount++;
|
| 776 |
} |
| 777 |
|
| 778 |
inst->monitor = 0;
|
| 779 |
} |
| 780 |
|
| 781 |
|
| 782 |
void instance_seteventtime(event_data_t *dw_event, uint8_t* timeStamp)
|
| 783 |
{
|
| 784 |
dw_event->timeStamp32l = (uint32_t)timeStamp[0] + ((uint32_t)timeStamp[1] << 8) + ((uint32_t)timeStamp[2] << 16) + ((uint32_t)timeStamp[3] << 24); |
| 785 |
dw_event->timeStamp = timeStamp[4];
|
| 786 |
dw_event->timeStamp <<= 32;
|
| 787 |
dw_event->timeStamp += dw_event->timeStamp32l; |
| 788 |
dw_event->timeStamp32h = ((uint32_t)timeStamp[4] << 24) + (dw_event->timeStamp32l >> 8); |
| 789 |
} |
| 790 |
|
| 791 |
|
| 792 |
int instance_peekevent(void) |
| 793 |
{
|
| 794 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 795 |
return inst->dwevent[inst->dweventPeek].type; //return the type of event that is in front of the queue |
| 796 |
} |
| 797 |
|
| 798 |
void instance_putevent(event_data_t newevent, uint8_t etype)
|
| 799 |
{
|
| 800 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 801 |
//newevent.gotit = 0 ; //newevent.eventtimeclr = 0;
|
| 802 |
|
| 803 |
//copy event
|
| 804 |
inst->dwevent[inst->dweventIdxIn] = newevent; |
| 805 |
|
| 806 |
//set type - this makes it a new event (making sure the event data is copied before event is set as new)
|
| 807 |
//to make sure that the get event function does not get an incomplete event
|
| 808 |
inst->dwevent[inst->dweventIdxIn].type = etype; |
| 809 |
|
| 810 |
inst->dweventIdxIn++; |
| 811 |
|
| 812 |
if(MAX_EVENT_NUMBER == inst->dweventIdxIn)
|
| 813 |
{
|
| 814 |
inst->dweventIdxIn = 0;
|
| 815 |
} |
| 816 |
} |
| 817 |
|
| 818 |
event_data_t dw_event_g; |
| 819 |
|
| 820 |
event_data_t* instance_getevent(int x)
|
| 821 |
{
|
| 822 |
(void) x;
|
| 823 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 824 |
int indexOut = inst->dweventIdxOut;
|
| 825 |
if(inst->dwevent[indexOut].type == 0) //exit with "no event" |
| 826 |
{
|
| 827 |
dw_event_g.type = 0;
|
| 828 |
//dw_event_g.typeSave = 0;
|
| 829 |
return &dw_event_g;
|
| 830 |
} |
| 831 |
|
| 832 |
//copy the event
|
| 833 |
//dw_event_g.typeSave = inst->dwevent[indexOut].typeSave ;
|
| 834 |
dw_event_g.typePend = inst->dwevent[indexOut].typePend ; |
| 835 |
dw_event_g.rxLength = inst->dwevent[indexOut].rxLength ; |
| 836 |
dw_event_g.timeStamp = inst->dwevent[indexOut].timeStamp ; |
| 837 |
dw_event_g.timeStamp32l = inst->dwevent[indexOut].timeStamp32l ; |
| 838 |
dw_event_g.timeStamp32h = inst->dwevent[indexOut].timeStamp32h ; |
| 839 |
dw_event_g.uTimeStamp = inst->dwevent[indexOut].uTimeStamp ; |
| 840 |
|
| 841 |
memcpy(&dw_event_g.msgu, &inst->dwevent[indexOut].msgu, sizeof(inst->dwevent[indexOut].msgu));
|
| 842 |
|
| 843 |
dw_event_g.type = inst->dwevent[indexOut].type ; |
| 844 |
|
| 845 |
//inst->dwevent[indexOut].gotit = x;
|
| 846 |
|
| 847 |
inst->dwevent[indexOut].type = 0; //clear the event |
| 848 |
|
| 849 |
inst->dweventIdxOut++; |
| 850 |
if(MAX_EVENT_NUMBER == inst->dweventIdxOut) //wrap the counter |
| 851 |
{
|
| 852 |
inst->dweventIdxOut = 0;
|
| 853 |
} |
| 854 |
inst->dweventPeek = inst->dweventIdxOut; //set the new peek value
|
| 855 |
|
| 856 |
return &dw_event_g;
|
| 857 |
} |
| 858 |
|
| 859 |
void instance_clearevents(void) |
| 860 |
{
|
| 861 |
int i = 0; |
| 862 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 863 |
|
| 864 |
for(i=0; i<MAX_EVENT_NUMBER; i++) |
| 865 |
{
|
| 866 |
memset(&inst->dwevent[i], 0, sizeof(event_data_t)); |
| 867 |
} |
| 868 |
|
| 869 |
inst->dweventIdxIn = 0;
|
| 870 |
inst->dweventIdxOut = 0;
|
| 871 |
inst->dweventPeek = 0;
|
| 872 |
|
| 873 |
} |
| 874 |
|
| 875 |
|
| 876 |
void instance_config_txpower(uint32_t txpower)
|
| 877 |
{
|
| 878 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 879 |
inst->txPower = txpower ; |
| 880 |
|
| 881 |
inst->txPowerChanged = 1;
|
| 882 |
|
| 883 |
} |
| 884 |
|
| 885 |
void instance_set_txpower(void) |
| 886 |
{
|
| 887 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 888 |
if(inst->txPowerChanged == 1) |
| 889 |
{
|
| 890 |
//Configure TX power
|
| 891 |
dwt_write32bitreg(0x1E, inst->txPower);
|
| 892 |
|
| 893 |
inst->txPowerChanged = 0;
|
| 894 |
} |
| 895 |
} |
| 896 |
|
| 897 |
void instance_config_antennadelays(uint16_t tx, uint16_t rx)
|
| 898 |
{
|
| 899 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 900 |
inst->txAntennaDelay = tx ; |
| 901 |
inst->rxAntennaDelay = rx ; |
| 902 |
|
| 903 |
inst->antennaDelayChanged = 1;
|
| 904 |
} |
| 905 |
|
| 906 |
void instance_set_antennadelays(void) |
| 907 |
{
|
| 908 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 909 |
if(inst->antennaDelayChanged == 1) |
| 910 |
{
|
| 911 |
dwt_setrxantennadelay(inst->rxAntennaDelay); |
| 912 |
dwt_settxantennadelay(inst->txAntennaDelay); |
| 913 |
|
| 914 |
inst->antennaDelayChanged = 0;
|
| 915 |
} |
| 916 |
} |
| 917 |
|
| 918 |
|
| 919 |
uint16_t instance_get_txantdly(void)
|
| 920 |
{
|
| 921 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 922 |
return inst->txAntennaDelay;
|
| 923 |
} |
| 924 |
|
| 925 |
uint16_t instance_get_rxantdly(void)
|
| 926 |
{
|
| 927 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 928 |
return inst->rxAntennaDelay;
|
| 929 |
} |
| 930 |
|
| 931 |
uint8_t instance_validranges(void)
|
| 932 |
{
|
| 933 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 934 |
uint8_t x = inst->rxResponseMaskReport; |
| 935 |
inst->rxResponseMaskReport = 0; //reset mask as we have printed out the ToFs |
| 936 |
return x;
|
| 937 |
} |
| 938 |
|
| 939 |
|
| 940 |
|
| 941 |
// -------------------------------------------------------------------------------------------------------------------
|
| 942 |
// function to set the fixed reply delay time (in us)
|
| 943 |
//
|
| 944 |
// This sets delay for RX to TX - Delayed Send, and for TX to RX delayed receive (wait for response) functionality,
|
| 945 |
// and the frame wait timeout value to use. This is a function of data rate, preamble length, and PRF
|
| 946 |
|
| 947 |
extern uint8_t dwnsSFDlen[];
|
| 948 |
|
| 949 |
float calc_length_data(float msgdatalen) |
| 950 |
{
|
| 951 |
instance_data_t* inst = instance_get_local_structure_ptr(0);
|
| 952 |
|
| 953 |
int x = 0; |
| 954 |
|
| 955 |
x = (int) ceil(msgdatalen*8/330.0f); |
| 956 |
|
| 957 |
msgdatalen = msgdatalen*8 + x*48; |
| 958 |
|
| 959 |
//assume PHR length is 172308ns for 110k and 21539ns for 850k/6.81M
|
| 960 |
if(inst->configData.dataRate == DWT_BR_110K)
|
| 961 |
{
|
| 962 |
msgdatalen *= 8205.13f; |
| 963 |
msgdatalen += 172308; // PHR length in nanoseconds |
| 964 |
|
| 965 |
} |
| 966 |
else if(inst->configData.dataRate == DWT_BR_850K) |
| 967 |
{
|
| 968 |
msgdatalen *= 1025.64f; |
| 969 |
msgdatalen += 21539; // PHR length in nanoseconds |
| 970 |
} |
| 971 |
else
|
| 972 |
{
|
| 973 |
msgdatalen *= 128.21f; |
| 974 |
msgdatalen += 21539; // PHR length in nanoseconds |
| 975 |
} |
| 976 |
|
| 977 |
return msgdatalen ;
|
| 978 |
} |
| 979 |
void instance_set_replydelay(int delayus) //delay in us |
| 980 |
{
|
| 981 |
instance_data_t *inst = &instance_data[0];
|
| 982 |
|
| 983 |
int margin = 3000; //2000 symbols |
| 984 |
int respframe = 0; |
| 985 |
int respframe_sy = 0; |
| 986 |
int pollframe_sy = 0; |
| 987 |
//int finalframeT_sy = 0;
|
| 988 |
int finalframeA_sy = 0; |
| 989 |
|
| 990 |
//configure the rx delay receive delay time, it is dependent on the message length
|
| 991 |
float msgdatalen_resp = 0; |
| 992 |
float msgdatalen_poll = 0; |
| 993 |
float msgdatalen_finalA = 0; |
| 994 |
//float msgdatalen_finalT = 0;
|
| 995 |
float preamblelen = 0; |
| 996 |
int sfdlen = 0; |
| 997 |
|
| 998 |
//Set the RX timeouts based on the longest expected message - the Final message
|
| 999 |
//Poll = 13, Response = 20, Final = 44 bytes
|
| 1000 |
//msgdatalen = TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
|
| 1001 |
msgdatalen_resp = calc_length_data(ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC); |
| 1002 |
msgdatalen_poll = calc_length_data(TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC); |
| 1003 |
|
| 1004 |
msgdatalen_finalA = calc_length_data(ANCH_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC); |
| 1005 |
//msgdatalen_finalT = calc_length_data(TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC);
|
| 1006 |
|
| 1007 |
//SFD length is 64 for 110k (always)
|
| 1008 |
//SFD length is 8 for 6.81M, and 16 for 850k, but can vary between 8 and 16 bytes
|
| 1009 |
sfdlen = dwnsSFDlen[inst->configData.dataRate]; |
| 1010 |
|
| 1011 |
switch (inst->configData.txPreambLength)
|
| 1012 |
{
|
| 1013 |
case DWT_PLEN_4096 : preamblelen = 4096.0f; break; |
| 1014 |
case DWT_PLEN_2048 : preamblelen = 2048.0f; break; |
| 1015 |
case DWT_PLEN_1536 : preamblelen = 1536.0f; break; |
| 1016 |
case DWT_PLEN_1024 : preamblelen = 1024.0f; break; |
| 1017 |
case DWT_PLEN_512 : preamblelen = 512.0f; break; |
| 1018 |
case DWT_PLEN_256 : preamblelen = 256.0f; break; |
| 1019 |
case DWT_PLEN_128 : preamblelen = 128.0f; break; |
| 1020 |
case DWT_PLEN_64 : preamblelen = 64.0f; break; |
| 1021 |
} |
| 1022 |
|
| 1023 |
//preamble = plen * (994 or 1018) depending on 16 or 64 PRF
|
| 1024 |
if(inst->configData.prf == DWT_PRF_16M)
|
| 1025 |
{
|
| 1026 |
preamblelen = (sfdlen + preamblelen) * 0.99359f; |
| 1027 |
} |
| 1028 |
else
|
| 1029 |
{
|
| 1030 |
preamblelen = (sfdlen + preamblelen) * 1.01763f; |
| 1031 |
} |
| 1032 |
|
| 1033 |
respframe_sy = (DW_RX_ON_DELAY + (int)((preamblelen + ((msgdatalen_resp + margin)/1000.0))/ 1.0256)) ; |
| 1034 |
pollframe_sy = (DW_RX_ON_DELAY + (int)((preamblelen + ((msgdatalen_poll + margin)/1000.0))/ 1.0256)) ; |
| 1035 |
|
| 1036 |
finalframeA_sy = (DW_RX_ON_DELAY + (int)((preamblelen + ((msgdatalen_finalA + margin)/1000.0))/ 1.0256)) ; |
| 1037 |
//finalframeT_sy = (DW_RX_ON_DELAY + (int)((preamblelen + ((msgdatalen_finalT + margin)/1000.0))/ 1.0256)) ;
|
| 1038 |
|
| 1039 |
//tag to anchor ranging consists of poll, 4xresponse and final
|
| 1040 |
//pollTx2FinalTxDelay delay is the time from start of sending of the poll to the start of sending of the final message
|
| 1041 |
//this is configured by the user with pollTxToFinalTxDly in sfConfig_t
|
| 1042 |
inst->pollTx2FinalTxDelay = instance_convert_usec_to_devtimeu (delayus); |
| 1043 |
|
| 1044 |
//the anchor to anchor ranging consist of A0 ranging to A1 and A2 and A1 ranging to A2
|
| 1045 |
//so there are a maximum of two responses, thus the poll to final delay can be shorter
|
| 1046 |
inst->pollTx2FinalTxDelayAnc = instance_convert_usec_to_devtimeu ((delayus >> 1) + RX_RESPONSE_TURNAROUND);
|
| 1047 |
|
| 1048 |
//this is the delay the anchors 1, 2, etc.. will send the response back at...
|
| 1049 |
//anchor 2 will have the delay set to 2 * fixedReplyDelayAnc
|
| 1050 |
//andhor 3 will have the delay set to 3 * fixedReplyDelayAnc and so on...
|
| 1051 |
//this delay depends on how quickly the tag can receive and process the message from previous anchor
|
| 1052 |
//(and also the frame length of course)
|
| 1053 |
respframe = (int)(preamblelen + (msgdatalen_resp/1000.0)); //length of response frame (micro seconds) |
| 1054 |
if(inst->configData.dataRate == DWT_BR_110K)
|
| 1055 |
{
|
| 1056 |
//set the frame wait timeout time - total time the frame takes in symbols
|
| 1057 |
inst->fwtoTime_sy = respframe_sy + RX_RESPONSE_TURNAROUND + 400; //add some margin because of the resp to resp RX turn on time |
| 1058 |
inst->preambleDuration32h = (uint32_t) (((uint64_t) instance_convert_usec_to_devtimeu (preamblelen)) >> 8) + DW_RX_ON_DELAY; //preamble duration + 16 us for RX on |
| 1059 |
} |
| 1060 |
else
|
| 1061 |
{
|
| 1062 |
//set the frame wait timeout time - total time the frame takes in symbols
|
| 1063 |
inst->fwtoTime_sy = respframe_sy + RX_RESPONSE_TURNAROUND; //add some margin because of the resp to resp RX turn on time
|
| 1064 |
inst->preambleDuration32h = (uint32_t) (((uint64_t) instance_convert_usec_to_devtimeu (preamblelen)) >> 8) + DW_RX_ON_DELAY; //preamble duration + 16 us for RX on |
| 1065 |
} |
| 1066 |
|
| 1067 |
inst->tagRespRxDelay_sy = RX_RESPONSE_TURNAROUND + respframe_sy - pollframe_sy; |
| 1068 |
|
| 1069 |
//anchors will reply after RX_RESPONSE_TURNAROUND time, also subtract 16 us for RX on delay
|
| 1070 |
inst->ancRespRxDelay_sy = RX_RESPONSE_TURNAROUND - DW_RX_ON_DELAY; |
| 1071 |
|
| 1072 |
inst->fixedReplyDelayAnc32h = ((uint64_t)instance_convert_usec_to_devtimeu (respframe + RX_RESPONSE_TURNAROUND) >> 8);
|
| 1073 |
|
| 1074 |
inst->fwto4RespFrame_sy = respframe_sy; |
| 1075 |
inst->fwto4FinalFrame_sy = finalframeA_sy + 200; //add some margin so we don't timeout too soon |
| 1076 |
|
| 1077 |
//RX Poll (from A0), the Final will come
|
| 1078 |
inst->anc1RespTx2FinalRxDelay_sy = ((delayus >> 1) + RX_RESPONSE_TURNAROUND) - (respframe_sy+RX_RESPONSE_TURNAROUND) - DW_RX_ON_DELAY - respframe;
|
| 1079 |
inst->anc2RespTx2FinalRxDelay_sy = (delayus >> 1) + RX_RESPONSE_TURNAROUND - 2*(respframe_sy+RX_RESPONSE_TURNAROUND) - DW_RX_ON_DELAY - respframe; |
| 1080 |
|
| 1081 |
} |
| 1082 |
|
| 1083 |
/* @fn instance_calc_ranges
|
| 1084 |
* @brief calculate range for each ToF in the array, and return a mask of valid ranges
|
| 1085 |
* */
|
| 1086 |
int instance_calc_ranges(uint32_t *array, uint16_t size, int reportRange, uint8_t* mask) |
| 1087 |
{
|
| 1088 |
int i;
|
| 1089 |
int newRange = TOF_REPORT_NUL;
|
| 1090 |
int distance = 0; |
| 1091 |
|
| 1092 |
for(i=0; i<size; i++) |
| 1093 |
{
|
| 1094 |
uint32_t tofx = array[i]; |
| 1095 |
if(tofx != INVALID_TOF) //if ToF == 0 - then no new range to report |
| 1096 |
{
|
| 1097 |
distance = instance_calculate_rangefromTOF(i, tofx); |
| 1098 |
} |
| 1099 |
|
| 1100 |
if(distance == 1) |
| 1101 |
{
|
| 1102 |
newRange = reportRange; |
| 1103 |
} |
| 1104 |
else
|
| 1105 |
{
|
| 1106 |
//clear mask
|
| 1107 |
*mask &= ~(0x1 << i) ;
|
| 1108 |
instance_cleardisttable(i); |
| 1109 |
} |
| 1110 |
array[i] = INVALID_TOF; |
| 1111 |
|
| 1112 |
distance = 0;
|
| 1113 |
} |
| 1114 |
|
| 1115 |
return newRange;
|
| 1116 |
} |
| 1117 |
|
| 1118 |
|
| 1119 |
/* ==========================================================
|
| 1120 |
|
| 1121 |
Notes:
|
| 1122 |
|
| 1123 |
Previously code handled multiple instances in a single console application
|
| 1124 |
|
| 1125 |
Now have changed it to do a single instance only. With minimal code changes...(i.e. kept [instance] index but it is always 0.
|
| 1126 |
|
| 1127 |
Windows application should call instance_init() once and then in the "main loop" call instance_run().
|
| 1128 |
|
| 1129 |
*/
|
| 1130 |
|
| 1131 |
#endif /* defined(AMIROLLD_CFG_USE_DW1000) */ |