├── .gitignore ├── CONTRIBUTING.md ├── Doxyfile ├── LICENSE ├── Pozyx.h ├── Pozyx_core.cpp ├── Pozyx_definitions.h ├── Pozyx_lib.cpp ├── README.md ├── changelog.md ├── docs ├── Makefile ├── api │ ├── index.rst │ ├── pozyx_class.rst │ ├── pozyx_functions.rst │ └── structs.rst ├── conf.py ├── doxygen_output │ └── xml │ │ ├── _c_o_n_t_r_i_b_u_t_i_n_g_8md.xml │ │ ├── _pozyx_8h.xml │ │ ├── _pozyx__core_8cpp.xml │ │ ├── _pozyx__definitions_8h.xml │ │ ├── _pozyx__lib_8cpp.xml │ │ ├── _r_e_a_d_m_e_8md.xml │ │ ├── changelog_8md.xml │ │ ├── class_pozyx_class.xml │ │ ├── combine.xslt │ │ ├── compound.xsd │ │ ├── group__communication__functions.xml │ │ ├── group__core.xml │ │ ├── group__device__list.xml │ │ ├── group__positioning__functions.xml │ │ ├── group__sensor__data.xml │ │ ├── group__system__functions.xml │ │ ├── index.xml │ │ ├── index.xsd │ │ ├── md__c_o_n_t_r_i_b_u_t_i_n_g.xml │ │ ├── md__r_e_a_d_m_e.xml │ │ ├── md_changelog.xml │ │ └── struct___u_w_b__settings.xml ├── general │ └── index.rst ├── getting_started │ └── index.rst ├── images │ └── logo.png ├── index.rst ├── make.bat ├── overview │ └── index.rst ├── requirements.txt ├── system │ └── index.rst ├── tips-and-tricks │ └── index.rst ├── troubleshooting │ └── index.rst └── tutorials │ └── index.rst ├── examples ├── chat_room │ └── chat_room.ino ├── multitag_positioning │ └── multitag_positioning.ino ├── old │ ├── orientation_3D │ │ └── orientation_3D.ino │ ├── ready_to_localize │ │ └── ready_to_localize.ino │ └── ready_to_range │ │ └── ready_to_range.ino ├── orientation_3D │ └── orientation_3D.ino ├── ready_to_localize │ └── ready_to_localize.ino ├── ready_to_range │ └── ready_to_range.ino └── useful │ ├── pozyx_UWB_configurator │ └── pozyx_UWB_configurator.ino │ ├── pozyx_anchor_configurator │ └── pozyx_anchor_configurator.ino │ ├── pozyx_basic_troubleshooting │ └── pozyx_basic_troubleshooting.ino │ ├── pozyx_change_network_id │ └── pozyx_change_network_id.ino │ ├── pozyx_check_new_position │ └── pozyx_check_new_position.ino │ ├── pozyx_doDiscovery │ └── pozyx_doDiscovery.ino │ ├── pozyx_gain_configurator │ └── pozyx_gain_configurator.ino │ └── pozyx_savechannel │ └── pozyx_savechannel.ino ├── library.properties └── unit_tests ├── README.md ├── unit_tests_core └── unit_tests_core.ino ├── unit_tests_flash └── unit_tests_flash.ino ├── unit_tests_interrupts └── unit_tests_interrupts.ino └── unit_tests_sensors └── unit_tests_sensors.ino /.gitignore: -------------------------------------------------------------------------------- 1 | # Doxyfile 2 | docs/doxygen_output/* 3 | *.gch 4 | docs/_build 5 | 6 | !docs/doxygen_output/xml 7 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contributing guide 2 | 3 | For now, this'll only be about documentation: 4 | 5 | ## Generating documentation 6 | 7 | To generate the documentation, you'll need to install both Doxygen and Sphinx. 8 | 9 | When these are installed, you can run the following in your favorite shell to build the documentation. 10 | 11 | * doxygen Doxyfile 12 | * cd docs 13 | * make html -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016 Pozyx Labs 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Pozyx_core.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Pozyx_core.cpp 3 | * -------------- 4 | * This file contains the defintion of the core POZYX functions and variables 5 | * 6 | */ 7 | 8 | #include "Pozyx.h" 9 | #include 10 | 11 | #if defined(__SAM3X8E__) 12 | // Arduino Due 13 | #define Wire Wire1 14 | #endif 15 | 16 | 17 | extern "C" { 18 | #include "Pozyx_definitions.h" 19 | } 20 | 21 | int PozyxClass::_interrupt; 22 | int PozyxClass::_mode; 23 | 24 | int PozyxClass::_hw_version; // pozyx harware version 25 | int PozyxClass::_fw_version; // pozyx firmware version. (By updating the firmware on the pozyx device, this value can change); 26 | 27 | /** 28 | * The interrupt handler for the pozyx device: keeping it uber short! 29 | */ 30 | void PozyxClass::IRQ() 31 | { 32 | _interrupt = 1; 33 | } 34 | 35 | boolean PozyxClass::waitForFlag(uint8_t interrupt_flag, int timeout_ms, uint8_t *interrupt) 36 | { 37 | long timer = millis(); 38 | int status; 39 | 40 | // stay in this loop until the event interrupt flag is set or until the the timer runs out 41 | while(millis()-timer < timeout_ms) 42 | { 43 | // in polling mode, we insert a small delay such that we don't swamp the i2c bus 44 | if( _mode == MODE_POLLING ){ 45 | delay(1); 46 | } 47 | 48 | if( (_interrupt == 1) || (_mode == MODE_POLLING)) 49 | { 50 | _interrupt = 0; 51 | 52 | // Read out the interrupt status register. After reading from this register, pozyx automatically clears the interrupt flags. 53 | uint8_t interrupt_status = 0; 54 | status = regRead(POZYX_INT_STATUS, &interrupt_status, 1); 55 | if((interrupt_status & interrupt_flag) && status == POZYX_SUCCESS) 56 | { 57 | // one of the interrupts we were waiting for arrived! 58 | if(interrupt != NULL) 59 | *interrupt = interrupt_status; 60 | return true; 61 | } 62 | } 63 | } 64 | // too bad, pozyx didn't respond 65 | // 1) pozyx can select from two pins to generate interrupts, make sure the correct pin is connected with the attachInterrupt() function. 66 | // 2) make sure the interrupt we are waiting for is enabled in the POZYX_INT_MASK register) 67 | return false; 68 | } 69 | 70 | boolean PozyxClass::waitForFlag_safe(uint8_t interrupt_flag, int timeout_ms, uint8_t *interrupt) 71 | { 72 | int tmp = _mode; 73 | _mode = MODE_POLLING; 74 | boolean result = waitForFlag(interrupt_flag, timeout_ms, interrupt); 75 | _mode = tmp; 76 | return result; 77 | } 78 | 79 | int PozyxClass::begin(boolean print_result, int mode, int interrupts, int interrupt_pin){ 80 | 81 | int status = POZYX_SUCCESS; 82 | 83 | if(print_result){ 84 | Serial.println("Pozyx Shield"); 85 | Serial.println("------------"); 86 | } 87 | 88 | // check if the mode parameter is valid 89 | if((mode != MODE_POLLING) && (mode != MODE_INTERRUPT)) 90 | return POZYX_FAILURE; 91 | 92 | // check if the pin is valid 93 | if((interrupt_pin != 0) && (interrupt_pin != 1)) 94 | return POZYX_FAILURE; 95 | 96 | Wire.begin(); 97 | 98 | // wait a bit until the pozyx board is up and running 99 | delay(250); 100 | 101 | _mode = mode; 102 | 103 | uint8_t whoami, selftest; 104 | uint8_t regs[3]; 105 | regs[2] = 0x12; 106 | 107 | // we read out the first 3 register values: who_am_i, firmware_version and harware version, respectively. 108 | if(regRead(POZYX_WHO_AM_I, regs, 3) == POZYX_FAILURE){ 109 | return POZYX_FAILURE; 110 | } 111 | whoami = regs[0]; 112 | _fw_version = regs[1]; 113 | _hw_version = regs[2]; 114 | 115 | if(print_result){ 116 | Serial.print("WhoAmI: 0x"); 117 | Serial.println(whoami, HEX); 118 | Serial.print("FW ver.: v"); 119 | Serial.print((_fw_version&0xF0)>>4); 120 | Serial.print("."); 121 | Serial.print((_fw_version&0x0F)); 122 | if(_fw_version < 0x10) 123 | Serial.print(" (please upgrade)"); 124 | Serial.print("\nHW ver.: "); 125 | Serial.println(_hw_version); 126 | } 127 | // verify if the whoami is correct 128 | if(whoami != 0x43) { 129 | // possibly the pozyx is not connected right. Also make sure the jumper of the boot pins is present. 130 | status = POZYX_FAILURE; 131 | } 132 | 133 | // readout the selftest registers to validate the proper functioning of pozyx 134 | if(regRead(POZYX_ST_RESULT, &selftest, 1) == POZYX_FAILURE){ 135 | return POZYX_FAILURE; 136 | } 137 | 138 | if(print_result){ 139 | Serial.print("selftest: 0b"); 140 | Serial.println(selftest, BIN); 141 | } 142 | 143 | if((_hw_version & POZYX_TYPE) == POZYX_TAG) 144 | { 145 | // check if the uwb, pressure sensor, accelerometer, magnetometer and gyroscope are working 146 | if(selftest != 0b00111111) { 147 | status = POZYX_FAILURE; 148 | } 149 | }else if((_hw_version & POZYX_TYPE) == POZYX_ANCHOR) 150 | { 151 | // check if the uwb transceiver and pressure sensor are working 152 | if(selftest != 0b00110000) { 153 | status = POZYX_FAILURE; 154 | } 155 | return status; 156 | } 157 | 158 | if(_mode == MODE_INTERRUPT){ 159 | // set the function that must be called upon an interrupt 160 | // put your main code here, to run repeatedly: 161 | #if defined(__SAMD21G18A__) || defined(__ATSAMD21G18A__) 162 | // Arduino Tian 163 | int tian_interrupt_pin = interrupt_pin; 164 | attachInterrupt(interrupt_pin+2, IRQ, RISING); 165 | #elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 166 | // Arduino UNO, Mega 167 | attachInterrupt(interrupt_pin, IRQ, RISING); 168 | #else 169 | Serial.println("This is not a board supported by Pozyx, interrupts may not work"); 170 | attachInterrupt(interrupt_pin, IRQ, RISING); 171 | #endif 172 | 173 | // use interrupt as provided and initiate the interrupt mask 174 | uint8_t int_mask = interrupts; 175 | configInterruptPin(5+interrupt_pin, PIN_MODE_PUSHPULL, PIN_ACTIVE_LOW, 0); 176 | 177 | if (regWrite(POZYX_INT_MASK, &int_mask, 1) == POZYX_FAILURE){ 178 | return POZYX_FAILURE; 179 | } 180 | } 181 | 182 | // all done 183 | delay(POZYX_DELAY_LOCAL_WRITE); 184 | return status; 185 | } 186 | 187 | /** 188 | * Reads a number of bytes from the specified pozyx register address using I2C 189 | */ 190 | int PozyxClass::regRead(uint8_t reg_address, uint8_t *pData, int size) 191 | { 192 | // BUFFER_LENGTH is defined in wire.h, it limits the maximum amount of bytes that can be transmitted/received with i2c in one go 193 | // because of this, we may have to split up the i2c reads in smaller chunks 194 | 195 | if(!IS_REG_READABLE(reg_address)) 196 | return POZYX_FAILURE; 197 | 198 | int n_runs = ceil((float)size / BUFFER_LENGTH); 199 | int i; 200 | int status = 1; 201 | uint8_t reg; 202 | 203 | for(i=0; i= size+1); // Arduino-specific code for the i2c 252 | assert(BUFFER_LENGTH >= param_size+1); // Arduino-specific code for the i2c 253 | 254 | if(!IS_FUNCTIONCALL(reg_address)) 255 | return POZYX_FAILURE; 256 | 257 | uint8_t status; 258 | 259 | // this feels a bit clumsy with all these memcpy's 260 | uint8_t write_data[param_size+1]; 261 | write_data[0] = reg_address; 262 | memcpy(write_data+1, params, param_size); 263 | uint8_t read_data[size+1]; 264 | 265 | // first write some data with i2c and then read some data 266 | status = i2cWriteRead(write_data, param_size + 1, read_data, size+1); 267 | if(status == POZYX_FAILURE) 268 | return status; 269 | 270 | memcpy(pData, read_data+1, size); 271 | 272 | 273 | // the first byte that a function returns is always it's success indicator, so we simply pass this through 274 | return read_data[0]; 275 | } 276 | 277 | 278 | /** 279 | * Wirelessly write a number of bytes to a specified register address on a remote Pozyx device using UWB. 280 | */ 281 | int PozyxClass::remoteRegWrite(uint16_t destination, uint8_t reg_address, uint8_t *pData, int size) 282 | { 283 | // some checks 284 | if(!IS_REG_WRITABLE(reg_address)) return POZYX_FAILURE; // the register is not writable 285 | if(size > MAX_BUF_SIZE-1) return POZYX_FAILURE; // trying to write too much data 286 | 287 | int status = 0; 288 | 289 | // first prepare the packet to send 290 | uint8_t tmp_data[size+1]; 291 | tmp_data[0] = 0; 292 | tmp_data[1] = reg_address; // the first byte is the register address we want to start writing to. 293 | memcpy(tmp_data+2, pData, size); // the remaining bytes are the data bytes to be written starting at the register address. 294 | status = regFunction(POZYX_TX_DATA, (uint8_t *)&tmp_data, size+2, NULL, 0); 295 | 296 | // stop if POZYX_TX_DATA returned an error. 297 | if(status == POZYX_FAILURE) 298 | return status; 299 | 300 | // send the packet 301 | uint8_t params[3]; 302 | params[0] = (uint8_t)destination; 303 | params[1] = (uint8_t)(destination>>8); 304 | params[2] = 0x04; // flag to indicate a register write 305 | 306 | uint8_t int_status = 0; 307 | regRead(POZYX_INT_STATUS, &int_status, 1); // first clear out the interrupt status register by reading from it 308 | status = regFunction(POZYX_TX_SEND, (uint8_t *)¶ms, 3, NULL, 0); 309 | 310 | if (waitForFlag_safe(POZYX_INT_STATUS_FUNC | POZYX_INT_STATUS_ERR, 100, &int_status)){ 311 | if((int_status & POZYX_INT_STATUS_ERR) == POZYX_INT_STATUS_ERR) 312 | { 313 | // An error occured during positioning. 314 | // Please read out the register POZYX_ERRORCODE to obtain more information about the error 315 | return POZYX_FAILURE; 316 | }else{ 317 | return POZYX_SUCCESS; 318 | } 319 | }else{ 320 | return POZYX_TIMEOUT; 321 | } 322 | 323 | return status; 324 | } 325 | 326 | /** 327 | * Wirelessly read a number of bytes from a specified register address on a remote Pozyx device using UWB. 328 | */ 329 | int PozyxClass::remoteRegRead(uint16_t destination, uint8_t reg_address, uint8_t *pData, int size) 330 | { 331 | // some checks 332 | if(!IS_REG_READABLE(reg_address)) return POZYX_FAILURE; // the register is not readable 333 | if(size > MAX_BUF_SIZE) return POZYX_FAILURE; // trying to read too much data 334 | if(destination == 0) return POZYX_FAILURE; // remote read not allowed in broadcast mode 335 | 336 | int status = 0; 337 | 338 | // first prepare the packet to send 339 | uint8_t tmp_data[3]; 340 | tmp_data[0] = 0; // the offset in the TX buffer 341 | tmp_data[1] = reg_address; // the first byte is the register address we want to start reading from 342 | tmp_data[2] = size; // the number of bytes to read starting from the register address 343 | status = regFunction(POZYX_TX_DATA, (uint8_t *)&tmp_data, 3, NULL, 0); 344 | 345 | // stop if POZYX_TX_DATA returned an error. 346 | if(status == POZYX_FAILURE) 347 | return status; 348 | 349 | // send the packet 350 | uint8_t params[3]; 351 | params[0] = (uint8_t)destination; 352 | params[1] = (uint8_t)(destination>>8); 353 | params[2] = 0x02; // flag to indicate a register read 354 | 355 | uint8_t int_status = 0; 356 | regRead(POZYX_INT_STATUS, &int_status, 1); // first clear out the interrupt status register by reading from it 357 | status = regFunction(POZYX_TX_SEND, (uint8_t *)¶ms, 3, NULL, 0); 358 | 359 | // stop if POZYX_TX_SEND returned an error. 360 | if(status == POZYX_FAILURE) 361 | return status; 362 | 363 | // wait up to x ms to receive a response 364 | if(waitForFlag_safe(POZYX_INT_STATUS_FUNC | POZYX_INT_STATUS_ERR, 1000, &int_status)) 365 | { 366 | if((int_status & POZYX_INT_STATUS_ERR) == POZYX_INT_STATUS_ERR) 367 | { 368 | // An error occured during positioning. 369 | // Please read out the register POZYX_ERRORCODE to obtain more information about the error 370 | return POZYX_FAILURE; 371 | }else{ 372 | // we received a response, now get some information about the response 373 | uint8_t rx_info[3]= {0,0,0}; 374 | regRead(POZYX_RX_NETWORK_ID, rx_info, 3); 375 | uint16_t remote_network_id = rx_info[0] + ((uint16_t)rx_info[1]<<8); 376 | uint8_t data_len = rx_info[2]; 377 | 378 | if( remote_network_id == destination && data_len == size) 379 | { 380 | status = readRXBufferData(pData, size); 381 | return status; 382 | }else{ 383 | return POZYX_FAILURE; 384 | } 385 | } 386 | 387 | }else{ 388 | // timeout 389 | return POZYX_TIMEOUT; 390 | } 391 | } 392 | 393 | /* 394 | * Wirelessly call a register function with given parameters on a remote Pozyx device using UWB, the data from the function is stored in pData 395 | */ 396 | int PozyxClass::remoteRegFunction(uint16_t destination, uint8_t reg_address, uint8_t *params, int param_size, uint8_t *pData, int size) 397 | { 398 | // some checks 399 | if(!IS_FUNCTIONCALL(reg_address)) return POZYX_FAILURE; // the register is not a function 400 | 401 | int status = 0; 402 | 403 | // first prepare the packet to send 404 | uint8_t tmp_data[param_size+2]; 405 | tmp_data[0] = 0; 406 | tmp_data[1] = reg_address; // the first byte is the function register address we want to call. 407 | memcpy(tmp_data+2, params, param_size); // the remaining bytes are the parameter bytes for the function. 408 | status = regFunction(POZYX_TX_DATA, tmp_data, param_size+2, NULL, 0); 409 | 410 | // stop if POZYX_TX_DATA returned an error. 411 | if(status == POZYX_FAILURE) 412 | { 413 | return status; 414 | } 415 | 416 | // send the packet 417 | uint8_t tx_params[3]; 418 | tx_params[0] = (uint8_t)destination; 419 | tx_params[1] = (uint8_t)(destination>>8); 420 | tx_params[2] = 0x08; // flag to indicate a register function call 421 | uint8_t int_status = 0; 422 | regRead(POZYX_INT_STATUS, &int_status, 1); // first clear out the interrupt status register by reading from it 423 | status = regFunction(POZYX_TX_SEND, tx_params, 3, NULL, 0); 424 | 425 | // stop if POZYX_TX_SEND returned an error. 426 | if(status == POZYX_FAILURE){ 427 | return status; 428 | } 429 | 430 | // wait up to x ms to receive a response 431 | if(waitForFlag_safe(POZYX_INT_STATUS_FUNC | POZYX_INT_STATUS_ERR, 1000, &int_status)) 432 | { 433 | if((int_status & POZYX_INT_STATUS_ERR) == POZYX_INT_STATUS_ERR) 434 | { 435 | return POZYX_FAILURE; 436 | }else 437 | { 438 | // we received a response, now get some information about the response 439 | uint8_t rx_info[3]; 440 | regRead(POZYX_RX_NETWORK_ID, rx_info, 3); 441 | uint16_t remote_network_id = rx_info[0] + ((uint16_t)rx_info[1]<<8); 442 | uint8_t data_len = rx_info[2]; 443 | 444 | if( remote_network_id == destination && data_len == size+1) 445 | { 446 | uint8_t return_data[size+1]; 447 | 448 | status = readRXBufferData(return_data, size+1); 449 | 450 | if(status == POZYX_FAILURE){ 451 | // debug information 452 | return status; 453 | } 454 | 455 | memcpy(pData, return_data+1, size); 456 | 457 | return return_data[0]; 458 | }else{ 459 | return POZYX_FAILURE; 460 | } 461 | } 462 | 463 | }else{ 464 | // timeout 465 | return POZYX_TIMEOUT; 466 | } 467 | } 468 | 469 | int PozyxClass::writeTXBufferData(uint8_t data[], int size, int offset) 470 | { 471 | if (offset + size > MAX_BUF_SIZE){ 472 | return POZYX_FAILURE; 473 | } 474 | 475 | int i, status = 1; 476 | int max_bytes = BUFFER_LENGTH-2; 477 | int n_runs = ceil((float)size / max_bytes); 478 | uint8_t params[BUFFER_LENGTH]; 479 | 480 | // read out the received data. 481 | for(i=0; i MAX_BUF_SIZE){ 499 | return POZYX_FAILURE; 500 | } 501 | 502 | int status; 503 | int i; 504 | uint8_t params[2]; 505 | int max_bytes = BUFFER_LENGTH-1; 506 | int n_runs = ceil((float)size / max_bytes); 507 | 508 | // read out the received data. 509 | for(i=0; i>8); 530 | params[2] = 0x06; 531 | status = regFunction(POZYX_TX_SEND, (uint8_t *)¶ms, 3, NULL, 0); 532 | delay(POZYX_DELAY_LOCAL_FUNCTION); 533 | 534 | return status; 535 | } 536 | 537 | 538 | /* 539 | * This function sends some data bytes to the destination 540 | */ 541 | int PozyxClass::sendData(uint16_t destination, uint8_t *pData, int size) 542 | { 543 | if(size > MAX_BUF_SIZE) return POZYX_FAILURE; // trying to send too much data 544 | 545 | uint8_t status = 0; 546 | 547 | uint8_t tmp_data[size+1]; 548 | tmp_data[0] = 0; // the first byte is the offset byte. 549 | memcpy(tmp_data+1, pData, size); 550 | 551 | // set the TX buffer 552 | status = regFunction(POZYX_TX_DATA, tmp_data, size+1, NULL, 0); 553 | 554 | // stop if POZYX_TX_DATA returned an error. 555 | if(status == POZYX_FAILURE) 556 | return status; 557 | 558 | // send the packet 559 | uint8_t params[3]; 560 | params[0] = (uint8_t)destination; 561 | params[1] = (uint8_t)(destination>>8); 562 | params[2] = 0x06; // flag to indicate we're just sending data 563 | status = regFunction(POZYX_TX_SEND, (uint8_t *)¶ms, 3, NULL, 0); 564 | 565 | return status; 566 | } 567 | 568 | /** 569 | * Writes a number of bytes to the specified pozyx register address using I2C 570 | */ 571 | int PozyxClass::i2cWriteWrite(const uint8_t reg_address, const uint8_t *pData, int size) 572 | { 573 | /*Serial.print("\t\ti2cWriteWrite(0x"); 574 | Serial.print(reg_address, HEX); 575 | Serial.print(", [0x"); 576 | for(int i = 0; i < size; i++) { 577 | if(i>0) 578 | Serial.print(", 0x"); 579 | Serial.print(pData[i], HEX); 580 | } 581 | Serial.print("], "); 582 | Serial.print(size); 583 | Serial.println(")");*/ 584 | 585 | int n, error; 586 | 587 | Wire.beginTransmission(POZYX_I2C_ADDRESS); 588 | // write the starting register address 589 | n = Wire.write(reg_address); 590 | if (n != 1) 591 | return (POZYX_FAILURE); 592 | 593 | error = Wire.endTransmission(false); // hold the bus for a repeated start 594 | if (error != 0) 595 | return (POZYX_FAILURE); 596 | 597 | Wire.beginTransmission(POZYX_I2C_ADDRESS); 598 | n = Wire.write(pData, size); // write data bytes 599 | if (n != size) 600 | return (POZYX_FAILURE); 601 | 602 | error = Wire.endTransmission(true); // release the I2C-bus 603 | if (error != 0) 604 | return (POZYX_FAILURE); 605 | 606 | //Serial.println("\t\t\tsuccess"); 607 | 608 | return (POZYX_SUCCESS); // return : no error 609 | } 610 | 611 | /** 612 | * Call a register function using I2C with given parameters 613 | */ 614 | int PozyxClass::i2cWriteRead(uint8_t* write_data, int write_len, uint8_t* read_data, int read_len) 615 | { 616 | /*Serial.print("\t\ti2cWriteRead([0x"); 617 | for(int i = 0; i < write_len; i++) { 618 | if(i>0) 619 | Serial.print(", 0x"); 620 | Serial.print(write_data[i],HEX); 621 | } 622 | Serial.print("], uint8_t* read_data, "); 623 | Serial.print(read_len); 624 | Serial.println(")");*/ 625 | 626 | int i, n; 627 | 628 | Wire.beginTransmission(POZYX_I2C_ADDRESS); 629 | for(i=0; i0) 663 | Serial.print(", 0x"); 664 | Serial.print(read_data[i], HEX); 665 | } 666 | Serial.println("]");*/ 667 | 668 | return (POZYX_SUCCESS); // return : no error 669 | } 670 | 671 | PozyxClass Pozyx; 672 | -------------------------------------------------------------------------------- /Pozyx_definitions.h: -------------------------------------------------------------------------------- 1 | /* 2 | Pozyx_definitions.h - Library for Arduino Pozyx shield. 3 | Copyright (c) Pozyx Laboratories. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | #ifndef POZYX_DEFINITIONS_h 21 | #define POZYX_DEFINITIONS_h 22 | 23 | #define _POZYX_LIB_VERSION 1 24 | #define _POZYX_FEET_PER_METER 3.2808399 25 | #define _POZYX_INCH_PER_METER 39.3700787 26 | 27 | #define _POZYX_FORMAT_METER 28 | #define _POZYX_FORMAT_FOOT 29 | #define _POZYX_FORMAT_INCH 30 | 31 | /* Maximum number of anchors to be stored in the interal anchor list on the pozyx device */ 32 | #define MAX_ANCHORS_IN_LIST 20 33 | 34 | #define POZYX_I2C_ADDRESS 0x4B 35 | 36 | /* Begin auto generated defines */ 37 | /* POZYX REGISTERS firmware version v1.0 */ 38 | 39 | /* Status registers */ 40 | 41 | #define POZYX_WHO_AM_I 0x0 /* Returns the constant value 0x43. */ 42 | #define POZYX_FIRMWARE_VER 0x1 /* Returns the POZYX firmware version. */ 43 | #define POZYX_HARDWARE_VER 0x2 /* Returns the POZYX hardware version. */ 44 | #define POZYX_ST_RESULT 0x3 /* Returns the self-test result */ 45 | #define POZYX_ERRORCODE 0x4 /* Describes a possibly system error. */ 46 | #define POZYX_INT_STATUS 0x5 /* Indicates the source of the interrupt. */ 47 | #define POZYX_CALIB_STATUS 0x6 /* Returns the calibration status. */ 48 | 49 | /* Configuration registers */ 50 | 51 | #define POZYX_INT_MASK 0x10 /* Indicates which interrupts are enabled. */ 52 | #define POZYX_INT_CONFIG 0x11 /* Configure the interrupt pin */ 53 | #define POZYX_POS_FILTER 0x14 /* Pozyx filter configuration register */ 54 | #define POZYX_CONFIG_LEDS 0x15 /* Configure the LEDs */ 55 | #define POZYX_POS_ALG 0x16 /* Algorithm used for positioning */ 56 | #define POZYX_POS_NUM_ANCHORS 0x17 /* Configure the number of anchors and selection procedure */ 57 | #define POZYX_POS_INTERVAL 0x18 /* Defines the update interval in ms in continuous positioning. */ 58 | #define POZYX_NETWORK_ID 0x1A /* The network id. */ 59 | #define POZYX_UWB_CHANNEL 0x1C /* UWB channel number. */ 60 | #define POZYX_UWB_RATES 0x1D /* Configure the UWB datarate and pulse repetition frequency (PRF) */ 61 | #define POZYX_UWB_PLEN 0x1E /* Configure the UWB preamble length. */ 62 | #define POZYX_UWB_GAIN 0x1F /* Configure the power gain for the UWB transmitter */ 63 | #define POZYX_UWB_XTALTRIM 0x20 /* Trimming value for the uwb crystal. */ 64 | #define POZYX_RANGE_PROTOCOL 0x21 /* The ranging protocol */ 65 | #define POZYX_OPERATION_MODE 0x22 /* Configure the mode of operation of the pozyx device */ 66 | #define POZYX_SENSORS_MODE 0x23 /* Configure the mode of operation of the sensors */ 67 | #define POZYX_CONFIG_GPIO1 0x27 /* Configure GPIO pin 1. */ 68 | #define POZYX_CONFIG_GPIO2 0x28 /* Configure GPIO pin 2. */ 69 | #define POZYX_CONFIG_GPIO3 0x29 /* Configure GPIO pin 3. */ 70 | #define POZYX_CONFIG_GPIO4 0x2A /* Configure GPIO pin 4. */ 71 | 72 | /* Positioning data */ 73 | 74 | #define POZYX_POS_X 0x30 /* x-coordinate of the device in mm. */ 75 | #define POZYX_POS_Y 0x34 /* y-coordinate of the device in mm. */ 76 | #define POZYX_POS_Z 0x38 /* z-coordinate of the device in mm. */ 77 | #define POZYX_POS_ERR_X 0x3C /* estimated error covariance of x */ 78 | #define POZYX_POS_ERR_Y 0x3E /* estimated error covariance of y */ 79 | #define POZYX_POS_ERR_Z 0x40 /* estimated error covariance of z */ 80 | #define POZYX_POS_ERR_XY 0x42 /* estimated covariance of xy */ 81 | #define POZYX_POS_ERR_XZ 0x44 /* estimated covariance of xz */ 82 | #define POZYX_POS_ERR_YZ 0x46 /* estimated covariance of yz */ 83 | 84 | #define POZYX_MAX_LIN_ACC 0x4E /* maximum linear acceleration since */ 85 | 86 | /* Sensor data */ 87 | 88 | #define POZYX_PRESSURE 0x50 /* Pressure data */ 89 | #define POZYX_ACCEL_X 0x54 /* Accelerometer data (in mg) */ 90 | #define POZYX_ACCEL_Y 0x56 /* */ 91 | #define POZYX_ACCEL_Z 0x58 /* */ 92 | #define POZYX_MAGN_X 0x5A /* Magnemtometer data */ 93 | #define POZYX_MAGN_Y 0x5C /* */ 94 | #define POZYX_MAGN_Z 0x5E /* */ 95 | #define POZYX_GYRO_X 0x60 /* Gyroscope data */ 96 | #define POZYX_GYRO_Y 0x62 /* */ 97 | #define POZYX_GYRO_Z 0x64 /* */ 98 | #define POZYX_EUL_HEADING 0x66 /* Euler angles heading (or yaw) */ 99 | #define POZYX_EUL_ROLL 0x68 /* Euler angles roll */ 100 | #define POZYX_EUL_PITCH 0x6A /* Euler angles pitch */ 101 | #define POZYX_QUAT_W 0x6C /* Weight of quaternion. */ 102 | #define POZYX_QUAT_X 0x6E /* x of quaternion */ 103 | #define POZYX_QUAT_Y 0x70 /* y of quaternion */ 104 | #define POZYX_QUAT_Z 0x72 /* z of quaternion */ 105 | #define POZYX_LIA_X 0x74 /* Linear acceleration in x-direction */ 106 | #define POZYX_LIA_Y 0x76 /* */ 107 | #define POZYX_LIA_Z 0x78 /* */ 108 | #define POZYX_GRAV_X 0x7A /* x-component of gravity vector */ 109 | #define POZYX_GRAV_Y 0x7C /* y-component of gravity vector */ 110 | #define POZYX_GRAV_Z 0x7E /* z-component of gravity vector */ 111 | #define POZYX_TEMPERATURE 0x80 /* Temperature */ 112 | 113 | /* General data */ 114 | 115 | #define POZYX_DEVICE_LIST_SIZE 0x81 /* Returns the number of devices stored internally */ 116 | #define POZYX_RX_NETWORK_ID 0x82 /* The network id of the latest received message */ 117 | #define POZYX_RX_DATA_LEN 0x84 /* The length of the latest received message */ 118 | #define POZYX_GPIO1 0x85 /* Value of the GPIO pin 1 */ 119 | #define POZYX_GPIO2 0x86 /* Value of the GPIO pin 2 */ 120 | #define POZYX_GPIO3 0x87 /* Value of the GPIO pin 3 */ 121 | #define POZYX_GPIO4 0x88 /* Value of the GPIO pin 4 */ 122 | 123 | /*Functions*/ 124 | 125 | #define POZYX_RESET_SYS 0xB0 /* Reset the Pozyx device */ 126 | #define POZYX_LED_CTRL 0xB1 /* Control LEDS 1 to 4 on the board */ 127 | #define POZYX_TX_DATA 0xB2 /* Write data in the UWB transmit (TX) buffer */ 128 | #define POZYX_TX_SEND 0xB3 /* Transmit the TX buffer to some other pozyx device */ 129 | #define POZYX_RX_DATA 0xB4 /* Read data from the UWB receive (RX) buffer */ 130 | #define POZYX_DO_RANGING 0xB5 /* Initiate ranging measurement */ 131 | #define POZYX_DO_POSITIONING 0xB6 /* Initiate the positioning process. */ 132 | #define POZYX_POS_SET_ANCHOR_IDS 0xB7 /* Set the list of anchor ID's used for positioning. */ 133 | #define POZYX_POS_GET_ANCHOR_IDS 0xB8 /* Read the list of anchor ID's used for positioning. */ 134 | #define POZYX_FLASH_RESET 0xB9 /* Reset a portion of the configuration in flash memory */ 135 | #define POZYX_FLASH_SAVE 0xBA /* Store a portion of the configuration in flash memory */ 136 | #define POZYX_FLASH_DETAILS 0xBB /* Return information on what is stored in flash */ 137 | 138 | /*Device list functions*/ 139 | 140 | #define POZYX_DEVICES_GETIDS 0xC0 /* Get all the network IDs's of devices in the device list. */ 141 | #define POZYX_DEVICES_DISCOVER 0xC1 /* Obtain the network ID's of all pozyx devices within range. */ 142 | #define POZYX_DEVICES_CALIBRATE 0xC2 /* Obtain the coordinates of the pozyx (anchor) devices within range. */ 143 | #define POZYX_DEVICES_CLEAR 0xC3 /* Clear the list of all pozyx devices. */ 144 | #define POZYX_DEVICE_ADD 0xC4 /* Add a pozyx device to the devices list */ 145 | #define POZYX_DEVICE_GETINFO 0xC5 /* Get the stored device information for a given pozyx device */ 146 | #define POZYX_DEVICE_GETCOORDS 0xC6 /* Get the stored coordinates of a given pozyx device */ 147 | #define POZYX_DEVICE_GETRANGEINFO 0xC7 /* Get the stored range inforamation of a given pozyx device */ 148 | #define POZYX_CIR_DATA 0xC8 /* Get the channel impulse response (CIR) coefficients */ 149 | 150 | /* Macro's to test if registers are readable/writable */ 151 | 152 | #define IS_REG_READABLE(x) (((((x)>=0x0)&&((x)<0x7)) || (((x)>=0x10)&&((x)<0x12)) || (((x)>=0x14)&&((x)<0x24)) || (((x)>=0x27)&&((x)<0x2b)) || (((x)>=0x30)&&((x)<0x48)) || (((x)>=0x4E)&&((x)<0x89)))?1:0) 153 | #define IS_REG_WRITABLE(x) (((((x)>=0x10)&&((x)<0x12)) || (((x)>=0x14)&&((x)<0x24)) || (((x)>=0x27)&&((x)<0x2b)) || (((x)>=0x30)&&((x)<0x3c)) || (((x)>=0x85)&&((x)<0x89)))?1:0) 154 | #define IS_FUNCTIONCALL(x) (((((x)>=0xb0)&&((x)<0xbc)) || (((x)>=0xc0)&&((x)<0xc9)))?1:0) 155 | 156 | /* End of auto generated defines */ 157 | 158 | /* Bit mask for POZYX_ST_RESULT */ 159 | #define POZYX_ST_RESULT_ACC 0x01 160 | #define POZYX_ST_RESULT_MAGN 0x02 161 | #define POZYX_ST_RESULT_GYR 0x04 162 | #define POZYX_ST_RESULT_MCU 0x08 163 | #define POZYX_ST_RESULT_PRES 0x10 164 | #define POZYX_ST_RESULT_UWB 0x20 165 | 166 | /* Bit mask for POZYX_INT_STATUS */ 167 | #define POZYX_INT_STATUS_ERR 0x01 168 | #define POZYX_INT_STATUS_POS 0x02 169 | #define POZYX_INT_STATUS_IMU 0x04 170 | #define POZYX_INT_STATUS_RX_DATA 0x08 171 | #define POZYX_INT_STATUS_FUNC 0x10 172 | 173 | /* Bit mask for POZYX_INT_MASK */ 174 | #define POZYX_INT_MASK_ERR 0x01 175 | #define POZYX_INT_MASK_POS 0x02 176 | #define POZYX_INT_MASK_IMU 0x04 177 | #define POZYX_INT_MASK_RX_DATA 0x08 178 | #define POZYX_INT_MASK_FUNC 0x10 179 | #define POZYX_INT_MASK_TDMA 0x40 180 | #define POZYX_INT_MASK_PIN 0x80 181 | 182 | /* Bit mask for POZYX_POS_ALG */ 183 | #define POZYX_POS_ALG_UWB_ONLY 0x00 184 | #define POZYX_POS_ALG_LS 0x00 // deprecated so default to UWB only 185 | #define POZYX_POS_ALG_TRACKING 0x04 186 | 187 | /* Bit masks for POZYX_POS_FILTER */ 188 | #define FILTER_TYPE_NONE 0x00 189 | #define FILTER_TYPE_FIR 0x01 190 | #define FILTER_TYPE_MOVINGAVERAGE 0x03 191 | #define FILTER_TYPE_MOVINGMEDIAN 0x04 192 | 193 | /* Bit mask for POZYX_RANGE_PROTOCOL */ 194 | #define POZYX_RANGE_PROTOCOL_PRECISION 0x00 195 | #define POZYX_RANGE_PROTOCOL_FAST 0x01 196 | #define POZYX_RANGE_PROTOCOL_TEST 0x02 197 | 198 | /* Bit mask for POZYX_LED_CTRL */ 199 | #define POZYX_LED_CTRL_LED1 0x01 200 | #define POZYX_LED_CTRL_LED2 0x02 201 | #define POZYX_LED_CTRL_LED3 0x04 202 | #define POZYX_LED_CTRL_LED4 0x08 203 | 204 | 205 | 206 | #define POZYX_TYPE 0xE0 207 | #define POZYX_ANCHOR 0x00 208 | #define POZYX_TAG 0x20 209 | 210 | #define MAX_BUF_SIZE 100 211 | 212 | 213 | #define POZYX_INT_MASK_ALL 0x1F 214 | #define POZYX_DELAY_LOCAL_WRITE 1 215 | #define POZYX_DELAY_LOCAL_FUNCTION 5 216 | #define POZYX_DELAY_REMOTE_WRITE 5 217 | #define POZYX_DELAY_REMOTE_FUNCTION 10 218 | #define POZYX_DELAY_INTERRUPT 100 219 | #define POZYX_DELAY_POSITIONING 1000 220 | #define POZYX_DELAY_REMOTE_POSITIONING 1000 221 | #define POZYX_DELAY_CALIBRATION 1000 222 | #define POZYX_FAILURE 0x0 223 | #define POZYX_SUCCESS 0x1 224 | #define POZYX_TIMEOUT 0x8 225 | #define POZYX_3D 3 226 | #define POZYX_2D 2 227 | #define POZYX_2_5D 1 228 | #define POZYX_INT_PIN0 0x0 229 | #define POZYX_INT_PIN1 0x1 230 | 231 | 232 | #define POZYX_LED_CTRL_LEDRX 0x10 233 | #define POZYX_LED_CTRL_LEDTX 0x20 234 | 235 | #define POZYX_ANCHOR_MODE 0 236 | #define POZYX_TAG_MODE 1 237 | 238 | // The GPIO modes 239 | #define POZYX_GPIO_DIGITAL_INPUT 0 240 | #define POZYX_GPIO_PUSHPULL 1 241 | #define POZYX_GPIO_OPENDRAIN 1 242 | 243 | // The GPIO pull resistor configuration 244 | #define POZYX_GPIO_NOPULL 0 245 | #define POZYX_GPIO_PULLUP 1 246 | #define POZYX_GPIO_PULLDOWN 2 247 | 248 | // anchor selection modes 249 | #define POZYX_ANCHOR_SEL_MANUAL 0 250 | #define POZYX_ANCHOR_SEL_AUTO 1 251 | 252 | // discovery options 253 | #define POZYX_DISCOVERY_ANCHORS_ONLY 0 254 | #define POZYX_DISCOVERY_TAGS_ONLY 1 255 | #define POZYX_DISCOVERY_ALL_DEVICES 2 256 | 257 | 258 | // how to intercept pozyx events: by polling or by interrupts 259 | #define MODE_POLLING 0 260 | #define MODE_INTERRUPT 1 261 | 262 | // Division factors for converting the raw register values to meaningful physical quantities 263 | #define POZYX_POS_DIV_MM 1.0f 264 | #define POZYX_PRESS_DIVPOSITIONING 1000 265 | #define POZYX_PRESS_DIVREMOTE_POSITIONING 1000 266 | #define POZYX_PRESS_DIV_PA 1000.0f 267 | #define POZYX_ACCEL_DIV_MG 1.0f 268 | #define POZYX_GYRO_DIV_DPS 16.0f 269 | #define POZYX_MAG_DIV_UT 16.0f 270 | #define POZYX_EULER_DIV_DEG 16.0f 271 | #define POZYX_QUAT_DIV 16384.0f 272 | #define POZYX_TEMP_DIV_CELSIUS 1.0f 273 | 274 | // error-code defintions 275 | #define POZYX_ERROR_NONE 0x00 276 | #define POZYX_ERROR_I2C_WRITE 0x01 277 | #define POZYX_ERROR_I2C_CMDFULL 0x02 278 | #define POZYX_ERROR_ANCHOR_ADD 0x03 279 | #define POZYX_ERROR_COMM_QUEUE_FULL 0x04 280 | #define POZYX_ERROR_I2C_READ 0x05 281 | #define POZYX_ERROR_UWB_CONFIG 0x06 282 | #define POZYX_ERROR_OPERATION_QUEUE_FULL 0x07 283 | #define POZYX_ERROR_TDMA 0xA0 284 | #define POZYX_ERROR_STARTUP_BUSFAULT 0x08 285 | #define POZYX_ERROR_FLASH_INVALID 0x09 286 | #define POZYX_ERROR_NOT_ENOUGH_ANCHORS 0x0A 287 | #define POZYX_ERROR_DISCOVERY 0X0B 288 | #define POZYX_ERROR_CALIBRATION 0x0C 289 | #define POZYX_ERROR_FUNC_PARAM 0x0D 290 | #define POZYX_ERROR_ANCHOR_NOT_FOUND 0x0E 291 | #define POZYX_ERROR_FLASH 0x0F 292 | #define POZYX_ERROR_MEMORY 0x10 293 | #define POZYX_ERROR_RANGING 0x11 294 | #define POZYX_ERROR_RTIMEOUT1 0x12 295 | #define POZYX_ERROR_RTIMEOUT2 0x13 296 | #define POZYX_ERROR_TXLATE 0x14 297 | #define POZYX_ERROR_UWB_BUSY 0x15 298 | #define POZYX_ERROR_POSALG 0x16 299 | #define POZYX_ERROR_NOACK 0x17 300 | #define POZYX_ERROR_SNIFF_OVERFLOW 0xE0 301 | #define POZYX_ERROR_NO_PPS 0xF0 302 | #define POZYX_ERROR_NEW_TASK 0xF1 303 | #define POZYX_ERROR_UNRECDEV 0xFE 304 | #define POZYX_ERROR_GENERAL 0xFF 305 | 306 | // flash configuration types 307 | #define POZYX_FLASH_REGS 1 308 | #define POZYX_FLASH_ANCHOR_IDS 2 309 | #define POZYX_FLASH_NETWORK 3 310 | 311 | 312 | // possible interrupt pin configuration settings 313 | #define PIN_MODE_PUSHPULL 0 314 | #define PIN_ACTIVE_LOW 0 315 | #define PIN_ACTIVE_HIGH 1 316 | 317 | #endif 318 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Pozyx-Arduino-library 2 | An Arduino library in C to work with the [Pozyx Creator kit](https://www.pozyx.io/creator). 3 | The [Pozyx](https://www.pozyx.io) creator kit has everything required to achieve accurate indoor positioning for a hobby project and consists out of an Arduino-compatible shield equiped with the Qorvo DW1000 UWB chip, as well as several stand-alone reference anchors. 4 | 5 | ## Prerequisites 6 | Please download a release version for a safely stable experience! 7 | 8 | The library requires **firmware version 1.1** installed on the Pozyx devices. 9 | If you're on an older version of the firmware and do not want to upgrade for some reason, please download an older release of the library according to their respective release description. 10 | 11 | ## Documentation and examples 12 | You can find the Arduino tutorials on our documentation site: [https://docs.pozyx.io/creator/latest/arduino.](https://docs.pozyx.io/creator/arduino) 13 | 14 | Documentation for the Arduino library can be found here: https://ardupozyx.readthedocs.io. 15 | 16 | If you encounter any issues, please send a mail to support@pozyx.io instead of creating an issue here. 17 | 18 | The following folders can be found together with this library: 19 | 20 | 1. **examples**. These example scripts showcase some basic functionality of the Pozyx device, each example comes with a tutorial that can be found on the pozyx website https://docs.pozyx.io/creator/arduino 21 | 2. **unit_test**. This folder contains a collection of Arduino scripts that can be run to test certain functionalities of the Pozyx device. They also serve as some good examples for some Arduino library functions. 22 | 3. **useful**. This folder contains a number of useful Arduino sketches that provide basic functionality such as discovering all pozyx devices or configuring them. 23 | -------------------------------------------------------------------------------- /changelog.md: -------------------------------------------------------------------------------- 1 | # Arduino Pozyx library change log 2 | 3 | ## Version 1.0.1 4 | - Added function documentation for doxygen 5 | - Added defines for parameter descriptions 6 | - Added assertions to all functions for checking API function parameters 7 | - Added function getErrorCode 8 | - Added function setTxPower, getTxPower 9 | - Added function setUWBChannel, getUWBChannel 10 | - Added unit tests for platform specific functions 11 | - The sensor functions now return the sensor values in metric numbers instead of raw numbers 12 | - Removed spurious Serial.print lines 13 | - Rename function getAcceleration to getAcceleration_mg 14 | - Rename function getGyro to getAngularVelocity_dps 15 | - Rename function getEulerAngles to getEulerAngles_deg 16 | - Rename function getGravity to getGravityVector_mg 17 | - Rename function getLinearAcceleration to getLinearAcceleration_mg 18 | - Rename function getSensorData to getAllSensorData 19 | - Rename function getPressure to getPressure_Pa 20 | - Rename function getMagnetic to getMagnetic_uT 21 | - Rename function getTemperature to getTemperature_c 22 | 23 | ## Version 1.0.0 24 | - initial release 25 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | SPHINXPROJ = PozyxArduinoLibrary 8 | SOURCEDIR = . 9 | BUILDDIR = _build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) -------------------------------------------------------------------------------- /docs/api/index.rst: -------------------------------------------------------------------------------- 1 | .. toctree:: 2 | :maxdepth: 2 3 | :caption: Contents: 4 | 5 | pozyx_functions 6 | pozyx_class 7 | structs 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /docs/api/pozyx_class.rst: -------------------------------------------------------------------------------- 1 | Pozyx Class Full 2 | ================ 3 | 4 | Members 5 | ------- 6 | 7 | .. doxygenclass:: PozyxClass 8 | :members: 9 | 10 | 11 | Protected 12 | --------- 13 | 14 | .. doxygenclass:: PozyxClass 15 | :protected-members: 16 | 17 | 18 | Private 19 | ------- 20 | 21 | .. doxygenclass:: PozyxClass 22 | :private-members: 23 | 24 | 25 | Undocumented... 26 | --------------- 27 | 28 | .. doxygenclass:: PozyxClass 29 | :undoc-members: -------------------------------------------------------------------------------- /docs/api/pozyx_functions.rst: -------------------------------------------------------------------------------- 1 | Pozyx Class Full 2 | ================ 3 | 4 | 5 | Core 6 | ---- 7 | 8 | .. doxygengroup:: core 9 | :members: 10 | 11 | 12 | System functions 13 | ---------------- 14 | 15 | .. doxygengroup:: System_functions 16 | :members: 17 | 18 | 19 | Communication 20 | ------------- 21 | 22 | .. doxygengroup:: Communication_functions 23 | :members: 24 | 25 | 26 | Device list 27 | ------------- 28 | 29 | .. doxygengroup:: device_list 30 | :members: 31 | 32 | 33 | Positioning 34 | ------------- 35 | 36 | .. doxygengroup:: Positioning_functions 37 | :members: 38 | 39 | 40 | Sensor Data 41 | ------------- 42 | 43 | .. doxygengroup:: sensor_data 44 | :members: 45 | 46 | -------------------------------------------------------------------------------- /docs/api/structs.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pozyxLabs/Pozyx-Arduino-library/e621c2e42aa55eb522a3e1ddbd91ddd80af42e09/docs/api/structs.rst -------------------------------------------------------------------------------- /docs/conf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # Configuration file for the Sphinx documentation builder. 4 | # 5 | # This file does only contain a selection of the most common options. For a 6 | # full list see the documentation: 7 | # http://www.sphinx-doc.org/en/master/config 8 | 9 | # -- Path setup -------------------------------------------------------------- 10 | 11 | # If extensions (or modules to document with autodoc) are in another directory, 12 | # add these directories to sys.path here. If the directory is relative to the 13 | # documentation root, use os.path.abspath to make it absolute, like shown here. 14 | # 15 | # import os 16 | # import sys 17 | # sys.path.insert(0, os.path.abspath('.')) 18 | 19 | 20 | # -- Project information ----------------------------------------------------- 21 | 22 | project = 'Pozyx Arduino Library' 23 | copyright = '2018, Samuel Van de Velde, Vadim Vermeiren, Laurent Van Acker' 24 | author = 'Samuel Van de Velde, Vadim Vermeiren, Laurent Van Acker' 25 | 26 | # The short X.Y version 27 | version = '1.2' 28 | # The full version, including alpha/beta/rc tags 29 | release = '1.2.2' 30 | 31 | 32 | # -- General configuration --------------------------------------------------- 33 | 34 | # If your documentation needs a minimal Sphinx version, state it here. 35 | # 36 | # needs_sphinx = '1.0' 37 | 38 | # Add any Sphinx extension module names here, as strings. They can be 39 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 40 | # ones. 41 | extensions = [ 42 | 'breathe', 43 | ] 44 | 45 | 46 | breathe_projects = { "ardupozyx": '/'.join(__file__.split('/')[:-1]) + "/doxygen_output/xml"} 47 | 48 | breathe_default_project = "ardupozyx" 49 | 50 | # Add any paths that contain templates here, relative to this directory. 51 | templates_path = ['_templates'] 52 | 53 | # The suffix(es) of source filenames. 54 | # You can specify multiple suffix as a list of string: 55 | # 56 | # source_suffix = ['.rst', '.md'] 57 | source_suffix = '.rst' 58 | 59 | # The master toctree document. 60 | master_doc = 'index' 61 | 62 | # The language for content autogenerated by Sphinx. Refer to documentation 63 | # for a list of supported languages. 64 | # 65 | # This is also used if you do content translation via gettext catalogs. 66 | # Usually you set "language" from the command line for these cases. 67 | language = None 68 | 69 | # List of patterns, relative to source directory, that match files and 70 | # directories to ignore when looking for source files. 71 | # This pattern also affects html_static_path and html_extra_path . 72 | exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] 73 | 74 | # The name of the Pygments (syntax highlighting) style to use. 75 | pygments_style = 'sphinx' 76 | 77 | 78 | # -- Options for HTML output ------------------------------------------------- 79 | 80 | # The theme to use for HTML and HTML Help pages. See the documentation for 81 | # a list of builtin themes. 82 | # 83 | html_theme = 'sphinx_rtd_theme' 84 | 85 | html_logo = "images/logo.png" 86 | 87 | # Theme options are theme-specific and customize the look and feel of a theme 88 | # further. For a list of options available for each theme, see the 89 | # documentation. 90 | # 91 | # html_theme_options = {} 92 | 93 | # Add any paths that contain custom static files (such as style sheets) here, 94 | # relative to this directory. They are copied after the builtin static files, 95 | # so a file named "default.css" will overwrite the builtin "default.css". 96 | html_static_path = ['_static'] 97 | 98 | # Custom sidebar templates, must be a dictionary that maps document names 99 | # to template names. 100 | # 101 | # The default sidebars (for documents that don't match any pattern) are 102 | # defined by theme itself. Builtin themes are using these templates by 103 | # default: ``['localtoc.html', 'relations.html', 'sourcelink.html', 104 | # 'searchbox.html']``. 105 | # 106 | # html_sidebars = {} 107 | 108 | 109 | # -- Options for HTMLHelp output --------------------------------------------- 110 | 111 | # Output file base name for HTML help builder. 112 | htmlhelp_basename = 'PozyxArduinoLibrarydoc' 113 | 114 | 115 | # -- Options for LaTeX output ------------------------------------------------ 116 | 117 | latex_elements = { 118 | # The paper size ('letterpaper' or 'a4paper'). 119 | # 120 | # 'papersize': 'letterpaper', 121 | 122 | # The font size ('10pt', '11pt' or '12pt'). 123 | # 124 | # 'pointsize': '10pt', 125 | 126 | # Additional stuff for the LaTeX preamble. 127 | # 128 | # 'preamble': '', 129 | 130 | # Latex figure (float) alignment 131 | # 132 | # 'figure_align': 'htbp', 133 | } 134 | 135 | # Grouping the document tree into LaTeX files. List of tuples 136 | # (source start file, target name, title, 137 | # author, documentclass [howto, manual, or own class]). 138 | latex_documents = [ 139 | (master_doc, 'PozyxArduinoLibrary.tex', 'Pozyx Arduino Library Documentation', 140 | 'Samuel Van de Velde, Vadim Vermeiren, Laurent Van Acker', 'manual'), 141 | ] 142 | 143 | 144 | # -- Options for manual page output ------------------------------------------ 145 | 146 | # One entry per manual page. List of tuples 147 | # (source start file, name, description, authors, manual section). 148 | man_pages = [ 149 | (master_doc, 'pozyxarduinolibrary', 'Pozyx Arduino Library Documentation', 150 | [author], 1) 151 | ] 152 | 153 | 154 | # -- Options for Texinfo output ---------------------------------------------- 155 | 156 | # Grouping the document tree into Texinfo files. List of tuples 157 | # (source start file, target name, title, author, 158 | # dir menu entry, description, category) 159 | texinfo_documents = [ 160 | (master_doc, 'PozyxArduinoLibrary', 'Pozyx Arduino Library Documentation', 161 | author, 'PozyxArduinoLibrary', 'One line description of project.', 162 | 'Miscellaneous'), 163 | ] -------------------------------------------------------------------------------- /docs/doxygen_output/xml/_c_o_n_t_r_i_b_u_t_i_n_g_8md.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | CONTRIBUTING.md 5 | 6 | 7 | 8 | 9 | 10 | #Contributingguide 11 | 12 | Fornow,this'llonlybeaboutdocumentation: 13 | 14 | ##Generatingdocumentation 15 | 16 | Togeneratethedocumentation,you'llneedtoinstallbothDoxygenandSphinx. 17 | 18 | Whentheseareinstalled,youcanrunthefollowinginyourfavoriteshelltobuildthedocumentation. 19 | 20 | *doxygenDoxyfile 21 | *cddocs 22 | *makehtml 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /docs/doxygen_output/xml/_r_e_a_d_m_e_8md.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | README.md 5 | 6 | 7 | 8 | 9 | 10 | #Pozyx-Arduino-library 11 | TheArduinolibraryforusewiththepozyxshield. 12 | 13 | Pleasedownloadareleaseversionforasafelystableexperience! 14 | 15 | Thelibraryrequires**firmwareversion1.1**installedonthePozyxdevices. 16 | Ifyou'reonanolderversionofthefirmwareanddonotwanttoupgradeforsomereason,pleasedownloadanolderreleaseofthelibraryaccordingtotheirrespectivereleasedescription. 17 | 18 | Documentationforthelibrarycanbefoundhere: 19 | https://www.pozyx.io/Documentation/Datasheet/arduino 20 | 21 | Ifyouencounteranyissues,pleasesendamailtosupport@pozyx.ioinsteadofcreatinganissuehere. 22 | 23 | Thefollowingfolderscanbefoundtogetherwiththislibrary: 24 | 25 | 1.**examples**.TheseexamplescriptsshowcasesomebasicfunctionalityofthePozyxdevice,eachexamplecomeswithatutorialthatcanbefoundonthepozyxwebsitehttps://www.pozyx.io/Documentation 26 | 2.**unit_test**.ThisfoldercontainsacollectionofArduinoscriptsthatcanberuntotestcertainfunctionalitiesofthePozyxdevice.TheyalsoserveassomegoodexamplesforsomeArduinolibraryfunctions. 27 | 3.**useful**.ThisfoldercontainsanumberofusefulArduinosketchesthatprovidebasicfunctionalitysuchasdiscoveringallpozyxdevicesorconfiguringthem. 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /docs/doxygen_output/xml/changelog_8md.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | changelog.md 5 | 6 | 7 | 8 | 9 | 10 | #ArduinoPozyxlibrarychangelog 11 | 12 | ##Version1.0.1 13 | -Addedfunctiondocumentationfordoxygen 14 | -Addeddefinesforparameterdescriptions 15 | -AddedassertionstoallfunctionsforcheckingAPIfunctionparameters 16 | -AddedfunctiongetErrorCode 17 | -AddedfunctionsetTxPower,getTxPower 18 | -AddedfunctionsetUWBChannel,getUWBChannel 19 | -Addedunittestsforplatformspecificfunctions 20 | -Thesensorfunctionsnowreturnthesensorvaluesinmetricnumbersinsteadofrawnumbers 21 | -RemovedspuriousSerial.printlines 22 | -RenamefunctiongetAccelerationtogetAcceleration_mg 23 | -RenamefunctiongetGyrotogetAngularVelocity_dps 24 | -RenamefunctiongetEulerAnglestogetEulerAngles_deg 25 | -RenamefunctiongetGravitytogetGravityVector_mg 26 | -RenamefunctiongetLinearAccelerationtogetLinearAcceleration_mg 27 | -RenamefunctiongetSensorDatatogetAllSensorData 28 | -RenamefunctiongetPressuretogetPressure_Pa 29 | -RenamefunctiongetMagnetictogetMagnetic_uT 30 | -RenamefunctiongetTemperaturetogetTemperature_c 31 | 32 | ##Version1.0.0 33 | -initialrelease 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /docs/doxygen_output/xml/combine.xslt: -------------------------------------------------------------------------------- 1 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /docs/doxygen_output/xml/index.xsd: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /docs/doxygen_output/xml/md__c_o_n_t_r_i_b_u_t_i_n_g.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | md_CONTRIBUTING 5 | Contributing guide 6 | 7 | 8 | 9 | For now, this'll only be about documentation:Generating documentation 10 | To generate the documentation, you'll need to install both Doxygen and Sphinx.When these are installed, you can run the following in your favorite shell to build the documentation. 11 | doxygen Doxyfilecd docsmake html 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /docs/doxygen_output/xml/md__r_e_a_d_m_e.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | md_README 5 | Pozyx-Arduino-library 6 | 7 | 8 | 9 | The Arduino library for use with the pozyx shield.Please download a release version for a safely stable experience!The library requires firmware version 1.1 installed on the Pozyx devices. If you're on an older version of the firmware and do not want to upgrade for some reason, please download an older release of the library according to their respective release description.Documentation for the library can be found here: https://www.pozyx.io/Documentation/Datasheet/arduinoIf you encounter any issues, please send a mail to support@pozyx.io instead of creating an issue here.The following folders can be found together with this library: 10 | examples. These example scripts showcase some basic functionality of the Pozyx device, each example comes with a tutorial that can be found on the pozyx website https://www.pozyx.io/Documentationunit_test. This folder contains a collection of Arduino scripts that can be run to test certain functionalities of the Pozyx device. They also serve as some good examples for some Arduino library functions.useful. This folder contains a number of useful Arduino sketches that provide basic functionality such as discovering all pozyx devices or configuring them. 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /docs/doxygen_output/xml/md_changelog.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | md_changelog 5 | Arduino Pozyx library change log 6 | 7 | 8 | 9 | Version 1.0.1 10 | 11 | Added function documentation for doxygenAdded defines for parameter descriptionsAdded assertions to all functions for checking API function parametersAdded function getErrorCodeAdded function setTxPower, getTxPowerAdded function setUWBChannel, getUWBChannelAdded unit tests for platform specific functionsThe sensor functions now return the sensor values in metric numbers instead of raw numbersRemoved spurious Serial.print linesRename function getAcceleration to getAcceleration_mgRename function getGyro to getAngularVelocity_dpsRename function getEulerAngles to getEulerAngles_degRename function getGravity to getGravityVector_mgRename function getLinearAcceleration to getLinearAcceleration_mgRename function getSensorData to getAllSensorDataRename function getPressure to getPressure_PaRename function getMagnetic to getMagnetic_uTRename function getTemperature to getTemperature_c 12 | Version 1.0.0 13 | 14 | initial release 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /docs/doxygen_output/xml/struct___u_w_b__settings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | _UWB_settings 5 | Pozyx.h 6 | 7 | 8 | uint8_t 9 | uint8_t _UWB_settings::channel 10 | 11 | channel 12 | 13 | 14 | 15 | The UWB channel number. Possible values are 1, 2, 3, 4, 5, 7. See the reg:POZYX_UWB_CHANNEL register for more information. 16 | 17 | 18 | 19 | 20 | 21 | uint8_t 22 | uint8_t _UWB_settings::bitrate 23 | 24 | bitrate 25 | 26 | 27 | 28 | The bitrate. Possible values are 29 | 0: 110kbits/s1: 850kbits/s2: 6.8Mbits/s. 30 | See the reg:POZYX_UWB_RATES register for more information 31 | 32 | 33 | 34 | 35 | 36 | uint8_t 37 | uint8_t _UWB_settings::prf 38 | 39 | prf 40 | 41 | 42 | 43 | The UWB pulse repetition frequency (PRF). Possible values are 44 | 1: 16MHz2: 64MHz 45 | See the reg:POZYX_UWB_RATES register for more information 46 | 47 | 48 | 49 | 50 | 51 | uint8_t 52 | uint8_t _UWB_settings::plen 53 | 54 | plen 55 | 56 | 57 | 58 | The preabmle length. Possible values are: 59 | 0x0C : 4096 symbols.0x28 : 2048 symbols.0x18 : 1536 symbols.0x08 : 1024 symbols.0x34 : 512 symbols.0x24 : 256 symbols.0x14 : 128 symbols.0x04 : 64 symbols. 60 | See the reg:POZYX_UWB_PLEN register for more information. 61 | 62 | 63 | 64 | 65 | 66 | float 67 | float _UWB_settings::gain_db 68 | 69 | gain_db 70 | 71 | 72 | 73 | The transmission gain in dB. Possible values are between 0dB and 33.5dB, with a resolution of 0.5dB. See the reg:POZYX_UWB_GAIN register for more information. 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | The UWB settings type defines all attributes needed to set the UWB (communication) parameters 83 | 84 | 85 | _UWB_settingsbitrate 86 | _UWB_settingschannel 87 | _UWB_settingsgain_db 88 | _UWB_settingsplen 89 | _UWB_settingsprf 90 | 91 | 92 | 93 | -------------------------------------------------------------------------------- /docs/general/index.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pozyxLabs/Pozyx-Arduino-library/e621c2e42aa55eb522a3e1ddbd91ddd80af42e09/docs/general/index.rst -------------------------------------------------------------------------------- /docs/getting_started/index.rst: -------------------------------------------------------------------------------- 1 | Getting started 2 | --------------- 3 | 4 | .. note:: 5 | 6 | In this text, the word tag and shield will be used interchangeably. 7 | 8 | Arduino 9 | ~~~~~~~~~~~~~~~~~~~~~~~~ 10 | 11 | If you're not familiar with the Arduino platform as a whole, `this `_ is a good place to get started and set up with testing your Arduino's basic functionality. 12 | 13 | 14 | Setup 15 | ~~~~ 16 | 17 | Role of Arduino 18 | _______________ 19 | 20 | The Arduino communicates to the Pozyx device using the I2C protocol. 21 | 22 | Something that has been confusing to a lot of people is how to set up the Pozyx on Arduino, especially 23 | when multiple devices come into play. 24 | 25 | And with the additional existence of the `Python library that provides direct USB access to the Pozyx `_, 26 | we have even seen people trying to use the Python library with an Arduino which had a 27 | Pozyx shield attached, which of course did not work. 28 | 29 | In this section, we intend to give you insight of the hardware you need! 30 | 31 | Pozyx on Arduino 32 | ________________ 33 | 34 | The only Pozyx device that needs an Arduino is the one you're running the positioning/ranging sketches on. 35 | This device is called the master device, as this will also direct the operation of other Pozyx devices through 36 | UWB communication. 37 | 38 | Remote Pozyx devices on Arduino 39 | _______________________________ 40 | 41 | Remote Pozyx tags don't need to have an Arduino attached! This is an important point. They just need to be powered. 42 | Phone powerbanks play well with the micro USB port on the tags! 43 | 44 | Only perform positioning and ranging functions on your master device. 45 | 46 | .. note:: 47 | 48 | You might want to have an Arduino on remote shields if you want to read their sensor data locally. 49 | However, do not use functions like positioning and ranging on multiple devices at the same time! 50 | 51 | An example is the Cloud example where an Arduino is used to read the tag's position. 52 | In this example, the positioning is directed by the Pozyx attached to the server, and the Arduino 53 | checks the tag's status to see whether a new position has been calculated. 54 | 55 | Pozyx via USB 56 | _______________ 57 | 58 | Instead of using an Arduino locally, you can skip the Arduino and use an USB cable directly. 59 | 60 | This has advantages like: 61 | 62 | * Very cross platform serial protocol. 63 | * Easy to use Python programming language for flexible functionality (and extendability). 64 | * Computer can be as powerful as you want. 65 | * You can implement the serial communication in any other programming language. 66 | 67 | But disadvantages too: 68 | 69 | * Arduino is very cheap and small standalone hardware, compared to a Raspberry Pi or regular PC. 70 | * The new Arduino Web IDE is amazing. 71 | * You're comfortable with Arduino programming but not with Python. 72 | 73 | Ultimately, the decision which you want to use depends on your application and your 74 | available hardware. 75 | 76 | The documentation for using USB directly can be found `here `_. 77 | 78 | Required headers 79 | ~~~~~~~~~~~~~~~~ 80 | 81 | To use the Pozyx library, you have to include the following headers: 82 | 83 | .. code-block:: cpp 84 | 85 | #include 86 | #include 87 | 88 | 89 | Connecting to the Pozyx 90 | ~~~~~~~~~~~~~~~~~~~~~~~ 91 | 92 | Connecting with the Pozyx is very straightforward. A safe way is presented here: 93 | 94 | .. code-block:: cpp 95 | 96 | void setup(){ 97 | Serial.begin(115200); 98 | if(Pozyx.begin() == POZYX_FAILURE){ 99 | Serial.println(F("ERROR: Unable to connect to POZYX shield")); 100 | Serial.println(F("Reset required")); 101 | delay(100); 102 | abort(); 103 | } 104 | } 105 | 106 | With this, you initialize the Pozyx and can use the entire API in the rest of your script! 107 | 108 | General philosophy 109 | ~~~~~~~~~~~~~~~~~~ 110 | 111 | Essentially, you can do three things with Pozyx: 112 | 113 | 1. Reading register data, which includes sensors and the device's configuration 114 | 2. Writing data to registers, making it possible to change the device's configuration ranging from its positioning algorithm to its very ID. 115 | 3. Performing Pozyx functions like ranging, positioning, saving the device's configuration to its flash memory... 116 | 117 | All these things are possible to do on the shield connected to your Arduino, and powered remote devices as well. In this section we'll go over all of these. 118 | 119 | Reading data 120 | ~~~~~~~~~~~~ 121 | 122 | To read data from the Pozyx, a simple pattern is followed. This pattern can be used with almost all methods starting with the words 'get': 123 | 124 | 1. Initialize the appropriate container for your data read. 125 | 2. Pass this container along with the get functions. 126 | 3. Check the status to see if the operation was successful and thus the data trustworthy. 127 | 128 | You can see the same pattern in action above when reading the UWB data. 129 | 130 | .. TODO An overview of all data containers, their usage and their particularities can be found here: 131 | 132 | .. TODO also mention that they all have human readable __str__ conversions 133 | 134 | .. code-block:: cpp 135 | 136 | // initialize the data container 137 | uint8_t who_am_i; 138 | uint8_t status = Pozyx.getWhoAmI(&whoami); 139 | 140 | // check the status to see if the read was successful. Handling failure is covered later. 141 | if (status == POZYX_SUCCESS) { 142 | // print the container. Note how a SingleRegister will print as a hex string by default. 143 | Serial.println(who_am_i); // will print '0x43' 144 | } 145 | 146 | # and repeat 147 | # initialize the data container 148 | acceleration_t acceleration; 149 | # get the data, passing along the container 150 | Pozyx.getAcceleration_mg(&acceleration); 151 | 152 | # initialize the data container 153 | euler_angles_t euler_angles; 154 | # get the data, passing along the container 155 | Pozyx.getEulerAngles_deg(&euler_angles) 156 | 157 | 158 | Writing data 159 | ~~~~~~~~~~~~ 160 | 161 | Writing data follows a similar pattern as reading, but making a container for the data is optional. This pattern can be used with all methods starting with the words 'set': 162 | 163 | 1. (Optional) Initialize the appropriate container with the right contents for your data write. 164 | 2. Pass this container or the right value along with the set functions. 165 | 3. Check the status to see if the operation was successful and thus the data written. 166 | 167 | Some typical write operations 168 | 169 | .. code-block:: cpp 170 | 171 | # initialize Pozyx as above 172 | 173 | uint8_t status = Pozyx.setPositionAlgorithm(POZYX_POS_ALG_UWB_ONLY); 174 | // Note: this shouldn't ever happen when writing locally. 175 | if (status == POZYX_FAILURE) { 176 | Serial.println("Settings the positioning algorithm failed"); 177 | } 178 | 179 | Pozyx.setPositioningFilter(FILTER_TYPE_MOVING_AVERAGE, 10); 180 | 181 | 182 | Performing functions 183 | ~~~~~~~~~~~~~~~~~~~~ 184 | 185 | Positioning, ranging, configuring the anchors for a tag to use... While the line is sometimes thin, 186 | these aren't per se writes or reads as they are implemented as functions on the Pozyx. 187 | 188 | A Pozyx device function typically can take a container object for storing the function's 189 | return data, and a container object for the function parameters. 190 | 191 | For example, when adding an anchor to a tag's device list, the anchor's ID and position 192 | are the function's parameters, but there is no return data. Thus, the function addDevice 193 | only needs a container object containing the anchor's properties. 194 | 195 | In the library, function wrappers are written in such a way that when no parameters are 196 | required, they are hidden from the user, and the same goes for return data. 197 | 198 | .. code-block:: cpp 199 | 200 | // assume an anchor 0x6038 that we want to add to the device list and 201 | // immediately save the device list after. 202 | device_coordinates_t anchor; 203 | anchor.network_id = 0x6038; 204 | anchor.flag = 0x1; 205 | anchor.pos.x = 5000; 206 | anchor.pos.y = 5000; 207 | anchor.pos.z = 0; 208 | Pozyx.addDevice(anchor); 209 | 210 | .. TODO find better example than positioning since that's a lie 211 | 212 | Remote 213 | ~~~~~~ 214 | 215 | To interface with a remote device, every function has a remote_id optional parameter. Thus, every function you just saw can be performed on a remote device as well! 216 | 217 | (The exceptions are doPositioning and doRanging, which have doRemotePositioning and doRemoteRanging) 218 | 219 | .. code-block:: cpp 220 | 221 | // let's assume there is another tag present with ID 0x6039 222 | uint16_t remote_device_id = 0x6039; 223 | 224 | // this will read the WHO_AM_I register of the remote tag 225 | uint8_t who_am_i; 226 | Pozyx.getWhoAmI(&whoami, remote_device_id); 227 | 228 | 229 | Saving writable register data 230 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 231 | 232 | Basically, every register you can write data to as a user can be saved in the device's flash memory. This means that when the device is powered on, its configuration will remain. Otherwise, the device will use its default values again. 233 | 234 | .. TODO add default values for registers so that users know what to expect. 235 | 236 | This is useful for multiple things: 237 | 238 | * Saving the UWB settings so all your devices remain on the same UWB settings. 239 | * Saving the anchors the tag uses for positioning. This means that after a reset, the tag can resume positioning immediately and doesn't need to be reconfigured! 240 | * Saving positioning algorithm, dimension, filter... you'll never lose your favorite settings when the device shuts down. 241 | 242 | There are various helpers in the library to help you save the settings you prefer, not requiring you to look up the relevant registers. 243 | 244 | .. 245 | .. code-block:: python 246 | # Saves the positioning settings 247 | pozyx.savePositioningSettings() 248 | # Saves the device list used for positioning 249 | pozyx.saveNetwork() 250 | # Saves the device's UWB settings 251 | pozyx.saveUWBSettings() 252 | 253 | 254 | Finding out the error 255 | ~~~~~~~~~~~~~~~~~~~~~ 256 | 257 | Pozyx functions typically return a status to indicate the success of the function. This is useful to indicate failure especially. When things go wrong, it's advised to read the error as well. 258 | 259 | A code snippet shows how this is typically done 260 | 261 | .. 262 | .. code-block:: python 263 | from pypozyx import PozyxSerial, get_first_pozyx_serial_port, POZYX_SUCCESS, SingleRegister 264 | # initialize Pozyx as above 265 | if pozyx.saveUWBSettings() != POZYX_SUCCESS: 266 | # this is one way which retrieves the error code 267 | error_code = SingleRegister() 268 | pozyx.getErrorCode(error_code) 269 | print('Pozyx error code: %s' % error_code) 270 | # the other method returns a descriptive string 271 | print(pozyx.getSystemError()) 272 | -------------------------------------------------------------------------------- /docs/images/logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pozyxLabs/Pozyx-Arduino-library/e621c2e42aa55eb522a3e1ddbd91ddd80af42e09/docs/images/logo.png -------------------------------------------------------------------------------- /docs/index.rst: -------------------------------------------------------------------------------- 1 | .. Pozyx Arduino Library documentation master file, created by 2 | sphinx-quickstart on Thu Jul 19 14:08:26 2018. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Welcome to Pozyx Arduino Library's documentation! 7 | ================================================= 8 | 9 | .. toctree:: 10 | :maxdepth: 2 11 | :caption: Contents: 12 | 13 | overview/index 14 | getting_started/index 15 | examples/index 16 | troubleshooting/index 17 | api/index 18 | 19 | -------------------------------------------------------------------------------- /docs/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=. 11 | set BUILDDIR=_build 12 | set SPHINXPROJ=PozyxArduinoLibrary 13 | 14 | if "%1" == "" goto help 15 | 16 | %SPHINXBUILD% >NUL 2>NUL 17 | if errorlevel 9009 ( 18 | echo. 19 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 20 | echo.installed, then set the SPHINXBUILD environment variable to point 21 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 22 | echo.may add the Sphinx directory to PATH. 23 | echo. 24 | echo.If you don't have Sphinx installed, grab it from 25 | echo.http://sphinx-doc.org/ 26 | exit /b 1 27 | ) 28 | 29 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% 30 | goto end 31 | 32 | :help 33 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% 34 | 35 | :end 36 | popd 37 | -------------------------------------------------------------------------------- /docs/overview/index.rst: -------------------------------------------------------------------------------- 1 | Arduino Pozyx 2 | ============= 3 | 4 | About 5 | ----- 6 | 7 | Arduino Pozyx is an Arduino library providing a high-level interface to a Pozyx shield attached to the Arduino, 8 | making the Pozyx shield compatible with Arduino Uno, Arduino Mega, and Arduino Nano. 9 | 10 | 11 | 12 | Features 13 | -------- 14 | 15 | * Easy to use, allowing both high-level and low-level interfacing with the Pozyx device. 16 | * Straightforward API 17 | * Specialized structs for all important Pozyx data. 18 | * Compatible with Arduino Uno, Mega, and Nano. 19 | 20 | Requirements 21 | ------------ 22 | 23 | * Arduino Web IDE or Arduino IDE 1.6+ 24 | 25 | Installation 26 | ------------ 27 | 28 | Web IDE 29 | ~~~~~~~ 30 | 31 | When you're using the Web IDE, Arduino Pozyx should install automagically 32 | when you include ````. 33 | 34 | Arduino Library Manager 35 | ~~~~~~~~~~~~~~~~~~~~~~~ 36 | 37 | Currently, the Arduino Pozyx library is easily installable from the Arduino Library 38 | manager. 39 | 40 | From source 41 | ~~~~~~~~~~~ 42 | 43 | To install from source, you'll have to download the source files first and put them in 44 | your Arduino sketchbook libraries folder. You can do this either by: 45 | 46 | * ``git clone https://github.com/pozyxLabs/Pozyx-Arduino-library`` and move the folder to your sketchbook libraries 47 | * Download it from `the repository `_ and extracting it. 48 | * Downloading the `zip file `_ directly, and extracting it. 49 | 50 | -------------------------------------------------------------------------------- /docs/requirements.txt: -------------------------------------------------------------------------------- 1 | breathe -------------------------------------------------------------------------------- /docs/system/index.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pozyxLabs/Pozyx-Arduino-library/e621c2e42aa55eb522a3e1ddbd91ddd80af42e09/docs/system/index.rst -------------------------------------------------------------------------------- /docs/tips-and-tricks/index.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pozyxLabs/Pozyx-Arduino-library/e621c2e42aa55eb522a3e1ddbd91ddd80af42e09/docs/tips-and-tricks/index.rst -------------------------------------------------------------------------------- /docs/troubleshooting/index.rst: -------------------------------------------------------------------------------- 1 | Troubleshooting 2 | =============== 3 | 4 | FAQ 5 | --- 6 | 7 | Lost a device? 8 | -------------- 9 | 10 | Contacting support 11 | ------------------ 12 | 13 | If you want to contact support, please include the following: 14 | 15 | * Run the pozyx_basic_troubleshooting.ino (provide link to github location of script) script and attach its output. 16 | * Mention what you want to achieve. Our support team has experience with many use cases and can set you on the right track. 17 | * If you get an error or exception, please include this in your mail instead of just saying something is broken. 18 | 19 | Ultimately, the more information you can provide our support team from the start, the less they'll have to ask of you and the quicker your problem resolution. 20 | -------------------------------------------------------------------------------- /docs/tutorials/index.rst: -------------------------------------------------------------------------------- 1 | Tutorials 2 | ========= 3 | 4 | Ready to range 5 | -------------- 6 | 7 | 8 | Ready to localize 9 | ----------------- 10 | 11 | 12 | Obtaining the sensor data 13 | ------------------------- 14 | 15 | 16 | Multitag positioning 17 | -------------------- -------------------------------------------------------------------------------- /examples/chat_room/chat_room.ino: -------------------------------------------------------------------------------- 1 | /** 2 | The pozyx chat demo 3 | please check out https://www.pozyx.io/Documentation/Tutorials/getting_started 4 | 5 | This demo requires at least two pozyx shields and an equal number of Arduino's. 6 | It demonstrates the wireless messaging capabilities of the pozyx device. 7 | 8 | This demo creates a chat room. Text written in the Serial monitor will be broadcasted to all other pozyx devices 9 | within range. They will see your message appear in their Serial monitor. 10 | */ 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | uint16_t source_id; // the network id of this device 17 | uint16_t destination_id = 0; // the destination network id. 0 means the message is broadcasted to every device in range 18 | String inputString = ""; // a string to hold incoming data 19 | boolean stringComplete = false; // whether the string is complete 20 | 21 | void setup(){ 22 | Serial.begin(115200); 23 | 24 | // initialize Pozyx 25 | if(! Pozyx.begin(false, MODE_INTERRUPT, POZYX_INT_MASK_RX_DATA, 0)){ 26 | Serial.println("ERROR: Unable to connect to POZYX shield"); 27 | Serial.println("Reset required"); 28 | abort(); 29 | } 30 | 31 | // read the network id of this device 32 | Pozyx.regRead(POZYX_NETWORK_ID, (uint8_t*)&source_id, 2); 33 | 34 | // reserve 100 bytes for the inputString: 35 | inputString.reserve(100); 36 | 37 | Serial.println("--- Pozyx Chat started ---"); 38 | } 39 | 40 | void loop(){ 41 | 42 | // check if we received a newline character and if so, broadcast the inputString. 43 | if(stringComplete){ 44 | Serial.print("Ox"); 45 | Serial.print(source_id, HEX); 46 | Serial.print(": "); 47 | Serial.println(inputString); 48 | 49 | int length = inputString.length(); 50 | uint8_t buffer[length]; 51 | inputString.getBytes(buffer, length); 52 | 53 | // write the message to the transmit (TX) buffer 54 | int status = Pozyx.writeTXBufferData(buffer, length); 55 | // broadcast the contents of the TX buffer 56 | status = Pozyx.sendTXBufferData(destination_id); 57 | 58 | inputString = ""; 59 | stringComplete = false; 60 | } 61 | 62 | // we wait up to 50ms to see if we have received an incoming message (if so we receive an RX_DATA interrupt) 63 | if(Pozyx.waitForFlag(POZYX_INT_STATUS_RX_DATA,50)) 64 | { 65 | // we have received a message! 66 | 67 | uint8_t length = 0; 68 | uint16_t messenger = 0x00; 69 | delay(1); 70 | // Let's read out some information about the message (i.e., how many bytes did we receive and who sent the message) 71 | Pozyx.getLastDataLength(&length); 72 | Pozyx.getLastNetworkId(&messenger); 73 | 74 | char data[length]; 75 | 76 | // read the contents of the receive (RX) buffer, this is the message that was sent to this device 77 | Pozyx.readRXBufferData((uint8_t *) data, length); 78 | Serial.print("Ox"); 79 | Serial.print(messenger, HEX); 80 | Serial.print(": "); 81 | Serial.println(data); 82 | } 83 | 84 | } 85 | 86 | /* 87 | SerialEvent occurs whenever a new data comes in the 88 | hardware serial RX. This routine is run between each 89 | time loop() runs, so using delay inside loop can delay 90 | response. Multiple bytes of data may be available. 91 | For more info check http://www.arduino.cc/en/Tutorial/SerialEvent 92 | */ 93 | void serialEvent() { 94 | while (Serial.available()) { 95 | // get the new byte: 96 | char inChar = (char)Serial.read(); 97 | 98 | // if the incoming character is a newline, set a flag 99 | // so the main loop can do something about it. 100 | // otherwise, add it to the inputString: 101 | if (inChar == '\n') { 102 | stringComplete = true; 103 | }else{ 104 | inputString += inChar; 105 | } 106 | } 107 | } 108 | -------------------------------------------------------------------------------- /examples/multitag_positioning/multitag_positioning.ino: -------------------------------------------------------------------------------- 1 | // Please read the ready-to-localize tuturial together with this example. 2 | // https://www.pozyx.io/Documentation/Tutorials/ready_to_localize 3 | /** 4 | The Pozyx ready to localize tutorial (c) Pozyx Labs 5 | 6 | Please read the tutorial that accompanies this sketch: https://www.pozyx.io/Documentation/Tutorials/ready_to_localize/Arduino 7 | 8 | This tutorial requires at least the contents of the Pozyx Ready to Localize kit. It demonstrates the positioning capabilities 9 | of the Pozyx device both locally and remotely. Follow the steps to correctly set up your environment in the link, change the 10 | parameters and upload this sketch. Watch the coordinates change as you move your device around! 11 | */ 12 | #include 13 | #include 14 | #include 15 | 16 | //////////////////////////////////////////////// 17 | ////////////////// PARAMETERS ////////////////// 18 | //////////////////////////////////////////////// 19 | 20 | const int num_tags = 3; 21 | uint16_t tags[num_tags] = {0x0001, 0x0002, 0x0003}; 22 | 23 | boolean use_processing = false; // set this to true to output data for the processing sketch 24 | 25 | const uint8_t num_anchors = 4; // the number of anchors 26 | uint16_t anchors[num_anchors] = {0x1156, 0x256B, 0x3325, 0x4244}; // the network id of the anchors: change these to the network ids of your anchors. 27 | int32_t anchors_x[num_anchors] = {0, 4500, 500, 4450}; // anchor x-coorindates in mm 28 | int32_t anchors_y[num_anchors] = {0, 0, 3300, 3500}; // anchor y-coordinates in mm 29 | int32_t heights[num_anchors] = {1500, 1800, 1100, 2000}; // anchor z-coordinates in mm 30 | 31 | uint8_t algorithm = POZYX_POS_ALG_UWB_ONLY; // positioning algorithm to use 32 | uint8_t dimension = POZYX_3D; // positioning dimension 33 | int32_t height = 1000; // height of device, required in 2.5D positioning 34 | 35 | 36 | //////////////////////////////////////////////// 37 | 38 | void setup(){ 39 | Serial.begin(115200); 40 | 41 | if(Pozyx.begin() == POZYX_FAILURE){ 42 | Serial.println(F("ERROR: Unable to connect to POZYX shield")); 43 | Serial.println(F("Reset required")); 44 | delay(100); 45 | abort(); 46 | } 47 | 48 | Serial.println(F("----------POZYX POSITIONING V1.1----------")); 49 | Serial.println(F("NOTES:")); 50 | Serial.println(F("- No parameters required.")); 51 | Serial.println(); 52 | Serial.println(F("- System will auto start anchor configuration")); 53 | Serial.println(); 54 | Serial.println(F("- System will auto start positioning")); 55 | Serial.println(F("----------POZYX POSITIONING V1.1----------")); 56 | Serial.println(); 57 | Serial.println(F("Performing manual anchor configuration:")); 58 | 59 | // configures all remote tags and prints the success of their configuration. 60 | setAnchorsManual(); 61 | setTagsAlgorithm(); 62 | delay(1000); 63 | 64 | Serial.println(F("Starting positioning: ")); 65 | } 66 | 67 | void loop(){ 68 | for (int i = 0; i < num_tags; i++){ 69 | coordinates_t position; 70 | int status = Pozyx.doRemotePositioning(tags[i], &position, dimension, height, algorithm); 71 | if (status == POZYX_SUCCESS){ 72 | // prints out the result 73 | printCoordinates(position, tags[i]); 74 | }else{ 75 | // prints out the error code 76 | printErrorCode("positioning", tags[i]); 77 | } 78 | } 79 | } 80 | 81 | // prints the coordinates for either humans or for processing 82 | void printCoordinates(coordinates_t coor, uint16_t network_id){ 83 | if(!use_processing){ 84 | Serial.print("POS ID 0x"); 85 | Serial.print(network_id, HEX); 86 | Serial.print(", x(mm): "); 87 | Serial.print(coor.x); 88 | Serial.print(", y(mm): "); 89 | Serial.print(coor.y); 90 | Serial.print(", z(mm): "); 91 | Serial.println(coor.z); 92 | }else{ 93 | Serial.print("POS,0x"); 94 | Serial.print(network_id, HEX); 95 | Serial.print(","); 96 | Serial.print(coor.x); 97 | Serial.print(","); 98 | Serial.print(coor.y); 99 | Serial.print(","); 100 | Serial.println(coor.z); 101 | } 102 | } 103 | 104 | // error printing function for debugging 105 | void printErrorCode(String operation, uint16_t network_id){ 106 | uint8_t error_code; 107 | int status = Pozyx.getErrorCode(&error_code, network_id); 108 | if(status == POZYX_SUCCESS){ 109 | Serial.print("ERROR "); 110 | Serial.print(operation); 111 | Serial.print(" on ID 0x"); 112 | Serial.print(network_id, HEX); 113 | Serial.print(", error code: 0x"); 114 | Serial.println(error_code, HEX); 115 | }else{ 116 | Pozyx.getErrorCode(&error_code); 117 | Serial.print("ERROR "); 118 | Serial.print(operation); 119 | Serial.print(", couldn't retrieve remote error code, local error: 0x"); 120 | Serial.println(error_code, HEX); 121 | } 122 | } 123 | 124 | void setTagsAlgorithm(){ 125 | for (int i = 0; i < num_tags; i++){ 126 | Pozyx.setPositionAlgorithm(algorithm, dimension, tags[i]); 127 | } 128 | } 129 | 130 | // function to manually set the anchor coordinates 131 | void setAnchorsManual(){ 132 | for (int i = 0; i < num_tags; i++){ 133 | int status = Pozyx.clearDevices(tags[i]); 134 | for(int j = 0; j < num_anchors; j++){ 135 | device_coordinates_t anchor; 136 | anchor.network_id = anchors[j]; 137 | anchor.flag = 0x1; 138 | anchor.pos.x = anchors_x[j]; 139 | anchor.pos.y = anchors_y[j]; 140 | anchor.pos.z = heights[j]; 141 | status &= Pozyx.addDevice(anchor, tags[i]); 142 | } 143 | if (num_anchors > 4){ 144 | Pozyx.setSelectionOfAnchors(POZYX_ANCHOR_SEL_AUTO, num_anchors, tags[i]); 145 | } 146 | if (status == POZYX_SUCCESS){ 147 | Serial.print("Configuring ID 0x"); 148 | Serial.print(tags[i], HEX); 149 | Serial.println(" success!"); 150 | }else{ 151 | printErrorCode("configuration", tags[i]); 152 | } 153 | } 154 | } 155 | -------------------------------------------------------------------------------- /examples/old/orientation_3D/orientation_3D.ino: -------------------------------------------------------------------------------- 1 | /** 2 | The pozyx ranging demo (c) Pozyx Labs 3 | please check out https://www.pozyx.io/Documentation/Tutorials/getting_started 4 | 5 | This demo requires one (or two) pozyx shields and one Arduino. It demonstrates the 3D orientation and the functionality 6 | to remotely read register data from a pozyx device. Place one of the pozyx shields on the Arduino and upload this sketch. 7 | 8 | This demo reads the following sensor data: 9 | - pressure 10 | - acceleration 11 | - magnetic field strength 12 | - angular velocity 13 | - the heading, roll and pitch 14 | - the quaternion rotation describing the 3D orientation of the device. This can be used to transform from the body coordinate system to the world coordinate system. 15 | - the linear acceleration (the acceleration excluding gravity) 16 | - the gravitational vector 17 | 18 | The data can be viewed in the Processing sketch orientation_3D.pde 19 | */ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | //////////////////////////////////////////////// 26 | ////////////////// PARAMETERS ////////////////// 27 | //////////////////////////////////////////////// 28 | 29 | boolean bRemote = false; // boolean to indicate if we want to read sensor data from the attached pozyx shield (value 0) or from a remote pozyx device (value 1) 30 | uint16_t destination_id = 0x6606; // the network id of the other pozyx device: fill in the network id of the other device 31 | uint32_t last_millis; // used to compute the measurement interval in milliseconds 32 | 33 | //////////////////////////////////////////////// 34 | 35 | void setup() 36 | { 37 | Serial.begin(115200); 38 | 39 | if(Pozyx.begin(false, MODE_INTERRUPT, POZYX_INT_MASK_IMU) == POZYX_FAILURE){ 40 | Serial.println("ERROR: Unable to connect to POZYX shield"); 41 | Serial.println("Reset required"); 42 | delay(100); 43 | abort(); 44 | } 45 | 46 | last_millis = millis(); 47 | delay(10); 48 | } 49 | 50 | void loop(){ 51 | 52 | int16_t sensor_data[24]; 53 | uint8_t calib_status = 0; 54 | int i, dt; 55 | 56 | if(bRemote == true) 57 | { 58 | // remotely read the sensor data 59 | int status = Pozyx.remoteRegRead(destination_id, POZYX_PRESSURE, (uint8_t*)&sensor_data, 24*sizeof(int16_t)); 60 | if(status != POZYX_SUCCESS){ 61 | return; 62 | } 63 | 64 | }else 65 | { 66 | // wait until this device gives an interrupt 67 | if (Pozyx.waitForFlag(POZYX_INT_STATUS_IMU, 10)) 68 | { 69 | // we received an interrupt from pozyx telling us new IMU data is ready, now let's read it! 70 | Pozyx.regRead(POZYX_PRESSURE, (uint8_t*)&sensor_data, 24*sizeof(int16_t)); 71 | 72 | // also read out the calibration status 73 | Pozyx.regRead(POZYX_CALIB_STATUS, &calib_status, 1); 74 | }else{ 75 | // we didn't receive an interrupt 76 | uint8_t interrupt_status = 0; 77 | Pozyx.regRead(POZYX_INT_STATUS, &interrupt_status, 1); 78 | 79 | return; 80 | } 81 | } 82 | 83 | // print out the results 84 | 85 | // print the measurement interval 86 | dt = millis() - last_millis; 87 | last_millis += dt; 88 | Serial.print(dt, DEC); 89 | 90 | // print out the presure (this is not an int16 but rather an uint32 91 | uint32_t pressure = ((uint32_t)sensor_data[0]) + (((uint32_t)sensor_data[1])<<16); 92 | Serial.print(","); 93 | Serial.print(pressure); 94 | 95 | // print out all remaining sensors 96 | for(i=2; i<24; i++){ 97 | Serial.print(","); 98 | Serial.print(sensor_data[i]); 99 | } 100 | 101 | // finally, print out the calibration status (remotely this is not available and all equal to zero) 102 | Serial.print(","); 103 | Serial.print(calib_status&0x03); 104 | Serial.print(","); 105 | Serial.print((calib_status&0x0C)>>2); 106 | Serial.print(","); 107 | Serial.print((calib_status&0x30)>>4); 108 | Serial.print(","); 109 | Serial.print((calib_status&0xC0)>>6); 110 | 111 | Serial.println(); 112 | } 113 | -------------------------------------------------------------------------------- /examples/old/ready_to_localize/ready_to_localize.ino: -------------------------------------------------------------------------------- 1 | // Please read the ready-to-localize tuturial together with this example. 2 | // https://www.pozyx.io/Documentation/Tutorials/ready_to_localize 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | //////////////////////////////////////////////// 9 | ////////////////// PARAMETERS ////////////////// 10 | //////////////////////////////////////////////// 11 | 12 | uint8_t num_anchors = 4; // the number of anchors 13 | uint16_t anchors[4] = {0x1156, 0x256B, 0x3325, 0x4244}; // the network id of the anchors: change these to the network ids of your anchors. 14 | int32_t heights[4] = {2750, 2000, 1900, 2350}; // anchor z-coordinates in mm 15 | boolean bProcessing = false; // set this to true to output data for the processing sketch 16 | 17 | // only required for manual anchor calibration. Please change this to the coordinates measured for the anchors 18 | int32_t anchors_x[4] = {0, 10000, 1000, 9000}; // anchor x-coorindates in mm 19 | int32_t anchors_y[4] = {0, 0, 7000, 8000}; // anchor y-coordinates in mm 20 | 21 | //////////////////////////////////////////////// 22 | 23 | void setup(){ 24 | Serial.begin(115200); 25 | 26 | if(Pozyx.begin() == POZYX_FAILURE){ 27 | Serial.println(F("ERROR: Unable to connect to POZYX shield")); 28 | Serial.println(F("Reset required")); 29 | delay(100); 30 | abort(); 31 | } 32 | 33 | Serial.println(F("----------POZYX POSITIONING V1.0----------")); 34 | Serial.println(F("NOTES:")); 35 | Serial.println(F("- No parameters required.")); 36 | Serial.println(); 37 | Serial.println(F("- System will auto start calibration")); 38 | Serial.println(); 39 | Serial.println(F("- System will auto start positioning")); 40 | Serial.println(F("----------POZYX POSITIONING V1.0----------")); 41 | Serial.println(); 42 | Serial.println(F("Performing auto anchor calibration:")); 43 | 44 | // clear all previous devices in the device list 45 | Pozyx.clearDevices(); 46 | 47 | int status = Pozyx.doAnchorCalibration(POZYX_2_5D, 10, num_anchors, anchors, heights); 48 | if (status != POZYX_SUCCESS){ 49 | Serial.println(status); 50 | Serial.println(F("ERROR: calibration")); 51 | Serial.println(F("Reset required")); 52 | delay(100); 53 | abort(); 54 | } 55 | 56 | // if the automatic anchor calibration is unsuccessful, try manually setting the anchor coordinates. 57 | // fot this, you must update the arrays anchors_x, anchors_y and heights above 58 | // comment out the doAnchorCalibration block and the if-statement above if you are using manual mode 59 | //SetAnchorsManual(); 60 | 61 | printCalibrationResult(); 62 | delay(3000); 63 | 64 | Serial.println(F("Starting positioning: ")); 65 | 66 | } 67 | 68 | void loop(){ 69 | 70 | coordinates_t position; 71 | int status = Pozyx.doPositioning(&position, POZYX_2_5D, 1000); 72 | 73 | if (status == POZYX_SUCCESS) 74 | { 75 | // print out the result 76 | if(!bProcessing){ 77 | printCoordinates(position); 78 | }else{ 79 | printCoordinatesProcessing(position); 80 | } 81 | } 82 | } 83 | 84 | // function to print the coordinates to the serial monitor 85 | void printCoordinates(coordinates_t coor){ 86 | 87 | Serial.print("x_mm: "); 88 | Serial.print(coor.x); 89 | Serial.print("\t"); 90 | Serial.print("y_mm: "); 91 | Serial.print(coor.y); 92 | Serial.print("\t"); 93 | Serial.print("z_mm: "); 94 | Serial.print(coor.z); 95 | Serial.println(); 96 | } 97 | 98 | // function to print out positoining data + ranges for the processing sketch 99 | void printCoordinatesProcessing(coordinates_t coor){ 100 | 101 | // get the network id and print it 102 | uint16_t network_id; 103 | Pozyx.getNetworkId(&network_id); 104 | 105 | Serial.print("POS,0x"); 106 | Serial.print(network_id,HEX); 107 | Serial.print(","); 108 | Serial.print(coor.x); 109 | Serial.print(","); 110 | Serial.print(coor.y); 111 | Serial.print(","); 112 | Serial.print(coor.z); 113 | Serial.print(","); 114 | 115 | // get information about the positioning error and print it 116 | pos_error_t pos_error; 117 | Pozyx.getPositionError(&pos_error); 118 | 119 | Serial.print(pos_error.x); 120 | Serial.print(","); 121 | Serial.print(pos_error.y); 122 | Serial.print(","); 123 | Serial.print(pos_error.z); 124 | Serial.print(","); 125 | Serial.print(pos_error.xy); 126 | Serial.print(","); 127 | Serial.print(pos_error.xz); 128 | Serial.print(","); 129 | Serial.print(pos_error.yz); 130 | 131 | // read out the ranges to each anchor and print it 132 | for (int i=0; i < num_anchors; i++){ 133 | device_range_t range; 134 | Pozyx.getDeviceRangeInfo(anchors[i], &range); 135 | Serial.print(","); 136 | Serial.print(range.distance); 137 | Serial.print(","); 138 | Serial.print(range.RSS); 139 | } 140 | Serial.println(); 141 | } 142 | 143 | // print out the anchor coordinates (also required for the processing sketch) 144 | void printCalibrationResult(){ 145 | uint8_t list_size; 146 | int status; 147 | 148 | status = Pozyx.getDeviceListSize(&list_size); 149 | Serial.print("list size: "); 150 | Serial.println(status*list_size); 151 | 152 | if(list_size == 0){ 153 | Serial.println("Calibration failed."); 154 | Serial.println(Pozyx.getSystemError()); 155 | return; 156 | } 157 | 158 | uint16_t device_ids[list_size]; 159 | status &= Pozyx.getDeviceIds(device_ids,list_size); 160 | 161 | Serial.println(F("Calibration result:")); 162 | Serial.print(F("Anchors found: ")); 163 | Serial.println(list_size); 164 | 165 | coordinates_t anchor_coor; 166 | for(int i=0; i 14 | #include 15 | #include 16 | 17 | //////////////////////////////////////////////// 18 | ////////////////// PARAMETERS ////////////////// 19 | //////////////////////////////////////////////// 20 | 21 | uint16_t destination_id = 0x6670; // the network id of the other pozyx device: fill in the network id of the other device 22 | signed int range_step_mm = 1000; // every 1000mm in range, one LED less will be giving light. 23 | 24 | //////////////////////////////////////////////// 25 | 26 | void setup(){ 27 | Serial.begin(115200); 28 | 29 | if(Pozyx.begin() == POZYX_FAILURE){ 30 | Serial.println("ERROR: Unable to connect to POZYX shield"); 31 | Serial.println("Reset required"); 32 | delay(100); 33 | abort(); 34 | } 35 | 36 | Serial.println("------------POZYX RANGING V1.0------------"); 37 | Serial.println("NOTES:"); 38 | Serial.println("- Change the parameters:\n\tdestination_id (target device)\n\trange_step (mm)\n\t"); 39 | Serial.println(); 40 | Serial.println("- Approach target device to see range and\n led control"); 41 | Serial.println("------------POZYX RANGING V1.0------------"); 42 | Serial.println(); 43 | Serial.println("START Ranging:"); 44 | 45 | // make sure the pozyx system has no control over the LEDs, we're the boss 46 | uint8_t configuration_leds = 0x0; 47 | Pozyx.regWrite(POZYX_CONFIG_LEDS, &configuration_leds, 1); 48 | 49 | // do the same with the remote device 50 | Pozyx.remoteRegWrite(destination_id, POZYX_CONFIG_LEDS, &configuration_leds, 1); 51 | } 52 | 53 | void loop(){ 54 | 55 | int status = 1; 56 | device_range_t range; 57 | 58 | // let's do ranging with the destination 59 | status &= Pozyx.doRanging(destination_id, &range); 60 | 61 | if (status == POZYX_SUCCESS){ 62 | Serial.print(range.timestamp); 63 | Serial.print("ms \t"); 64 | Serial.print(range.distance); 65 | Serial.println("mm \t"); 66 | 67 | // now control some LEDs; the closer the two devices are, the more LEDs will be lit 68 | if (ledControl(range.distance) == POZYX_FAILURE){ 69 | Serial.println("ERROR: setting (remote) leds"); 70 | } 71 | } 72 | else{ 73 | Serial.println("ERROR: ranging"); 74 | } 75 | } 76 | 77 | int ledControl(uint32_t range){ 78 | int status = 1; 79 | 80 | // set the LEDs of this pozyx device 81 | status &= Pozyx.setLed(4, (range < range_step_mm)); 82 | status &= Pozyx.setLed(3, (range < 2*range_step_mm)); 83 | status &= Pozyx.setLed(2, (range < 3*range_step_mm)); 84 | status &= Pozyx.setLed(1, (range < 4*range_step_mm)); 85 | 86 | // set the LEDs of the remote pozyx device 87 | status &= Pozyx.setLed(4, (range < range_step_mm), destination_id); 88 | status &= Pozyx.setLed(3, (range < 2*range_step_mm), destination_id); 89 | status &= Pozyx.setLed(2, (range < 3*range_step_mm), destination_id); 90 | status &= Pozyx.setLed(1, (range < 4*range_step_mm), destination_id); 91 | 92 | // status will be zero if setting the LEDs failed somewhere along the way 93 | return status; 94 | } 95 | -------------------------------------------------------------------------------- /examples/orientation_3D/orientation_3D.ino: -------------------------------------------------------------------------------- 1 | /** 2 | The pozyx ranging demo (c) Pozyx Labs 3 | please check out https://www.pozyx.io/Documentation/Tutorials/getting_started/Arduino 4 | 5 | This demo requires one (or two) pozyx shields and one Arduino. It demonstrates the 3D orientation and the functionality 6 | to remotely read register data from a pozyx device. Place one of the pozyx shields on the Arduino and upload this sketch. 7 | 8 | This demo reads the following sensor data: 9 | - pressure 10 | - acceleration 11 | - magnetic field strength 12 | - angular velocity 13 | - the heading, roll and pitch 14 | - the quaternion rotation describing the 3D orientation of the device. This can be used to transform from the body coordinate system to the world coordinate system. 15 | - the linear acceleration (the acceleration excluding gravity) 16 | - the gravitational vector 17 | 18 | The data can be viewed in the Processing sketch orientation_3D.pde 19 | */ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | //////////////////////////////////////////////// 26 | ////////////////// PARAMETERS ////////////////// 27 | //////////////////////////////////////////////// 28 | 29 | boolean remote = false; // boolean to indicate if we want to read sensor data from the attached pozyx shield (value 0) or from a remote pozyx device (value 1) 30 | uint16_t remote_id = 0x6606; // the network id of the other pozyx device: fill in the network id of the other device 31 | uint32_t last_millis; // used to compute the measurement interval in milliseconds 32 | 33 | //////////////////////////////////////////////// 34 | 35 | void setup() 36 | { 37 | Serial.begin(115200); 38 | 39 | if(Pozyx.begin(false, MODE_INTERRUPT, POZYX_INT_MASK_IMU) == POZYX_FAILURE){ 40 | Serial.println("ERROR: Unable to connect to POZYX shield"); 41 | Serial.println("Reset required"); 42 | delay(100); 43 | abort(); 44 | } 45 | 46 | if(!remote) 47 | remote_id = NULL; 48 | 49 | last_millis = millis(); 50 | delay(10); 51 | } 52 | 53 | void loop(){ 54 | sensor_raw_t sensor_raw; 55 | uint8_t calibration_status = 0; 56 | int dt; 57 | int status; 58 | if(remote){ 59 | status = Pozyx.getRawSensorData(&sensor_raw, remote_id); 60 | status &= Pozyx.getCalibrationStatus(&calibration_status, remote_id); 61 | if(status != POZYX_SUCCESS){ 62 | return; 63 | } 64 | }else{ 65 | if (Pozyx.waitForFlag(POZYX_INT_STATUS_IMU, 10) == POZYX_SUCCESS){ 66 | Pozyx.getRawSensorData(&sensor_raw); 67 | Pozyx.getCalibrationStatus(&calibration_status); 68 | }else{ 69 | uint8_t interrupt_status = 0; 70 | Pozyx.getInterruptStatus(&interrupt_status); 71 | return; 72 | } 73 | } 74 | 75 | dt = millis() - last_millis; 76 | last_millis += dt; 77 | // print time difference between last measurement in ms, sensor data, and calibration data 78 | Serial.print(dt, DEC); 79 | Serial.print(","); 80 | printRawSensorData(sensor_raw); 81 | Serial.print(","); 82 | // will be zeros for remote devices as unavailable remotely. 83 | printCalibrationStatus(calibration_status); 84 | Serial.println(); 85 | } 86 | 87 | void printRawSensorData(sensor_raw_t sensor_raw){ 88 | Serial.print(sensor_raw.pressure); 89 | Serial.print(","); 90 | Serial.print(sensor_raw.acceleration[0]); 91 | Serial.print(","); 92 | Serial.print(sensor_raw.acceleration[1]); 93 | Serial.print(","); 94 | Serial.print(sensor_raw.acceleration[2]); 95 | Serial.print(","); 96 | Serial.print(sensor_raw.magnetic[0]); 97 | Serial.print(","); 98 | Serial.print(sensor_raw.magnetic[1]); 99 | Serial.print(","); 100 | Serial.print(sensor_raw.magnetic[2]); 101 | Serial.print(","); 102 | Serial.print(sensor_raw.angular_vel[0]); 103 | Serial.print(","); 104 | Serial.print(sensor_raw.angular_vel[1]); 105 | Serial.print(","); 106 | Serial.print(sensor_raw.angular_vel[2]); 107 | Serial.print(","); 108 | Serial.print(sensor_raw.euler_angles[0]); 109 | Serial.print(","); 110 | Serial.print(sensor_raw.euler_angles[1]); 111 | Serial.print(","); 112 | Serial.print(sensor_raw.euler_angles[2]); 113 | Serial.print(","); 114 | Serial.print(sensor_raw.quaternion[0]); 115 | Serial.print(","); 116 | Serial.print(sensor_raw.quaternion[1]); 117 | Serial.print(","); 118 | Serial.print(sensor_raw.quaternion[2]); 119 | Serial.print(","); 120 | Serial.print(sensor_raw.quaternion[3]); 121 | Serial.print(","); 122 | Serial.print(sensor_raw.linear_acceleration[0]); 123 | Serial.print(","); 124 | Serial.print(sensor_raw.linear_acceleration[1]); 125 | Serial.print(","); 126 | Serial.print(sensor_raw.linear_acceleration[2]); 127 | Serial.print(","); 128 | Serial.print(sensor_raw.gravity_vector[0]); 129 | Serial.print(","); 130 | Serial.print(sensor_raw.gravity_vector[1]); 131 | Serial.print(","); 132 | Serial.print(sensor_raw.gravity_vector[2]); 133 | Serial.print(","); 134 | Serial.print(sensor_raw.temperature); 135 | } 136 | 137 | void printCalibrationStatus(uint8_t calibration_status){ 138 | Serial.print(calibration_status & 0x03); 139 | Serial.print(","); 140 | Serial.print((calibration_status & 0x0C) >> 2); 141 | Serial.print(","); 142 | Serial.print((calibration_status & 0x30) >> 4); 143 | Serial.print(","); 144 | Serial.print((calibration_status & 0xC0) >> 6); 145 | } 146 | 147 | -------------------------------------------------------------------------------- /examples/ready_to_localize/ready_to_localize.ino: -------------------------------------------------------------------------------- 1 | // Please read the ready-to-localize tuturial together with this example. 2 | // https://www.pozyx.io/Documentation/Tutorials/ready_to_localize 3 | /** 4 | The Pozyx ready to localize tutorial (c) Pozyx Labs 5 | 6 | Please read the tutorial that accompanies this sketch: https://www.pozyx.io/Documentation/Tutorials/ready_to_localize/Arduino 7 | 8 | This tutorial requires at least the contents of the Pozyx Ready to Localize kit. It demonstrates the positioning capabilities 9 | of the Pozyx device both locally and remotely. Follow the steps to correctly set up your environment in the link, change the 10 | parameters and upload this sketch. Watch the coordinates change as you move your device around! 11 | */ 12 | #include 13 | #include 14 | #include 15 | 16 | //////////////////////////////////////////////// 17 | ////////////////// PARAMETERS ////////////////// 18 | //////////////////////////////////////////////// 19 | 20 | uint16_t remote_id = 0x6000; // set this to the ID of the remote device 21 | bool remote = false; // set this to true to use the remote ID 22 | 23 | boolean use_processing = false; // set this to true to output data for the processing sketch 24 | 25 | const uint8_t num_anchors = 4; // the number of anchors 26 | uint16_t anchors[num_anchors] = {0x1156, 0x256B, 0x3325, 0x4244}; // the network id of the anchors: change these to the network ids of your anchors. 27 | int32_t anchors_x[num_anchors] = {0, 4500, 500, 4450}; // anchor x-coorindates in mm 28 | int32_t anchors_y[num_anchors] = {0, 0, 3300, 3500}; // anchor y-coordinates in mm 29 | int32_t heights[num_anchors] = {1500, 1800, 1100, 2000}; // anchor z-coordinates in mm 30 | 31 | uint8_t algorithm = POZYX_POS_ALG_UWB_ONLY; // positioning algorithm to use. try POZYX_POS_ALG_TRACKING for fast moving objects. 32 | uint8_t dimension = POZYX_3D; // positioning dimension 33 | int32_t height = 1000; // height of device, required in 2.5D positioning 34 | 35 | 36 | //////////////////////////////////////////////// 37 | 38 | void setup(){ 39 | Serial.begin(115200); 40 | 41 | if(Pozyx.begin() == POZYX_FAILURE){ 42 | Serial.println(F("ERROR: Unable to connect to POZYX shield")); 43 | Serial.println(F("Reset required")); 44 | delay(100); 45 | abort(); 46 | } 47 | 48 | if(!remote){ 49 | remote_id = NULL; 50 | } 51 | 52 | Serial.println(F("----------POZYX POSITIONING V1.1----------")); 53 | Serial.println(F("NOTES:")); 54 | Serial.println(F("- No parameters required.")); 55 | Serial.println(); 56 | Serial.println(F("- System will auto start anchor configuration")); 57 | Serial.println(); 58 | Serial.println(F("- System will auto start positioning")); 59 | Serial.println(F("----------POZYX POSITIONING V1.1----------")); 60 | Serial.println(); 61 | Serial.println(F("Performing manual anchor configuration:")); 62 | 63 | // clear all previous devices in the device list 64 | Pozyx.clearDevices(remote_id); 65 | // sets the anchor manually 66 | setAnchorsManual(); 67 | // sets the positioning algorithm 68 | Pozyx.setPositionAlgorithm(algorithm, dimension, remote_id); 69 | 70 | printCalibrationResult(); 71 | delay(2000); 72 | 73 | Serial.println(F("Starting positioning: ")); 74 | } 75 | 76 | void loop(){ 77 | coordinates_t position; 78 | int status; 79 | if(remote){ 80 | status = Pozyx.doRemotePositioning(remote_id, &position, dimension, height, algorithm); 81 | }else{ 82 | status = Pozyx.doPositioning(&position, dimension, height, algorithm); 83 | } 84 | 85 | if (status == POZYX_SUCCESS){ 86 | // prints out the result 87 | printCoordinates(position); 88 | }else{ 89 | // prints out the error code 90 | printErrorCode("positioning"); 91 | } 92 | } 93 | 94 | // prints the coordinates for either humans or for processing 95 | void printCoordinates(coordinates_t coor){ 96 | uint16_t network_id = remote_id; 97 | if (network_id == NULL){ 98 | network_id = 0; 99 | } 100 | if(!use_processing){ 101 | Serial.print("POS ID 0x"); 102 | Serial.print(network_id, HEX); 103 | Serial.print(", x(mm): "); 104 | Serial.print(coor.x); 105 | Serial.print(", y(mm): "); 106 | Serial.print(coor.y); 107 | Serial.print(", z(mm): "); 108 | Serial.println(coor.z); 109 | }else{ 110 | Serial.print("POS,0x"); 111 | Serial.print(network_id,HEX); 112 | Serial.print(","); 113 | Serial.print(coor.x); 114 | Serial.print(","); 115 | Serial.print(coor.y); 116 | Serial.print(","); 117 | Serial.println(coor.z); 118 | } 119 | } 120 | 121 | // error printing function for debugging 122 | void printErrorCode(String operation){ 123 | uint8_t error_code; 124 | if (remote_id == NULL){ 125 | Pozyx.getErrorCode(&error_code); 126 | Serial.print("ERROR "); 127 | Serial.print(operation); 128 | Serial.print(", local error code: 0x"); 129 | Serial.println(error_code, HEX); 130 | return; 131 | } 132 | int status = Pozyx.getErrorCode(&error_code, remote_id); 133 | if(status == POZYX_SUCCESS){ 134 | Serial.print("ERROR "); 135 | Serial.print(operation); 136 | Serial.print(" on ID 0x"); 137 | Serial.print(remote_id, HEX); 138 | Serial.print(", error code: 0x"); 139 | Serial.println(error_code, HEX); 140 | }else{ 141 | Pozyx.getErrorCode(&error_code); 142 | Serial.print("ERROR "); 143 | Serial.print(operation); 144 | Serial.print(", couldn't retrieve remote error code, local error: 0x"); 145 | Serial.println(error_code, HEX); 146 | } 147 | } 148 | 149 | // print out the anchor coordinates (also required for the processing sketch) 150 | void printCalibrationResult(){ 151 | uint8_t list_size; 152 | int status; 153 | 154 | status = Pozyx.getDeviceListSize(&list_size, remote_id); 155 | Serial.print("list size: "); 156 | Serial.println(status*list_size); 157 | 158 | if(list_size == 0){ 159 | printErrorCode("configuration"); 160 | return; 161 | } 162 | 163 | uint16_t device_ids[list_size]; 164 | status &= Pozyx.getDeviceIds(device_ids, list_size, remote_id); 165 | 166 | Serial.println(F("Calibration result:")); 167 | Serial.print(F("Anchors found: ")); 168 | Serial.println(list_size); 169 | 170 | coordinates_t anchor_coor; 171 | for(int i = 0; i < list_size; i++) 172 | { 173 | Serial.print("ANCHOR,"); 174 | Serial.print("0x"); 175 | Serial.print(device_ids[i], HEX); 176 | Serial.print(","); 177 | Pozyx.getDeviceCoordinates(device_ids[i], &anchor_coor, remote_id); 178 | Serial.print(anchor_coor.x); 179 | Serial.print(","); 180 | Serial.print(anchor_coor.y); 181 | Serial.print(","); 182 | Serial.println(anchor_coor.z); 183 | } 184 | } 185 | 186 | // function to manually set the anchor coordinates 187 | void setAnchorsManual(){ 188 | for(int i = 0; i < num_anchors; i++){ 189 | device_coordinates_t anchor; 190 | anchor.network_id = anchors[i]; 191 | anchor.flag = 0x1; 192 | anchor.pos.x = anchors_x[i]; 193 | anchor.pos.y = anchors_y[i]; 194 | anchor.pos.z = heights[i]; 195 | Pozyx.addDevice(anchor, remote_id); 196 | } 197 | if (num_anchors > 4){ 198 | Pozyx.setSelectionOfAnchors(POZYX_ANCHOR_SEL_AUTO, num_anchors, remote_id); 199 | } 200 | } 201 | -------------------------------------------------------------------------------- /examples/ready_to_range/ready_to_range.ino: -------------------------------------------------------------------------------- 1 | /** 2 | The Pozyx ready to range tutorial (c) Pozyx Labs 3 | Please read the tutorial that accompanies this sketch: https://www.pozyx.io/Documentation/Tutorials/ready_to_range/Arduino 4 | 5 | This demo requires two Pozyx devices and one Arduino. It demonstrates the ranging capabilities and the functionality to 6 | to remotely control a Pozyx device. Place one of the Pozyx shields on the Arduino and upload this sketch. Move around 7 | with the other Pozyx device. 8 | 9 | This demo measures the range between the two devices. The closer the devices are to each other, the more LEDs will 10 | light up on both devices. 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | //////////////////////////////////////////////// 18 | ////////////////// PARAMETERS ////////////////// 19 | //////////////////////////////////////////////// 20 | 21 | uint16_t destination_id = 0x6069; // the network id of the other pozyx device: fill in the network id of the other device 22 | signed int range_step_mm = 1000; // every 1000mm in range, one LED less will be giving light. 23 | 24 | uint8_t ranging_protocol = POZYX_RANGE_PROTOCOL_PRECISION; // ranging protocol of the Pozyx. 25 | 26 | uint16_t remote_id = 0x605D; // the network ID of the remote device 27 | bool remote = false; // whether to use the given remote device for ranging 28 | 29 | //////////////////////////////////////////////// 30 | 31 | void setup(){ 32 | Serial.begin(115200); 33 | 34 | if(Pozyx.begin() == POZYX_FAILURE){ 35 | Serial.println("ERROR: Unable to connect to POZYX shield"); 36 | Serial.println("Reset required"); 37 | delay(100); 38 | abort(); 39 | } 40 | // setting the remote_id back to NULL will use the local Pozyx 41 | if (!remote){ 42 | remote_id = NULL; 43 | } 44 | Serial.println("------------POZYX RANGING V1.1------------"); 45 | Serial.println("NOTES:"); 46 | Serial.println("- Change the parameters:"); 47 | Serial.println("\tdestination_id (target device)"); 48 | Serial.println("\trange_step (mm)"); 49 | Serial.println(); 50 | Serial.println("- Approach target device to see range and"); 51 | Serial.println("led control"); 52 | Serial.println("------------POZYX RANGING V1.1------------"); 53 | Serial.println(); 54 | Serial.println("START Ranging:"); 55 | 56 | // make sure the pozyx system has no control over the LEDs, we're the boss 57 | uint8_t led_config = 0x0; 58 | Pozyx.setLedConfig(led_config, remote_id); 59 | // do the same with the remote device 60 | Pozyx.setLedConfig(led_config, destination_id); 61 | // set the ranging protocol 62 | Pozyx.setRangingProtocol(ranging_protocol, remote_id); 63 | } 64 | 65 | void loop(){ 66 | device_range_t range; 67 | int status = 0; 68 | 69 | // let's perform ranging with the destination 70 | if(!remote) 71 | status = Pozyx.doRanging(destination_id, &range); 72 | else 73 | status = Pozyx.doRemoteRanging(remote_id, destination_id, &range); 74 | 75 | if (status == POZYX_SUCCESS){ 76 | Serial.print(range.timestamp); 77 | Serial.print("ms, "); 78 | Serial.print(range.distance); 79 | Serial.print("mm, "); 80 | Serial.print(range.RSS); 81 | Serial.println("dBm"); 82 | 83 | // now control some LEDs; the closer the two devices are, the more LEDs will be lit 84 | if (ledControl(range.distance) == POZYX_FAILURE){ 85 | Serial.println("ERROR: setting (remote) leds"); 86 | } 87 | } 88 | else{ 89 | Serial.println("ERROR: ranging"); 90 | } 91 | } 92 | 93 | int ledControl(uint32_t range){ 94 | int status = POZYX_SUCCESS; 95 | // set the LEDs of the pozyx device 96 | status &= Pozyx.setLed(4, (range < range_step_mm), remote_id); 97 | status &= Pozyx.setLed(3, (range < 2*range_step_mm), remote_id); 98 | status &= Pozyx.setLed(2, (range < 3*range_step_mm), remote_id); 99 | status &= Pozyx.setLed(1, (range < 4*range_step_mm), remote_id); 100 | 101 | // set the LEDs of the destination pozyx device 102 | status &= Pozyx.setLed(4, (range < range_step_mm), destination_id); 103 | status &= Pozyx.setLed(3, (range < 2*range_step_mm), destination_id); 104 | status &= Pozyx.setLed(2, (range < 3*range_step_mm), destination_id); 105 | status &= Pozyx.setLed(1, (range < 4*range_step_mm), destination_id); 106 | 107 | // status will be zero if setting the LEDs failed somewhere along the way 108 | return status; 109 | } 110 | -------------------------------------------------------------------------------- /examples/useful/pozyx_UWB_configurator/pozyx_UWB_configurator.ino: -------------------------------------------------------------------------------- 1 | /** 2 | * This sketch will scan for ALL pozyx devices on all UWB settings 3 | * After scanning, it is possible to select a device and set its UWB settings for later later use 4 | * All settings configured this way are saved in flash memory. 5 | * 6 | * Note that in order to use this sketch, it is necessary to enable the CR (carriage return) in the Serial Monitor 7 | * 8 | * Example use: 9 | * --------------------------------------- 10 | * wait untill all devices are found. 11 | * input '1' + 'enter' to select the first device, which is the local device 12 | * next, input 'channel' + 'enter', then '3' + 'enter', to switch to the 3th channel 13 | * next, input 'bitrate' + 'enter', then '850'+'enter', to select bitrate 850kb/sec 14 | * finally, input 'save' + 'enter' to save the settings 15 | * 16 | * Author, Samuel Van de Velde, Pozyx Labs 17 | * 18 | */ 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | // all possible options 25 | uint8_t channels[6] = {1,2,3,4,5,7}; 26 | uint8_t bitrate[3] = {0, 1, 2}; 27 | uint8_t prf[2] = {1, 2}; 28 | uint8_t plens[8] = {0x0C, 0x28, 0x18, 0x08, 0x34, 0x24, 0x14, 0x04}; 29 | int plen_vals[8] = {4096, 2048, 1536, 1024, 512 , 256 , 128 , 64}; 30 | 31 | // data about the devices found 32 | #define MAX_DEVICES 20 33 | int devices_found; 34 | uint16_t devId[MAX_DEVICES]; 35 | UWB_settings_t devSettings[MAX_DEVICES]; 36 | 37 | bool long_plen = false; 38 | 39 | void setup() { 40 | Serial.begin(115200); 41 | while(!Serial); // for the Arduino Leonardo/Micro only 42 | 43 | Pozyx.begin(false); 44 | 45 | Serial.println(F("Perform scanning")); 46 | Serial.println(F("---------------------------------------------------------\n")); 47 | 48 | Serial.println("Local device: "); 49 | Pozyx.getNetworkId(&devId[devices_found]); 50 | Pozyx.getUWBSettings(&devSettings[devices_found]); 51 | devices_found++; 52 | Serial.print("1) 0x"); 53 | Serial.print(devId[0], HEX); 54 | Serial.print(": channel "); 55 | Serial.print(devSettings[0].channel); 56 | printChannelSettings(devSettings[0]); 57 | 58 | Serial.println(F("\nScanning remote devices..")); 59 | 60 | int chan_num, bitrate_num, prf_num, plen_num; 61 | 62 | for(chan_num = 0; chan_num < 6; chan_num++) 63 | { 64 | Serial.print(F("\nChannel: ")); 65 | Serial.print(channels[chan_num]); 66 | for(plen_num = 0; plen_num < 8; plen_num++) { 67 | for(prf_num = 0; prf_num < 2; prf_num++) { 68 | for(bitrate_num = 0; bitrate_num < 3; bitrate_num++) { 69 | 70 | UWB_settings_t UWB_settings; 71 | UWB_settings.channel = channels[chan_num]; 72 | UWB_settings.bitrate = bitrate[bitrate_num]; 73 | UWB_settings.prf = prf[prf_num]; 74 | UWB_settings.plen = plens[plen_num]; 75 | UWB_settings.gain_db = 0.0f; 76 | int result = Pozyx.setUWBSettings(&UWB_settings); 77 | delay(20); 78 | 79 | Pozyx.clearDevices(); 80 | if (long_plen) { 81 | Pozyx.doDiscovery(POZYX_DISCOVERY_ALL_DEVICES, 3, 200); 82 | } else { 83 | Pozyx.doDiscovery(POZYX_DISCOVERY_ALL_DEVICES); 84 | } 85 | 86 | uint8_t list_size = 0; 87 | result = Pozyx.getDeviceListSize(&list_size); 88 | if(list_size > 0) 89 | { 90 | uint16_t devices[list_size]; 91 | result = Pozyx.getDeviceIds(devices, list_size); 92 | 93 | int i; 94 | int bFirstFound = 1; 95 | for(i=0; i>5; 119 | 120 | Serial.print("\t"); 121 | Serial.print(devices_found); 122 | Serial.print(") 0x"); 123 | Serial.print(devices[i], HEX); 124 | 125 | if(dev == 0) 126 | Serial.print(F(": anchor v1.")); 127 | else 128 | Serial.print(F(": tag v1.")); 129 | Serial.print(data[1]&0x1F); 130 | Serial.print(", FW v"); 131 | Serial.print((data[0]&0xF0)>>4); 132 | Serial.print("."); 133 | Serial.print((data[0]&0x0F)); 134 | 135 | if(dev == 0){ 136 | if(data[2] != 0b110000){ 137 | Serial.print(F(". WARNING SELFTEST: ")); 138 | Serial.print(data[2], BIN); 139 | } 140 | }else if(dev == 1) 141 | { 142 | if(data[2] != 0b111111){ 143 | Serial.print(". WARNING SELFTEST: "); 144 | Serial.print(data[2], BIN); 145 | } 146 | } 147 | Serial.println(); 148 | 149 | } 150 | } 151 | } 152 | } 153 | } 154 | } 155 | 156 | Serial.println(F("\nScan ended.")); 157 | 158 | if(devices_found == 0) 159 | abort(); 160 | 161 | Serial.print(F("\nPlease select a device [1-")); 162 | Serial.print(devices_found); 163 | Serial.println("]: "); 164 | 165 | } 166 | 167 | #define STATE_DEV_SELECTION 0 168 | #define STATE_DEV_OPTION 1 169 | #define STATE_DEV_OPTION_CHANNEL 2 170 | #define STATE_DEV_OPTION_BITRATE 3 171 | #define STATE_DEV_OPTION_PRF 4 172 | #define STATE_DEV_OPTION_PLEN 5 173 | #define STATE_DEV_OPTION_SAVE 6 174 | 175 | String input; 176 | int state = STATE_DEV_SELECTION; 177 | UWB_settings_t tmp_settings; 178 | int dev_Selected; 179 | 180 | void loop() { 181 | 182 | if (Serial.available() > 0) { // is a character available? 183 | char rx_byte = Serial.read(); // get the character 184 | 185 | if(rx_byte != '\r' && rx_byte != '\n') 186 | input += rx_byte; 187 | 188 | if(rx_byte == '\r') { 189 | 190 | if(state == STATE_DEV_SELECTION) 191 | { 192 | int deviceIdx = input.toInt()-1; 193 | if(deviceIdx >=0 && deviceIdx < devices_found) 194 | { 195 | dev_Selected = deviceIdx; 196 | Serial.print(F("\tDevice selected: 0x")); 197 | Serial.println(devId[deviceIdx], HEX); 198 | Serial.println(F("\tOptions [channel, bitrate, prf, plen, save, return]:")); 199 | tmp_settings = devSettings[dev_Selected]; 200 | state = STATE_DEV_OPTION; 201 | }else{ 202 | Serial.println(F("\tUnknown device")); 203 | Serial.print(F("\nPlease select a device [1-")); 204 | Serial.print(devices_found); 205 | Serial.println("]: "); 206 | } 207 | }else if(state == STATE_DEV_OPTION) 208 | { 209 | if(input == "channel") { 210 | Serial.println(F("\t\tPlease select channel [1,2,3,4,5,7,return]: ")); 211 | state = STATE_DEV_OPTION_CHANNEL; 212 | }else if(input == "bitrate") { 213 | Serial.println(F("\t\tPlease select bitrate [110,850,6810,return]: ")); 214 | state = STATE_DEV_OPTION_BITRATE; 215 | }else if(input == "prf") { 216 | Serial.println(F("\t\tPlease select channel [16,64,return]: ")); 217 | state = STATE_DEV_OPTION_PRF; 218 | }else if(input == "plen") { 219 | Serial.println(F("\t\tPlease select plen [64,128,256,512,1024,1536,2048,4096,return]: ")); 220 | state = STATE_DEV_OPTION_PLEN; 221 | }else if(input == "save") 222 | { 223 | if(dev_Selected != 0) 224 | { 225 | // go to the old settings of the target device 226 | Pozyx.setUWBSettings(&devSettings[dev_Selected]); 227 | delay(20); 228 | // update the settings of the target device 229 | tmp_settings.gain_db = 0; 230 | Pozyx.setUWBSettings(&tmp_settings, devId[dev_Selected]); 231 | devSettings[dev_Selected] = tmp_settings; 232 | delay(20); 233 | 234 | // go to the new settings of the target device 235 | Pozyx.setUWBSettings(&devSettings[dev_Selected]); 236 | delay(20); 237 | 238 | // save the settings on the target device 239 | uint8_t reg[3] = {POZYX_UWB_CHANNEL, POZYX_UWB_RATES, POZYX_UWB_PLEN}; 240 | Pozyx.saveConfiguration(POZYX_FLASH_REGS, reg, 3, devId[dev_Selected]); 241 | 242 | int result = Pozyx.isRegisterSaved(POZYX_UWB_CHANNEL, devId[dev_Selected] ); 243 | if(result != POZYX_SUCCESS) { 244 | Serial.println(F("\t\tSave unsuccessful.")); 245 | Serial.println(result); 246 | }else{ 247 | Serial.println(F("\t\tSettings saved. ")); 248 | } 249 | }else{ 250 | devSettings[0] = tmp_settings; 251 | devSettings[0].gain_db = 0; 252 | Pozyx.setUWBSettings(&devSettings[0]); 253 | delay(20); 254 | 255 | // save the settings on the target device 256 | uint8_t reg[3] = {POZYX_UWB_CHANNEL, POZYX_UWB_RATES, POZYX_UWB_PLEN}; 257 | Pozyx.saveConfiguration(POZYX_FLASH_REGS, reg, 3); 258 | 259 | int result = Pozyx.isRegisterSaved(POZYX_UWB_CHANNEL); 260 | if(result != POZYX_SUCCESS) { 261 | Serial.println(F("\t\tSave unsuccessful.")); 262 | Serial.println(result); 263 | }else{ 264 | Serial.println(F("\t\tSettings saved. ")); 265 | } 266 | } 267 | goToDeviceSelect(); 268 | }else if(input == "return") 269 | { 270 | goToDeviceSelect(); 271 | }else{ 272 | Serial.println(state); 273 | Serial.println(input); 274 | Serial.println(F("\t\tInvalid option")); 275 | state = STATE_DEV_OPTION; 276 | } 277 | }else 278 | { 279 | if(input == "return") 280 | state = STATE_DEV_OPTION; 281 | else{ 282 | int int_input = input.toInt(); 283 | boolean success = false; 284 | 285 | if(state == STATE_DEV_OPTION_CHANNEL && int_input > 0 && int_input <=7 && int_input != 6) 286 | { 287 | tmp_settings.channel = int_input; 288 | success = true; 289 | }else if(state == STATE_DEV_OPTION_PRF) 290 | { 291 | if(int_input == 16){ 292 | tmp_settings.prf = 1; success = true; 293 | }else if(int_input == 64){ 294 | tmp_settings.prf = 2; success = true; 295 | } 296 | }else if(state == STATE_DEV_OPTION_BITRATE) 297 | { 298 | if(int_input == 110){ 299 | tmp_settings.bitrate = 0; success = true; 300 | }else if(int_input == 850){ 301 | tmp_settings.bitrate = 1; success = true; 302 | }else if(int_input == 6810){ 303 | tmp_settings.bitrate = 2; success = true; 304 | } 305 | }else if(state == STATE_DEV_OPTION_PLEN) 306 | { 307 | int j; 308 | for(j=0; j<8; j++) 309 | { 310 | if(int_input == plen_vals[j]){ 311 | tmp_settings.plen = plens[j]; success = true; 312 | } 313 | } 314 | }else{ 315 | } 316 | 317 | if(success) 318 | Serial.println(F("\t\tOption set")); 319 | else 320 | Serial.println(F("\t\tOption not set")); 321 | state = STATE_DEV_OPTION; 322 | } 323 | } 324 | 325 | input = ""; 326 | } 327 | 328 | } // end: if (Serial.available() > 0) 329 | 330 | } 331 | 332 | int getPreambleLength(int plen_code) 333 | { 334 | if(plen_code == 0x0C) 335 | return 4096; 336 | else if(plen_code == 0x28 ) 337 | return 2048; 338 | else if(plen_code == 0x18 ) 339 | return 1536; 340 | else if(plen_code == 0x08 ) 341 | return 1024; 342 | else if(plen_code == 0x34 ) 343 | return 512; 344 | else if(plen_code == 0x24 ) 345 | return 256; 346 | else if(plen_code == 0x14 ) 347 | return 128; 348 | else if(plen_code == 0x04 ) 349 | return 64; 350 | else 351 | Serial.println("Illigal preamble code"); 352 | 353 | return 0; 354 | } 355 | 356 | void printChannelSettings(UWB_settings_t UWB_settings) 357 | { 358 | Serial.print(" - bitrate: "); 359 | if(UWB_settings.bitrate == 0) 360 | Serial.print("110kbit/s"); 361 | else if(UWB_settings.bitrate == 1) 362 | Serial.print("850kbit/s"); 363 | else if(UWB_settings.bitrate == 2) 364 | Serial.print("6.81Mbit/s"); 365 | 366 | Serial.print(" - PRF: "); 367 | if(UWB_settings.prf == 1) 368 | Serial.print("16MHz"); 369 | else 370 | Serial.print("64MHz"); 371 | 372 | Serial.print(" - plen: "); 373 | Serial.println(getPreambleLength(UWB_settings.plen)); 374 | } 375 | 376 | void goToDeviceSelect() 377 | { 378 | Serial.println(F("------------------------------------------------------------")); 379 | int i; 380 | for(i=0; i 2 | #include 3 | #include 4 | #include 5 | 6 | // change this if you want to perform remote configuration 7 | uint16_t remote_id = NULL; 8 | 9 | #define MAX_DEVICES 20 10 | uint8_t devlist_size = 0; 11 | uint16_t device_ids[MAX_DEVICES]; 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | while(!Serial); // for the Arduino Leonardo/Micro only 16 | 17 | Pozyx.begin(); 18 | 19 | Serial.println("\n\nAnchor configurator started."); 20 | Serial.println("------------------------------------"); 21 | 22 | if(remote_id == NULL) { 23 | Serial.print(F("Configuring the local pozyx device with network id: 0x")); 24 | uint16_t netid = 0; 25 | Pozyx.getNetworkId(&netid); 26 | Serial.println(netid, HEX); 27 | }else{ 28 | uint8_t whoami=0; 29 | Pozyx.getWhoAmI(&whoami, remote_id); 30 | if(whoami == 0x43){ 31 | Serial.print(F("Configuring the remote pozyx device with network id: 0x")); 32 | Serial.println(remote_id, HEX); 33 | }else{ 34 | Serial.print(F("Cannot connect to the remote device with network id 0x")); 35 | Serial.println(remote_id, HEX); 36 | delay(50); 37 | abort(); 38 | } 39 | } 40 | 41 | printLocalizationOptions(remote_id); 42 | printMain(remote_id); 43 | } 44 | 45 | #define STATE_MAIN 0 46 | #define STATE_ADD_HEX 1 47 | #define STATE_ADD_X 2 48 | #define STATE_ADD_Y 3 49 | #define STATE_ADD_Z 4 50 | #define STATE_REMOVE_ID 5 51 | #define STATE_OPTION_ALGORITHM 6 52 | #define STATE_OPTION_DIM 7 53 | #define STATE_OPTION_NUM_ANCHORS 8 54 | #define STATE_OPTION_ANCHOR_SEL 9 55 | #define STATE_MANUAL_IDS 10 56 | 57 | String input; 58 | int state = STATE_MAIN; 59 | int dev_Selected; 60 | device_coordinates_t new_device; 61 | int algorithm = -1; 62 | int numAnchors; 63 | int selected_anchor_num = 0; 64 | uint16_t anchor_sel[15]; 65 | 66 | void loop() { 67 | 68 | if (Serial.available() > 0) { // is a character available? 69 | char rx_byte = Serial.read(); // get the character 70 | 71 | if(rx_byte != '\r' && rx_byte != '\n') 72 | input += rx_byte; 73 | 74 | if(rx_byte == '\r') { 75 | 76 | if(state == STATE_MAIN) 77 | { 78 | if(input == "add") 79 | { 80 | Serial.println("\tAdding new anchor:"); 81 | Serial.print("\tplease give network id (hex): "); 82 | state = STATE_ADD_HEX; 83 | } 84 | else if(input == "remove") 85 | { 86 | Serial.print("\tGive network id of device to remove (hex): "); 87 | state = STATE_REMOVE_ID; 88 | } 89 | else if(input == "remove all") 90 | { 91 | Pozyx.clearDevices(remote_id); 92 | Pozyx.saveConfiguration(POZYX_FLASH_NETWORK, NULL, 0, remote_id); 93 | printMain(remote_id); 94 | } 95 | else if(input == "localization options") 96 | { 97 | Serial.println("\tselect positioning algoirthm [LS, UWB_only, default] "); 98 | state = STATE_OPTION_ALGORITHM; 99 | }else{ 100 | Serial.println("\tUnknown command."); 101 | } 102 | }else if(state == STATE_ADD_HEX) 103 | { 104 | char input_hex[sizeof(input)]; 105 | input.toCharArray(input_hex, sizeof(input)); 106 | long network_id = strtol(input_hex, NULL, 16); 107 | if(network_id >0 && network_id <= 0xFFFF) 108 | { 109 | new_device.network_id = (uint16_t)network_id; 110 | Serial.print("0x"); 111 | Serial.println(network_id, HEX); 112 | Serial.print("\tSet anchor x-coordinate(mm): "); 113 | state = STATE_ADD_X; 114 | }else{ 115 | Serial.print(F("\n\tNot a valid network id, try again: ")); 116 | } 117 | }else if(state == STATE_ADD_X) 118 | { 119 | int x_mm = input.toInt(); 120 | if(x_mm >=-100000 && x_mm < 100000) 121 | { 122 | new_device.pos.x = x_mm; 123 | Serial.println(x_mm); 124 | Serial.print(F("\tSet anchor y-coordinate(mm): ")); 125 | state = STATE_ADD_Y; 126 | }else{ 127 | Serial.print(F("\n\tNot a valid x-coordinate, try again: ")); 128 | } 129 | }else if(state == STATE_ADD_Y) 130 | { 131 | int y_mm = input.toInt(); 132 | if(y_mm >=-100000 && y_mm < 100000) 133 | { 134 | new_device.pos.y = y_mm; 135 | Serial.println(y_mm); 136 | Serial.print(F("\tSet anchor z-coordinate(mm): ")); 137 | state = STATE_ADD_Z; 138 | }else{ 139 | Serial.print(F("\n\tNot a valid y-coordinate, try again: ")); 140 | } 141 | }else if(state == STATE_ADD_Z) 142 | { 143 | int z_mm = input.toInt(); 144 | if(z_mm >=-100000 && z_mm < 100000) 145 | { 146 | new_device.pos.z = z_mm; 147 | Serial.println(z_mm); 148 | 149 | new_device.flag = 0x1; 150 | int result = Pozyx.addDevice(new_device, remote_id); 151 | Pozyx.saveConfiguration(POZYX_FLASH_NETWORK, NULL, 0, remote_id); 152 | if(result == POZYX_SUCCESS) 153 | { 154 | Serial.println(F("\tAnchor successfully added.")); 155 | }else{ 156 | Serial.println(F("\tFailed to add anchor.")); 157 | } 158 | state = STATE_MAIN; 159 | printMain(remote_id); 160 | }else{ 161 | Serial.print(F("\n\tNot a valid x-coordinate, try again: ")); 162 | } 163 | }else if(state == STATE_REMOVE_ID) 164 | { 165 | char input_hex[sizeof(input)]; 166 | input.toCharArray(input_hex, sizeof(input)); 167 | long network_id = strtol(input_hex, NULL, 16); 168 | if(network_id >0 && network_id <= 0xFFFF) 169 | { 170 | Serial.print("0x"); 171 | Serial.println(network_id, HEX); 172 | // remove anchor 173 | remove_anchor(network_id, remote_id); 174 | Pozyx.saveConfiguration(POZYX_FLASH_NETWORK, NULL, 0, remote_id); 175 | Serial.println("\tAnchor removed"); 176 | 177 | state = STATE_MAIN; 178 | printMain(remote_id); 179 | 180 | }else{ 181 | Serial.println(F("\n\tNot a valid network id, try again:")); 182 | } 183 | } else if(state == STATE_OPTION_ALGORITHM) 184 | { 185 | algorithm = -1; 186 | 187 | if(input == "LS") { 188 | algorithm = POZYX_POS_ALG_LS; 189 | }else if(input == "UWB_only" || input == "default") { 190 | algorithm = POZYX_POS_ALG_UWB_ONLY ; 191 | }else{ 192 | Serial.println("\tInvalid input"); 193 | } 194 | 195 | if(algorithm != -1) 196 | { 197 | Serial.println(F("\tSelect the localization dimension [2D, 2.5D, 3D]")); 198 | state = STATE_OPTION_DIM; 199 | } 200 | }else if(state == STATE_OPTION_DIM) 201 | { 202 | int dim = -1; 203 | if(input == "2D") 204 | { 205 | dim = 2; 206 | }else if(input == "2.5D"){ 207 | dim = 1; 208 | }else if(input == "3D"){ 209 | dim = 3; 210 | }else{ 211 | Serial.println("\tInvalid input"); 212 | } 213 | 214 | if(dim != -1) 215 | { 216 | Pozyx.setPositionAlgorithm(algorithm,dim, remote_id); 217 | Serial.println(F("\tChoose the number of anchors to use [3-15]")); 218 | state = STATE_OPTION_NUM_ANCHORS; 219 | } 220 | }else if(state == STATE_OPTION_NUM_ANCHORS) 221 | { 222 | numAnchors = input.toInt(); 223 | if(numAnchors >= 3 && numAnchors <= 15 ) 224 | { 225 | Serial.println(F("\tSelect the anchor selection method [manual, automatic]")); 226 | state = STATE_OPTION_ANCHOR_SEL; 227 | }else{ 228 | Serial.println(F("\tInvalid input")); 229 | } 230 | }else if(state == STATE_OPTION_ANCHOR_SEL) 231 | { 232 | if(input == "manual") 233 | { 234 | Pozyx.setSelectionOfAnchors(POZYX_ANCHOR_SEL_MANUAL, numAnchors, remote_id ); 235 | uint8_t regs[2] = {POZYX_POS_ALG, POZYX_POS_NUM_ANCHORS}; 236 | Pozyx.saveConfiguration(POZYX_FLASH_REGS, regs, 2, remote_id); 237 | state = STATE_MANUAL_IDS; 238 | Serial.print(F("\tenter network id of anchor 1 (hex): ")); 239 | selected_anchor_num = 1; 240 | }else if(input == "automatic") 241 | { 242 | Pozyx.setSelectionOfAnchors(POZYX_ANCHOR_SEL_AUTO , numAnchors, remote_id ); 243 | uint8_t regs[2] = {POZYX_POS_ALG, POZYX_POS_NUM_ANCHORS}; 244 | Pozyx.saveConfiguration(POZYX_FLASH_REGS, regs, 2, remote_id); 245 | Serial.println(F("\tLocalization options saved")); 246 | printLocalizationOptions(remote_id); 247 | state = STATE_MAIN; 248 | printMain(remote_id); 249 | }else{ 250 | Serial.println(F("\tInvalid input, please try again")); 251 | } 252 | }else if(state == STATE_MANUAL_IDS) 253 | { 254 | char input_hex[sizeof(input)]; 255 | input.toCharArray(input_hex, sizeof(input)); 256 | long network_id = strtol(input_hex, NULL, 16); 257 | if(network_id >0 && network_id <= 0xFFFF) 258 | { 259 | Serial.print("0x"); 260 | Serial.println(network_id, HEX); 261 | 262 | anchor_sel[selected_anchor_num-1] = network_id; 263 | 264 | if(selected_anchor_num < numAnchors) 265 | { 266 | Serial.print(F("\tenter network id of anchor ")); 267 | Serial.print(++selected_anchor_num); 268 | Serial.print(F(" (hex): ")); 269 | }else 270 | { 271 | Pozyx.setPositioningAnchorIds(anchor_sel, numAnchors, remote_id); 272 | Pozyx.saveConfiguration(POZYX_FLASH_ANCHOR_IDS, NULL, 0, remote_id); 273 | printLocalizationOptions(remote_id); 274 | state = STATE_MAIN; 275 | printMain(remote_id); 276 | } 277 | 278 | }else{ 279 | Serial.print(F("\n\tNot a valid network id, try again: ")); 280 | } 281 | } 282 | 283 | input = ""; 284 | } 285 | 286 | } // end: if (Serial.available() > 0) 287 | 288 | } 289 | 290 | void printLocalizationOptions(uint16_t remote_id) 291 | { 292 | uint8_t algorithm = 0; 293 | Pozyx.getPositionAlgorithm(&algorithm, remote_id); 294 | uint8_t dimension = 0; 295 | Pozyx.getPositionDimension(&dimension, remote_id); 296 | 297 | Serial.print(F("using the ")); 298 | if(algorithm == POZYX_POS_ALG_UWB_ONLY ) 299 | Serial.print(F("UWB-only")); 300 | else if(algorithm == POZYX_POS_ALG_LS ) 301 | Serial.print(F("Least squares")); 302 | Serial.print(F(" algorithm in ")); 303 | if(dimension == POZYX_2D ) 304 | Serial.println(F("2D.")); 305 | else if(dimension == POZYX_2_5D ) 306 | Serial.println(F("3D with fixed height (2.5D).")); 307 | else if(dimension == POZYX_3D ) 308 | Serial.println(F("3D.")); 309 | 310 | uint8_t mode = 0; 311 | Pozyx.getAnchorSelectionMode(&mode, remote_id); 312 | uint8_t numAnchors = 0; 313 | Pozyx.getNumberOfAnchors(&numAnchors, remote_id); 314 | 315 | Serial.print(F("using ")); 316 | Serial.print(numAnchors); 317 | Serial.print(F(" anchors that are ")); 318 | if(mode == POZYX_ANCHOR_SEL_MANUAL) 319 | { 320 | Serial.println(F("manually selected.")); 321 | uint16_t anchors[numAnchors]; 322 | for(int i=0;i 0) 343 | { 344 | uint16_t devices[devlist_size]; 345 | result = Pozyx.getDeviceIds(device_ids, devlist_size, remote_id); 346 | 347 | int i; 348 | for(i=0; i 0) 377 | { 378 | uint16_t device_ids[devlist_size]; 379 | device_coordinates_t devices[devlist_size]; 380 | result = Pozyx.getDeviceIds(device_ids, devlist_size, remote_id); 381 | 382 | // load all the current devices 383 | for(int i=0; i 2 | #include 3 | #include 4 | 5 | 6 | void setup(){ 7 | Serial.begin(115200); 8 | 9 | if(Pozyx.begin() == POZYX_FAILURE){ 10 | Serial.println("ERROR: Unable to connect to POZYX shield"); 11 | Serial.println("Reset required"); 12 | delay(100); 13 | abort(); 14 | } 15 | 16 | // this performs device check locally. 17 | device_check(NULL); 18 | 19 | // uncomment and fill in own remote ID to check that device. 20 | // device_check(0x10); 21 | 22 | network_check_discovery(); 23 | 24 | } 25 | 26 | void device_check(uint16_t remote_id){ 27 | uint8_t data[5] = {0,0,0,0,0}; 28 | 29 | if (remote_id == NULL){ 30 | Pozyx.regRead(POZYX_WHO_AM_I, data, 5); 31 | Serial.println("local device:"); 32 | }else{ 33 | Pozyx.remoteRegRead(remote_id, POZYX_WHO_AM_I, data, 5); 34 | Serial.print("device 0x"); 35 | Serial.println(remote_id, HEX); 36 | } 37 | 38 | Serial.print("who am i: "); 39 | Serial.println(data[0], HEX); 40 | Serial.print("firmware version: 0x"); 41 | Serial.println(data[1], HEX); 42 | Serial.print("hardware version: 0x"); 43 | Serial.println(data[2], HEX); 44 | Serial.print("self test result: 0b"); 45 | Serial.println(data[3], BIN); 46 | Serial.print("error: 0x"); 47 | Serial.println(data[4], HEX); 48 | } 49 | 50 | 51 | void network_check_discovery(){ 52 | if( Pozyx.doDiscovery(POZYX_DISCOVERY_ALL_DEVICES) == POZYX_SUCCESS){ 53 | uint8_t num_devices = 0; 54 | Pozyx.getDeviceListSize(&num_devices); 55 | Serial.print("Discovery found: "); 56 | Serial.print(num_devices); 57 | Serial.println(" device(s)."); 58 | uint16_t tags[num_devices]; 59 | if (num_devices == 0){ 60 | Serial.println("FIN."); 61 | return; 62 | } 63 | Pozyx.getDeviceIds(tags, num_devices); 64 | 65 | for(int i = 0; i < num_devices; i++){ 66 | Serial.print("0x"); 67 | Serial.print(tags[i], HEX); 68 | if (i != num_devices - 1){ 69 | Serial.print(", "); 70 | } 71 | } 72 | Serial.println(); 73 | } 74 | } 75 | 76 | void loop() { 77 | 78 | } 79 | -------------------------------------------------------------------------------- /examples/useful/pozyx_change_network_id/pozyx_change_network_id.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | 6 | uint16_t new_id = 0x6000; // the new network id of the pozyx device, change as desired 7 | bool remote = false; // whether to use the remote device 8 | uint16_t remote_id = 0x6068; // the remote ID 9 | 10 | void setup(){ 11 | Serial.begin(115200); 12 | Serial.print("Setting the POZYX ID to 0x"); 13 | Serial.println(new_id, HEX); 14 | 15 | if (!remote){ 16 | remote_id = NULL; 17 | } 18 | 19 | if(Pozyx.begin() == POZYX_FAILURE){ 20 | Serial.println("ERROR: Unable to connect to POZYX shield"); 21 | Serial.println("Reset required"); 22 | delay(100); 23 | abort(); 24 | } 25 | 26 | Pozyx.setNetworkId(new_id, remote_id); 27 | 28 | uint8_t regs[1] = {POZYX_NETWORK_ID}; 29 | 30 | int status = Pozyx.saveConfiguration(POZYX_FLASH_REGS, regs, 1, remote_id); 31 | if(status == POZYX_SUCCESS){ 32 | Serial.println("Saving to flash was successful! Resetting system..."); 33 | Pozyx.resetSystem(remote_id); 34 | }else{ 35 | Serial.println("Saving to flash was unsuccessful!"); 36 | } 37 | } 38 | 39 | void loop() { 40 | 41 | } 42 | -------------------------------------------------------------------------------- /examples/useful/pozyx_check_new_position/pozyx_check_new_position.ino: -------------------------------------------------------------------------------- 1 | /** 2 | * This sketch assumes that the tag is doing positioning through a master tag (or through the cloud app) 3 | * This sketch will check on the pozyx device if a new position is available. 4 | * The sketch itself does not initiate the positioning, it is assumed that this is iniated by another tag through remote positioning. 5 | * When a tag is being remotely positioned, it is only possible to perform read operations, any other operations 6 | * may result in problems positioning. 7 | * 8 | * Note that in order to use this sketch, it is necessary to enable the CR (carriage return) in the Serial Monitor 9 | * 10 | * Author, Laurent Van Acker, Pozyx Labs 11 | * 12 | */ 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | void setup(){ 19 | Serial.begin(115200); 20 | if(Pozyx.begin() == POZYX_FAILURE){ 21 | Serial.println(F("ERROR: Unable to connect to POZYX shield")); 22 | Serial.println(F("Reset required")); 23 | delay(100); 24 | abort(); 25 | } 26 | } 27 | 28 | void loop(){ 29 | coordinates_t position; 30 | int status = checkLocalNewPosition(&position); 31 | if (status == POZYX_SUCCESS){ 32 | // prints out the result 33 | printCoordinates(position); 34 | }else{ 35 | // prints out the error code 36 | printErrorCode("positioning"); 37 | } 38 | } 39 | 40 | int checkLocalNewPosition(coordinates_t *position) 41 | { 42 | assert(position != NULL); 43 | int status; 44 | uint8_t int_status = 0; 45 | // now wait for the positioning to finish or generate an error 46 | if (Pozyx.waitForFlag_safe(POZYX_INT_STATUS_POS | POZYX_INT_STATUS_ERR, 2*POZYX_DELAY_INTERRUPT, &int_status)){ 47 | if((int_status & POZYX_INT_STATUS_ERR) == POZYX_INT_STATUS_ERR) 48 | { 49 | // An error occured during positioning. 50 | // Please read out the register POZYX_ERRORCODE to obtain more information about the error 51 | return POZYX_FAILURE; 52 | }else{ 53 | status = Pozyx.getCoordinates(position); 54 | return status; 55 | } 56 | }else{ 57 | return POZYX_TIMEOUT; 58 | } 59 | } 60 | 61 | // prints the coordinates for either humans or for processing 62 | void printCoordinates(coordinates_t coor){ 63 | Serial.print("POS"); 64 | Serial.print(", x(mm): "); 65 | Serial.print(coor.x); 66 | Serial.print(", y(mm): "); 67 | Serial.print(coor.y); 68 | Serial.print(", z(mm): "); 69 | Serial.println(coor.z); 70 | } 71 | 72 | // error printing function for debugging 73 | void printErrorCode(String operation){ 74 | uint8_t error_code; 75 | Pozyx.getErrorCode(&error_code); 76 | Serial.print("ERROR "); 77 | Serial.print(operation); 78 | Serial.print(", local error code: 0x"); 79 | Serial.println(error_code, HEX); 80 | } -------------------------------------------------------------------------------- /examples/useful/pozyx_doDiscovery/pozyx_doDiscovery.ino: -------------------------------------------------------------------------------- 1 | /** 2 | * This sketch performs continuous discovery, to discover any pozyx devices nearby using the same UWB settings as the connected pozyx device 3 | * It is possible to use custom UWB settings to look for differently configured pozyx devices 4 | * 5 | * Author, Samuel Van de Velde, Pozyx Labs 6 | * 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | // indicate if we must use the configured UWB settings of the device, or that we want to use the settings below 14 | boolean bUseDefaultSettings = true; 15 | 16 | // UWB settings to use when bUseDefaultSettings == false 17 | uint8_t channel = 3; // values 1,2,3,4,5,7 18 | uint8_t bitrate = 0; // values 0, 1, 2 19 | uint8_t prf = 2; // values 1, 2 20 | uint8_t plen = 0x08; 21 | 22 | int prev_list_size = -1; 23 | uint16_t* devices = new uint16_t[0]; 24 | uint16_t* prev_devices = new uint16_t[0]; 25 | 26 | void setup() { 27 | Serial.begin(115200); 28 | while(!Serial); // for the Arduino Leonardo/Micro only 29 | 30 | Wire.begin(); 31 | 32 | Serial.println(F("Perform discovery")); 33 | Serial.println(F("---------------------------------------------------------\n")); 34 | Serial.println(F("Channel parameters for discovery: ")); 35 | 36 | if(!bUseDefaultSettings) 37 | { 38 | UWB_settings_t UWB_settings; 39 | UWB_settings.channel = channel; 40 | UWB_settings.bitrate = bitrate; 41 | UWB_settings.prf = prf; 42 | UWB_settings.plen = plen; 43 | UWB_settings.gain_db = 0.0f; 44 | int result = Pozyx.setUWBSettings(&UWB_settings); 45 | delay(100); 46 | } 47 | 48 | print_uwb_settings(); 49 | 50 | Serial.println(F("\nContinuously performing discovery..")); 51 | 52 | } 53 | 54 | void loop() { 55 | // Get new device list 56 | Pozyx.clearDevices(); 57 | int status = Pozyx.doDiscovery(POZYX_DISCOVERY_ALL_DEVICES); 58 | uint8_t list_size = 0; 59 | status = Pozyx.getDeviceListSize(&list_size); 60 | devices = new uint16_t[list_size]; 61 | if (list_size>0) 62 | status = Pozyx.getDeviceIds(devices, list_size); 63 | 64 | // Check if device list has changed 65 | bool hasChanged; 66 | if (prev_list_size != list_size) { 67 | hasChanged = true; 68 | } else { 69 | hasChanged = !deviceListsAreEqual(devices, prev_devices, list_size); 70 | } 71 | 72 | // Print device list when changed 73 | if(hasChanged) 74 | { 75 | Serial.print("Number of pozyx devices discovered: "); 76 | Serial.println(list_size); 77 | 78 | if(list_size > 0) 79 | { 80 | Serial.println("List of device IDs: "); 81 | for(int i=0; i 17 | #include 18 | #include 19 | 20 | //////////////////////////////////////////////// 21 | ////////////////// PARAMETERS ////////////////// 22 | //////////////////////////////////////////////// 23 | 24 | float desired_gain = 15.0f; 25 | boolean save_gain = false; 26 | 27 | const int num_devices = 3; 28 | uint16_t devices[num_devices] = {0x0001, 0x0002, 0x0003}; 29 | 30 | //////////////////////////////////////////////// 31 | 32 | void setup(){ 33 | Serial.begin(115200); 34 | 35 | if(Pozyx.begin() == POZYX_FAILURE){ 36 | Serial.println(F("ERROR: Unable to connect to POZYX shield")); 37 | Serial.println(F("Reset required")); 38 | delay(100); 39 | abort(); 40 | } 41 | 42 | for (int i = 0; i < num_devices; i++){ 43 | int status = Pozyx.setTxPower(desired_gain, devices[i]); 44 | uint8_t registers[1] = {POZYX_UWB_GAIN}; 45 | if (save_gain){ 46 | status &= Pozyx.saveRegisters(registers, 1, devices[i]); 47 | } 48 | if (status == POZYX_SUCCESS){ 49 | printSuccess(devices[i]); 50 | } 51 | else{ 52 | printFailure(devices[i]); 53 | } 54 | } 55 | } 56 | 57 | void printSuccess(uint16_t device_id){ 58 | Serial.print("Configuration of device 0x"); 59 | Serial.print(device_id, 16); 60 | Serial.print(" was successful!\n"); 61 | } 62 | 63 | void printFailure(uint16_t device_id){ 64 | Serial.print("Configuration of device 0x"); 65 | Serial.print(device_id, 16); 66 | Serial.print(" failed!\n"); 67 | } 68 | 69 | void loop(){ 70 | delay(10000); 71 | } 72 | -------------------------------------------------------------------------------- /examples/useful/pozyx_savechannel/pozyx_savechannel.ino: -------------------------------------------------------------------------------- 1 | /** 2 | * This sketch showcases how to save the UWB channel number into flash memory so that next time, this channel number will be used by default 3 | * 4 | * Author, Samuel Van de Velde, Pozyx Labs 5 | * 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | // channel to set this device to 13 | uint8_t channel_num = 3; 14 | 15 | void setup() { 16 | Serial.begin(115200); 17 | while(!Serial); // for the Arduino Leonardo/Micro only 18 | 19 | Wire.begin(); 20 | 21 | Serial.println(F("Changing UWB channel")); 22 | Serial.println(F("---------------------------------------------------------\n")); 23 | 24 | uint8_t result, num_regs; 25 | 26 | uint8_t channel_old = 0; 27 | result = Pozyx.regRead(POZYX_UWB_CHANNEL, &channel_old, 1); 28 | Serial.print("Previously set channel: "); 29 | Serial.println(channel_old); 30 | 31 | if(channel_old == channel_num) 32 | { 33 | Serial.println("Not changing anything"); 34 | }else 35 | { 36 | // if you want: you can perform a memory clear 37 | //result = PozyxNew.clearConfiguration(); 38 | 39 | // write a new value in the register 40 | result = Pozyx.regWrite(POZYX_UWB_CHANNEL, &channel_num, 1); 41 | delay(100); 42 | 43 | // save to flash 44 | uint8_t reg = POZYX_UWB_CHANNEL; 45 | result = Pozyx.saveConfiguration(POZYX_FLASH_REGS, ®, 1); 46 | if(result != POZYX_SUCCESS) 47 | { 48 | Serial.println(result); 49 | Serial.println(POZYX_SUCCESS); 50 | Serial.println("could not save the UWB channel"); 51 | } 52 | 53 | // now read the number of registers that have been saved in flash, should be zero because we just cleared the memory 54 | num_regs = Pozyx.getNumRegistersSaved(); 55 | Serial.print("Number of registers saved: "); 56 | Serial.println(num_regs); 57 | 58 | result = Pozyx.isRegisterSaved(POZYX_UWB_CHANNEL); 59 | if(result != POZYX_SUCCESS) { 60 | Serial.println("Channel was not saved."); 61 | Serial.println(result); 62 | } 63 | 64 | // reset the Pozyx device to verify if it worked 65 | Pozyx.resetSystem(); 66 | 67 | channel_num = 0; 68 | result = Pozyx.regRead(POZYX_UWB_CHANNEL, &channel_num, 1); 69 | Serial.println("Channel after reset: "); 70 | Serial.println(channel_num); 71 | } 72 | 73 | } 74 | 75 | void loop() { 76 | // put your main code here, to run repeatedly: 77 | 78 | } 79 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=Pozyx 2 | version=1.2.2 3 | author=Pozyx Labs 4 | maintainer=Pozyx Labs support@pozyx.io 5 | sentence=Library for the pozyx indoor positioning shield 6 | paragraph=Library for the pozyx indoor positioning shield 7 | category=Sensors 8 | url=https://github.com/pozyxLabs/Pozyx-Arduino-library 9 | architectures=* 10 | -------------------------------------------------------------------------------- /unit_tests/README.md: -------------------------------------------------------------------------------- 1 | # Unit tests for the pozyx library 2 | 3 | These files require the library ArduinoUnit which can be downloaded here: https://github.com/mmurdoch/arduinounit 4 | 5 | -------------------------------------------------------------------------------- /unit_tests/unit_tests_core/unit_tests_core.ino: -------------------------------------------------------------------------------- 1 | #line 2 "unit_tests_core.ino" 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | // can we read a single byte 9 | test(regRead_singlebyte) 10 | { 11 | uint8_t whoami = 0; 12 | int status = Pozyx.regRead(POZYX_WHO_AM_I, &whoami, 1); 13 | 14 | assertEqual(status, POZYX_SUCCESS); 15 | assertEqual(whoami, 0x43); 16 | } 17 | 18 | // can we write a single byte 19 | test(regWrite_singlebyte) 20 | { 21 | uint8_t writebyte = 19; 22 | 23 | // write the value 1 to this register 24 | int status = Pozyx.regWrite(POZYX_POS_X, &writebyte, 1); 25 | 26 | assertEqual(status, POZYX_SUCCESS); 27 | 28 | // this delay is necessary because pozyx is still processing the write operation first. 29 | delay(1); 30 | 31 | uint8_t testbyte = 0; 32 | Pozyx.regRead(POZYX_POS_X, &testbyte, 1); 33 | 34 | assertEqual(testbyte, writebyte); 35 | } 36 | 37 | // can we write multiple bytes at once 38 | test(regWrite_multibyte) 39 | { 40 | uint32_t pos[3] = {1, 2, 3}; 41 | 42 | // write the value 1 to this register 43 | int status = Pozyx.regWrite(POZYX_POS_X, (uint8_t*)&pos, 3*sizeof(uint32_t)); 44 | 45 | assertEqual(status, POZYX_SUCCESS); 46 | 47 | // this delay is necessary because pozyx is still processing the write operation first. 48 | delay(1); 49 | 50 | pos[0] = 0; 51 | pos[1] = 0; 52 | pos[2] = 0; 53 | Pozyx.regRead(POZYX_POS_X, (uint8_t*)&pos, 3*sizeof(uint32_t)); 54 | 55 | assertEqual(pos[0], 1); 56 | assertEqual(pos[1], 2); 57 | assertEqual(pos[2], 3); 58 | } 59 | 60 | // can we write a single byte quickly after one-another. 61 | test(regWrite_quick) 62 | { 63 | int num_cycles = 10; 64 | uint8_t i = 0; 65 | for(i=1; i <= num_cycles; i++){ 66 | int status = Pozyx.regWrite(POZYX_POS_X, (uint8_t*)&i, 1); 67 | assertEqual(status, POZYX_SUCCESS); 68 | } 69 | 70 | // this delay is necessary because pozyx is still processing the write operation first. 71 | delay(1); 72 | 73 | uint8_t result = 0; 74 | Pozyx.regRead(POZYX_POS_X, (uint8_t*)&result, 1); 75 | 76 | assertEqual(result, num_cycles); 77 | } 78 | 79 | test(regFunction_simple) 80 | { 81 | uint8_t input = 0x88; 82 | 83 | // call function to write to some buffer 84 | int status = Pozyx.regFunction( POZYX_LED_CTRL, (uint8_t*)&input, 1, NULL, 0); 85 | assertEqual(status, POZYX_SUCCESS); 86 | } 87 | 88 | // can we call a register function? 89 | test(regFunction) 90 | { 91 | uint16_t input[3] = {1, 2, 3}; 92 | 93 | // call function to write to some buffer 94 | int status = Pozyx.regFunction( POZYX_POS_SET_ANCHOR_IDS, (uint8_t*)&input, 3*sizeof(uint16_t), NULL, 0); 95 | assertEqual(status, POZYX_SUCCESS); 96 | 97 | // call function to read from some buffer 98 | uint16_t result[3] = {0,0,0}; 99 | status = Pozyx.regFunction( POZYX_POS_GET_ANCHOR_IDS, NULL, 0, (uint8_t*)&result, 3*sizeof(uint16_t)); 100 | 101 | assertEqual(result[0], 1); 102 | assertEqual(result[1], 2); 103 | assertEqual(result[2], 3); 104 | 105 | assertEqual(status, 3); 106 | } 107 | 108 | test(waitForFlag) 109 | { 110 | Pozyx.begin(false, MODE_INTERRUPT, POZYX_INT_STATUS_ERR); 111 | //Pozyx.begin(false, MODE_POLLING, POZYX_INT_STATUS_ERR); 112 | 113 | // read out the interrupt status register to reset it. 114 | uint8_t dummy; 115 | Pozyx.regRead(POZYX_INT_STATUS, &dummy, 1); 116 | 117 | // cause an error interrupt by writing to a read-only register 118 | uint8_t causes_error = 0x01; 119 | Pozyx.regWrite(POZYX_WHO_AM_I, &causes_error, 1); 120 | 121 | // if the interrupt was caught, the result is true 122 | boolean result = Pozyx.waitForFlag(POZYX_INT_STATUS_ERR, 100); 123 | 124 | assertTrue(result); 125 | } 126 | 127 | void setup() 128 | { 129 | Serial.begin(115200); 130 | while(!Serial); // for the Arduino Leonardo/Micro only 131 | 132 | Wire.begin(); 133 | } 134 | 135 | void loop() 136 | { 137 | Test::run(); 138 | } 139 | -------------------------------------------------------------------------------- /unit_tests/unit_tests_flash/unit_tests_flash.ino: -------------------------------------------------------------------------------- 1 | #line 2 "unit_tests_core.ino" 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | /** 9 | * These unit tests verify that the all functionalities regarding saving and loading from/to the flash memory work. 10 | * These functionalities allow a configuration to be saved and remembered even after a pozyx device is reset. 11 | * This requires firmware version v1.0 12 | * 13 | * Author, Samuel Van de Velde, Pozyx Labs 14 | * 15 | * functions tested: 16 | * - Pozyx.reset 17 | * - Pozyx.saveConfiguration 18 | * - Pozyx.clearConfiguration 19 | * - Pozyx.getNumRegistersSaved 20 | * - Pozyx.isRegisterSaved 21 | * 22 | * This tests the registers: 23 | * - POZYX_FLASH_DETAILS 24 | * - POZYX_FLASH_RESET 25 | * - POZYX_FLASH_SAVE 26 | */ 27 | 28 | 29 | void setup() 30 | { 31 | Serial.begin(115200); 32 | while(!Serial); // for the Arduino Leonardo/Micro only 33 | 34 | Wire.begin(); 35 | 36 | delay(5000); 37 | 38 | Serial.println(F("Start testing flash functionality")); 39 | Serial.println(F("---------------------------------------------------------\n")); 40 | 41 | Test::exclude("flash*"); 42 | //Test::exclude("reset*"); 43 | } 44 | 45 | void loop() 46 | { 47 | Test::run(); 48 | } 49 | 50 | // can we reset the device? we test this to check if the value for the led_config register 51 | // is reset to default after reset. 52 | test(reset) 53 | { 54 | uint8_t ledconfig, ledconfig2; 55 | int result; 56 | 57 | // read the register and change the content 58 | ledconfig = 0x20; 59 | Pozyx.regWrite(POZYX_POS_ALG, &ledconfig, 1); 60 | delay(2); 61 | Pozyx.regRead(POZYX_POS_ALG, &ledconfig2, 1); 62 | assertEqual(ledconfig, ledconfig2); 63 | 64 | // reset the system 65 | Pozyx.resetSystem(); 66 | 67 | // after the reset, we should read the default value again. 68 | ledconfig2 = 0; 69 | Pozyx.regRead(POZYX_POS_ALG, &ledconfig2, 1); 70 | assertNotEqual(ledconfig, ledconfig2); 71 | } 72 | 73 | // can we reset the device multiple times? we test this by checking if the value for the led_config register 74 | // is reset to default after reset. 75 | test(multi_reset) 76 | { 77 | uint8_t ledconfig, ledconfig2; 78 | int i; 79 | 80 | // read the register and change the content 81 | for(i=0; i<10; i++) 82 | { 83 | ledconfig = 0x20; 84 | Pozyx.regWrite(POZYX_POS_ALG, &ledconfig, 1); 85 | delay(2); 86 | Pozyx.regRead(POZYX_POS_ALG, &ledconfig2, 1); 87 | assertEqual(ledconfig, ledconfig2); 88 | 89 | 90 | // reset the system 91 | Pozyx.resetSystem(); 92 | 93 | // after the reset, we should read the default value again. 94 | ledconfig2 = 0; 95 | Pozyx.regRead(POZYX_POS_ALG, &ledconfig2, 1); 96 | assertNotEqual(ledconfig, ledconfig2); 97 | } 98 | } 99 | 100 | /* 101 | * Test if we can save a single 8-bit register to flash memory 102 | */ 103 | test(flash8bitRegSave) 104 | { 105 | uint8_t result, num_regs; 106 | 107 | // first perform a memory clear 108 | result = Pozyx.clearConfiguration(); 109 | assertEqual(result, POZYX_SUCCESS); 110 | 111 | // now read the number of registers that have been saved in flash, should be zero because we just cleared the memory 112 | num_regs = Pozyx.getNumRegistersSaved(); 113 | assertEqual(num_regs, 0); 114 | 115 | // same if we check if the register is saved 116 | result = Pozyx.isRegisterSaved(POZYX_POS_ALG); 117 | assertEqual(num_regs, 0); 118 | 119 | // write a new value in the register 120 | uint8_t reg_saved = 0x32; 121 | result = Pozyx.regWrite(POZYX_POS_ALG, ®_saved, 1); 122 | assertEqual(result, POZYX_SUCCESS); 123 | 124 | // save to flash 125 | uint8_t reg = POZYX_POS_ALG; 126 | result = Pozyx.saveConfiguration(POZYX_FLASH_REGS, ®, 1); 127 | assertEqual(result, POZYX_SUCCESS); 128 | 129 | // now read the number of registers that have been saved in flash again, we saved one register so this should be one 130 | num_regs = 0; 131 | num_regs = Pozyx.getNumRegistersSaved(); 132 | assertEqual(num_regs, 1); 133 | 134 | result = Pozyx.isRegisterSaved(POZYX_POS_ALG); 135 | assertEqual(result, 1); 136 | 137 | // reset the system to see if saving worked 138 | Pozyx.resetSystem(); 139 | 140 | // now read the number of registers that have been saved in flash again, we saved one register so this should be one 141 | num_regs = 0; 142 | num_regs = Pozyx.getNumRegistersSaved(); 143 | assertEqual(num_regs, 1); 144 | 145 | result = Pozyx.isRegisterSaved(POZYX_POS_ALG); 146 | assertEqual(result, 1); 147 | 148 | uint8_t reg_check = 0; 149 | Pozyx.regRead(POZYX_POS_ALG, ®_check, 1); 150 | assertEqual(reg_check, reg_saved); 151 | 152 | // finally perform a memory clear 153 | result = Pozyx.clearConfiguration(); 154 | assertEqual(result, POZYX_SUCCESS); 155 | 156 | // reset the system such that all registers are loaded with their default values 157 | Pozyx.resetSystem(); 158 | } 159 | 160 | /* 161 | * Test if we can save a single 32bit register to flash memory 162 | */ 163 | test(flash32bitRegSave) 164 | { 165 | uint8_t result, num_regs; 166 | 167 | // first perform a memory clear 168 | result = Pozyx.clearConfiguration(); 169 | assertEqual(result, POZYX_SUCCESS); 170 | 171 | // now read the number of registers that have been saved in flash, should be zero because we just cleared the memory 172 | num_regs = Pozyx.getNumRegistersSaved(); 173 | assertEqual(num_regs, 0); 174 | 175 | // same if we check if the register is saved 176 | result = Pozyx.isRegisterSaved(POZYX_POS_X); 177 | assertEqual(result, 0); 178 | 179 | // write a new value in the register 180 | uint32_t reg_saved = 0x69C1C7D5; 181 | result = Pozyx.regWrite(POZYX_POS_X, (uint8_t*)®_saved, 4); 182 | assertEqual(result, POZYX_SUCCESS); 183 | 184 | // save to flash 185 | uint8_t reg = POZYX_POS_X; 186 | result = Pozyx.saveConfiguration(POZYX_FLASH_REGS, ®, 1); 187 | assertEqual(result, POZYX_SUCCESS); 188 | 189 | // now read the number of registers that have been saved in flash again, we saved one register so this should be one 190 | num_regs = 0; 191 | num_regs = Pozyx.getNumRegistersSaved(); 192 | assertEqual(num_regs, 1); 193 | 194 | result = Pozyx.isRegisterSaved(POZYX_POS_X); 195 | assertEqual(result, 1); 196 | 197 | // reset the system to see if saving worked 198 | Pozyx.resetSystem(); 199 | 200 | // now read the number of registers that have been saved in flash again, we saved one register so this should be one 201 | num_regs = 0; 202 | num_regs = Pozyx.getNumRegistersSaved(); 203 | assertEqual(num_regs, 1); 204 | 205 | result = Pozyx.isRegisterSaved(POZYX_POS_X); 206 | assertEqual(result, 1); 207 | result = Pozyx.isRegisterSaved(POZYX_POS_X+1); 208 | assertEqual(result, 0); 209 | 210 | uint32_t reg_check = 0; 211 | Pozyx.regRead(POZYX_POS_X, (uint8_t*)®_check, 4); 212 | assertEqual(reg_check, reg_saved); 213 | 214 | // finally perform a memory clear 215 | result = Pozyx.clearConfiguration(); 216 | assertEqual(result, POZYX_SUCCESS); 217 | 218 | // reset the system such that all registers are loaded with their default values 219 | Pozyx.resetSystem(); 220 | } 221 | 222 | /* 223 | * Test if we can save a multiple registers to flash memory 224 | */ 225 | test(flashMultiRegSave) 226 | { 227 | uint8_t result, num_regs; 228 | uint8_t registers[10] = {POZYX_POS_X, POZYX_POS_Z, POZYX_POS_Y, POZYX_INT_CONFIG, POZYX_NETWORK_ID, POZYX_UWB_XTALTRIM, POZYX_INT_MASK, POZYX_GPIO1, POZYX_POS_ALG, POZYX_POS_NUM_ANCHORS}; 229 | int reg_size[10] = {4,4,4,1,2,1,1,1,1,1}; 230 | uint32_t reg_vals[10] = {0x12345678, 0x23456789, 0xABCDEF09, 0x01, 0x7784, 0x05, 0x03, 0x1 ,0x32, 7}; 231 | int i; 232 | 233 | // first perform a memory clear 234 | result = Pozyx.clearConfiguration(); 235 | assertEqual(result, POZYX_SUCCESS); 236 | 237 | // now read the number of registers that have been saved in flash, should be zero because we just cleared the memory 238 | num_regs = Pozyx.getNumRegistersSaved(); 239 | assertEqual(num_regs, 0); 240 | 241 | // write new values in the registers 242 | for(i=0; i< 5; i++) 243 | { 244 | result = Pozyx.regWrite(registers[i], (uint8_t*)®_vals[i], reg_size[i]); 245 | assertEqual(result, POZYX_SUCCESS); 246 | delay(5); 247 | uint32_t reg_check = 0; 248 | Pozyx.regRead(registers[i], (uint8_t*)®_check, reg_size[i]); 249 | assertEqual(reg_check, reg_vals[i]); 250 | } 251 | 252 | // save the first 5 registers to flash 253 | result = Pozyx.saveConfiguration(POZYX_FLASH_REGS, registers, 5); 254 | assertEqual(result, POZYX_SUCCESS); 255 | 256 | // now read the number of registers that have been saved in flash again, we saved one register so this should be one 257 | num_regs = 0; 258 | num_regs = Pozyx.getNumRegistersSaved(); 259 | assertEqual(num_regs, 5); 260 | 261 | for(i=0; i< 10; i++) 262 | { 263 | result = Pozyx.isRegisterSaved(registers[i]); 264 | if(i<5){ 265 | assertEqual(result, 1); 266 | }else{ 267 | assertEqual(result, 0); 268 | } 269 | } 270 | 271 | // write new values in the registers 272 | for(i=5; i< 10; i++) 273 | { 274 | result = Pozyx.regWrite(registers[i], (uint8_t*)®_vals[i], reg_size[i]); 275 | assertEqual(result, POZYX_SUCCESS); 276 | delay(2); 277 | uint32_t reg_check = 0; 278 | Pozyx.regRead(registers[i], (uint8_t*)®_check, reg_size[i]); 279 | assertEqual(reg_check, reg_vals[i]); 280 | } 281 | 282 | // save the remaining 5 registers to flash 283 | result = Pozyx.saveConfiguration(POZYX_FLASH_REGS, registers+5, 5); 284 | assertEqual(result, POZYX_SUCCESS); 285 | 286 | // now read the number of registers that have been saved in flash again, we saved one register so this should be one 287 | num_regs = 0; 288 | num_regs = Pozyx.getNumRegistersSaved(); 289 | assertEqual(num_regs, 10); 290 | 291 | for(i=0; i< 10; i++){ 292 | result = Pozyx.isRegisterSaved(registers[i]); 293 | assertEqual(result, 1); 294 | } 295 | 296 | // reset the system to see if saving worked 297 | Pozyx.resetSystem(); 298 | 299 | // now read the number of registers that have been saved in flash again, we saved one register so this should be one 300 | num_regs = 0; 301 | num_regs = Pozyx.getNumRegistersSaved(); 302 | assertEqual(num_regs, 10); 303 | 304 | for(i=0; i< 10; i++) 305 | { 306 | result = Pozyx.isRegisterSaved(registers[i]); 307 | assertEqual(result, 1); 308 | 309 | uint32_t reg_check = 0; 310 | Pozyx.regRead(registers[i], (uint8_t*)®_check, reg_size[i]); 311 | assertEqual(reg_check, reg_vals[i]); 312 | } 313 | 314 | // finally perform a memory clear 315 | result = Pozyx.clearConfiguration(); 316 | assertEqual(result, POZYX_SUCCESS); 317 | 318 | // reset the system such that all registers are loaded with their default values 319 | Pozyx.resetSystem(); 320 | } 321 | 322 | 323 | /* 324 | * Test if we can save the list of anchors, set by setPositioningAnchorIds() 325 | */ 326 | test(flashAnchorsSave){ 327 | uint16_t anchors[4], anchors_afterreset[4]; 328 | int result, i; 329 | 330 | // perform a memory clear 331 | result = Pozyx.clearConfiguration(); 332 | assertEqual(result, POZYX_SUCCESS); 333 | 334 | // create some anchor ids and store them 335 | for(i=0; i<4; i++){ 336 | anchors[i] = i*500; 337 | } 338 | result = Pozyx.setPositioningAnchorIds(anchors, 4); 339 | assertEqual(result, POZYX_SUCCESS); 340 | 341 | // now save the anchors ids 342 | result = Pozyx.saveConfiguration(POZYX_FLASH_ANCHOR_IDS); 343 | assertEqual(result, POZYX_SUCCESS); 344 | 345 | // reset the system 346 | Pozyx.resetSystem(); 347 | delay(500); 348 | 349 | // read out the anchors again 350 | result = Pozyx.getPositioningAnchorIds(anchors_afterreset, 4); 351 | for(i=0; i<4; i++){ 352 | assertEqual(anchors_afterreset[i], anchors[i]); 353 | } 354 | 355 | // finally perform a memory clear 356 | result = Pozyx.clearConfiguration(); 357 | assertEqual(result, POZYX_SUCCESS); 358 | 359 | // reset the system 360 | Pozyx.resetSystem(); 361 | } 362 | 363 | test(flashDeviceListSave) 364 | { 365 | int result; 366 | uint8_t list_size; 367 | 368 | // perform a memory clear 369 | result = Pozyx.clearConfiguration(); 370 | assertEqual(result, POZYX_SUCCESS); 371 | 372 | // reset the system 373 | Pozyx.resetSystem(); 374 | 375 | // verify that the list is clear 376 | list_size = 10; 377 | result = Pozyx.getDeviceListSize(&list_size); 378 | assertEqual(result, POZYX_SUCCESS); 379 | assertEqual(list_size, 0); 380 | 381 | // add some devices to the list 382 | int i=0; 383 | for(i=0; i<4; i++){ 384 | device_coordinates_t anchor; 385 | anchor.network_id = i+1; 386 | anchor.flag = 0x1; 387 | anchor.pos.x = i+1; 388 | anchor.pos.y = i+2; 389 | anchor.pos.z = i+3; 390 | Pozyx.addDevice(anchor); 391 | } 392 | 393 | // verify that the list is clear 394 | list_size = 10; 395 | result = Pozyx.getDeviceListSize(&list_size); 396 | assertEqual(result, POZYX_SUCCESS); 397 | assertEqual(list_size, 4); 398 | 399 | // save the list to flash 400 | result = Pozyx.saveConfiguration(POZYX_FLASH_NETWORK); 401 | assertEqual(result, POZYX_SUCCESS); 402 | 403 | // reset the system 404 | Pozyx.resetSystem(); 405 | 406 | list_size = 0; 407 | result = Pozyx.getDeviceListSize(&list_size); 408 | assertEqual(result, POZYX_SUCCESS); 409 | assertEqual(list_size, 4); 410 | 411 | uint16_t device_ids[4]; 412 | Pozyx.getDeviceIds(device_ids,4); 413 | 414 | // verify that the information about each device is still the same 415 | for(i=0; i<4; i++){ 416 | assertEqual(device_ids[i], i+1); 417 | coordinates_t coordinates; 418 | result = Pozyx.getDeviceCoordinates(device_ids[i], &coordinates); 419 | assertEqual(result, POZYX_SUCCESS); 420 | assertEqual(coordinates.x, i+1); 421 | assertEqual(coordinates.y, i+2); 422 | assertEqual(coordinates.z, i+3); 423 | } 424 | } -------------------------------------------------------------------------------- /unit_tests/unit_tests_interrupts/unit_tests_interrupts.ino: -------------------------------------------------------------------------------- 1 | #line 2 "unit_tests_core.ino" 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | /** 9 | * These unit tests verify that the interrupts work under different settings. 10 | * This requires firmware version v1.0 11 | * 12 | * Author, Samuel Van de Velde, Pozyx Labs 13 | * 14 | * functions tested: 15 | * - Pozyx.configInterruptPin 16 | * 17 | * This tests the registers: 18 | * - POZYX_INT_CONFIG 19 | * 20 | */ 21 | void test_pin(int pinnum, int arduino_pinnum); 22 | void test_pinlatch(int pinnum, int arduino_pinnum); 23 | void trigger_interrupt(); 24 | 25 | void setup() 26 | { 27 | Serial.begin(115200); 28 | while(!Serial); // for the Arduino Leonardo/Micro only 29 | 30 | Wire.begin(); 31 | 32 | Serial.println(F("Start testing interrupts")); 33 | Serial.println(F("This test requires the Arduino Uno.")); 34 | Serial.println(F("Minimum firmware version v1.0.\n")); 35 | Serial.println(F("---------------------------------------------------------\n")); 36 | 37 | } 38 | 39 | void loop() 40 | { 41 | Test::run(); 42 | } 43 | 44 | // Test pin 1 45 | test(pin1){ 46 | test_pin(1, 9); 47 | } 48 | test(pin1_latch){ 49 | test_pinlatch(1, 9); 50 | } 51 | 52 | // Test pin 2 53 | test(pin2){ 54 | test_pin(2, 10); 55 | } 56 | test(pin2_latch){ 57 | test_pinlatch(2, 10); 58 | } 59 | 60 | // Test pin 3 61 | test(pin3){ 62 | test_pin(3, 11); 63 | } 64 | test(pin3_latch){ 65 | test_pinlatch(3, 11); 66 | } 67 | 68 | // Test pin 4 69 | test(pin4){ 70 | test_pin(4, 12); 71 | } 72 | test(pin4_latch){ 73 | test_pinlatch(4, 12); 74 | } 75 | 76 | // Test pin 5 77 | test(pin5){ 78 | test_pin(5, 2); 79 | } 80 | test(pin5_latch){ 81 | test_pinlatch(5, 2); 82 | } 83 | 84 | // Test pin 6 85 | test(pin6){ 86 | test_pin(6, 3); 87 | } 88 | test(pin6_latch){ 89 | test_pinlatch(6, 3); 90 | } 91 | 92 | void test_pin(int pinnum, int arduino_pinnum) 93 | { 94 | uint8_t int_mask = 0; 95 | Pozyx.regWrite(POZYX_INT_MASK, &int_mask, 1); 96 | 97 | // configure pin 1 in push pull active low, no latch 98 | // verify that the pin give a high signal (not active) 99 | Pozyx.configInterruptPin(pinnum, PIN_MODE_PUSHPULL, PIN_ACTIVE_LOW, 0); 100 | delay(2); 101 | int level = digitalRead(arduino_pinnum); 102 | assertEqual(level, HIGH); 103 | 104 | // configure pin 1 in push pull active high, no latch 105 | // verify that the pin give a high signal (not active) 106 | Pozyx.configInterruptPin(pinnum, PIN_MODE_PUSHPULL, PIN_ACTIVE_HIGH, 0); 107 | delay(2); 108 | level = digitalRead(arduino_pinnum); 109 | assertEqual(level, LOW); 110 | } 111 | 112 | void test_pinlatch(int pinnum, int arduino_pinnum) 113 | { 114 | int level; 115 | uint8_t int_mask; 116 | uint8_t int_status; 117 | 118 | ///////////////////////////////////// 119 | // test for active low 120 | ///////////////////////////////////// 121 | 122 | // turn on the interrupt mask for errors 123 | int_mask = POZYX_INT_MASK_ERR; 124 | Pozyx.regWrite(POZYX_INT_MASK, &int_mask, 1); 125 | 126 | // configure pin 1 in push pull active low, latch 127 | // verify that the pin give a high signal (not active) 128 | Pozyx.configInterruptPin(pinnum, PIN_MODE_PUSHPULL, PIN_ACTIVE_LOW, 1); 129 | delay(2); 130 | level = digitalRead(arduino_pinnum); 131 | assertEqual(level, HIGH); 132 | 133 | // trigger an interrupt, the level should go to low 134 | trigger_interrupt(); 135 | level = digitalRead(arduino_pinnum); 136 | assertEqual(level, LOW); 137 | 138 | // after reading the interrupt status, the level should be high again 139 | int_status =0; 140 | Pozyx.regRead(POZYX_INT_STATUS, &int_status, 1); 141 | level = digitalRead(arduino_pinnum); 142 | assertEqual(level, HIGH); 143 | 144 | ///////////////////////////////////// 145 | // test for active high 146 | ///////////////////////////////////// 147 | 148 | // configure pin 1 in push pull active low, latch 149 | // verify that the pin give a high signal (not active) 150 | Pozyx.configInterruptPin(pinnum, PIN_MODE_PUSHPULL, PIN_ACTIVE_HIGH, 1); 151 | delay(2); 152 | level = digitalRead(arduino_pinnum); 153 | assertEqual(level, LOW); 154 | 155 | // trigger an interrupt, the level should go to low 156 | trigger_interrupt(); 157 | level = digitalRead(arduino_pinnum); 158 | assertEqual(level, HIGH); 159 | 160 | // after reading the interrupt status, the level should be high again 161 | int_status =0; 162 | Pozyx.regRead(POZYX_INT_STATUS, &int_status, 1); 163 | level = digitalRead(arduino_pinnum); 164 | assertEqual(level, LOW); 165 | } 166 | 167 | void trigger_interrupt() 168 | { 169 | // call this function without parameters: this will result in an error 170 | Pozyx.regFunction(POZYX_LED_CTRL); 171 | delay(1); 172 | } 173 | 174 | /* 175 | test(open_drain) 176 | { 177 | int pinnum = 5; 178 | int arduino_pinnum = 2; 179 | int level; 180 | uint8_t int_status; 181 | 182 | ///////////////////////////////////// 183 | // test for active low 184 | ///////////////////////////////////// 185 | 186 | // turn on the interrupt mask for errors 187 | uint8_t int_mask = POZYX_INT_MASK_ERR; 188 | Pozyx.regWrite(POZYX_INT_MASK, &int_mask, 1); 189 | 190 | pinMode(arduino_pinnum, INPUT); // set pin to input 191 | digitalWrite(arduino_pinnum, HIGH); // turn on pullup resistors 192 | 193 | level = digitalRead(arduino_pinnum); 194 | assertEqual(level, HIGH); 195 | 196 | 197 | // opendrain, active low, latching 198 | Pozyx.configInterruptPin(pinnum, PIN_MODE_OPENDRAIN, PIN_ACTIVE_LOW, 1); 199 | delay(2); 200 | level = digitalRead(arduino_pinnum); 201 | assertEqual(level, HIGH); 202 | 203 | // trigger an interrupt, the level should go to low 204 | trigger_interrupt(); 205 | delay(2); 206 | level = digitalRead(arduino_pinnum); 207 | assertEqual(level, LOW); 208 | 209 | // after reading the interrupt status, the level should be high again 210 | int_status =0; 211 | Pozyx.regRead(POZYX_INT_STATUS, &int_status, 1); 212 | delay(2); 213 | level = digitalRead(arduino_pinnum); 214 | assertEqual(level, HIGH); 215 | 216 | 217 | ///////////////////////////////////// 218 | // test for active low 219 | ///////////////////////////////////// 220 | 221 | level = digitalRead(arduino_pinnum); 222 | assertEqual(level, LOW); 223 | 224 | // opendrain, active low, latching 225 | Pozyx.configInterruptPin(pinnum, PIN_MODE_OPENDRAIN, PIN_ACTIVE_HIGH, 1); 226 | delay(5); 227 | level = digitalRead(arduino_pinnum); 228 | assertEqual(level, LOW); 229 | 230 | // trigger an interrupt, the level should go high 231 | trigger_interrupt(); 232 | delay(2); 233 | level = digitalRead(arduino_pinnum); 234 | assertEqual(level, HIGH); 235 | 236 | // after reading the interrupt status, the level should be low again 237 | int_status =0; 238 | Pozyx.regRead(POZYX_INT_STATUS, &int_status, 1); 239 | delay(2); 240 | level = digitalRead(arduino_pinnum); 241 | assertEqual(level, LOW); 242 | 243 | } 244 | */ 245 | -------------------------------------------------------------------------------- /unit_tests/unit_tests_sensors/unit_tests_sensors.ino: -------------------------------------------------------------------------------- 1 | #line 2 "unit_tests_core.ino" 2 | #include 3 | 4 | #define NDEBUG 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | /** 11 | * Unit tests for the Pozyx sensors: accelerometer, gyroscope, magnetometer, pressure sensor, temperature sensor, etc.. 12 | * This test can be run on an Arduino Uno. 13 | * 14 | * Author, Samuel Van de Velde, Pozyx Labs 15 | * 16 | * functions tested: 17 | * - getAcceleration_mg 18 | * - getAllSensorData 19 | * - getAngularVelocity_dps 20 | * - getEulerAngles_deg 21 | * - getGravityVector_mg 22 | * - getLinearAcceleration_mg 23 | * - getMagnetic_uT 24 | * - getPressure_Pa 25 | * - getQuaternion 26 | * - getSensorMode 27 | * - getTemperature_c 28 | * - setSensorMode 29 | * 30 | * Todo: 31 | * ----- 32 | * - better test for the magnetometer 33 | * - better test for the quaternions 34 | * - we don't test if the sensor mode actually *does* something 35 | */ 36 | 37 | 38 | void setup() 39 | { 40 | Serial.begin(115200); 41 | while(!Serial); // for the Arduino Leonardo/Micro only 42 | 43 | Wire.begin(); 44 | 45 | Serial.println(F("Start testing the Pozyx sensors")); 46 | Serial.println(F("This test expects the tag to lay still on a flat surface.\n")); 47 | Serial.println(F("---------------------------------------------------------\n")); 48 | 49 | Pozyx.setSensorMode(12); 50 | delay(500); 51 | } 52 | 53 | void loop() 54 | { 55 | Test::run(); 56 | } 57 | 58 | /* 59 | * Test if pozyx produces sensor values at 100Hz, by checking the interrrupt status. 60 | * We check this by counting the number of new measurements during a fixed interval 61 | */ 62 | test(update_rate) 63 | { 64 | int counter = 0; 65 | int timewindow_ms = 2000; // the clock on the arduino is pretty crappy, so this can be more or less this value 66 | long chrono_ms = millis(); 67 | uint8_t int_status; 68 | 69 | while(millis() - chrono_ms < timewindow_ms) 70 | { 71 | Pozyx.regRead(POZYX_INT_STATUS, &int_status, 1); 72 | if((int_status & POZYX_INT_STATUS_IMU) == POZYX_INT_STATUS_IMU) 73 | counter++; 74 | delay(1); 75 | } 76 | 77 | // 100Hz should be about one measurement per 10ms 78 | float avg_delay = timewindow_ms/counter; 79 | assertLess(avg_delay, 12); 80 | assertMore(avg_delay, 5); 81 | } 82 | 83 | test(getPressure_Pa) 84 | { 85 | float pressure; 86 | int result; 87 | 88 | result = Pozyx.getPressure_Pa( &pressure); 89 | assertEqual(result, POZYX_SUCCESS); 90 | assertLess(pressure, 120000); 91 | assertMore(pressure, 80000); 92 | } 93 | 94 | test(getTemperature_c) 95 | { 96 | float temp; 97 | int result; 98 | 99 | result = Pozyx.getTemperature_c( &temp); 100 | assertEqual(result, POZYX_SUCCESS); 101 | assertLess(temp, 50); 102 | assertMore(temp, -10); 103 | } 104 | 105 | /* 106 | * Test the acceleration 107 | */ 108 | test(getAcceleration_mg) 109 | { 110 | acceleration_t acceleration; 111 | int result; 112 | 113 | result = Pozyx.getAcceleration_mg( &acceleration); 114 | assertEqual(result, POZYX_SUCCESS); 115 | 116 | assertLess(abs(acceleration.x), 1200); 117 | assertLess(abs(acceleration.y), 1200); 118 | assertLess(abs(acceleration.z), 1200); 119 | 120 | float norm = acceleration.x*acceleration.x + acceleration.y*acceleration.y +acceleration.z*acceleration.z; 121 | norm = sqrt(norm); 122 | 123 | assertLess(norm, 1100); 124 | assertMore(norm, 900); 125 | } 126 | 127 | /* 128 | * Test the angular velocity 129 | */ 130 | test(getAngularVelocity_dps) 131 | { 132 | angular_vel_t angular_vel; 133 | int result; 134 | 135 | result = Pozyx.getAngularVelocity_dps( &angular_vel); 136 | assertEqual(result, POZYX_SUCCESS); 137 | 138 | assertLess(abs(angular_vel.x), 1); 139 | assertLess(abs(angular_vel.y), 1); 140 | assertLess(abs(angular_vel.z), 1); 141 | } 142 | 143 | /* 144 | * Test the magnetic field sensor 145 | */ 146 | test(getMagnetic_uT) 147 | { 148 | magnetic_t magn; 149 | int result; 150 | 151 | result = Pozyx.getMagnetic_uT( &magn); 152 | assertEqual(result, POZYX_SUCCESS); 153 | 154 | /*Serial.println(magn.x); 155 | Serial.println(magn.y); 156 | Serial.println(magn.z); 157 | */ 158 | 159 | assertLess(abs(magn.x), 500); 160 | assertLess(abs(magn.y), 500); 161 | assertLess(abs(magn.z), 500); 162 | 163 | float norm = magn.x*magn.x + magn.y*magn.y +magn.z*magn.z; 164 | norm = sqrt(norm); 165 | 166 | //Serial.println(norm); 167 | 168 | assertLess(norm, 500); 169 | assertMore(norm, 10); 170 | } 171 | 172 | test(getLinearAcceleration_mg) 173 | { 174 | acceleration_t lin_acceleration; 175 | float acc_treshold_mg = 100; 176 | int result; 177 | 178 | result = Pozyx.getLinearAcceleration_mg( &lin_acceleration); 179 | assertEqual(result, POZYX_SUCCESS); 180 | 181 | assertLess(abs(lin_acceleration.x), acc_treshold_mg); 182 | assertLess(abs(lin_acceleration.y), acc_treshold_mg); 183 | assertLess(abs(lin_acceleration.z), acc_treshold_mg); 184 | 185 | float norm = lin_acceleration.x*lin_acceleration.x + lin_acceleration.y*lin_acceleration.y +lin_acceleration.z*lin_acceleration.z; 186 | norm = sqrt(norm); 187 | 188 | assertLess(norm, acc_treshold_mg); 189 | } 190 | 191 | test(getGravityVector_mg) 192 | { 193 | acceleration_t grav; 194 | float acc_treshold_mg = 1200; 195 | int result; 196 | 197 | result = Pozyx.getGravityVector_mg( &grav); 198 | assertEqual(result, POZYX_SUCCESS); 199 | 200 | assertLess(abs(grav.x), acc_treshold_mg); 201 | assertLess(abs(grav.y), acc_treshold_mg); 202 | assertLess(abs(grav.z), acc_treshold_mg); 203 | 204 | float norm = grav.x*grav.x + grav.y*grav.y +grav.z*grav.z; 205 | norm = sqrt(norm); 206 | 207 | assertLess(norm, acc_treshold_mg); 208 | assertMore(norm, 800); 209 | } 210 | 211 | test(getEulerAngles_deg) 212 | { 213 | euler_angles_t euler_angles; 214 | int result; 215 | 216 | result = Pozyx.getEulerAngles_deg( &euler_angles); 217 | assertEqual(result, POZYX_SUCCESS); 218 | 219 | assertLess(abs(euler_angles.heading), 361); 220 | assertLess(abs(euler_angles.roll), 361); 221 | assertLess(abs(euler_angles.pitch), 361); 222 | } 223 | 224 | test(getQuaternion) 225 | { 226 | quaternion_t quat; 227 | int result; 228 | 229 | result = Pozyx.getQuaternion( &quat); 230 | assertEqual(result, POZYX_SUCCESS); 231 | 232 | // when the device is laying flat, this should be around 1. 233 | // when the device is upside down, it will be negative 234 | float gz = quat.weight*quat.weight - quat.x*quat.x - quat.y*quat.y + quat.z*quat.z; 235 | assertMore(gz, 0.9); 236 | assertLess(gz, 1.1); 237 | } 238 | 239 | test(sensorMode) 240 | { 241 | uint8_t sensor_modes[13] = {0,1,2,3,4,5,6,7,8,9,10,11,12}; 242 | uint8_t sensor_mode; 243 | int i, result; 244 | 245 | /* 246 | sensor_data_t sensor_data; 247 | sensor_data_t sensor_data_empty; 248 | memset(&sensor_data_empty, 0, sizeof(sensor_data_t)); 249 | 250 | // test MODE_OFF 251 | result = Pozyx.setSensorMode(1); 252 | assertEqual(result, POZYX_SUCCESS); 253 | delay(5000); 254 | result = Pozyx.getSensorMode(&sensor_mode); 255 | assertEqual(result, POZYX_SUCCESS); 256 | delay(10); 257 | 258 | memset(&sensor_data, 0, sizeof(sensor_data_t)); 259 | Pozyx.getAllSensorData(&sensor_data); 260 | result = memcmp(&sensor_data+4, &sensor_data_empty+4, sizeof(sensor_data_t)-4); 261 | 262 | for(i=1; i<(sizeof(sensor_data_t)-2)/4; i++) 263 | { 264 | Serial.print(*((float32_t*)&sensor_data+i)); 265 | Serial.print(" "); 266 | } 267 | */ 268 | 269 | // test the other modes 270 | for(i = 0; i<13; i++) 271 | { 272 | result = Pozyx.setSensorMode(sensor_modes[i]); 273 | assertEqual(result, POZYX_SUCCESS); 274 | delay(10); 275 | result = Pozyx.getSensorMode(&sensor_mode); 276 | assertEqual(result, POZYX_SUCCESS); 277 | 278 | assertEqual(sensor_mode, sensor_modes[i]); 279 | } 280 | } 281 | --------------------------------------------------------------------------------