diff --git a/examples/twr_aloha/Makefile b/examples/twr_aloha/Makefile index 88f6a82a0f..8fb25d0fe5 100644 --- a/examples/twr_aloha/Makefile +++ b/examples/twr_aloha/Makefile @@ -17,6 +17,9 @@ DEVELHELP ?= 1 # Include uwb-core, uwb-dw1000 USEPKG += uwb-core USEPKG += uwb-dw1000 +# Use event threads instead of a custom event queue +USEMODULE += uwb-core_event_thread +CFLAGS += -DEVENT_THREAD_MEDIUM_STACKSIZE=THREAD_STACKSIZE_DEFAULT # Include all ranging algorithms USEMODULE += uwb-core_twr_ss @@ -25,14 +28,15 @@ USEMODULE += uwb-core_twr_ss_ext USEMODULE += uwb-core_twr_ds USEMODULE += uwb-core_twr_ds_ext +# System modules used by this application USEMODULE += shell USEMODULE += shell_commands USEMODULE += ps -USEMODULE += ztimer_usec - -# Set the device role: 0x0 for tag, 0x4 for an anchor -DW1000_ROLE ?= 0x00 -CFLAGS += -DDW1000_ROLE_DEFAULT=$(DW1000_ROLE) +USEMODULE += l2util +USEMODULE += event_callback +USEMODULE += event_timeout_ztimer +USEMODULE += event_periodic +USEMODULE += test_utils_result_output # All uwb-core applications need to enable `-fms-extensions` CFLAGS += -fms-extensions @@ -40,19 +44,4 @@ 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/README.md b/examples/twr_aloha/README.md index bae09b34b6..f5d16ee437 100644 --- a/examples/twr_aloha/README.md +++ b/examples/twr_aloha/README.md @@ -2,67 +2,147 @@ 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). +pkg. + + +#### IoT-LAB Setup + +IoT-LAB can be used to test the application. + +##### Prerequisites + +1. Install iotlab-cli tools + + - `pip install iotlabcli` + +2. Create an iotlab account + + - https://www.iot-lab.info/testbed/signup + +3. Authenticate: + +```shell +$ iotlab-auth --user --password +``` + +##### Launch an Experiment + +1. Submit a 60 minutes experiment with 2 dwm1001 boards: + +```shell +$ iotlab-experiment submit -n "twr-aloha" -d 60 -l 2,archi=dwm1001:dw1000+site=saclay +``` + +2. Wait for the experiment to be in the Running state: + +```shell +$ iotlab-experiment wait --timeout 30 --cancel-on-timeout +``` + +**Note:** If the command above returns the message `Timeout reached, cancelling experiment `, try to re-submit your experiment later. + +3. Get the experiment nodes list: + +```shell +$ iotlab-experiment --jmespath="items[*].network_address | sort(@)" get --nodes +[ + "dwm1001-1.saclay.iot-lab.info", + "dwm1001-10.saclay.iot-lab.info" +] +``` + +When flashing the devices set `IOTLAB_NODE` to one of the above values, e.g. for +the firs node: `IOTLAB_NODE=dwm1001-2.saclay.iot-lab.info`. + +##### Free up the resources + +You are done with the experiment, so stop your experiment to free up the +devices: + +```shell +$ iotlab-experiment stop +``` ### Setup -1. Flash one node as the tag (Default configuration) +1. In two separate terminals flash two nodes and open a serial connection to the +board, prefix with IOTLAB_NODE if using IoT-LAB - $ 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]} +```shell +$ make -C examples/twr_aloha/ flash term -j +$ make -C examples/twr_aloha/ flash term -j ``` -4. Trying different ranging algorithms +1. On at least one node enable listening and get its short address -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. +```shell +> twr lst on +[twr]: start listening +> ifconfig +Iface 3 HWaddr: C8:0C NID: DE:CA -To use only one algorithm, compile as follows: + Long HWaddr: 08:2B:31:07:CC:52:C8:0C +``` - $ UWB_TWR_ALGORITHM_ONLY_ONE=UWB_DATA_CODE_SS_TWR make -C examples/twr_aloha/ flash term +1. On the second node send a request to neighbors short address: -The different algorithm options are: +```shell +>twr req C8:0C +[twr]: start ranging +``` -- 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 +Both nodes should show the ranging result: -### Notes +```shell +> {"t": 55158, "src": "02:A1", "dst": "C8:0C", "d_cm": 39} +``` -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. +By default `twr-ss` (Single-Sided Two-Way-Ranging) is used, try using different +protocols, check the ones available with `help` command: -This value can be overridden with a `CFLAG += -DANCHOR_ADDRESS=$(VALUE)`. +```shell +> twr req --help +Usage: + twr req [-p ] [-c ] [-h][-i ] + - short_addr: short address of request destination + - count: number of requests (default: 1) + - ms interval: wait interval milliseconds between requests(default: 1000) + - proto: twr protocol (ss|ss-ext|ss-ack|ds|ds-ext, default: ss) + twr lst on: start listening for ranging requests + twr lst off: stop listening for ranging requests +``` -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. +1. Perform range requests with different intervals, protocols etc... + +```shell +> twr req C8:0C -c 5 -i 10 +[twr]: start ranging +{"t": 251588, "src": "02:A1", "dst": "C8:0C", "d_cm": 38} +{"t": 251600, "src": "02:A1", "dst": "C8:0C", "d_cm": 37} +{"t": 251610, "src": "02:A1", "dst": "C8:0C", "d_cm": 38} +{"t": 251620, "src": "02:A1", "dst": "C8:0C", "d_cm": 37} +``` + +```shell +> twr req C8:0C -c 5 -i 10 -p ds +[twr]: start ranging +{"t": 312513, "src": "C8:0C", "dst": "02:A1", "d_cm": 36} +{"t": 312525, "src": "C8:0C", "dst": "02:A1", "d_cm": 37} +{"t": 312535, "src": "C8:0C", "dst": "02:A1", "d_cm": 35} +{"t": 312545, "src": "C8:0C", "dst": "02:A1", "d_cm": 35} +``` + +### Automatic Test + +Pre-Requisites: + +- `pip install rioctrl pyserial` + +A basic automatic test for the provided shell is included, run with: + +```shell +$ make -C examples/twr_aloha flash test +``` + +The application provides a `ShellInteraction` that can be used for +scripting UWB experiments or testing. diff --git a/examples/twr_aloha/app.config.test b/examples/twr_aloha/app.config.test index 47383cda5d..9a6860d329 100644 --- a/examples/twr_aloha/app.config.test +++ b/examples/twr_aloha/app.config.test @@ -1,12 +1,15 @@ CONFIG_PACKAGE_UWB-CORE=y CONFIG_PACKAGE_UWB-DW1000=y +CONFIG_MODULE_UWB-CORE_EVENT_THREAD=y CONFIG_MODULE_UWB-CORE_TWR_SS=y CONFIG_MODULE_UWB-CORE_TWR_SS_ACK=y CONFIG_MODULE_UWB-CORE_TWR_SS_EXT=y CONFIG_MODULE_UWB-CORE_TWR_DS=y CONFIG_MODULE_UWB-CORE_TWR_DS_EXT=y CONFIG_MODULE_SHELL=y -CONFIG_MODULE_PS=y CONFIG_MODULE_SHELL_COMMANDS=y -CONFIG_MODULE_ZTIMER=y -CONFIG_ZTIMER_USEC=y +CONFIG_MODULE_PS=y +CONFIG_MODULE_L2UTIL=y +CONFIG_MODULE_EVENT_PERIODIC=y +CONFIG_MODULE_EVENT_TIMEOUT_ZTIMER=y +CONFIG_MODULE_TEST_UTILS_RESULT_OUTPUT=y diff --git a/examples/twr_aloha/control.c b/examples/twr_aloha/control.c index fc81a50455..748c9430f1 100644 --- a/examples/twr_aloha/control.c +++ b/examples/twr_aloha/control.c @@ -22,49 +22,54 @@ #include #include +#include "control.h" #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 "mutex.h" +#include "timex.h" +#include "fmt.h" +#include "test_utils/result_output.h" +#include "event/callback.h" +#include "event/periodic.h" -static struct dpl_callout _rng_req_callout; -static uint8_t _ranging_enabled_flag; -static struct dpl_event _slot_event; +#include "net/ieee802154.h" +#include "net/l2util.h" + +#define IEEE802154_SHORT_ADDRESS_LEN_STR_MAX \ + (sizeof("00:00")) + +/* setup timeout to the maximum value */ +#define UWB_CORE_RNG_RX_TIMEOUT 0xfffff /* 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; +/* currently there can only be one so lets just keep a pointer to it */ +struct uwb_rng_instance *_rng; +struct uwb_dev *_udev; - if (!strcmp(argv[1], "start")) { - printf("Start ranging\n"); - dpl_callout_reset(&_rng_req_callout, RANGE_REQUEST_T_MS); - _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"); - } +/* forward declaration */ +static void _rng_listen(void *arg); +static void _rng_request(void *arg); +static event_callback_t _rng_listen_event = EVENT_CALLBACK_INIT(_rng_listen, NULL); - return 0; -} +#define TWR_STATUS_INITIATOR (1 << 0) +#define TWR_STATUS_RESPONDER (1 << 1) +static uint8_t _status; -static const shell_command_t shell_commands[] = { - { "range", "Control command", _range_cli_cmd }, - { NULL, NULL, NULL } +typedef struct uwb_core_rng_event { + event_callback_t event; /**< the event callback */ + event_periodic_t periodic; /**< the periodic event struct */ + twr_protocol_t proto; /**< protocol to use */ + uint16_t addr; /**< dest short address */ +} uwb_core_rng_event_t; + +static uwb_core_rng_event_t _rng_request_event = { + .event = EVENT_CALLBACK_INIT(_rng_request, &_rng_request_event), + .addr = 0xffff, + .proto = TWR_PROTOCOL_SS, }; /* Structure of extension callbacks common for mac layer */ @@ -74,6 +79,45 @@ static struct uwb_mac_interface _uwb_mac_cbs = (struct uwb_mac_interface){ .rx_timeout_cb = _rx_timeout_cb, }; +/* callback to offload printing */ +static void _print_rng_data_cb(void *arg) +{ + uwb_core_rng_data_t *rng_data = (uwb_core_rng_data_t *)arg; + turo_t ctx; + + char addr_str[IEEE802154_SHORT_ADDRESS_LEN_STR_MAX]; + uint8_t buffer[IEEE802154_SHORT_ADDRESS_LEN]; + + turo_init(&ctx); + turo_dict_open(&ctx); + turo_dict_key(&ctx, "t"); + turo_u32(&ctx, rng_data->time); + turo_dict_key(&ctx, "src"); + byteorder_htobebufs(buffer, rng_data->src); + l2util_addr_to_str(buffer, IEEE802154_SHORT_ADDRESS_LEN, addr_str); + turo_string(&ctx, addr_str); + turo_dict_key(&ctx, "dst"); + byteorder_htobebufs(buffer, rng_data->dest); + l2util_addr_to_str(buffer, IEEE802154_SHORT_ADDRESS_LEN, addr_str); + turo_string(&ctx, addr_str); + turo_dict_key(&ctx, "d_cm"); + turo_s32(&ctx, rng_data->d_cm); +#if IS_USED(MODULE_UWB_CORE_RNG_TRX_INFO) + turo_dict_key(&ctx, "tof"); + turo_float(&ctx, rng_data->tof); + turo_dict_key(&ctx, "los"); + turo_float(&ctx, rng_data->los); + turo_dict_key(&ctx, "rssi"); + turo_float(&ctx, rng_data->rssi); +#endif + turo_dict_close(&ctx); + print_str("\n"); +} + +static uwb_core_rng_data_t _rng_data; +static event_callback_t _print_rng_data_event = EVENT_CALLBACK_INIT( + _print_rng_data_cb, &_rng_data); + /** * @brief Range request complete callback. * @@ -99,19 +143,44 @@ static struct uwb_mac_interface _uwb_mac_cbs = (struct uwb_mac_interface){ 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)) { + /* on completion of a range request setup an event to keep listening */ + event_post(uwb_core_get_eventq(), &_rng_listen_event.super); 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); + /* get received frame */ + struct uwb_rng_instance *rng = (struct uwb_rng_instance *)cbs->inst_ptr; + + rng->idx_current = (rng->idx) % rng->nframes; + twr_frame_t *frame = rng->frames[rng->idx_current]; + /* parse data*/ + uwb_core_rng_data_t data; + + data.src = frame->src_address; + data.dest = frame->dst_address; + data.time = ztimer_now(ZTIMER_MSEC); + float range_f = + uwb_rng_tof_to_meters(uwb_rng_twr_to_tof(rng, rng->idx_current)); + + /* convert from float meters to cm */ + data.d_cm = ((int32_t)(range_f * 100)); +#if IS_USED(MODULE_UWB_CORE_RNG_TRX_INFO) + data.rssi = (int16_t)uwb_calc_rssi(inst, inst->rxdiag); + data.fppl = uwb_calc_fppl(inst, inst->rxdiag); + data.tof = uwb_rng_twr_to_tof(rng, rng->idx_current); + data.los = uwb_estimate_los(rng->dev_inst, data.rssi, data.fppl); +#endif + /* offload range data printing */ + memcpy(&_rng_data, &data, sizeof(uwb_core_rng_data_t)); + event_post(uwb_core_get_eventq(), &_print_rng_data_event.super); + /* on completion of a range request setup an event to keep listening */ + event_post(uwb_core_get_eventq(), &_rng_listen_event.super); return true; } /** - * @brief API for receive timeout callback. + * @brief API for receive timeout callback. * * @param[in] inst Pointer to struct uwb_dev. * @param[in] cbs Pointer to struct uwb_mac_interface. @@ -120,131 +189,103 @@ 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) { - 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); - } + (void)inst; + (void)cbs; + event_post(uwb_core_get_eventq(), &_rng_listen_event.super); 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. + * @brief An event callback to update the remaining listening time */ -static void _slot_complete_cb(struct dpl_event *ev) +static void _rng_listen(void *arg) { - assert(ev != NULL); + (void)arg; - 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); + if (!ztimer_is_set(ZTIMER_MSEC, &_rng_request_event.periodic.timer.timer)) { + _status &= ~TWR_STATUS_INITIATOR; } + if (_status && TWR_STATUS_RESPONDER) { + /* wake up if needed */ + if (_udev->status.sleeping) { + uwb_wakeup(_udev); + } + /* start listening */ + if (dpl_sem_get_count(&_rng->sem) == 1) { + uwb_rng_listen(_rng, UWB_CORE_RNG_RX_TIMEOUT, UWB_NONBLOCKING); + } + } + else { + /* go to sleep if possible */ + if (_rng_request_event.periodic.timer.interval > CONFIG_TWR_MIN_IDLE_SLEEP_MS || + !(_status && TWR_STATUS_INITIATOR)) { + uwb_sleep_config(_udev); + uwb_enter_sleep(_udev); + } + } +} + +void uwb_core_rng_listen_enable(void) +{ + _status |= TWR_STATUS_RESPONDER; + /* post event to start listening */ + event_post(uwb_core_get_eventq(), &_rng_listen_event.super); +} + +void uwb_core_rng_listen_disable(void) +{ + _status &= ~TWR_STATUS_RESPONDER; } /** - * @brief An event callback to send range request every RANGE_REQUEST_T_MS. - * On every request it will switch the used ranging algorithm. + * @brief An event callback to send a range request */ -static void uwb_ev_cb(struct dpl_event *ev) +static void _rng_request(void *arg) { - struct uwb_rng_instance *rng = (struct uwb_rng_instance *)ev->ev.arg; - struct uwb_dev *inst = rng->dev_inst; + uwb_core_rng_event_t *event = (uwb_core_rng_event_t *)arg; - if (inst->role & UWB_ROLE_ANCHOR) { - if (dpl_sem_get_count(&rng->sem) == 1) { - uwb_rng_listen(rng, 0xfffff, UWB_NONBLOCKING); - } + /* wake up if needed */ + if (_udev->status.sleeping) { + uwb_wakeup(_udev); } - 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); - } + /* force transition to trxoff instead of waiting for rng_listen operations + to finish */ + if (dpl_sem_get_count(&_rng->sem) == 0) { + uwb_phy_forcetrxoff(_udev); } - /* reset the callback if ranging is enabled */ - if (_ranging_enabled_flag) { - dpl_callout_reset(&_rng_req_callout, RANGE_REQUEST_T_MS); - } + uwb_rng_request(_rng, event->addr, (uwb_dataframe_code_t)event->proto); } -void init_ranging(void) +void uwb_core_rng_start(uint16_t addr, twr_protocol_t proto, uint32_t interval, + uint32_t count) { - 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); + event_periodic_stop(&_rng_request_event.periodic); - assert(rng); + _rng_request_event.proto = proto; + _rng_request_event.addr = addr; + _status |= TWR_STATUS_INITIATOR; + + event_periodic_set_count(&_rng_request_event.periodic, count); + event_periodic_start(&_rng_request_event.periodic, interval); +} + +void uwb_core_rng_init(void) +{ + _udev = uwb_dev_idx_lookup(0); + _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 = ztimer_now(ZTIMER_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))); - - dpl_callout_init(&_rng_req_callout, dpl_eventq_dflt_get(), - uwb_ev_cb, rng); - dpl_callout_reset(&_rng_req_callout, RANGE_REQUEST_T_MS); - dpl_event_init(&_slot_event, _slot_complete_cb, rng); - - 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); + _uwb_mac_cbs.inst_ptr = _rng; + uwb_mac_append_interface(_udev, &_uwb_mac_cbs); + /* init request periodic event */ + event_periodic_init(&_rng_request_event.periodic, ZTIMER_MSEC, + uwb_core_get_eventq(), &_rng_request_event.event.super); + /* set initial status */ + _status = 0; + /* got to sleep on boot */ + struct uwb_dev*_udev = uwb_dev_idx_lookup(0); + uwb_sleep_config(_udev); + uwb_enter_sleep(_udev); } diff --git a/examples/twr_aloha/control.h b/examples/twr_aloha/control.h index 1f4f8f081a..50859f3880 100644 --- a/examples/twr_aloha/control.h +++ b/examples/twr_aloha/control.h @@ -24,26 +24,81 @@ extern "C" { #endif -#include "timex.h" +#include "uwb/uwb.h" +#include "uwb/uwb_ftypes.h" /** - * @brief Anchor address + * @brief Block after a request is sent */ -#ifndef ANCHOR_ADDRESS -#define ANCHOR_ADDRESS 0x1234 +#ifndef CONFIG_TWR_SHELL_BLOCKING +#define CONFIG_TWR_SHELL_BLOCKING 1 #endif /** - * @brief Range request period + * @brief Minimum idle time to enable putting the radio to sleep */ -#ifndef RANGE_REQUEST_T_MS -#define RANGE_REQUEST_T_MS (40) +#ifndef CONFIG_TWR_MIN_IDLE_SLEEP_MS +#define CONFIG_TWR_MIN_IDLE_SLEEP_MS 20 #endif /** - * @brief Starts ranging + * @brief uwb rng data structure */ -void init_ranging(void); +typedef struct uwb_core_rng_data_t { +#if IS_USED(MODULE_UWB_CORE_RNG_TRX_INFO) + float tof; /**< range request time of flight */ + float los; /**< range request line of sight estimation [0..1] */ + float fppl; /**< range request first path phase loss */ +#endif + uint32_t time; /**< range request timestamp ms */ + int32_t d_cm; /**< range request range estimation (cm) */ + uint16_t src; /**< source short address */ + uint16_t dest; /**< destination short address */ +#if IS_USED(MODULE_UWB_CORE_RNG_TRX_INFO) + int16_t rssi; /**< range request rssi */ +#endif +} uwb_core_rng_data_t; + +/** + * @brief TWR algorithms + */ +typedef enum { + TWR_PROTOCOL_NONE = 0, + TWR_PROTOCOL_SS = UWB_DATA_CODE_SS_TWR, /**< single sided twr */ + TWR_PROTOCOL_SS_ACK = UWB_DATA_CODE_SS_TWR_ACK, /**< single sided twr with + hw ACK as response */ + TWR_PROTOCOL_SS_EXT = UWB_DATA_CODE_SS_TWR_EXT, /**< single sided twr with + extended frames */ + TWR_PROTOCOL_DS = UWB_DATA_CODE_DS_TWR, /**< double sided twr */ + TWR_PROTOCOL_DS_ACK = UWB_DATA_CODE_DS_TWR_EXT, /**< double sided twr with + extended frames */ +} twr_protocol_t; + +/** + * @brief Initialize the uwb_rng wrapper + */ +void uwb_core_rng_init(void); + +/** + * @brief Listen for rng requests + */ +void uwb_core_rng_listen_enable(void); + +/** + * @brief Do not listen for rng requests + */ +void uwb_core_rng_listen_disable(void); + +/** + * @brief Start performing range requests + * + * @param[in] addr short address of destination + * @param[in] proto twr protocol to use + * @param[in] interval interval between requests in ms + * @param[in] count number of requests to perform + */ +void uwb_core_rng_start(uint16_t addr, twr_protocol_t proto, uint32_t interval, + uint32_t count); #ifdef __cplusplus } diff --git a/examples/twr_aloha/main.c b/examples/twr_aloha/main.c index cb549a4b98..b87a1159f8 100644 --- a/examples/twr_aloha/main.c +++ b/examples/twr_aloha/main.c @@ -19,13 +19,26 @@ */ #include +#include "shell.h" +#include "shell_commands.h" #include "control.h" +extern int _twr_handler(int argc, char **argv); +extern int _twr_ifconfig(int argc, char **argv); + +static const shell_command_t shell_commands[] = { + { "twr", "Two-way-ranging (TWR) cli", _twr_handler }, + { "ifconfig", "Network interface information", _twr_ifconfig}, + { NULL, NULL, NULL } +}; + int main(void) { - puts("pkg uwb-dw1000 + uwb-core test application"); /* this should start ranging... */ - init_ranging(); + uwb_core_rng_init(); + /* define buffer to be used by the shell */ + char line_buf[SHELL_DEFAULT_BUFSIZE]; + shell_run(shell_commands, line_buf, SHELL_DEFAULT_BUFSIZE); return 1; } diff --git a/examples/twr_aloha/tests/01-run.py b/examples/twr_aloha/tests/01-run.py new file mode 100755 index 0000000000..3346919b27 --- /dev/null +++ b/examples/twr_aloha/tests/01-run.py @@ -0,0 +1,82 @@ +#! /usr/bin/env python3 + +# Copyright (C) 2021 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. + +import logging +import sys +import unittest + +from riotctrl.ctrl import RIOTCtrl +from riotctrl_shell.sys import Reboot +from twr_shell import TwrCmd, TwrIfconfigParser + + +class TwrShell(Reboot, TwrCmd): + """Convenience class inheriting from the Reboot and TwrCmd shell""" + + _netif = { + "netif": None, + "hwaddr": None, + "hwaddr64": None, + "panid": None, + } + + def parse_netif(self): + parser = TwrIfconfigParser() + self._netif = parser.parse(self.ifconfig()) + + def hwaddr(self): + return self._netif["hwaddr"] + + def netif(self): + return self._netif["netif"] + + def hwaddr64(self): + return self._netif["hwaddr64"] + + def panid(self): + return self._netif["panid"] + + +class TestTWRBase(unittest.TestCase): + DEBUG = False + + @classmethod + def setUpClass(cls): + cls.ctrl = RIOTCtrl() + cls.ctrl.reset() + cls.ctrl.start_term() + if cls.DEBUG: + cls.ctrl.term.logfile = sys.stdout + cls.shell = TwrShell(cls.ctrl) + cls.logger = logging.getLogger(cls.__name__) + if cls.DEBUG: + cls.logger.setLevel(logging.DEBUG) + + @classmethod + def tearDownClass(cls): + cls.ctrl.stop_term() + + +class TestTWR(TestTWRBase): + def test_ifconfig(self): + self.shell.parse_netif() + assert self.shell.panid() == "DE:CA" + assert self.shell.hwaddr() is not None + assert self.shell.hwaddr64() is not None + assert self.shell.panid() is not None + + def test_listen(self): + assert "[twr]: start listening" in self.shell.twr_listen(on=True) + assert "[twr]: stop listening" in self.shell.twr_listen(on=False) + + def test_req(self): + assert "[twr]: start ranging" in self.shell.twr_req() + + +if __name__ == "__main__": + unittest.main() diff --git a/examples/twr_aloha/twr_shell.c b/examples/twr_aloha/twr_shell.c new file mode 100644 index 0000000000..edceb07caf --- /dev/null +++ b/examples/twr_aloha/twr_shell.c @@ -0,0 +1,191 @@ +/* + * Copyright (C) 2021 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 "net/ieee802154.h" +#include "net/l2util.h" +#include "shell_commands.h" +#include "shell.h" +#include "ztimer.h" + +#include "control.h" + +#define IEEE802154_LONG_ADDRESS_LEN_STR_MAX \ + (sizeof("00:00:00:00:00:00:00:00")) +#define IEEE802154_SHORT_ADDRESS_LEN_STR_MAX \ + (sizeof("00:00")) + +int _twr_ifconfig(int argc, char **argv) +{ + (void)argc; + (void)argv; + + char addr_str[IEEE802154_LONG_ADDRESS_LEN_STR_MAX]; + struct uwb_dev *udev = uwb_dev_idx_lookup(0); + uint8_t buffer[IEEE802154_LONG_ADDRESS_LEN]; + + printf("Iface %d", udev->task_str.t.pid); + byteorder_htobebufs(buffer, udev->uid); + printf("\tHWaddr: %s ", + l2util_addr_to_str(buffer, IEEE802154_SHORT_ADDRESS_LEN, addr_str)); + byteorder_htobebufs(buffer, udev->pan_id); + printf("NID: %s\n\n", + l2util_addr_to_str(buffer, IEEE802154_SHORT_ADDRESS_LEN, addr_str)); + byteorder_htobebufll(buffer, udev->euid); + printf("\t\tLong HWaddr: %s\n", + l2util_addr_to_str(buffer, IEEE802154_LONG_ADDRESS_LEN, addr_str)); + + return 0; +} + +static void _print_usage(void) +{ + puts("Usage:"); + puts("\ttwr req [-p ] [-c ] [-h]" + "[-i ] "); + puts("\t - short_addr: short address of request destination"); + puts("\t - count: number of requests (default: 1)"); + puts("\t - ms interval: wait interval milliseconds between requests" + "(default: 1000)"); + puts("\t - proto: twr protocol (ss|ss-ext|ss-ack|ds|ds-ext, default: ss)"); + puts("\ttwr lst on: start listening for ranging requests"); + puts("\ttwr lst off: stop listening for ranging requests"); +} + +int _twr_handler(int argc, char **argv) +{ + if (argc < 2) { + _print_usage(); + return -1; + } + + if (!strcmp(argv[1], "lst")) { + if (argc == 3) { + if (!strcmp(argv[2], "on")) { + puts("[twr]: start listening"); + uwb_core_rng_listen_enable(); + return 0; + } + else if (!strcmp(argv[2], "off")) { + puts("[twr]: stop listening"); + uwb_core_rng_listen_disable(); + return 0; + } + } + _print_usage(); + return -1; + } + + if (!strcmp(argv[1], "req")) { + uint32_t count = 1; + uint32_t interval_ms = 1000; + int proto = UWB_DATA_CODE_SS_TWR; + uint8_t addr[IEEE802154_SHORT_ADDRESS_LEN_STR_MAX]; + int res = 0; + if (argc < 3) { + _print_usage(); + return -1; + } + /* parse command line arguments */ + for (int i = 2; i < argc; i++) { + char *arg = argv[i]; + if (arg[0] != '-') { + size_t addr_len = l2util_addr_from_str(arg, addr); + if (addr_len != 2) { + puts("[Error]: unable to parse address.\n" + "Must be of format [0-9a-fA-F]{2}(:[0-9a-fA-F]{2})*\n" + "(hex pairs delimited by colons)"); + res = 1; + } + } + else { + switch (arg[1]) { + case 'c': + if (((i++) + 1) < argc) { + count = atoi(argv[i]); + if (count > 0) { + continue; + } + res = 1; + } + /* intentionally falls through */ + case 'h': + res = 1; + continue; + /* intentionally falls through */ + case 'p': + if ((++i) < argc) { + if (!strcmp(argv[i], "ss")) { + proto = UWB_DATA_CODE_SS_TWR; + continue; + } + else if (!strcmp(argv[i], "ss-ack")) { + proto = UWB_DATA_CODE_SS_TWR_ACK; + continue; + } + else if (!strcmp(argv[i], "ss-ext")) { + proto = UWB_DATA_CODE_SS_TWR_EXT; + continue; + } + else if (!strcmp(argv[i], "ds")) { + proto = UWB_DATA_CODE_DS_TWR; + continue; + } + else if (!strcmp(argv[i], "ds-ext")) { + proto = UWB_DATA_CODE_DS_TWR_EXT; + continue; + } + else { + printf("[ERROR]: invalid protocol %s\n", argv[i]); + res = 1; + } + } + /* intentionally falls through */ + case 'i': + if ((++i) < argc) { + interval_ms = atoi(argv[i]); + continue; + } + /* intentionally falls through */ + default: + res = 1; + break; + } + } + } + if (res != 0) { + _print_usage(); + return 1; + } + uint16_t short_addr = addr[1] + (addr[0] << 8); + puts("[twr]: start ranging"); + uwb_core_rng_start(short_addr, proto, interval_ms, count); + if (IS_ACTIVE(CONFIG_TWR_SHELL_BLOCKING)) { + ztimer_sleep(ZTIMER_MSEC, interval_ms * (count + 1)); + } + return 0; + } + + _print_usage(); + return -1; +} diff --git a/examples/twr_aloha/twr_shell.py b/examples/twr_aloha/twr_shell.py new file mode 100644 index 0000000000..13dde0b159 --- /dev/null +++ b/examples/twr_aloha/twr_shell.py @@ -0,0 +1,67 @@ +# Copyright (C) 2021 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. + +""" +twr-aloha shell interactions +""" + +import re + +from riotctrl.shell import ShellInteraction, ShellInteractionParser + + +class TwrIfconfigParser(ShellInteractionParser): + iface_c = re.compile(r"Iface\s+(?P\d+)\s") + hwaddr_c = re.compile(r"HWaddr:\s+(?P[0-9a-fA-F:]+)\s") + hwaddr64_c = re.compile(r"Long HWaddr:\s+(?P[0-9a-fA-F:]+)") + panid_c = re.compile(r"NID:\s+(?P[0-9a-fA-F:]+)") + + def parse(self, cmd_output): + netif = { + "netif": None, + "hwaddr": None, + "hwaddr64": None, + "panid": None, + } + for line in cmd_output.splitlines(): + m = self.iface_c.search(line) + if m is not None: + netif["netif"] = m.group("name") + m = self.hwaddr_c.search(line) + if m is not None: + netif["hwaddr"] = m.group("name") + m = self.hwaddr64_c.search(line) + if m is not None: + netif["hwaddr64"] = m.group("name") + m = self.panid_c.search(line) + if m is not None: + netif["panid"] = m.group("name") + return netif + + +class TwrCmd(ShellInteraction): + @ShellInteraction.check_term + def twr_cmd(self, args=None, timeout=-1, async_=False): + cmd = "twr" + if args is not None: + cmd += " {args}".format(args=" ".join(str(a) for a in args)) + return self.cmd(cmd, timeout=timeout, async_=False) + + def twr_listen(self, on=True, timeout=-1, async_=False): + return self.twr_cmd( + args=("lst", "on" if on else "off"), timeout=timeout, async_=async_ + ) + + def twr_req(self, count=1, interval=1000, proto="ss", timeout=-1, async_=False): + return self.twr_cmd( + args=("req", f"-c {count}", f"-p {proto}", f"-i {interval}"), + timeout=timeout, + async_=async_, + ) + + @ShellInteraction.check_term + def ifconfig(self, timeout=-1, async_=False): + return self.cmd("ifconfig", timeout, async_) diff --git a/pkg/uwb-core/Kconfig b/pkg/uwb-core/Kconfig index 92acdb1a58..b8610693dd 100644 --- a/pkg/uwb-core/Kconfig +++ b/pkg/uwb-core/Kconfig @@ -59,10 +59,10 @@ config MODULE_UWB-CORE_RNG bool "uwb-core ranging module" select MODULE_UWB-CORE_RNG_MATH select MODULE_UWB-CORE_DSP - select MODULE_UWB-CORE_UWB_JSON config MODULE_UWB-CORE_UWB_JSON bool "uwb-core JSON utilities" + select MODULE_FMT config MODULE_UWB-CORE_DSP bool "uwb-core DSP module" @@ -76,9 +76,12 @@ config MODULE_UWB-CORE_UWBCFG config MODULE_UWB-CORE_EVENT_THREAD bool "Use event-thread loop as uwb-core's event loop" - select MODULE_UWB-CORE_CONFIG + select MODULE_EVENT_THREAD config MODULE_UWB-CORE_CONFIG bool +config MODULE_UWB-CORE_TRX_INFO + bool "Enable uwb-core diagnostic data: rssi, tof, los" + endif # PACKAGE_UWB-CORE diff --git a/pkg/uwb-core/Makefile.dep b/pkg/uwb-core/Makefile.dep index 589ce0fb53..bac356bea9 100644 --- a/pkg/uwb-core/Makefile.dep +++ b/pkg/uwb-core/Makefile.dep @@ -3,7 +3,6 @@ USEMODULE += uwb-core_contrib DEFAULT_MODULE += auto_init_uwb-core -USEMODULE += fmt FEATURES_REQUIRED += periph_gpio_irq FEATURES_REQUIRED += periph_spi @@ -15,7 +14,6 @@ endif ifneq (,$(filter uwb-core_rng,$(USEMODULE))) USEMODULE += uwb-core_rng_math USEMODULE += uwb-core_dsp - USEMODULE += uwb-core_uwb_json endif ifneq (,$(filter uwb-core_uwbcfg,$(USEMODULE))) @@ -34,6 +32,9 @@ ifneq (,$(filter uwb-core_event_thread,$(USEMODULE))) USEMODULE += event_thread endif +ifneq (,$(filter uwb-core_rng_json,$(USEMODULE))) + USEMODULE += fmt +endif # Some stdlib functions used by the pkg are not in avr-gcc FEATURES_BLACKLIST += arch_avr8 # uwb-core has specific compilation sources when compiling kernel diff --git a/pkg/uwb-core/Makefile.include b/pkg/uwb-core/Makefile.include index 95a7b949c0..61a3407add 100644 --- a/pkg/uwb-core/Makefile.include +++ b/pkg/uwb-core/Makefile.include @@ -15,6 +15,12 @@ INCLUDES += -I$(PKGDIRBASE)/uwb-core/hw/drivers/uwb/include/ \ # PSEUDOMODULES += uwb-core_dpl +PSEUDOMODULES += uwb-core_rng_trx_info DIRS += $(RIOTPKG)/uwb-core/contrib \ # + +ifneq (,$(filter uwb-core_uwb_rng_trx_info,$(USEMODULE))) + # Enable RX diagnostics + CFLAGS += -DDW1000_RX_DIAGNOSTIC=1 +endif diff --git a/sys/Makefile.dep b/sys/Makefile.dep index 2a553173c1..a1d6651908 100644 --- a/sys/Makefile.dep +++ b/sys/Makefile.dep @@ -548,7 +548,6 @@ endif ifneq (,$(filter event_periodic,$(USEMODULE))) USEMODULE += event_timeout_ztimer - USEMODULE += ztimer_periodic endif ifneq (,$(filter event,$(USEMODULE))) diff --git a/sys/event/Kconfig b/sys/event/Kconfig index 5ce394dac5..3a2704e39e 100644 --- a/sys/event/Kconfig +++ b/sys/event/Kconfig @@ -18,6 +18,10 @@ menuconfig MODULE_EVENT if MODULE_EVENT +config MODULE_EVENT_PERIODIC + bool "Provides functionality to trigger periodic events" + depends on MODULE_EVENT_TIMEOUT_ZTIMER + config MODULE_EVENT_CALLBACK bool "Support for callback-with-argument event type"