diff --git a/examples/twr_aloha/Makefile b/examples/twr_aloha/Makefile new file mode 100644 index 0000000000..9a03b53c5f --- /dev/null +++ b/examples/twr_aloha/Makefile @@ -0,0 +1,57 @@ +APPLICATION = twr-aloha + +# If no BOARD is found in the environment, use this default: +BOARD ?= dwm1001 + +# This has to be the absolute path to the RIOT base directory: +RIOTBASE ?= $(CURDIR)/../.. + +# Change this to 0 show compiler invocation lines by default: +QUIET ?= 1 + +# Comment this out to disable code in RIOT that does safety checking +# which is not needed in a production environment but helps in the +# development process: +DEVELHELP ?= 1 + +# Include uwb-core, uwb-dw1000 +USEPKG += uwb-core +USEPKG += uwb-dw1000 + +# Include all ranging algorithms +USEMODULE += uwb-core_twr_ss +USEMODULE += uwb-core_twr_ss_ack +USEMODULE += uwb-core_twr_ss_ext +USEMODULE += uwb-core_twr_ds +USEMODULE += uwb-core_twr_ds_ext + +USEMODULE += shell +USEMODULE += shell_commands +USEMODULE += ps + +# Set the device role: 0x0 for tag, 0x4 for an anchor +DW1000_ROLE ?= 0x00 +CFLAGS += -DDW1000_ROLE_DEFAULT=$(DW1000_ROLE) + +# All uwb-core applications need to enable `-fms-extensions` +CFLAGS += -fms-extensions +ifneq (,$(filter llvm,$(TOOLCHAIN))) + CFLAGS += -Wno-microsoft-anon-tag +endif + +# Enable verbose mode to get all logs +CFLAGS += -DMYNEWT_VAL_RNG_VERBOSE=2 +# Enable RX diagnostics +CFLAGS += -DDW1000_RX_DIAGNOSTIC=1 + +# Fix the TWR algorithm: +# - UWB_DATA_CODE_SS_TWR +# - UWB_DATA_CODE_SS_TWR_EXT +# - UWB_DATA_CODE_SS_TWR_ACK +# - UWB_DATA_CODE_DS_TWR +# - UWB_DATA_CODE_DS_TWR_EXT +UWB_TWR_ALGORITHM_ONLY_ONE ?= UWB_DATA_CODE_SS_TWR +# Uncomment to fix the TWR algoritm +# CFLAGS += -DUWB_TWR_ALGORITHM_ONLY_ONE=$(UWB_TWR_ALGORITHM_ONLY_ONE) + +include $(RIOTBASE)/Makefile.include diff --git a/examples/twr_aloha/Makefile.ci b/examples/twr_aloha/Makefile.ci new file mode 100644 index 0000000000..8cc220c328 --- /dev/null +++ b/examples/twr_aloha/Makefile.ci @@ -0,0 +1,12 @@ +BOARD_INSUFFICIENT_MEMORY := \ + i-nucleo-lrwan1 \ + nucleo-f031k6 \ + nucleo-f042k6 \ + nucleo-l011k4 \ + nucleo-l031k6 \ + nucleo-l053r8 \ + stk3200 \ + stm32f030f4-demo \ + stm32f0discovery \ + stm32l0538-disco \ + # diff --git a/examples/twr_aloha/README.md b/examples/twr_aloha/README.md new file mode 100644 index 0000000000..bae09b34b6 --- /dev/null +++ b/examples/twr_aloha/README.md @@ -0,0 +1,68 @@ +## Decawave TWR_ALOHA Example + +This example allows testing different two-way ranging algorithms between +two boards supporting a dw1000 device. This makes use of the uwb-core +pkg. This example is based on the twr-aloha example in +[uwb-apps](https://github.com/Decawave/uwb-apps/tree/master/apps/twr_aloha). + +### Setup + +1. Flash one node as the tag (Default configuration) + + $ make -C examples/twr_aloha/ flash term + +2. Flash the second node as an anchor + + $ DW1000_ROLE=0x4 make -C examples/twr_aloha/ flash term + +3. On the tag node begin ranging, you will see ranging estimations where +the "raz" field is "range" in meters. + +``` + main(): This is RIOT! (Version: 2020.10-devel-1384-g5b7ad-wip/uwb-dw1000) + pkg uwb-dw1000 + uwb-core test application + {"utime": 49412,"exec": "/home/francisco/workspace/RIOT/examples/twr_aloha/control.c"} + {"device_id"="deca0130","panid="DECA","addr"="1303","part_id"="cad11303","lot_id"="402c188"} + {"utime": 49412,"msg": "frame_duration = 201 usec"} + {"utime": 49412,"msg": "SHR_duration = 139 usec"} + {"utime": 49412,"msg": "holdoff = 821 usec"} + Node role: TAG + {"utime": 120995,"c": 274,"uid": 4867,"ouid": 4660,"raz": [0.766359],"los": [1.000000]} +> range start + range start + Start ranging + {"utime": 5214098,"c": 274,"uid": 4867,"ouid": 4660,"raz": [0.778875],"los": [1.000000]} + {"utime": 5232368,"c": 274,"uid": 4867,"ouid": 4660,"raz": [0.777146],"los": [1.000000]} + {"utime": 5250631,"c": 274,"uid": 4867,"ouid": 4660,"raz": [0.796009],"los": [1.000000]} + {"utime": 5268894,"c": 274,"uid": 4867,"ouid": 4660,"raz": [0.756952],"los": [1.000000]} + {"utime": 5287157,"c": 274,"uid": 4867,"ouid": 4660,"raz": [0.787994],"los": [1.000000]} +``` + +4. Trying different ranging algorithms + +The algorithm used for ranging is selected by modifying the `mode` variable +in the uwb_ev_cb function in main.c. If multiple modes are enabled it will switch among them. + +To use only one algorithm, compile as follows: + + $ UWB_TWR_ALGORITHM_ONLY_ONE=UWB_DATA_CODE_SS_TWR make -C examples/twr_aloha/ flash term + +The different algorithm options are: + +- UWB_DATA_CODE_SS_TWR +- UWB_DATA_CODE_SS_TWR_EXT +- UWB_DATA_CODE_SS_TWR_ACK +- UWB_DATA_CODE_DS_TWR +- UWB_DATA_CODE_DS_TWR_EXT + +### Notes + +The application example fixes the `short_address` of the anchor to +`ANCHOR_ADDRESS`, by default `0x1234`. All tags will send ranging requests +to that address. + +This value can be overridden with a `CFLAG += -DANCHOR_ADDRESS=$(VALUE)`. + +Ranging request are sent every 40ms, this value can also be overridden +by changing the default value of `RANGE_REQUEST_T_US`, e.g: +`CFLAG += -DRANGE_REQUEST_T_US=10000` to set it at 10ms. diff --git a/examples/twr_aloha/control.c b/examples/twr_aloha/control.c new file mode 100644 index 0000000000..492584cbb6 --- /dev/null +++ b/examples/twr_aloha/control.c @@ -0,0 +1,260 @@ +/* + * Copyright (C) 2020 Inria + * + * This file is subject to the terms and conditions of the GNU Lesser + * General Public License v2.1. See the file LICENSE in the top level + * directory for more details. + */ + +/** + * @ingroup examples + * @{ + * + * @file + * + * @author Francisco Molina + * + * @} + */ + +#include +#include +#include +#include + +#include "uwb/uwb.h" +#include "uwb_rng/uwb_rng.h" +#include "dpl/dpl.h" +#include "control.h" + +#include "shell_commands.h" +#include "shell.h" +#include "xtimer.h" + +static struct dpl_callout _rng_req_callout; +static uint8_t _ranging_enabled_flag; +static struct dpl_event _slot_event; + +/* forward declaration */ +static void _slot_complete_cb(struct dpl_event *ev); +static bool _complete_cb(struct uwb_dev *inst, struct uwb_mac_interface *cbs); +static bool _rx_timeout_cb(struct uwb_dev *inst, struct uwb_mac_interface *cbs); + +static int _range_cli_cmd(int argc, char **argv) +{ + (void)argc; + + if (!strcmp(argv[1], "start")) { + printf("Start ranging\n"); + dpl_callout_reset(&_rng_req_callout, RANGE_REQUEST_T_US); + _ranging_enabled_flag = 1; + } + else if (!strcmp(argv[1], "stop")) { + printf("Stop ranging\n"); + dpl_callout_stop(&_rng_req_callout); + _ranging_enabled_flag = 0; + } + else { + puts("Usage:"); + puts("\trange start: to start ranging"); + puts("\trange stop: to stop ranging"); + } + + return 0; +} + +static const shell_command_t shell_commands[] = { + { "range", "Control command", _range_cli_cmd }, + { NULL, NULL, NULL } +}; + +/* Structure of extension callbacks common for mac layer */ +static struct uwb_mac_interface _uwb_mac_cbs = (struct uwb_mac_interface){ + .id = UWBEXT_APP0, + .complete_cb = _complete_cb, + .rx_timeout_cb = _rx_timeout_cb, +}; + +/** + * @brief Range request complete callback. + * + * This callback is part of the struct uwb_mac_interface extension + * interface and invoked of the completion of a range request in the + * context of this example. The struct uwb_mac_interface is in the + * interrupt context and is used to schedule events an event queue. + * + * Processing should be kept to a minimum given the interrupt context. + * All algorithms activities should be deferred to a thread on an event + * queue. The callback should return true if and only if it can determine + * if it is the sole recipient of this event. + * + * @note The MAC extension interface is a linked-list of callbacks, + * subsequentcallbacks on the list will be not be called if the callback + * returns true. + * + * @param[in] inst Pointer to struct uwb_dev. + * @param[in] cbs Pointer to struct uwb_mac_interface. + * * + * @return true if valid recipient for the event, false otherwise + */ +static bool _complete_cb(struct uwb_dev *inst, struct uwb_mac_interface *cbs) +{ + (void)cbs; + + if (inst->fctrl != FCNTL_IEEE_RANGE_16 && + inst->fctrl != (FCNTL_IEEE_RANGE_16 | UWB_FCTRL_ACK_REQUESTED)) { + return false; + } + /* on completion of a range request setup an event to keep listening, + if is anchor */ + dpl_eventq_put(dpl_eventq_dflt_get(), &_slot_event); + return true; +} + +/** + * @brief API for receive timeout callback. + * + * @param[in] inst Pointer to struct uwb_dev. + * @param[in] cbs Pointer to struct uwb_mac_interface. + * + * @return true on success + */ +static bool _rx_timeout_cb(struct uwb_dev *inst, struct uwb_mac_interface *cbs) +{ + struct uwb_rng_instance *rng = (struct uwb_rng_instance *)cbs->inst_ptr; + + if (inst->role & UWB_ROLE_ANCHOR) { + /* Restart receiver */ + uwb_phy_forcetrxoff(inst); + uwb_rng_listen(rng, 0xfffff, UWB_NONBLOCKING); + } + return true; +} + +/** + * @brief In the example this function represents the event context + * processing of the received range request which has been offloaded from + * ISR context to an event queue. + */ +static void _slot_complete_cb(struct dpl_event *ev) +{ + assert(ev != NULL); + + struct uwb_rng_instance *rng = (struct uwb_rng_instance *) + dpl_event_get_arg(ev); + struct uwb_dev *inst = rng->dev_inst; + + if (inst->role & UWB_ROLE_ANCHOR) { + uwb_rng_listen(rng, 0xfffff, UWB_NONBLOCKING); + } +} + +/** + * @brief An event callback to send range request every RANGE_REQUEST_T_US. + * On every request it will switch the used ranging algorithm. + */ +static void uwb_ev_cb(struct dpl_event *ev) +{ + struct uwb_rng_instance *rng = (struct uwb_rng_instance *)ev->arg; + struct uwb_dev *inst = rng->dev_inst; + + if (inst->role & UWB_ROLE_ANCHOR) { + if (dpl_sem_get_count(&rng->sem) == 1) { + uwb_rng_listen(rng, 0xfffff, UWB_NONBLOCKING); + } + } + else { + int mode_v[8] = { 0 }, mode_i = 0, mode = -1; + static int last_used_mode = 0; + if (IS_USED(MODULE_UWB_CORE_TWR_SS)) { + mode_v[mode_i++] = UWB_DATA_CODE_SS_TWR; + } + if (IS_USED(MODULE_UWB_CORE_TWR_SS_ACK)) { + mode_v[mode_i++] = UWB_DATA_CODE_SS_TWR_ACK; + } + if (IS_USED(MODULE_UWB_CORE_TWR_SS_EXT)) { + mode_v[mode_i++] = UWB_DATA_CODE_SS_TWR_EXT; + } + if (IS_USED(MODULE_UWB_CORE_TWR_DS)) { + mode_v[mode_i++] = UWB_DATA_CODE_DS_TWR; + } + if (IS_USED(MODULE_UWB_CORE_TWR_DS_EXT)) { + mode_v[mode_i++] = UWB_DATA_CODE_DS_TWR_EXT; + } + if (++last_used_mode >= mode_i) { + last_used_mode = 0; + } + mode = mode_v[last_used_mode]; +#ifdef UWB_TWR_ALGORITHM_ONLY_ONE + mode = UWB_TWR_ALGORITHM_ONLY_ONE; +#endif + if (mode > 0) { + uwb_rng_request(rng, ANCHOR_ADDRESS, mode); + } + } + + /* reset the callback if ranging is enabled */ + if (_ranging_enabled_flag) { + dpl_callout_reset(&_rng_req_callout, RANGE_REQUEST_T_US); + } +} + +void init_ranging(void) +{ + struct uwb_dev *udev = uwb_dev_idx_lookup(0); + struct uwb_rng_instance *rng = + (struct uwb_rng_instance *)uwb_mac_find_cb_inst_ptr(udev, UWBEXT_RNG); + + assert(rng); + + /* set up local mac callbacks */ + _uwb_mac_cbs.inst_ptr = rng; + uwb_mac_append_interface(udev, &_uwb_mac_cbs); + + uint32_t utime = xtimer_now_usec(); + + printf("{\"utime\": %" PRIu32 ",\"exec\": \"%s\"}\n", utime, __FILE__); + printf("{\"device_id\"=\"%" PRIx32 "\"", udev->device_id); + printf(",\"panid=\"%X\"", udev->pan_id); + printf(",\"addr\"=\"%X\"", udev->uid); + printf(",\"part_id\"=\"%" PRIx32 "\"", + (uint32_t)(udev->euid & 0xffffffff)); + printf(",\"lot_id\"=\"%" PRIx32 "\"}\n", (uint32_t)(udev->euid >> 32)); + printf("{\"utime\": %"PRIu32",\"msg\": \"frame_duration = %d usec\"}\n", + utime, uwb_phy_frame_duration(udev, sizeof(twr_frame_final_t))); + printf("{\"utime\": %"PRIu32",\"msg\": \"SHR_duration = %d usec\"}\n", + utime, uwb_phy_SHR_duration(udev)); + printf("{\"utime\": %"PRIu32",\"msg\": \"holdoff = %d usec\"}\n", utime, + (uint16_t)ceilf(uwb_dwt_usecs_to_usecs(rng->config. + tx_holdoff_delay))); + + if (IS_USED(MODULE_UWB_CORE_TWR_SS_ACK)) { + uwb_set_autoack(udev, true); + uwb_set_autoack_delay(udev, 12); + } + + dpl_callout_init(&_rng_req_callout, dpl_eventq_dflt_get(), + uwb_ev_cb, rng); + dpl_callout_reset(&_rng_req_callout, RANGE_REQUEST_T_US); + dpl_event_init(&_slot_event, _slot_complete_cb, rng); + + /* Apply config */ + uwb_mac_config(udev, NULL); + uwb_txrf_config(udev, &udev->config.txrf); + + if ((udev->role & UWB_ROLE_ANCHOR)) { + printf("Node role: ANCHOR \n"); + udev->my_short_address = ANCHOR_ADDRESS; + uwb_set_uid(udev, udev->my_short_address); + /* anchor starts listening by default */ + _ranging_enabled_flag = 1; + } + else { + _ranging_enabled_flag = 0; + printf("Node role: TAG \n"); + } + + /* define buffer to be used by the shell */ + char line_buf[SHELL_DEFAULT_BUFSIZE]; + shell_run(shell_commands, line_buf, SHELL_DEFAULT_BUFSIZE); +} diff --git a/examples/twr_aloha/control.h b/examples/twr_aloha/control.h new file mode 100644 index 0000000000..2c3e467d91 --- /dev/null +++ b/examples/twr_aloha/control.h @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2020 Inria + * + * This file is subject to the terms and conditions of the GNU Lesser + * General Public License v2.1. See the file LICENSE in the top level + * directory for more details. + */ + +/** + * @ingroup examples + * @{ + * + * @file + * + * @author Francisco Molina + * + * @} + */ + + +#ifndef CONTROL_H +#define CONTROL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "timex.h" + +/** + * @brief Anchor address + */ +#ifndef ANCHOR_ADDRESS +#define ANCHOR_ADDRESS 0x1234 +#endif + +/** + * @brief Range request period + */ +#ifndef RANGE_REQUEST_T_US +#define RANGE_REQUEST_T_US (40 * US_PER_MS) +#endif + +/** + * @brief Starts ranging + */ +void init_ranging(void); + +#ifdef __cplusplus +} +#endif + +#endif /* CONTROL_H */ diff --git a/examples/twr_aloha/main.c b/examples/twr_aloha/main.c new file mode 100644 index 0000000000..cb549a4b98 --- /dev/null +++ b/examples/twr_aloha/main.c @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2020 Inria + * + * This file is subject to the terms and conditions of the GNU Lesser + * General Public License v2.1. See the file LICENSE in the top level + * directory for more details. + */ + +/** + * @ingroup examples + * @{ + * + * @file + * @brief Two way ranging example + * + * @author Francisco Molina + * + * @} + */ + +#include + +#include "control.h" + +int main(void) +{ + puts("pkg uwb-dw1000 + uwb-core test application"); + /* this should start ranging... */ + init_ranging(); + return 1; +}