diff --git a/.env b/.env new file mode 100644 index 0000000..5baca68 --- /dev/null +++ b/.env @@ -0,0 +1,2 @@ +EXPORT_DIR="export" +CACHE_DIR="cache" \ No newline at end of file diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c629ecb --- /dev/null +++ b/.gitignore @@ -0,0 +1,12 @@ +/.idea +/build +.DS_Store +/scripts/testbed/__pycache__/trento_b.cpython-38.pyc +/scripts/testbed/__pycache__/trento_b.cpython-310.pyc +/scripts/testbed/__pycache__/trento_a.cpython-310.pyc +/scripts/testbed/__pycache__/lille.cpython-38.pyc +/scripts/testbed/__pycache__/lille.cpython-310.pyc +__pycache__ +/scripts/.DS_Store +*.pyc +/cache diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..f3c5b38 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,6 @@ +cmake_minimum_required(VERSION 3.20.0) + +find_package(Zephyr) +project(uwb_swarm_ranging) + +target_sources(app PRIVATE src/main.c src/testbed.c src/nodes.c src/measurements.c src/estimation.c src/uart.c src/history.c) \ No newline at end of file diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..a1dffd2 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,33 @@ +FROM --platform=linux/amd64 zephyrprojectrtos/zephyr-build:latest + +# Switch to root for installations +USER root + +# Setup essentials +RUN apt-get update -y +RUN apt-get install -y git nano build-essential + +# Install dependencies +RUN apt-get install -y gcc-multilib g++-multilib fftw3-dev + +WORKDIR / + +RUN mkdir -p /app +RUN mkdir -p /zephyr + +RUN chown -Rf user:user /app +RUN chown -Rf user:user /zephyr + +USER user + +WORKDIR /zephyr + +ENV ZEPHYR_BASE=/zephyr/zephyr + +ENV ZEPHYR_REVISION=v2.7.2 +RUN west init --mr $ZEPHYR_REVISION +RUN west update +RUN west zephyr-export + +# Switch back to app directory +WORKDIR /app \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..1ea97da --- /dev/null +++ b/README.md @@ -0,0 +1,50 @@ +# ALADIn: Autonomous Linear Antenna Delay Inference on Resource-Constrained Ultra-Wideband Devices + +This project contains the PoC for ALADIN: All-to-all Linear Antenna Delay Inference on Ultra-Wideband Devices. It is currently under revision and clean-up. + + +## Setup +Setup Zephyr and West as usual. As the Decawave Driver does not allow precise timings, we added some overrides which need to be manually applied (see override directory). As an alternative, you can checkout the related [Zephyr feature branch](https://github.com/prathje/zephyr/tree/feature/dwm_1001_ranging_api). +Tested based on commit 6d56b829423056819c4baaafd6c66957752e22f8, while commit eeb4434d2eb5f2c978c59a439688c1f3f46e8bf8 has been reverted due to scheduling exceptions (already included in the overrides). + +## Build + +Build the project for the Decawave DWM1001 module: + +```bash +west build -b decawave_dwm1001_dev --pristine auto +``` + +You can then flash the boards one by one: +```bash +west flash +``` + +## Build With Docker + +You can also use the included Dockerfile / docker compose file configuration to build (warning this might take a bit of time): + +```commandline +docker compose up -d --build + +docker compose exec -it build /bin/bash +cp -Rf /app/override/* /zephyr/zephyr/ +west build -b decawave_dwm1001_dev --pristine auto +``` + + +To run and deploy on Lille: +```commandline +scp ./build/zephyr/zephyr.elf USER@lille.iot-lab.info:~ && ssh USER@lille.iot-lab.info 'iotlab-experiment submit -d 5 -l lille,dwm1001,1-14,zephyr.elf' +``` + + +In the container you can also easily install script dependencies as follows: +```commandline +pip3 install numpy pandas matplotlib +``` +## Scripts + +Evaluation scripts are in the scripts directory. + + diff --git a/docker-compose.yaml b/docker-compose.yaml new file mode 100644 index 0000000..748d5a8 --- /dev/null +++ b/docker-compose.yaml @@ -0,0 +1,14 @@ +services: + build: + build: . + volumes: + - ./:/app/ + environment: + ZEPHYR_REVISION: "v3.2.0" + tty: true +# command: +# - /bin/sh +# - -c +# - | +# cp -Rf /app/override/* /zephyr/zephyr/ +# west build -b decawave_dwm1001_dev --pristine always \ No newline at end of file diff --git a/override/drivers/ieee802154/ieee802154_dw1000.c b/override/drivers/ieee802154/ieee802154_dw1000.c new file mode 100644 index 0000000..00dcd04 --- /dev/null +++ b/override/drivers/ieee802154/ieee802154_dw1000.c @@ -0,0 +1,1840 @@ +/* + * Copyright (c) 2020 PHYTEC Messtechnik GmbH + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +LOG_MODULE_REGISTER(dw1000, LOG_LEVEL_INF); + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include "ieee802154_dw1000_regs.h" +#include + +#define DT_DRV_COMPAT decawave_dw1000 + +#define DWT_FCS_LENGTH 2U +#define DWT_SPI_CSWAKEUP_FREQ 500000U +#define DWT_SPI_SLOW_FREQ 2000000U +#define DWT_SPI_TRANS_MAX_HDR_LEN 3 +#define DWT_SPI_TRANS_REG_MAX_RANGE 0x3F +#define DWT_SPI_TRANS_SHORT_MAX_OFFSET 0x7F +#define DWT_SPI_TRANS_WRITE_OP BIT(7) +#define DWT_SPI_TRANS_SUB_ADDR BIT(6) +#define DWT_SPI_TRANS_EXTEND_ADDR BIT(7) + +#define DWT_TS_TIME_UNITS_FS 15650U /* DWT_TIME_UNITS in fs */ + +#define DW1000_TX_ANT_DLY 16450 +#define DW1000_RX_ANT_DLY 16450 + +/* SHR Symbol Duration in ns */ +#define UWB_PHY_TPSYM_PRF64 1017.63 +#define UWB_PHY_TPSYM_PRF16 993.59 + +#define UWB_PHY_NUMOF_SYM_SHR_SFD 8 + +/* PHR Symbol Duration Tdsym in ns */ +#define UWB_PHY_TDSYM_PHR_110K 8205.13 +#define UWB_PHY_TDSYM_PHR_850K 1025.64 +#define UWB_PHY_TDSYM_PHR_6M8 1025.64 + +#define UWB_PHY_NUMOF_SYM_PHR 18 + +/* Data Symbol Duration Tdsym in ns */ +#define UWB_PHY_TDSYM_DATA_110K 8205.13 +#define UWB_PHY_TDSYM_DATA_850K 1025.64 +#define UWB_PHY_TDSYM_DATA_6M8 128.21 + +#define DWT_WORK_QUEUE_STACK_SIZE 512 + +#define DWT_OTP_DELAY_ADDR 0x1C + +static struct k_work_q dwt_work_queue; +static K_KERNEL_STACK_DEFINE(dwt_work_queue_stack, +DWT_WORK_QUEUE_STACK_SIZE); + +struct dwt_phy_config { + uint8_t channel; /* Channel 1, 2, 3, 4, 5, 7 */ + uint8_t dr; /* Data rate DWT_BR_110K, DWT_BR_850K, DWT_BR_6M8 */ + uint8_t prf; /* PRF DWT_PRF_16M or DWT_PRF_64M */ + + uint8_t rx_pac_l; /* DWT_PAC8..DWT_PAC64 */ + uint8_t rx_shr_code; /* RX SHR preamble code */ + uint8_t rx_ns_sfd; /* non-standard SFD */ + uint16_t rx_sfd_to; /* SFD timeout value (in symbols) + * (tx_shr_nsync + 1 + SFD_length - rx_pac_l) + */ + + uint8_t tx_shr_code; /* TX SHR preamble code */ + uint32_t tx_shr_nsync; /* PLEN index, e.g. DWT_PLEN_64 */ + + float t_shr; + float t_phr; + float t_dsym; +}; + +struct dwt_hi_cfg { + struct spi_dt_spec bus; + struct gpio_dt_spec irq_gpio; + struct gpio_dt_spec rst_gpio; +}; + +#define DWT_STATE_TX 0 +#define DWT_STATE_CCA 1 +#define DWT_STATE_RX_DEF_ON 2 + +struct dwt_context { + const struct device *dev; + struct net_if *iface; + const struct spi_config *spi_cfg; + struct spi_config spi_cfg_slow; + struct gpio_callback gpio_cb; + struct k_sem dev_lock; + struct k_sem phy_sem; + struct k_work irq_cb_work; + struct k_thread thread; + struct dwt_phy_config rf_cfg; + atomic_t state; + bool cca_busy; + uint16_t sleep_mode; + uint8_t mac_addr[8]; + + bool delayed_tx_short_ts_set; + uint32_t delayed_tx_short_ts; + + uint64_t rx_ts; +}; + +static const struct dwt_hi_cfg dw1000_0_config = { + .bus = SPI_DT_SPEC_INST_GET(0, SPI_WORD_SET(8), 0), + .irq_gpio = GPIO_DT_SPEC_INST_GET(0, int_gpios), + .rst_gpio = GPIO_DT_SPEC_INST_GET(0, reset_gpios), +}; + +static struct dwt_context dwt_0_context = { + .dev_lock = Z_SEM_INITIALIZER(dwt_0_context.dev_lock, 1, 1), + .phy_sem = Z_SEM_INITIALIZER(dwt_0_context.phy_sem, 0, 1), + .rf_cfg = { + .channel = 5, + .dr = DWT_BR_6M8, + .prf = DWT_PRF_64M, + + .rx_pac_l = DWT_PAC8, + .rx_shr_code = 10, + .rx_ns_sfd = 0, + .rx_sfd_to = (129 + 8 - 8), + + .tx_shr_code = 10, + .tx_shr_nsync = DWT_PLEN_128, + }, +}; + +/* This struct is used to read all additional RX frame info at one push */ +struct dwt_rx_info_regs { + uint8_t rx_fqual[DWT_RX_FQUAL_LEN]; + uint8_t rx_ttcki[DWT_RX_TTCKI_LEN]; + uint8_t rx_ttcko[DWT_RX_TTCKO_LEN]; + /* RX_TIME without RX_RAWST */ + uint8_t rx_time[DWT_RX_TIME_FP_RAWST_OFFSET]; +} _packed; + +static int dwt_configure_rf_phy(const struct device *dev); + +static int dwt_spi_read(const struct device *dev, + uint16_t hdr_len, const uint8_t *hdr_buf, + uint32_t data_len, uint8_t *data) +{ + struct dwt_context *ctx = dev->data; + const struct dwt_hi_cfg *hi_cfg = dev->config; + const struct spi_buf tx_buf = { + .buf = (uint8_t *)hdr_buf, + .len = hdr_len + }; + const struct spi_buf_set tx = { + .buffers = &tx_buf, + .count = 1 + }; + struct spi_buf rx_buf[2] = { + { + .buf = NULL, + .len = hdr_len, + }, + { + .buf = (uint8_t *)data, + .len = data_len, + }, + }; + const struct spi_buf_set rx = { + .buffers = rx_buf, + .count = 2 + }; + + LOG_DBG("spi read, header length %u, data length %u", + (uint16_t)hdr_len, (uint32_t)data_len); + LOG_HEXDUMP_DBG(hdr_buf, (uint16_t)hdr_len, "rd: header"); + + if (spi_transceive(hi_cfg->bus.bus, ctx->spi_cfg, &tx, &rx)) { + LOG_ERR("SPI transfer failed"); + return -EIO; + } + + LOG_HEXDUMP_DBG(data, (uint32_t)data_len, "rd: data"); + + return 0; +} + + +static int dwt_spi_write(const struct device *dev, + uint16_t hdr_len, const uint8_t *hdr_buf, + uint32_t data_len, const uint8_t *data) +{ + struct dwt_context *ctx = dev->data; + const struct dwt_hi_cfg *hi_cfg = dev->config; + struct spi_buf buf[2] = { + {.buf = (uint8_t *)hdr_buf, .len = hdr_len}, + {.buf = (uint8_t *)data, .len = data_len} + }; + struct spi_buf_set buf_set = {.buffers = buf, .count = 2}; + + LOG_DBG("spi write, header length %u, data length %u", + (uint16_t)hdr_len, (uint32_t)data_len); + LOG_HEXDUMP_DBG(hdr_buf, (uint16_t)hdr_len, "wr: header"); + LOG_HEXDUMP_DBG(data, (uint32_t)data_len, "wr: data"); + + if (spi_write(hi_cfg->bus.bus, ctx->spi_cfg, &buf_set)) { + LOG_ERR("SPI read failed"); + return -EIO; + } + + return 0; +} + +/* See 2.2.1.2 Transaction formats of the SPI interface */ +static int dwt_spi_transfer(const struct device *dev, + uint8_t reg, uint16_t offset, + size_t buf_len, uint8_t *buf, bool write) +{ + uint8_t hdr[DWT_SPI_TRANS_MAX_HDR_LEN] = {0}; + size_t hdr_len = 0; + + hdr[0] = reg & DWT_SPI_TRANS_REG_MAX_RANGE; + hdr_len += 1; + + if (offset != 0) { + hdr[0] |= DWT_SPI_TRANS_SUB_ADDR; + hdr[1] = (uint8_t)offset & DWT_SPI_TRANS_SHORT_MAX_OFFSET; + hdr_len += 1; + + if (offset > DWT_SPI_TRANS_SHORT_MAX_OFFSET) { + hdr[1] |= DWT_SPI_TRANS_EXTEND_ADDR; + hdr[2] = (uint8_t)(offset >> 7); + hdr_len += 1; + } + + } + + if (write) { + hdr[0] |= DWT_SPI_TRANS_WRITE_OP; + return dwt_spi_write(dev, hdr_len, hdr, buf_len, buf); + } else { + return dwt_spi_read(dev, hdr_len, hdr, buf_len, buf); + } +} + +static int dwt_register_read(const struct device *dev, + uint8_t reg, uint16_t offset, size_t buf_len, uint8_t *buf) +{ + return dwt_spi_transfer(dev, reg, offset, buf_len, buf, false); +} + +static int dwt_register_write(const struct device *dev, + uint8_t reg, uint16_t offset, size_t buf_len, uint8_t *buf) +{ + return dwt_spi_transfer(dev, reg, offset, buf_len, buf, true); +} + +static inline uint32_t dwt_reg_read_u32(const struct device *dev, + uint8_t reg, uint16_t offset) +{ + uint8_t buf[sizeof(uint32_t)]; + + dwt_spi_transfer(dev, reg, offset, sizeof(buf), buf, false); + + return sys_get_le32(buf); +} + +static inline uint16_t dwt_reg_read_u16(const struct device *dev, + uint8_t reg, uint16_t offset) +{ + uint8_t buf[sizeof(uint16_t)]; + + dwt_spi_transfer(dev, reg, offset, sizeof(buf), buf, false); + + return sys_get_le16(buf); +} + +static inline uint8_t dwt_reg_read_u8(const struct device *dev, + uint8_t reg, uint16_t offset) +{ + uint8_t buf; + + dwt_spi_transfer(dev, reg, offset, sizeof(buf), &buf, false); + + return buf; +} + +static inline void dwt_reg_write_u32(const struct device *dev, + uint8_t reg, uint16_t offset, uint32_t val) +{ + uint8_t buf[sizeof(uint32_t)]; + + sys_put_le32(val, buf); + dwt_spi_transfer(dev, reg, offset, sizeof(buf), buf, true); +} + +static inline void dwt_reg_write_u16(const struct device *dev, + uint8_t reg, uint16_t offset, uint16_t val) +{ + uint8_t buf[sizeof(uint16_t)]; + + sys_put_le16(val, buf); + dwt_spi_transfer(dev, reg, offset, sizeof(buf), buf, true); +} + +static inline void dwt_reg_write_u8(const struct device *dev, + uint8_t reg, uint16_t offset, uint8_t val) +{ + dwt_spi_transfer(dev, reg, offset, sizeof(uint8_t), &val, true); +} + +static ALWAYS_INLINE void dwt_setup_int(const struct device *dev, + bool enable) +{ + const struct dwt_hi_cfg *hi_cfg = dev->config; + + unsigned int flags = enable + ? GPIO_INT_EDGE_TO_ACTIVE + : GPIO_INT_DISABLE; + + gpio_pin_interrupt_configure_dt(&hi_cfg->irq_gpio, flags); +} + +static void dwt_reset_rfrx(const struct device *dev) +{ + /* + * Apply a receiver-only soft reset, + * see SOFTRESET field description in DW1000 User Manual. + */ + dwt_reg_write_u8(dev, DWT_PMSC_ID, DWT_PMSC_CTRL0_SOFTRESET_OFFSET, + DWT_PMSC_CTRL0_RESET_RX); + dwt_reg_write_u8(dev, DWT_PMSC_ID, DWT_PMSC_CTRL0_SOFTRESET_OFFSET, + DWT_PMSC_CTRL0_RESET_CLEAR); +} + +static void dwt_disable_txrx(const struct device *dev) +{ + dwt_setup_int(dev, false); + + dwt_reg_write_u8(dev, DWT_SYS_CTRL_ID, DWT_SYS_CTRL_OFFSET, + DWT_SYS_CTRL_TRXOFF); + + dwt_reg_write_u32(dev, DWT_SYS_STATUS_ID, DWT_SYS_STATUS_OFFSET, + (DWT_SYS_STATUS_ALL_RX_GOOD | + DWT_SYS_STATUS_ALL_RX_TO | + DWT_SYS_STATUS_ALL_RX_ERR | + DWT_SYS_STATUS_ALL_TX)); + + dwt_setup_int(dev, true); +} + +/* timeout time in units of 1.026 microseconds */ +static int dwt_enable_rx(const struct device *dev, uint16_t timeout) +{ + uint32_t sys_cfg; + uint16_t sys_ctrl = DWT_SYS_CTRL_RXENAB; + + sys_cfg = dwt_reg_read_u32(dev, DWT_SYS_CFG_ID, 0); + + if (timeout != 0) { + dwt_reg_write_u16(dev, DWT_RX_FWTO_ID, DWT_RX_FWTO_OFFSET, + timeout); + sys_cfg |= DWT_SYS_CFG_RXWTOE; + } else { + sys_cfg &= ~DWT_SYS_CFG_RXWTOE; + } + + dwt_reg_write_u32(dev, DWT_SYS_CFG_ID, 0, sys_cfg); + dwt_reg_write_u16(dev, DWT_SYS_CTRL_ID, DWT_SYS_CTRL_OFFSET, sys_ctrl); + + return 0; +} + +static inline void dwt_irq_handle_rx_cca(const struct device *dev) +{ + struct dwt_context *ctx = dev->data; + + k_sem_give(&ctx->phy_sem); + ctx->cca_busy = true; + + /* Clear all RX event bits */ + dwt_reg_write_u32(dev, DWT_SYS_STATUS_ID, 0, + DWT_SYS_STATUS_ALL_RX_GOOD); +} + +static inline void dwt_irq_handle_rx(const struct device *dev, uint32_t sys_stat) +{ + struct dwt_context *ctx = dev->data; + struct net_pkt *pkt = NULL; + struct dwt_rx_info_regs rx_inf_reg; + float a_const; + uint32_t rx_finfo; + uint32_t ttcki; + uint32_t rx_pacc; + uint32_t cir_pwr; + uint32_t flags_to_clear; + int32_t ttcko; + uint16_t pkt_len; + uint8_t *fctrl; + int8_t rx_level = INT8_MIN; + + LOG_DBG("RX OK event, SYS_STATUS 0x%08x", sys_stat); + flags_to_clear = sys_stat & DWT_SYS_STATUS_ALL_RX_GOOD; + + rx_finfo = dwt_reg_read_u32(dev, DWT_RX_FINFO_ID, DWT_RX_FINFO_OFFSET); + pkt_len = rx_finfo & DWT_RX_FINFO_RXFLEN_MASK; + rx_pacc = (rx_finfo & DWT_RX_FINFO_RXPACC_MASK) >> + DWT_RX_FINFO_RXPACC_SHIFT; + + if (!(IS_ENABLED(CONFIG_IEEE802154_RAW_MODE))) { + pkt_len -= DWT_FCS_LENGTH; + } + + pkt = net_pkt_alloc_with_buffer(ctx->iface, pkt_len, + AF_UNSPEC, 0, K_NO_WAIT); + if (!pkt) { + LOG_ERR("No buf available"); + goto rx_out_enable_rx; + } + + dwt_register_read(dev, DWT_RX_BUFFER_ID, 0, pkt_len, pkt->buffer->data); + dwt_register_read(dev, DWT_RX_FQUAL_ID, 0, sizeof(rx_inf_reg), + (uint8_t *)&rx_inf_reg); + net_buf_add(pkt->buffer, pkt_len); + fctrl = pkt->buffer->data; + + /* + * Get Ranging tracking offset and tracking interval + * for Crystal characterization + */ + ttcki = sys_get_le32(rx_inf_reg.rx_ttcki); + ttcko = sys_get_le32(rx_inf_reg.rx_ttcko) & DWT_RX_TTCKO_RXTOFS_MASK; + /* Tracking offset value is a 19-bit signed integer */ + if (ttcko & BIT(18)) { + ttcko |= ~DWT_RX_TTCKO_RXTOFS_MASK; + } + + /* TODO add: + * net_pkt_set_ieee802154_tcki(pkt, ttcki); + * net_pkt_set_ieee802154_tcko(pkt, ttcko); + */ + LOG_DBG("ttcko %d ttcki: 0x%08x", ttcko, ttcki); + + if (IS_ENABLED(CONFIG_NET_PKT_TIMESTAMP)) { + uint8_t ts_buf[sizeof(uint64_t)] = {0}; + struct net_ptp_time timestamp; + uint64_t ts_fsec; + + memcpy(ts_buf, rx_inf_reg.rx_time, DWT_RX_TIME_RX_STAMP_LEN); + uint64_t rx_ts = sys_get_le64(ts_buf); + ctx->rx_ts = rx_ts; + + ts_fsec = rx_ts * DWT_TS_TIME_UNITS_FS; + timestamp.second = (ts_fsec / 1000000) / NSEC_PER_SEC; + timestamp.nanosecond = (ts_fsec / 1000000) % NSEC_PER_SEC; + net_pkt_set_timestamp(pkt, ×tamp); + } + + /* See 4.7.2 Estimating the receive signal power */ + cir_pwr = sys_get_le16(&rx_inf_reg.rx_fqual[6]); + if (ctx->rf_cfg.prf == DWT_PRF_16M) { + a_const = DWT_RX_SIG_PWR_A_CONST_PRF16; + } else { + a_const = DWT_RX_SIG_PWR_A_CONST_PRF64; + } + + if (rx_pacc != 0) { + + //ctx->rx_cir_pwr = cir_pwr; + //ctx->rx_pacc = rx_pacc; + //ctx->rx_a_const = a_const + +#if defined(CONFIG_NEWLIB_LIBC) + /* From 4.7.2 Estimating the receive signal power */ + rx_level = 10.0 * log10f(cir_pwr * BIT(17) / + (rx_pacc * rx_pacc)) - a_const; +#endif + } + + net_pkt_set_ieee802154_rssi(pkt, rx_level); + + /* + * Workaround for AAT status bit issue, + * From 5.3.5 Host Notification in DW1000 User Manual: + * "Note: there is a situation that can result in the AAT bit being set + * for the current frame as a result of a previous frame that was + * received and rejected due to frame filtering." + */ + if ((sys_stat & DWT_SYS_STATUS_AAT) && ((fctrl[0] & 0x20) == 0)) { + flags_to_clear |= DWT_SYS_STATUS_AAT; + } + + if (ieee802154_radio_handle_ack(ctx->iface, pkt) == NET_OK) { + LOG_INF("ACK packet handled"); + goto rx_out_unref_pkt; + } + + /* LQI not implemented */ + LOG_DBG("Caught a packet (%u) (RSSI: %d)", + pkt_len, (int8_t)net_pkt_ieee802154_rssi(pkt)); + LOG_HEXDUMP_DBG(pkt->buffer->data, pkt_len, "RX buffer:"); + + if (net_recv_data(ctx->iface, pkt) == NET_OK) { + goto rx_out_enable_rx; + } else { + LOG_DBG("Packet dropped by NET stack"); + } + + rx_out_unref_pkt: + if (pkt) { + net_pkt_unref(pkt); + } + + rx_out_enable_rx: + dwt_reg_write_u32(dev, DWT_SYS_STATUS_ID, 0, flags_to_clear); + LOG_DBG("Cleared SYS_STATUS flags 0x%08x", flags_to_clear); + if (atomic_test_bit(&ctx->state, DWT_STATE_RX_DEF_ON)) { + /* + * Re-enable reception but in contrast to dwt_enable_rx() + * without to read SYS_STATUS and set delayed option. + */ + dwt_reg_write_u16(dev, DWT_SYS_CTRL_ID, DWT_SYS_CTRL_OFFSET, + DWT_SYS_CTRL_RXENAB); + } +} + +static void dwt_irq_handle_tx(const struct device *dev, uint32_t sys_stat) +{ + struct dwt_context *ctx = dev->data; + + /* Clear TX event bits */ + dwt_reg_write_u32(dev, DWT_SYS_STATUS_ID, 0, + DWT_SYS_STATUS_ALL_TX); + + LOG_DBG("TX confirmed event"); + k_sem_give(&ctx->phy_sem); +} + +static void dwt_irq_handle_rxto(const struct device *dev, uint32_t sys_stat) +{ + struct dwt_context *ctx = dev->data; + + /* Clear RX timeout event bits */ + dwt_reg_write_u32(dev, DWT_SYS_STATUS_ID, 0, + DWT_SYS_STATUS_RXRFTO); + + dwt_disable_txrx(dev); + /* Receiver reset necessary, see 4.1.6 RX Message timestamp */ + dwt_reset_rfrx(dev); + + LOG_DBG("RX timeout event"); + + if (atomic_test_bit(&ctx->state, DWT_STATE_CCA)) { + k_sem_give(&ctx->phy_sem); + ctx->cca_busy = false; + } +} + +static void dwt_irq_handle_error(const struct device *dev, uint32_t sys_stat) +{ + struct dwt_context *ctx = dev->data; + + /* Clear RX error event bits */ + dwt_reg_write_u32(dev, DWT_SYS_STATUS_ID, 0, DWT_SYS_STATUS_ALL_RX_ERR); + + dwt_disable_txrx(dev); + /* Receiver reset necessary, see 4.1.6 RX Message timestamp */ + dwt_reset_rfrx(dev); + + if(sys_stat & DWT_SYS_STATUS_RXPHE) { + LOG_INF("RX error DWT_SYS_STATUS_RXPHE"); + } + + if(sys_stat & DWT_SYS_STATUS_RXFCE) { + LOG_INF("RX error DWT_SYS_STATUS_RXFCE"); + } + + if(sys_stat & DWT_SYS_STATUS_RXRFSL) { + LOG_INF("RX error DWT_SYS_STATUS_RXRFSL"); + } + + if(sys_stat & DWT_SYS_STATUS_RXOVRR) { + LOG_INF("RX error DWT_SYS_STATUS_RXOVRR"); + } + + if(sys_stat & DWT_SYS_STATUS_RXSFDTO) { + LOG_INF("RX error DWT_SYS_STATUS_RXSFDTO"); + } + + if(sys_stat & DWT_SYS_STATUS_AFFREJ) { + LOG_INF("RX error DWT_SYS_STATUS_AFFREJ"); + } + + if (atomic_test_bit(&ctx->state, DWT_STATE_CCA)) { + k_sem_give(&ctx->phy_sem); + ctx->cca_busy = true; + return; + } + + if (atomic_test_bit(&ctx->state, DWT_STATE_RX_DEF_ON)) { + dwt_enable_rx(dev, 0); + } +} + +static void dwt_irq_work_handler(struct k_work *item) +{ + struct dwt_context *ctx = CONTAINER_OF(item, struct dwt_context, + irq_cb_work); + const struct device *dev = ctx->dev; + uint32_t sys_stat; + + k_sem_take(&ctx->dev_lock, K_FOREVER); + + sys_stat = dwt_reg_read_u32(dev, DWT_SYS_STATUS_ID, 0); + + if (sys_stat & DWT_SYS_STATUS_RXFCG) { + if (atomic_test_bit(&ctx->state, DWT_STATE_CCA)) { + dwt_irq_handle_rx_cca(dev); + } else { + dwt_irq_handle_rx(dev, sys_stat); + } + } + + if (sys_stat & DWT_SYS_STATUS_TXFRS) { + dwt_irq_handle_tx(dev, sys_stat); + } + + if (sys_stat & DWT_SYS_STATUS_ALL_RX_TO) { + dwt_irq_handle_rxto(dev, sys_stat); + } + + if (sys_stat & DWT_SYS_STATUS_ALL_RX_ERR) { + dwt_irq_handle_error(dev, sys_stat); + } + + k_sem_give(&ctx->dev_lock); +} + +static void dwt_gpio_callback(const struct device *dev, + struct gpio_callback *cb, uint32_t pins) +{ + struct dwt_context *ctx = CONTAINER_OF(cb, struct dwt_context, gpio_cb); + + LOG_DBG("IRQ callback triggered %p", ctx); + k_work_submit(&ctx->irq_cb_work); + //k_work_submit_to_queue(&dwt_work_queue, &ctx->irq_cb_work); +} + +static enum ieee802154_hw_caps dwt_get_capabilities(const struct device *dev) +{ + return IEEE802154_HW_FCS | + IEEE802154_HW_2_4_GHZ | /* FIXME: add IEEE802154_HW_UWB_PHY */ + IEEE802154_HW_FILTER; +} + +static uint32_t dwt_get_pkt_duration_ns(struct dwt_context *ctx, uint8_t psdu_len) +{ + struct dwt_phy_config *rf_cfg = &ctx->rf_cfg; + float t_psdu = rf_cfg->t_dsym * psdu_len * 8; + + return (rf_cfg->t_shr + rf_cfg->t_phr + t_psdu); +} + +static int dwt_cca(const struct device *dev) +{ + struct dwt_context *ctx = dev->data; + uint32_t cca_dur = (dwt_get_pkt_duration_ns(ctx, 127) + + dwt_get_pkt_duration_ns(ctx, 5)) / + UWB_PHY_TDSYM_PHR_6M8; + + if (atomic_test_and_set_bit(&ctx->state, DWT_STATE_CCA)) { + LOG_ERR("Transceiver busy"); + return -EBUSY; + } + + /* Perform CCA Mode 5 */ + k_sem_take(&ctx->dev_lock, K_FOREVER); + dwt_disable_txrx(dev); + LOG_DBG("CCA duration %u us", cca_dur); + + dwt_enable_rx(dev, cca_dur); + k_sem_give(&ctx->dev_lock); + + k_sem_take(&ctx->phy_sem, K_FOREVER); + LOG_DBG("CCA finished %p", ctx); + + atomic_clear_bit(&ctx->state, DWT_STATE_CCA); + if (atomic_test_bit(&ctx->state, DWT_STATE_RX_DEF_ON)) { + k_sem_take(&ctx->dev_lock, K_FOREVER); + dwt_enable_rx(dev, 0); + k_sem_give(&ctx->dev_lock); + } + + return ctx->cca_busy ? -EBUSY : 0; +} + +static int dwt_ed(const struct device *dev, uint16_t duration, + energy_scan_done_cb_t done_cb) +{ + /* TODO: see description Sub-Register 0x23:02 – AGC_CTRL1 */ + + return -ENOTSUP; +} + +static int dwt_set_channel(const struct device *dev, uint16_t channel) +{ + struct dwt_context *ctx = dev->data; + struct dwt_phy_config *rf_cfg = &ctx->rf_cfg; + + rf_cfg->channel = channel; + LOG_INF("Set channel %u", channel); + + k_sem_take(&ctx->dev_lock, K_FOREVER); + + dwt_disable_txrx(dev); + dwt_configure_rf_phy(dev); + + if (atomic_test_bit(&ctx->state, DWT_STATE_RX_DEF_ON)) { + dwt_enable_rx(dev, 0); + } + + k_sem_give(&ctx->dev_lock); + + return 0; +} + +static int dwt_set_pan_id(const struct device *dev, uint16_t pan_id) +{ + struct dwt_context *ctx = dev->data; + + k_sem_take(&ctx->dev_lock, K_FOREVER); + dwt_reg_write_u16(dev, DWT_PANADR_ID, DWT_PANADR_PAN_ID_OFFSET, pan_id); + k_sem_give(&ctx->dev_lock); + + LOG_INF("Set PAN ID 0x%04x %p", pan_id, ctx); + + return 0; +} + +static int dwt_set_short_addr(const struct device *dev, uint16_t short_addr) +{ + struct dwt_context *ctx = dev->data; + + k_sem_take(&ctx->dev_lock, K_FOREVER); + dwt_reg_write_u16(dev, DWT_PANADR_ID, DWT_PANADR_SHORT_ADDR_OFFSET, + short_addr); + k_sem_give(&ctx->dev_lock); + + LOG_INF("Set short 0x%x %p", short_addr, ctx); + + return 0; +} + +static int dwt_set_ieee_addr(const struct device *dev, + const uint8_t *ieee_addr) +{ + struct dwt_context *ctx = dev->data; + + LOG_INF("IEEE address %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x", + ieee_addr[7], ieee_addr[6], ieee_addr[5], ieee_addr[4], + ieee_addr[3], ieee_addr[2], ieee_addr[1], ieee_addr[0]); + + k_sem_take(&ctx->dev_lock, K_FOREVER); + dwt_register_write(dev, DWT_EUI_64_ID, DWT_EUI_64_OFFSET, + DWT_EUI_64_LEN, (uint8_t *)ieee_addr); + k_sem_give(&ctx->dev_lock); + + return 0; +} + +static int dwt_filter(const struct device *dev, + bool set, + enum ieee802154_filter_type type, + const struct ieee802154_filter *filter) +{ + if (!set) { + return -ENOTSUP; + } + + if (type == IEEE802154_FILTER_TYPE_IEEE_ADDR) { + return dwt_set_ieee_addr(dev, filter->ieee_addr); + } else if (type == IEEE802154_FILTER_TYPE_SHORT_ADDR) { + return dwt_set_short_addr(dev, filter->short_addr); + } else if (type == IEEE802154_FILTER_TYPE_PAN_ID) { + return dwt_set_pan_id(dev, filter->pan_id); + } + + return -ENOTSUP; +} + +static int dwt_set_power(const struct device *dev, int16_t dbm) +{ + struct dwt_context *ctx = dev->data; + + LOG_INF("set_txpower not supported %p", ctx); + + return 0; +} + + +static int dwt_tx(const struct device *dev, enum ieee802154_tx_mode tx_mode, + struct net_pkt *pkt, struct net_buf *frag) +{ + struct dwt_context *ctx = dev->data; + size_t len = frag->len; + uint32_t tx_time = 0; + struct net_ptp_time *txts; + uint64_t tmp_fs; + uint32_t tx_fctrl; + uint8_t sys_ctrl = DWT_SYS_CTRL_TXSTRT; + + if (atomic_test_and_set_bit(&ctx->state, DWT_STATE_TX)) { + LOG_ERR("Transceiver busy"); + return -EBUSY; + } + + k_sem_reset(&ctx->phy_sem); + k_sem_take(&ctx->dev_lock, K_FOREVER); + + switch (tx_mode) { + case IEEE802154_TX_MODE_DIRECT: + break; + case IEEE802154_TX_MODE_TXTIME: + /* + * tx_time is the high 32-bit of the 40-bit system + * time value at which to send the message. + */ + if (ctx->delayed_tx_short_ts_set) { + tx_time = ctx->delayed_tx_short_ts; + ctx->delayed_tx_short_ts_set = 0; // disable the flag again + } else { + txts = net_pkt_timestamp(pkt); + tmp_fs = txts->second * NSEC_PER_SEC + txts->nanosecond; + tmp_fs *= 1000U * 1000U; + tx_time = (tmp_fs / DWT_TS_TIME_UNITS_FS) >> 8; + } + + sys_ctrl |= DWT_SYS_CTRL_TXDLYS; + /* DX_TIME is 40-bit register */ + dwt_reg_write_u32(dev, DWT_DX_TIME_ID, 1, tx_time); + + LOG_DBG("ntx hi32 %x", tx_time); + LOG_DBG("sys hi32 %x", + dwt_reg_read_u32(dev, DWT_SYS_TIME_ID, 1)); + + break; + default: + LOG_ERR("TX mode %d not supported", tx_mode); + goto error; + } + + LOG_HEXDUMP_DBG(frag->data, len, "TX buffer:"); + + /* + * See "3 Message Transmission" in DW1000 User Manual for + * more details about transmission configuration. + */ + if (dwt_register_write(dev, DWT_TX_BUFFER_ID, 0, len, frag->data)) { + LOG_ERR("Failed to write TX data"); + goto error; + } + + tx_fctrl = dwt_reg_read_u32(dev, DWT_TX_FCTRL_ID, 0); + /* Clear TX buffer index offset, frame length, and length extension */ + tx_fctrl &= ~(DWT_TX_FCTRL_TFLEN_MASK | DWT_TX_FCTRL_TFLE_MASK | + DWT_TX_FCTRL_TXBOFFS_MASK); + /* Set frame length and ranging flag */ + tx_fctrl |= (len + DWT_FCS_LENGTH) & DWT_TX_FCTRL_TFLEN_MASK; + tx_fctrl |= DWT_TX_FCTRL_TR; + /* Update Transmit Frame Control register */ + dwt_reg_write_u32(dev, DWT_TX_FCTRL_ID, 0, tx_fctrl); + + dwt_disable_txrx(dev); + + /* Begin transmission */ + dwt_reg_write_u8(dev, DWT_SYS_CTRL_ID, DWT_SYS_CTRL_OFFSET, sys_ctrl); + + if (sys_ctrl & DWT_SYS_CTRL_TXDLYS) { + uint32_t sys_stat = dwt_reg_read_u32(dev, DWT_SYS_STATUS_ID, 0); + + if (sys_stat & DWT_SYS_STATUS_HPDWARN) { + //LOG_WRN("Half Period Delay Warning Current: %llu", dwt_system_ts(dev)); + uint64_t planned_us = dwt_short_ts_to_fs(tx_time) / 1000000000U; + uint64_t current_us = dwt_ts_to_fs(dwt_system_ts(dev)) / 1000000000U; + LOG_WRN("Half Period Delay Warning (planned: %llu, current: %llu)", planned_us, current_us); + } + } + + k_sem_give(&ctx->dev_lock); + /* Wait for the TX confirmed event */ + k_sem_take(&ctx->phy_sem, K_FOREVER); + + if (IS_ENABLED(CONFIG_NET_PKT_TIMESTAMP)) { + uint8_t ts_buf[sizeof(uint64_t)] = {0}; + struct net_ptp_time timestamp; + + k_sem_take(&ctx->dev_lock, K_FOREVER); + dwt_register_read(dev, DWT_TX_TIME_ID, + DWT_TX_TIME_TX_STAMP_OFFSET, + DWT_TX_TIME_TX_STAMP_LEN, + ts_buf); + LOG_DBG("ts hi32 %x", (uint32_t)(sys_get_le64(ts_buf) >> 8)); + LOG_DBG("sys hi32 %x", dwt_reg_read_u32(dev, DWT_SYS_TIME_ID, 1)); + k_sem_give(&ctx->dev_lock); + + tmp_fs = sys_get_le64(ts_buf) * DWT_TS_TIME_UNITS_FS; + timestamp.second = (tmp_fs / 1000000) / NSEC_PER_SEC; + timestamp.nanosecond = (tmp_fs / 1000000) % NSEC_PER_SEC; + net_pkt_set_timestamp(pkt, ×tamp); + } + + atomic_clear_bit(&ctx->state, DWT_STATE_TX); + + if (atomic_test_bit(&ctx->state, DWT_STATE_RX_DEF_ON)) { + k_sem_take(&ctx->dev_lock, K_FOREVER); + dwt_enable_rx(dev, 0); + k_sem_give(&ctx->dev_lock); + } + + return 0; + + error: + atomic_clear_bit(&ctx->state, DWT_STATE_TX); + k_sem_give(&ctx->dev_lock); + + return -EIO; +} + +void dwt_set_frame_filter(const struct device *dev, + bool ff_enable, uint8_t ff_type) +{ + uint32_t sys_cfg_ff = ff_enable ? DWT_SYS_CFG_FFE : 0; + + sys_cfg_ff |= ff_type & DWT_SYS_CFG_FF_ALL_EN; + + dwt_reg_write_u8(dev, DWT_SYS_CFG_ID, 0, (uint8_t)sys_cfg_ff); +} + +static int dwt_configure(const struct device *dev, + enum ieee802154_config_type type, + const struct ieee802154_config *config) +{ + struct dwt_context *ctx = dev->data; + + LOG_DBG("API configure %p", ctx); + + switch (type) { + case IEEE802154_CONFIG_AUTO_ACK_FPB: + LOG_DBG("IEEE802154_CONFIG_AUTO_ACK_FPB"); + break; + + case IEEE802154_CONFIG_ACK_FPB: + LOG_DBG("IEEE802154_CONFIG_ACK_FPB"); + break; + + case IEEE802154_CONFIG_PAN_COORDINATOR: + LOG_DBG("IEEE802154_CONFIG_PAN_COORDINATOR"); + break; + + case IEEE802154_CONFIG_PROMISCUOUS: + LOG_DBG("IEEE802154_CONFIG_PROMISCUOUS"); + break; + + case IEEE802154_CONFIG_EVENT_HANDLER: + LOG_DBG("IEEE802154_CONFIG_EVENT_HANDLER"); + break; + + default: + return -EINVAL; + } + + return -ENOTSUP; +} + +/* + * Note, the DW_RESET pin should not be driven high externally. + */ +static int dwt_hw_reset(const struct device *dev) +{ + const struct dwt_hi_cfg *hi_cfg = dev->config; + + if (gpio_pin_configure_dt(&hi_cfg->rst_gpio, GPIO_OUTPUT_ACTIVE)) { + LOG_ERR("Failed to configure GPIO pin %u", hi_cfg->rst_gpio.pin); + return -EINVAL; + } + + k_sleep(K_MSEC(1)); + gpio_pin_set_dt(&hi_cfg->rst_gpio, 0); + k_sleep(K_MSEC(5)); + + if (gpio_pin_configure_dt(&hi_cfg->rst_gpio, GPIO_INPUT)) { + LOG_ERR("Failed to configure GPIO pin %u", hi_cfg->rst_gpio.pin); + return -EINVAL; + } + + return 0; +} + +/* + * SPI speed in INIT state or for wake-up sequence, + * see 2.3.2 Overview of main operational states + */ +static void dwt_set_spi_slow(const struct device *dev, const uint32_t freq) +{ + struct dwt_context *ctx = dev->data; + + ctx->spi_cfg_slow.frequency = freq; + ctx->spi_cfg = &ctx->spi_cfg_slow; +} + +/* SPI speed in IDLE, RX, and TX state */ +static void dwt_set_spi_fast(const struct device *dev) +{ + const struct dwt_hi_cfg *hi_cfg = dev->config; + struct dwt_context *ctx = dev->data; + + ctx->spi_cfg = &hi_cfg->bus.config; +} + +static void dwt_set_rx_mode(const struct device *dev) +{ + struct dwt_context *ctx = dev->data; + struct dwt_phy_config *rf_cfg = &ctx->rf_cfg; + uint32_t pmsc_ctrl0; + uint32_t t_on_us; + uint8_t rx_sniff[2]; + + /* SNIFF Mode ON time in units of PAC */ + rx_sniff[0] = CONFIG_IEEE802154_DW1000_SNIFF_ONT & + DWT_RX_SNIFF_SNIFF_ONT_MASK; + /* SNIFF Mode OFF time in microseconds */ + rx_sniff[1] = CONFIG_IEEE802154_DW1000_SNIFF_OFFT; + + t_on_us = (rx_sniff[0] + 1) * (BIT(3) << rf_cfg->rx_pac_l); + LOG_INF("RX duty cycle %u%%", t_on_us * 100 / (t_on_us + rx_sniff[1])); + + dwt_register_write(dev, DWT_RX_SNIFF_ID, DWT_RX_SNIFF_OFFSET, + sizeof(rx_sniff), rx_sniff); + + pmsc_ctrl0 = dwt_reg_read_u32(dev, DWT_PMSC_ID, DWT_PMSC_CTRL0_OFFSET); + /* Enable PLL2 on/off sequencing for SNIFF mode */ + pmsc_ctrl0 |= DWT_PMSC_CTRL0_PLL2_SEQ_EN; + dwt_reg_write_u32(dev, DWT_PMSC_ID, DWT_PMSC_CTRL0_OFFSET, pmsc_ctrl0); +} + +static int dwt_start(const struct device *dev) +{ + struct dwt_context *ctx = dev->data; + uint8_t cswakeup_buf[32] = {0}; + + k_sem_take(&ctx->dev_lock, K_FOREVER); + + /* Set SPI clock to lowest frequency */ + dwt_set_spi_slow(dev, DWT_SPI_CSWAKEUP_FREQ); + + if (dwt_reg_read_u32(dev, DWT_DEV_ID_ID, 0) != DWT_DEVICE_ID) { + /* Keep SPI CS line low for 500 microseconds */ + dwt_register_read(dev, 0, 0, sizeof(cswakeup_buf), + cswakeup_buf); + /* Give device time to initialize */ + k_sleep(K_MSEC(5)); + + if (dwt_reg_read_u32(dev, DWT_DEV_ID_ID, 0) != DWT_DEVICE_ID) { + LOG_ERR("Failed to wake-up %p", dev); + k_sem_give(&ctx->dev_lock); + return -1; + } + } else { + LOG_WRN("Device not in a sleep mode"); + } + + /* Restore SPI clock settings */ + dwt_set_spi_slow(dev, DWT_SPI_SLOW_FREQ); + dwt_set_spi_fast(dev); + + dwt_setup_int(dev, true); + dwt_disable_txrx(dev); + dwt_reset_rfrx(dev); + + if (CONFIG_IEEE802154_DW1000_SNIFF_ONT != 0) { + dwt_set_rx_mode(dev); + } + + /* Re-enable RX after packet reception */ + atomic_set_bit(&ctx->state, DWT_STATE_RX_DEF_ON); + dwt_enable_rx(dev, 0); + k_sem_give(&ctx->dev_lock); + + LOG_INF("Started %p", dev); + + return 0; +} + +static int dwt_stop(const struct device *dev) +{ + struct dwt_context *ctx = dev->data; + + k_sem_take(&ctx->dev_lock, K_FOREVER); + dwt_disable_txrx(dev); + dwt_reset_rfrx(dev); + dwt_setup_int(dev, false); + + /* Copy the user configuration and enter sleep mode */ + dwt_reg_write_u8(dev, DWT_AON_ID, DWT_AON_CTRL_OFFSET, + DWT_AON_CTRL_SAVE); + k_sem_give(&ctx->dev_lock); + + LOG_INF("Stopped %p", dev); + + return 0; +} + +static inline void dwt_set_sysclks_xti(const struct device *dev, bool ldeload) +{ + uint16_t clks = BIT(9) | DWT_PMSC_CTRL0_SYSCLKS_19M; + + /* + * See Table 4: Register accesses required to load LDE microcode, + * set PMSC_CTRL0 0x0301, load LDE, set PMSC_CTRL0 0x0200. + */ + if (ldeload) { + clks |= BIT(8); + } + + /* Force system clock to be the 19.2 MHz XTI clock */ + dwt_reg_write_u16(dev, DWT_PMSC_ID, DWT_PMSC_CTRL0_OFFSET, clks); +} + +static inline void dwt_set_sysclks_auto(const struct device *dev) +{ + uint8_t sclks = DWT_PMSC_CTRL0_SYSCLKS_AUTO | + DWT_PMSC_CTRL0_RXCLKS_AUTO | + DWT_PMSC_CTRL0_TXCLKS_AUTO; + + dwt_reg_write_u8(dev, DWT_PMSC_ID, DWT_PMSC_CTRL0_OFFSET, sclks); +} + +static uint32_t dwt_otpmem_read(const struct device *dev, uint16_t otp_addr) +{ + dwt_reg_write_u16(dev, DWT_OTP_IF_ID, DWT_OTP_ADDR, otp_addr); + + dwt_reg_write_u8(dev, DWT_OTP_IF_ID, DWT_OTP_CTRL, + DWT_OTP_CTRL_OTPREAD | DWT_OTP_CTRL_OTPRDEN); + /* OTPREAD is self clearing but OTPRDEN is not */ + dwt_reg_write_u8(dev, DWT_OTP_IF_ID, DWT_OTP_CTRL, 0x00); + + /* Read read data, available 40ns after rising edge of OTP_READ */ + return dwt_reg_read_u32(dev, DWT_OTP_IF_ID, DWT_OTP_RDAT); +} + +void dwt_set_antenna_delay_rx(const struct device *dev, uint16_t rx_delay_ts) { + dwt_reg_write_u16(dev, DWT_LDE_IF_ID, DWT_LDE_RXANTD_OFFSET, rx_delay_ts); +} +void dwt_set_antenna_delay_tx(const struct device *dev, uint16_t tx_delay_ts) { + dwt_reg_write_u16(dev, DWT_TX_ANTD_ID, DWT_TX_ANTD_OFFSET, tx_delay_ts); +} +uint16_t dwt_antenna_delay_rx(const struct device *dev) { + return dwt_reg_read_u16(dev, DWT_LDE_IF_ID, DWT_LDE_RXANTD_OFFSET); +} + +inline uint16_t dwt_antenna_delay_tx(const struct device *dev) { + return dwt_reg_read_u16(dev, DWT_TX_ANTD_ID, DWT_TX_ANTD_OFFSET); +} + +uint32_t dwt_otp_antenna_delay(const struct device *dev) { + return dwt_otpmem_read(dev, DWT_OTP_DELAY_ADDR); +} + +inline void dwt_set_delayed_tx_short_ts(const struct device *dev, uint32_t short_ts) { + struct dwt_context *ctx = dev->data; + ctx->delayed_tx_short_ts = short_ts; + ctx->delayed_tx_short_ts_set = 1; +} + +inline uint64_t dwt_system_ts(const struct device *dev) { + uint8_t ts_buf[sizeof(uint64_t)] = {0}; + + // TODO: Should we lock?! + //k_sem_take(&ctx->dev_lock, K_FOREVER); + dwt_register_read(dev, DWT_SYS_TIME_ID, DWT_SYS_TIME_OFFSET, DWT_SYS_TIME_LEN, ts_buf); + //k_sem_give(&ctx->dev_lock); + return sys_get_le64(ts_buf); +} + +inline uint32_t dwt_system_short_ts(const struct device *dev) { + uint64_t ts = dwt_system_ts(dev); + return (uint32_t)(ts >> 8); +} + +uint64_t dwt_ts_to_fs(uint64_t ts) { + return ts * DWT_TS_TIME_UNITS_FS; +} + +uint64_t dwt_fs_to_ts(uint64_t fs) { + return fs / DWT_TS_TIME_UNITS_FS; +} + +uint64_t dwt_short_ts_to_fs(uint32_t ts) { + uint64_t tmp_ts = (uint64_t)ts; + return (tmp_ts << 8) * DWT_TS_TIME_UNITS_FS; +} + +uint32_t dwt_fs_to_short_ts(uint64_t fs) { + return (fs / DWT_TS_TIME_UNITS_FS) >> 8; +} + +inline uint64_t dwt_estimate_actual_tx_ts(uint32_t planned_short_ts, uint16_t tx_antenna_delay) { +return (((uint64_t)(planned_short_ts & 0xFFFFFFFEUL)) << 8) + tx_antenna_delay; +} + +uint64_t dwt_plan_delayed_tx(const struct device *dev, uint64_t uus_delay) { + // query the antenna delay before + uint16_t antenna_delay = dwt_antenna_delay_tx(dev); + uint64_t cur_ts = dwt_system_ts(dev); + /* UWB microsecond (uus) to device time unit (dtu, around 15.65 ps) conversion factor. + * 1 uus = 512 / 499.2 µs and 1 µs = 499.2 * 128 dtu. */ + uint32_t planned_tx_short_ts = (cur_ts + (uus_delay * 65536)) >> 8; + //LOG_WRN("current %u planned %u", (uint32_t)(cur_ts >> 8), planned_tx_short_ts); + dwt_set_delayed_tx_short_ts(dev, planned_tx_short_ts); + + uint64_t estimated_ts = dwt_estimate_actual_tx_ts(planned_tx_short_ts, antenna_delay); + //LOG_WRN("Current: %llu, Plan: %llu, Est: %llu", cur_ts, (uint64_t)(planned_tx_short_ts) << 8, estimated_ts); + //LOG_WRN("Current: %llu, Plan: %llu, Est: %llu", dwt_ts_to_fs(cur_ts) / 1000000000U, dwt_ts_to_fs((uint64_t)(planned_tx_short_ts) << 8) / 1000000000U, dwt_ts_to_fs(estimated_ts) / 1000000000U); + + return estimated_ts; +} + +uint64_t dwt_rx_ts(const struct device *dev) { + struct dwt_context *ctx = dev->data; + return ctx->rx_ts; +} + +#define B20_SIGN_EXTEND_TEST (0x00100000UL) +#define B20_SIGN_EXTEND_MASK (0xFFF00000UL) + +int dwt_readcarrierintegrator(const struct device *dev) +{ + uint32_t regval = 0 ; + uint8_t buf[DWT_DRX_CARRIER_INT_LEN]; + /* Read 3 bytes into buffer (21-bit quantity) */ + dwt_spi_transfer(dev, DWT_DRX_CONF_ID, DWT_DRX_CARRIER_INT_OFFSET, sizeof(buf), buf, false); + + for (int j = 2 ; j >= 0 ; j --) // arrange the three bytes into an unsigned integer value + { + regval = (regval << 8) + buf[j] ; + } + + if (regval & B20_SIGN_EXTEND_TEST) { + regval |= B20_SIGN_EXTEND_MASK ; // sign extend bit #20 to whole word + } + else { + regval &= DWT_DRX_CARRIER_INT_MASK ; // make sure upper bits are clear if not sign extending + } + + return (int) regval ; // cast unsigned value to signed quantity. +} + +// Multiplication factors to convert carrier integrator value to a frequency offset in Hertz + +#define DWT_FREQ_OFFSET_MULTIPLIER (998.4e6/2.0/1024.0/131072.0) +#define DWT_FREQ_OFFSET_MULTIPLIER_110KB (998.4e6/2.0/8192.0/131072.0) + +// Multiplication factors to convert frequency offset in Hertz to PPM crystal offset +// NB: also changes sign so a positive value means the local RX clock is running slower than the remote TX device. + +#define DWT_HERTZ_TO_PPM_MULTIPLIER_CHAN_1 (-1.0e6/3494.4e6) +#define DWT_HERTZ_TO_PPM_MULTIPLIER_CHAN_2 (-1.0e6/3993.6e6) +#define DWT_HERTZ_TO_PPM_MULTIPLIER_CHAN_3 (-1.0e6/4492.8e6) +#define DWT_HERTZ_TO_PPM_MULTIPLIER_CHAN_5 (-1.0e6/6489.6e6) + +float dwt_rx_clock_ratio_offset(const struct device *dev) { + //TODO: Use correct channel! + return dwt_readcarrierintegrator(dev) * (DWT_FREQ_OFFSET_MULTIPLIER * DWT_HERTZ_TO_PPM_MULTIPLIER_CHAN_5 / 1.0e6); +} + + +static int dwt_initialise_dev(const struct device *dev) +{ + struct dwt_context *ctx = dev->data; + uint32_t otp_val = 0; + uint32_t otp_antenna_delay = 0; + uint8_t xtal_trim; + + ctx->delayed_tx_short_ts_set = 0; + ctx->delayed_tx_short_ts = 0; + + dwt_set_sysclks_xti(dev, false); + ctx->sleep_mode = 0; + + /* Disable PMSC control of analog RF subsystem */ + dwt_reg_write_u16(dev, DWT_PMSC_ID, DWT_PMSC_CTRL1_OFFSET, + DWT_PMSC_CTRL1_PKTSEQ_DISABLE); + + /* Clear all status flags */ + dwt_reg_write_u32(dev, DWT_SYS_STATUS_ID, 0, DWT_SYS_STATUS_MASK_32); + + /* + * Apply soft reset, + * see SOFTRESET field description in DW1000 User Manual. + */ + dwt_reg_write_u8(dev, DWT_PMSC_ID, DWT_PMSC_CTRL0_SOFTRESET_OFFSET, + DWT_PMSC_CTRL0_RESET_ALL); + k_sleep(K_MSEC(1)); + dwt_reg_write_u8(dev, DWT_PMSC_ID, DWT_PMSC_CTRL0_SOFTRESET_OFFSET, + DWT_PMSC_CTRL0_RESET_CLEAR); + + dwt_set_sysclks_xti(dev, false); + + /* + * This bit (a.k.a PLLLDT) should be set to ensure reliable + * operation of the CPLOCK bit. + */ + dwt_reg_write_u8(dev, DWT_EXT_SYNC_ID, DWT_EC_CTRL_OFFSET, + DWT_EC_CTRL_PLLLCK); + + /* Kick LDO if there is a value programmed. */ + otp_val = dwt_otpmem_read(dev, DWT_OTP_LDOTUNE_ADDR); + if ((otp_val & 0xFF) != 0) { + dwt_reg_write_u8(dev, DWT_OTP_IF_ID, DWT_OTP_SF, + DWT_OTP_SF_LDO_KICK); + ctx->sleep_mode |= DWT_AON_WCFG_ONW_LLDO; + LOG_INF("Load LDOTUNE_CAL parameter"); + } + + otp_val = dwt_otpmem_read(dev, DWT_OTP_XTRIM_ADDR); + xtal_trim = otp_val & DWT_FS_XTALT_MASK; + LOG_INF("OTP Revision 0x%02x, XTAL Trim 0x%02x", + (uint8_t)(otp_val >> 8), xtal_trim); + + LOG_DBG("CHIP ID 0x%08x", dwt_otpmem_read(dev, DWT_OTP_PARTID_ADDR)); + LOG_DBG("LOT ID 0x%08x", dwt_otpmem_read(dev, DWT_OTP_LOTID_ADDR)); + LOG_DBG("Vbat 0x%02x", dwt_otpmem_read(dev, DWT_OTP_VBAT_ADDR)); + LOG_DBG("Vtemp 0x%02x", dwt_otpmem_read(dev, DWT_OTP_VTEMP_ADDR)); + + otp_antenna_delay = dwt_otpmem_read(dev, DWT_OTP_DELAY_ADDR); + LOG_INF("OTP AntDelay 0x%02x", otp_antenna_delay); //TODO: change this to a debug log + + if (xtal_trim == 0) { + /* Set to default */ + xtal_trim = DWT_FS_XTALT_MIDRANGE; + } + + /* For FS_XTALT bits 7:5 must always be set to binary “011” */ + xtal_trim |= BIT(6) | BIT(5); + dwt_reg_write_u8(dev, DWT_FS_CTRL_ID, DWT_FS_XTALT_OFFSET, xtal_trim); + + /* Load LDE microcode into RAM, see 2.5.5.10 LDELOAD */ + dwt_set_sysclks_xti(dev, true); + dwt_reg_write_u16(dev, DWT_OTP_IF_ID, DWT_OTP_CTRL, + DWT_OTP_CTRL_LDELOAD); + k_sleep(K_MSEC(1)); + dwt_set_sysclks_xti(dev, false); + ctx->sleep_mode |= DWT_AON_WCFG_ONW_LLDE; + + dwt_set_sysclks_auto(dev); + + if (!(dwt_reg_read_u8(dev, DWT_SYS_STATUS_ID, 0) & + DWT_SYS_STATUS_CPLOCK)) { + LOG_WRN("PLL has not locked"); + return -EIO; + } + + dwt_set_spi_fast(dev); + + /* Setup default antenna delay values */ + dwt_set_antenna_delay_rx(dev, DW1000_RX_ANT_DLY); + dwt_set_antenna_delay_tx(dev, DW1000_TX_ANT_DLY); + + /* Clear AON_CFG1 register */ + dwt_reg_write_u8(dev, DWT_AON_ID, DWT_AON_CFG1_OFFSET, 0); + /* + * Configure sleep mode: + * - On wake-up load configurations from the AON memory + * - preserve sleep mode configuration + * - On Wake-up load the LDE microcode + * - When available, on wake-up load the LDO tune value + */ + ctx->sleep_mode |= DWT_AON_WCFG_ONW_LDC | + DWT_AON_WCFG_PRES_SLEEP; + dwt_reg_write_u16(dev, DWT_AON_ID, DWT_AON_WCFG_OFFSET, + ctx->sleep_mode); + LOG_DBG("sleep mode 0x%04x", ctx->sleep_mode); + /* Enable sleep and wake using SPI CSn */ + dwt_reg_write_u8(dev, DWT_AON_ID, DWT_AON_CFG0_OFFSET, + DWT_AON_CFG0_WAKE_SPI | DWT_AON_CFG0_SLEEP_EN); + + return 0; +} + +/* + * RF PHY configuration. Must be carried out as part of initialization and + * for every channel change. See also 2.5 Default Configuration on Power Up. + */ +static int dwt_configure_rf_phy(const struct device *dev) +{ + struct dwt_context *ctx = dev->data; + struct dwt_phy_config *rf_cfg = &ctx->rf_cfg; + uint8_t chan = rf_cfg->channel; + uint8_t prf_idx = rf_cfg->prf; + uint32_t chan_ctrl = 0; + uint8_t rxctrlh; + uint8_t pll_tune; + uint8_t tune4h; + uint8_t pgdelay; + uint16_t lde_repc; + uint16_t agc_tune1; + uint16_t sfdto; + uint16_t tune1a; + uint16_t tune0b; + uint16_t tune1b; + uint32_t txctrl; + uint32_t pll_cfg; + uint32_t tune2; + uint32_t sys_cfg; + uint32_t tx_fctrl; + uint32_t power; + + if ((chan < 1) || (chan > 7) || (chan == 6)) { + LOG_ERR("Channel not supported %u", chan); + return -ENOTSUP; + } + + if (rf_cfg->rx_shr_code >= ARRAY_SIZE(dwt_lde_repc_defs)) { + LOG_ERR("Preamble code not supported %u", + rf_cfg->rx_shr_code); + return -ENOTSUP; + } + + if (prf_idx >= DWT_NUMOF_PRFS) { + LOG_ERR("PRF not supported %u", prf_idx); + return -ENOTSUP; + } + + if (rf_cfg->rx_pac_l >= DWT_NUMOF_PACS) { + LOG_ERR("RX PAC not supported %u", rf_cfg->rx_pac_l); + return -ENOTSUP; + } + + if (rf_cfg->rx_ns_sfd > 1) { + LOG_ERR("Wrong NS SFD configuration"); + return -ENOTSUP; + } + + if (rf_cfg->tx_shr_nsync >= DWT_NUM_OF_PLEN) { + LOG_ERR("Wrong SHR configuration"); + return -ENOTSUP; + } + + lde_repc = dwt_lde_repc_defs[rf_cfg->rx_shr_code]; + agc_tune1 = dwt_agc_tune1_defs[prf_idx]; + sfdto = rf_cfg->rx_sfd_to; + rxctrlh = dwt_rxctrlh_defs[dwt_ch_to_cfg[chan]]; + txctrl = dwt_txctrl_defs[dwt_ch_to_cfg[chan]]; + pll_tune = dwt_plltune_defs[dwt_ch_to_cfg[chan]]; + pll_cfg = dwt_pllcfg_defs[dwt_ch_to_cfg[chan]]; + tune2 = dwt_tune2_defs[prf_idx][rf_cfg->rx_pac_l]; + tune1a = dwt_tune1a_defs[prf_idx]; + tune0b = dwt_tune0b_defs[rf_cfg->dr][rf_cfg->rx_ns_sfd]; + pgdelay = dwt_pgdelay_defs[dwt_ch_to_cfg[chan]]; + + sys_cfg = dwt_reg_read_u32(dev, DWT_SYS_CFG_ID, 0); + tx_fctrl = dwt_reg_read_u32(dev, DWT_TX_FCTRL_ID, 0); + + /* Don't allow 0 - SFD timeout will always be enabled */ + if (sfdto == 0) { + sfdto = DWT_SFDTOC_DEF; + } + + /* Set IEEE 802.15.4 compliant mode */ + sys_cfg &= ~DWT_SYS_CFG_PHR_MODE_11; + + if (rf_cfg->dr == DWT_BR_110K) { + /* Set Receiver Mode 110 kbps data rate */ + sys_cfg |= DWT_SYS_CFG_RXM110K; + lde_repc = lde_repc >> 3; + tune1b = DWT_DRX_TUNE1b_110K; + tune4h = DWT_DRX_TUNE4H_PRE64; + } else { + sys_cfg &= ~DWT_SYS_CFG_RXM110K; + if (rf_cfg->tx_shr_nsync == DWT_PLEN_64) { + tune1b = DWT_DRX_TUNE1b_6M8_PRE64; + tune4h = DWT_DRX_TUNE4H_PRE64; + } else { + tune1b = DWT_DRX_TUNE1b_850K_6M8; + tune4h = DWT_DRX_TUNE4H_PRE128PLUS; + } + } + + if (sys_cfg & DWT_SYS_CFG_DIS_STXP) { + if (rf_cfg->prf == DWT_PRF_64M) { + power = dwt_txpwr_stxp1_64[dwt_ch_to_cfg[chan]]; + } else { + power = dwt_txpwr_stxp1_16[dwt_ch_to_cfg[chan]]; + } + } else { + if (rf_cfg->prf == DWT_PRF_64M) { + power = dwt_txpwr_stxp0_64[dwt_ch_to_cfg[chan]]; + } else { + power = dwt_txpwr_stxp0_16[dwt_ch_to_cfg[chan]]; + } + } + + dwt_reg_write_u32(dev, DWT_SYS_CFG_ID, 0, sys_cfg); + LOG_DBG("SYS_CFG: 0x%08x", sys_cfg); + + dwt_reg_write_u16(dev, DWT_LDE_IF_ID, DWT_LDE_REPC_OFFSET, lde_repc); + LOG_DBG("LDE_REPC: 0x%04x", lde_repc); + + dwt_reg_write_u8(dev, DWT_LDE_IF_ID, DWT_LDE_CFG1_OFFSET, + DWT_DEFAULT_LDE_CFG1); + + if (rf_cfg->prf == DWT_PRF_64M) { + dwt_reg_write_u16(dev, DWT_LDE_IF_ID, DWT_LDE_CFG2_OFFSET, + DWT_DEFAULT_LDE_CFG2_PRF64); + LOG_DBG("LDE_CFG2: 0x%04x", DWT_DEFAULT_LDE_CFG2_PRF64); + } else { + dwt_reg_write_u16(dev, DWT_LDE_IF_ID, DWT_LDE_CFG2_OFFSET, + DWT_DEFAULT_LDE_CFG2_PRF16); + LOG_DBG("LDE_CFG2: 0x%04x", DWT_DEFAULT_LDE_CFG2_PRF16); + } + + /* Configure PLL2/RF PLL block CFG/TUNE (for a given channel) */ + dwt_reg_write_u32(dev, DWT_FS_CTRL_ID, DWT_FS_PLLCFG_OFFSET, pll_cfg); + LOG_DBG("PLLCFG: 0x%08x", pll_cfg); + dwt_reg_write_u8(dev, DWT_FS_CTRL_ID, DWT_FS_PLLTUNE_OFFSET, pll_tune); + LOG_DBG("PLLTUNE: 0x%02x", pll_tune); + /* Configure RF RX blocks (for specified channel/bandwidth) */ + dwt_reg_write_u8(dev, DWT_RF_CONF_ID, DWT_RF_RXCTRLH_OFFSET, rxctrlh); + LOG_DBG("RXCTRLH: 0x%02x", rxctrlh); + /* Configure RF/TX blocks for specified channel and PRF */ + dwt_reg_write_u32(dev, DWT_RF_CONF_ID, DWT_RF_TXCTRL_OFFSET, txctrl); + LOG_DBG("TXCTRL: 0x%08x", txctrl); + + /* Digital receiver configuration, DRX_CONF */ + dwt_reg_write_u16(dev, DWT_DRX_CONF_ID, DWT_DRX_TUNE0b_OFFSET, tune0b); + LOG_DBG("DRX_TUNE0b: 0x%04x", tune0b); + dwt_reg_write_u16(dev, DWT_DRX_CONF_ID, DWT_DRX_TUNE1a_OFFSET, tune1a); + LOG_DBG("DRX_TUNE1a: 0x%04x", tune1a); + dwt_reg_write_u16(dev, DWT_DRX_CONF_ID, DWT_DRX_TUNE1b_OFFSET, tune1b); + LOG_DBG("DRX_TUNE1b: 0x%04x", tune1b); + dwt_reg_write_u32(dev, DWT_DRX_CONF_ID, DWT_DRX_TUNE2_OFFSET, tune2); + LOG_DBG("DRX_TUNE2: 0x%08x", tune2); + dwt_reg_write_u8(dev, DWT_DRX_CONF_ID, DWT_DRX_TUNE4H_OFFSET, tune4h); + LOG_DBG("DRX_TUNE4H: 0x%02x", tune4h); + dwt_reg_write_u16(dev, DWT_DRX_CONF_ID, DWT_DRX_SFDTOC_OFFSET, sfdto); + LOG_DBG("DRX_SFDTOC: 0x%04x", sfdto); + + /* Automatic Gain Control configuration and control, AGC_CTRL */ + dwt_reg_write_u16(dev, DWT_AGC_CTRL_ID, DWT_AGC_TUNE1_OFFSET, + agc_tune1); + LOG_DBG("AGC_TUNE1: 0x%04x", agc_tune1); + dwt_reg_write_u32(dev, DWT_AGC_CTRL_ID, DWT_AGC_TUNE2_OFFSET, + DWT_AGC_TUNE2_VAL); + + if (rf_cfg->rx_ns_sfd) { + /* + * SFD_LENGTH, length of the SFD sequence used when + * the data rate is 850 kbps or 6.8 Mbps, + * must be set to either 8 or 16. + */ + dwt_reg_write_u8(dev, DWT_USR_SFD_ID, 0x00, + dwt_ns_sfdlen[rf_cfg->dr]); + LOG_DBG("USR_SFDLEN: 0x%02x", dwt_ns_sfdlen[rf_cfg->dr]); + chan_ctrl |= DWT_CHAN_CTRL_DWSFD; + } + + /* Set RX_CHAN and TX CHAN */ + chan_ctrl |= (chan & DWT_CHAN_CTRL_TX_CHAN_MASK) | + ((chan << DWT_CHAN_CTRL_RX_CHAN_SHIFT) & + DWT_CHAN_CTRL_RX_CHAN_MASK); + + /* Set RXPRF */ + chan_ctrl |= (BIT(rf_cfg->prf) << DWT_CHAN_CTRL_RXFPRF_SHIFT) & + DWT_CHAN_CTRL_RXFPRF_MASK; + + /* Set TX_PCOD */ + chan_ctrl |= (rf_cfg->tx_shr_code << DWT_CHAN_CTRL_TX_PCOD_SHIFT) & + DWT_CHAN_CTRL_TX_PCOD_MASK; + + /* Set RX_PCOD */ + chan_ctrl |= (rf_cfg->rx_shr_code << DWT_CHAN_CTRL_RX_PCOD_SHIFT) & + DWT_CHAN_CTRL_RX_PCOD_MASK; + + /* Set Channel Control */ + dwt_reg_write_u32(dev, DWT_CHAN_CTRL_ID, 0, chan_ctrl); + LOG_DBG("CHAN_CTRL 0x%08x", chan_ctrl); + + /* Set up TX Preamble Size, PRF and Data Rate */ + tx_fctrl = dwt_plen_cfg[rf_cfg->tx_shr_nsync] | + (BIT(rf_cfg->prf) << DWT_TX_FCTRL_TXPRF_SHFT) | + (rf_cfg->dr << DWT_TX_FCTRL_TXBR_SHFT); + + dwt_reg_write_u32(dev, DWT_TX_FCTRL_ID, 0, tx_fctrl); + LOG_DBG("TX_FCTRL 0x%08x", tx_fctrl); + + /* Set the Pulse Generator Delay */ + dwt_reg_write_u8(dev, DWT_TX_CAL_ID, DWT_TC_PGDELAY_OFFSET, pgdelay); + LOG_DBG("PGDELAY 0x%02x", pgdelay); + /* Set Transmit Power Control */ + dwt_reg_write_u32(dev, DWT_TX_POWER_ID, 0, power); + LOG_DBG("TX_POWER 0x%08x", power); + + /* + * From 5.3.1.2 SFD Initialisation, + * SFD sequence initialisation for Auto ACK frame. + */ + dwt_reg_write_u8(dev, DWT_SYS_CTRL_ID, DWT_SYS_CTRL_OFFSET, + DWT_SYS_CTRL_TXSTRT | DWT_SYS_CTRL_TRXOFF); + + /* + * Calculate PHY timing parameters + * + * From (9.4) Std 802.15.4-2011 + * Tshr = Tpsym * (NSYNC + NSFD ) + * Tphr = NPHR * Tdsym1m + * Tpsdu = Tdsym * NPSDU * NSYMPEROCTET / Rfec + * + * PRF: pulse repetition frequency + * PSR: preamble symbol repetitions + * SFD: start of frame delimiter + * SHR: synchronisation header (SYNC + SFD) + * PHR: PHY header + */ + uint16_t nsync = BIT(rf_cfg->tx_shr_nsync + 6); + + if (rf_cfg->prf == DWT_PRF_64M) { + rf_cfg->t_shr = UWB_PHY_TPSYM_PRF64 * + (nsync + UWB_PHY_NUMOF_SYM_SHR_SFD); + } else { + rf_cfg->t_shr = UWB_PHY_TPSYM_PRF16 * + (nsync + UWB_PHY_NUMOF_SYM_SHR_SFD); + } + + if (rf_cfg->dr == DWT_BR_6M8) { + rf_cfg->t_phr = UWB_PHY_NUMOF_SYM_PHR * UWB_PHY_TDSYM_PHR_6M8; + rf_cfg->t_dsym = UWB_PHY_TDSYM_DATA_6M8 / 0.44; + } else if (rf_cfg->dr == DWT_BR_850K) { + rf_cfg->t_phr = UWB_PHY_NUMOF_SYM_PHR * UWB_PHY_TDSYM_PHR_850K; + rf_cfg->t_dsym = UWB_PHY_TDSYM_DATA_850K / 0.44; + } else { + rf_cfg->t_phr = UWB_PHY_NUMOF_SYM_PHR * UWB_PHY_TDSYM_PHR_110K; + rf_cfg->t_dsym = UWB_PHY_TDSYM_DATA_110K / 0.44; + } + + return 0; +} + +static int dw1000_init(const struct device *dev) +{ + struct dwt_context *ctx = dev->data; + const struct dwt_hi_cfg *hi_cfg = dev->config; + + LOG_INF("Initialize DW1000 Transceiver"); + k_sem_init(&ctx->phy_sem, 0, 1); + + /* slow SPI config */ + memcpy(&ctx->spi_cfg_slow, &hi_cfg->bus.config, sizeof(ctx->spi_cfg_slow)); + ctx->spi_cfg_slow.frequency = DWT_SPI_SLOW_FREQ; + + if (!spi_is_ready(&hi_cfg->bus)) { + LOG_ERR("SPI device not ready"); + return -ENODEV; + } + + dwt_set_spi_slow(dev, DWT_SPI_SLOW_FREQ); + + /* Initialize IRQ GPIO */ + if (!device_is_ready(hi_cfg->irq_gpio.port)) { + LOG_ERR("IRQ GPIO device not ready"); + return -ENODEV; + } + + if (gpio_pin_configure_dt(&hi_cfg->irq_gpio, GPIO_INPUT)) { + LOG_ERR("Unable to configure GPIO pin %u", hi_cfg->irq_gpio.pin); + return -EINVAL; + } + + gpio_init_callback(&(ctx->gpio_cb), dwt_gpio_callback, + BIT(hi_cfg->irq_gpio.pin)); + + if (gpio_add_callback(hi_cfg->irq_gpio.port, &(ctx->gpio_cb))) { + LOG_ERR("Failed to add IRQ callback"); + return -EINVAL; + } + + /* Initialize RESET GPIO */ + if (!device_is_ready(hi_cfg->rst_gpio.port)) { + LOG_ERR("Reset GPIO device not ready"); + return -ENODEV; + } + + if (gpio_pin_configure_dt(&hi_cfg->rst_gpio, GPIO_INPUT)) { + LOG_ERR("Unable to configure GPIO pin %u", hi_cfg->rst_gpio.pin); + return -EINVAL; + } + + LOG_INF("GPIO and SPI configured"); + + dwt_hw_reset(dev); + + if (dwt_reg_read_u32(dev, DWT_DEV_ID_ID, 0) != DWT_DEVICE_ID) { + LOG_ERR("Failed to read device ID %p", dev); + return -ENODEV; + } + + if (dwt_initialise_dev(dev)) { + LOG_ERR("Failed to initialize DW1000"); + return -EIO; + } + + if (dwt_configure_rf_phy(dev)) { + LOG_ERR("Failed to configure RF PHY"); + return -EIO; + } + + /* Allow Beacon, Data, Acknowledgement, MAC command */ + dwt_set_frame_filter(dev, true, DWT_SYS_CFG_FFAB | DWT_SYS_CFG_FFAD | + DWT_SYS_CFG_FFAA | DWT_SYS_CFG_FFAM); + /* + * Enable system events: + * - transmit frame sent, + * - receiver FCS good, + * - receiver PHY header error, + * - receiver FCS error, + * - receiver Reed Solomon Frame Sync Loss, + * - receive Frame Wait Timeout, + * - preamble detection timeout, + * - receive SFD timeout + */ + dwt_reg_write_u32(dev, DWT_SYS_MASK_ID, 0, + DWT_SYS_MASK_MTXFRS | + DWT_SYS_MASK_MRXFCG | + DWT_SYS_MASK_MRXPHE | + DWT_SYS_MASK_MRXFCE | + DWT_SYS_MASK_MRXRFSL | + DWT_SYS_MASK_MRXRFTO | + DWT_SYS_MASK_MRXPTO | + DWT_SYS_MASK_MRXSFDTO); + + /* Initialize IRQ event work queue */ + ctx->dev = dev; + + k_work_queue_start(&dwt_work_queue, dwt_work_queue_stack, + K_KERNEL_STACK_SIZEOF(dwt_work_queue_stack), + CONFIG_SYSTEM_WORKQUEUE_PRIORITY, NULL); + + k_work_init(&ctx->irq_cb_work, dwt_irq_work_handler); + + dwt_setup_int(dev, true); + + LOG_INF("DW1000 device initialized and configured"); + + return 0; +} + +static inline uint8_t *get_mac(const struct device *dev) +{ + struct dwt_context *dw1000 = dev->data; + uint32_t *ptr = (uint32_t *)(dw1000->mac_addr); + + UNALIGNED_PUT(sys_rand32_get(), ptr); + ptr = (uint32_t *)(dw1000->mac_addr + 4); + UNALIGNED_PUT(sys_rand32_get(), ptr); + + dw1000->mac_addr[0] = (dw1000->mac_addr[0] & ~0x01) | 0x02; + + return dw1000->mac_addr; +} + +uint8_t *dwt_get_mac(const struct device *dev) +{ + return get_mac(dev); +} + +static void dwt_iface_api_init(struct net_if *iface) +{ + const struct device *dev = net_if_get_device(iface); + struct dwt_context *dw1000 = dev->data; + uint8_t *mac = get_mac(dev); + + net_if_set_link_addr(iface, mac, 8, NET_LINK_IEEE802154); + + dw1000->iface = iface; + + ieee802154_init(iface); + + LOG_INF("Iface initialized"); +} + +static struct ieee802154_radio_api dwt_radio_api = { + .iface_api.init = dwt_iface_api_init, + + .get_capabilities = dwt_get_capabilities, + .cca = dwt_cca, + .set_channel = dwt_set_channel, + .filter = dwt_filter, + .set_txpower = dwt_set_power, + .start = dwt_start, + .stop = dwt_stop, + .configure = dwt_configure, + .ed_scan = dwt_ed, + .tx = dwt_tx, +}; + +#define DWT_PSDU_LENGTH (127 - DWT_FCS_LENGTH) + +#if defined(CONFIG_IEEE802154_RAW_MODE) +DEVICE_DT_INST_DEFINE(0, dw1000_init, NULL, + &dwt_0_context, &dw1000_0_config, + POST_KERNEL, CONFIG_IEEE802154_DW1000_INIT_PRIO, + &dwt_radio_api); +#else +NET_DEVICE_DT_INST_DEFINE(0, +dw1000_init, +NULL, +&dwt_0_context, +&dw1000_0_config, +CONFIG_IEEE802154_DW1000_INIT_PRIO, +&dwt_radio_api, +IEEE802154_L2, +NET_L2_GET_CTX_TYPE(IEEE802154_L2), +DWT_PSDU_LENGTH); +#endif diff --git a/override/include/drivers/ieee802154/dw1000.h b/override/include/drivers/ieee802154/dw1000.h new file mode 100644 index 0000000..5da7716 --- /dev/null +++ b/override/include/drivers/ieee802154/dw1000.h @@ -0,0 +1,53 @@ +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_INCLUDE_DRIVERS_IEEE802154_DW1000_H_ +#define ZEPHYR_INCLUDE_DRIVERS_IEEE802154_DW1000_H_ + +#include + +/** + * Set the upper 32 bits of the dwt timestamp + * This method should be called just before the invocation of the tx method and from within the same thread + */ +void dwt_set_delayed_tx_short_ts(const struct device *dev, uint32_t short_ts); + +/** + * Sets the delayed tx and returns the estimated tx ts + * @param dev The dw1000 device + * @param uus_delay the delay in uwb microseconds + * @return estimated tx ts (corrected by antenna delay) + */ +uint64_t dwt_plan_delayed_tx(const struct device *dev, uint64_t uus_delay); + +void dwt_set_antenna_delay_rx(const struct device *dev, uint16_t rx_delay_ts); +void dwt_set_antenna_delay_tx(const struct device *dev, uint16_t tx_delay_ts); + +uint16_t dwt_antenna_delay_rx(const struct device *dev); +uint16_t dwt_antenna_delay_tx(const struct device *dev); + +uint64_t dwt_rx_ts(const struct device *dev); + +uint64_t dwt_system_ts(const struct device *dev); +uint32_t dwt_system_short_ts(const struct device *dev); + +uint64_t dwt_ts_to_fs(uint64_t ts); +uint64_t dwt_fs_to_ts(uint64_t fs); + +uint64_t dwt_short_ts_to_fs(uint32_t ts); +uint32_t dwt_fs_to_short_ts(uint64_t fs); + +uint64_t dwt_calculate_actual_tx_ts(uint32_t planned_short_ts, uint16_t tx_antenna_delay); + +void dwt_set_frame_filter(const struct device *dev, bool ff_enable, uint8_t ff_type); + + +int dwt_readcarrierintegrator(const struct device *dev); +float dwt_rx_clock_ratio_offset(const struct device *dev); + +uint8_t *dwt_get_mac(const struct device *dev); + +#endif /* ZEPHYR_INCLUDE_DRIVERS_IEEE802154_DW1000_H_ */ diff --git a/prj.conf b/prj.conf new file mode 100644 index 0000000..871a6e3 --- /dev/null +++ b/prj.conf @@ -0,0 +1,31 @@ +CONFIG_LOG=y +CONFIG_LOG_DEFAULT_LEVEL=4 +CONFIG_LOG_MODE_IMMEDIATE=y +CONFIG_LOG_PRINTK=y + +CONFIG_SCHED_SCALABLE=y + +#CONFIG_NET_TC_THREAD_COOPERATIVE=y +CONFIG_NETWORKING=y +CONFIG_IEEE802154=y +CONFIG_IEEE802154_DW1000=y +CONFIG_NET_PKT_TIMESTAMP=y +CONFIG_IEEE802154_RAW_MODE=y + +CONFIG_NEWLIB_LIBC=y +CONFIG_NEWLIB_LIBC_NANO=n + +#CONFIG_CBPRINTF_FP_SUPPORT=y +#CONFIG_NEWLIB_LIBC_FLOAT_PRINTF=y + +CONFIG_SERIAL=y +#CONFIG_NET_L2_IEEE802154=n + +#CONFIG_NO_OPTIMIZATIONS=n + + +CONFIG_CMSIS_DSP=y +CONFIG_CMSIS_DSP_MATRIX=y +CONFIG_CMSIS_DSP_MATRIXCHECK=y + +CONFIG_HWINFO=y \ No newline at end of file diff --git a/scripts/base.py b/scripts/base.py new file mode 100644 index 0000000..b7a84a2 --- /dev/null +++ b/scripts/base.py @@ -0,0 +1,61 @@ +import json +import math +import numpy as np + +SPEED_OF_LIGHT = 299792458.0 + +DWT_FREQ_OFFSET_MULTIPLIER = (998.4e6/2.0/1024.0/131072.0) +DWT_HERTZ_TO_PPM_MULTIPLIER_CHAN_5 = (-1.0e6/6489.6e6) + +S_PER_DT_TS = 1.0E-15 * 15650.0 +METER_PER_DWT_TS = ((SPEED_OF_LIGHT * 1.0E-15) * 15650.0) + +DBM_TWR_DIST_ADJUST = { + -61: -11, + -63: -10.5, + -65: -10.0, + -67: -9.3, + -69: -8.2, + -71: -6.9, + -73: -5.1, + -75: -2.7, + -77: 0.0, + -79: 2.1, + -81: 3.5, + -83: 4.2, + -85: 4.9, + -87: 6.2, + -89: 7.1, + -91: 7.6, + -93: 8.1 +} + +def get_dist(pos_a, pos_b): + pos_a = np.array(pos_a) + pos_b = np.array(pos_b) + return np.linalg.norm(pos_a - pos_b) + +def pair_index(a, b): + if a > b: + return int((a * (a - 1)) / 2 + b) + else: + return pair_index(b, a) + + +def convert_ts_to_sec(ts): + return ts*S_PER_DT_TS + +def convert_sec_to_ts(sec): + ts = sec / S_PER_DT_TS + assert convert_ts_to_sec(ts)-sec < 0.00001 + return ts + +def convert_ts_to_m(ts): + return convert_ts_to_sec(ts)*SPEED_OF_LIGHT + +def convert_m_to_ts(m): + return convert_sec_to_ts(m/SPEED_OF_LIGHT) + + +def ci_to_rd(ci): + return 1.0 - ci*(DWT_FREQ_OFFSET_MULTIPLIER * DWT_HERTZ_TO_PPM_MULTIPLIER_CHAN_5 / 1.0e6) \ No newline at end of file diff --git a/scripts/eval.py b/scripts/eval.py new file mode 100644 index 0000000..18e1a74 --- /dev/null +++ b/scripts/eval.py @@ -0,0 +1,772 @@ +import os +import progressbar +import numpy as np +import json + +from testbed import lille, trento_a, trento_b + +from logs import gen_estimations_from_testbed_run, gen_measurements_from_testbed_run, gen_delay_estimates_from_testbed_run +from base import get_dist, pair_index, convert_ts_to_sec, convert_sec_to_ts, convert_ts_to_m, convert_m_to_ts, ci_to_rd + + +import matplotlib +import matplotlib.pyplot as plt +from utility import slugify, cached, init_cache, load_env_config + +import pandas as pd + +METHOD_PREFIX = 'export_' + +CONFIDENCE_FILL_COLOR = '0.8' +PERCENTILES_FILL_COLOR = '0.5' +COLOR_MAP = 'tab10' + +c_in_air = 299702547.236 + + +PROTOCOL_NAME = "ALADIn" + + +runs = { + 'trento_a': 'job_fixed', # [(6,3)], + 'trento_b': 'job_fixed', + 'lille': 'job_fixed' # [(11,3), (10,3), (7,1), (5,0)], +} + +src_devs = { + 'trento_a': 'dwm1001.1', # [(6,3)], + 'trento_b': 'dwm1001.160', + 'lille': 'dwm1001-1' # [(11,3), (10,3), (7,1), (5,0)], +} + +def load_plot_defaults(): + # Configure as needed + plt.rc('lines', linewidth=2.0) + plt.rc('legend', framealpha=1.0, fancybox=True) + plt.rc('errorbar', capsize=3) + plt.rc('pdf', fonttype=42) + plt.rc('ps', fonttype=42) + plt.rc('font', size=11) + #plt.rc('font', size=8, family="serif", serif=['Times New Roman'] + plt.rcParams['font.serif']) + plt.rcParams['axes.axisbelow'] = True + + +def export_simulation_performance(config, export_dir): + + from sim import get_sim_data_rows + + xs = [16, 64, 256, 1024] + num_repetitions = 100 + + def proc(): + return get_sim_data_rows(xs, num_repetitions) + + data_rows = cached(('sim', xs, num_repetitions), proc) + + df = pd.DataFrame(data_rows) + + # df.plot.bar(x='pair',y=['dist', 'est_distance_uncalibrated', 'est_distance_factory', 'est_distance_calibrated']) + df = df.rename(columns={"gn_mean": "Gauss-Newton", "tdoa_mean": "TDoA", "our_mean": PROTOCOL_NAME}) + + stds = [df['tdoa_std'], df['gn_std'], df['our_std']] + + plt.clf() + + ax = df.plot.bar(x='num_measurements', y=['TDoA', 'Gauss-Newton', PROTOCOL_NAME], yerr=stds, width=0.8) + + + plt.ylim(0.0, 14.0) + ax.set_axisbelow(True) + ax.set_xlabel("Number of Measurement Rounds") + ax.set_ylabel("Mean RMSE [cm]") + + + stds_as_np = np.array(stds) + stds_as_np = np.ravel(stds_as_np, order='C') + + counter = 0 + for p in ax.patches: + height = p.get_height() + if np.isnan(height): + height = 0 + + ax.text(p.get_x() + p.get_width() / 2., height+stds_as_np[counter], "{:.1f}\n[{:.1f}]".format(height, stds_as_np[counter]), fontsize=9, color='black', ha='center', va='bottom') + + # ax.text(p.get_x() + p.get_width()/2., 0.5, '%.2f' % stds[offset], fontsize=12, color='black', ha='center', va='bottom') + counter += 1 + + plt.grid(color='lightgray', linestyle='dashed') + + plt.gcf().set_size_inches(6.0, 5.5) + plt.tight_layout() + + plt.savefig("{}/sim_rmse.pdf".format(export_dir), bbox_inches = 'tight', pad_inches = 0) + #plt.show() + + plt.close() + + + +def export_testbed_variance(config, export_dir): + + std_upper_lim = 10.0 + + for (c, t) in enumerate([lille, trento_a, trento_b]): + + def proc(): + meas_df = pd.DataFrame.from_records(gen_measurements_from_testbed_run(t, runs[t.name], src_dev=src_devs[t.name])) + meas_df['estimated_m'] = meas_df['estimated_tof'] + meas_df = meas_df[['pair', 'estimated_m', 'dist']] + + ma = np.zeros((len(t.devs), len(t.devs))) + res = meas_df.groupby('pair').aggregate(func=['mean', 'std']) + + for a in range(len(t.devs)): + for b in range(len(t.devs)): + if a > b: + e = res.loc['{}-{}'.format(a, b), ('estimated_m', 'std')]*100 # in cm + ma[a, b] = e + ma[b, a] = e + return ma + + ma = np.array(cached(('meas_var', t.name, runs[t.name], src_devs[t.name], 6), proc)) + print(t.name, "mean std", ma.mean(), "median",np.median(ma), "max", np.max(ma), "90% quantile", np.quantile(ma, 0.9), "95% quantile", np.quantile(ma, 0.95)) + + plt.clf() + fig, ax = plt.subplots(figsize=(7.5, 7.5)) + ax.matshow(ma, vmin=0.0, vmax=std_upper_lim, norm='asinh') + + for i in range(ma.shape[0]): + for j in range(ma.shape[1]): + if i != j: + e = ma[i, j] + if e > 100: + e = int(ma[i, j]) + else: + e = round(ma[i, j], 1) + + if e >= std_upper_lim: + s = r"\underline{" + str(e) + "}" + else: + s = str(e) + ax.text(x=j, y=i, s=s, va='center', ha='center', usetex=True) + + ax.xaxis.set_major_formatter(lambda x, pos: int(x+1)) + ax.yaxis.set_major_formatter(lambda x, pos: int(x+1)) + fig.set_size_inches(4.75, 4.75) + plt.tight_layout() + + plt.savefig("{}/var_ma_{}.pdf".format(export_dir, t.name), bbox_inches='tight', pad_inches=0) + + plt.close() + + #plt.show() + +def export_testbed_variance_from_device(config, export_dir): + + std_upper_lim = 10.0 + + for (c, t) in enumerate([lille, trento_a, trento_b]): + + def proc(): + est_df = pd.DataFrame.from_records(gen_estimations_from_testbed_run(t, runs[t.name], src_dev=src_devs[t.name])) + + round = est_df['round'].max() + est_df = est_df[(est_df['round'] == round)] + + + ma = np.zeros((len(t.devs), len(t.devs))) + + for a in range(len(t.devs)): + for b in range(len(t.devs)): + if a > b: + res = est_df.loc[est_df['pair'] == '{}-{}'.format(a, b), "var_measurement"] + + e = 0.0 + if len(res) > 0: + var = (res.to_numpy())[0] + if var is not None: + e = np.sqrt(var)*100 + + ma[a, b] = e + ma[b, a] = e + print(ma) + return ma + + ma = np.array(cached(('meas_var_device', t.name, runs[t.name], src_devs[t.name], 7), proc)) + print(t.name, "mean std", ma.mean(), "median",np.median(ma), "max", np.max(ma), "90% quantile", np.quantile(ma, 0.9), "95% quantile", np.quantile(ma, 0.95)) + + plt.clf() + fig, ax = plt.subplots(figsize=(7.5, 7.5)) + ax.matshow(ma, vmin=0.0, vmax=std_upper_lim, norm='asinh') + + for i in range(ma.shape[0]): + for j in range(ma.shape[1]): + if i != j: + e = ma[i, j] + if e > 100: + e = int(ma[i, j]) + else: + e = round(ma[i, j], 1) + + if e >= std_upper_lim: + s = r"\underline{" + str(e) + "}" + else: + s = str(e) + ax.text(x=j, y=i, s=s, va='center', ha='center', usetex=True) + + ax.xaxis.set_major_formatter(lambda x, pos: int(x+1)) + ax.yaxis.set_major_formatter(lambda x, pos: int(x+1)) + fig.set_size_inches(6.0, 6.0) + plt.tight_layout() + + plt.savefig("{}/var_ma_{}_from_device.pdf".format(export_dir, t.name), bbox_inches='tight', pad_inches=0) + + plt.close() + + #plt.show() + + + +def export_testbed_layouts(config, export_dir): + + # we draw every layout + + # TODO: Extract them from the actual data! + high_variance_connections = { + 'trento_a': [],#[(6,3)], + 'trento_b': [], + 'lille': [] #[(11,3), (10,3), (7,1), (5,0)], + } + + always_drawn ={ + 'lille': [], + 'trento_a': [], + 'trento_b': [] + } + + limits = { + 'lille': [-0.5, 10.0, 29, 22.5], + 'trento_a': [71, 81, -1.25, 8.5], + 'trento_b': [118.5, 131.5, -1, 8.0] + } + + + for (c,t) in enumerate([lille, trento_a, trento_b]): + ys = [] + xs = [] + ns = [] + + for (i,k) in enumerate(t.devs): + pos = t.dev_positions[k] + xs.append(pos[0]) + ys.append(pos[1]) + n = i+1 #k.replace("dwm1001.", "").replace("dwm1001-", "") + ns.append(n) + + plt.clf() + + + + fig, ax = plt.subplots() + + t.draw_layout(plt) + + ax.scatter(xs, ys, color='C'+str(c), marker='o') + ax.set_aspect('equal', adjustable='box') + + + for (a,b) in high_variance_connections[t.name]: + plt.plot([xs[a], xs[b]], [ys[a], ys[b]], 'r', zorder=-10) + + for a in always_drawn[t.name]: + for b in range(len(t.devs)): + if a > a: + plt.plot([xs[a], xs[b]], [ys[a], ys[b]], 'r', zorder=-10) + + + for i, txt in enumerate(ns): + ax.annotate(txt, (xs[i], ys[i]), xytext=(-3, 0), textcoords='offset points', va='center', ha='right') + + ax.set_axisbelow(True) + ax.set_xlabel("Position X [m]") + ax.set_ylabel("Position Y [m]") + + if t.name == 'lille': + ax.invert_yaxis() + + plt.axis(limits[t.name]) + + fig.set_size_inches(4.0, 3.5) + plt.tight_layout() + + #plt.show() + plt.savefig("{}/layout_{}.pdf".format(export_dir, t.name),bbox_inches = 'tight', pad_inches = 0, dpi=600) + + plt.close() + + + +def export_overall_rmse_reduction(config, export_dir): + + + # we extract values for: + # uncalibrated, factory, calibrated_unfiltered, calibrated_filtered + + errs = {} + ts = [trento_a, trento_b, lille] + + for (c, t) in enumerate(ts): + def proc(): + est_df = pd.DataFrame.from_records(gen_estimations_from_testbed_run(t, runs[t.name], src_dev=src_devs[t.name])) + + round = est_df['round'].max() + + + + est_df = est_df[(est_df['round'] == round) & (est_df['mean_measurement'].notna())] + est_df['err_uncalibrated'] = est_df['est_distance_uncalibrated'] - est_df['dist'] + est_df['err_factory'] = est_df['est_distance_factory'] - est_df['dist'] + est_df['err_calibrated'] = est_df['est_distance_calibrated'] - est_df['dist'] + est_df['err_calibrated_filtered'] = est_df['est_distance_calibrated_filtered'] - est_df['dist'] + + #est_df['std'] = est_df['var_measurement'].apply(np.sqrt) * 100.0 + + return { + 'err_uncalibrated': est_df['err_uncalibrated'].to_numpy(), + 'err_factory': est_df['err_factory'].to_numpy(), + 'err_calibrated': est_df['err_calibrated'].to_numpy() + #'err_calibrated_filtered': est_df['err_calibrated_filtered'].to_numpy() + } + + # est_df['abs_err_uncalibrated'] = est_df['err_uncalibrated'].apply(np.abs) + # est_df['abs_err_factory'] = est_df['err_factory'].apply(np.abs) + # est_df['abs_err_calibrated'] = est_df['err_calibrated'].apply(np.abs) + # + # est_df['squared_err_uncalibrated'] = est_df['err_uncalibrated'].apply(np.square) + # est_df['squared_err_factory'] = est_df['err_factory'].apply(np.square) + # est_df['squared_err_calibrated'] = est_df['err_calibrated'].apply(np.square) + # + # # we determine the mean squared error of all pairs + # + # est_df = est_df[['pair', 'squared_err_uncalibrated', 'squared_err_factory', 'squared_err_calibrated']] + # res = est_df.aggregate(func=['mean']) + # + # return { + # 'rmse_uncalibrated': np.sqrt(res['squared_err_uncalibrated'][0]), + # 'rmse_factory': np.sqrt(res['squared_err_factory'][0]), + # 'rmse_calibrated': np.sqrt(res['squared_err_calibrated'][0]) + # } + + data = cached(('overall', t.name, runs[t.name], src_devs[t.name], 17), proc) + for k in data: + if k not in errs: + errs[k] = [] + errs[k].append(data[k]*100) + + def rmse(xs): + return np.sqrt(np.mean(np.square(np.array(xs)))), 0 + + + stds = {} + + for k in errs: + stds[k] = [] + for (i, xs) in enumerate(errs[k]): + e, sd = rmse(xs) + errs[k][i] = e*100 + stds[k].append(sd*100) + + scenarios = ["CLOVES-7", "CLOVES-14", "IoT-LAB-14"] + + x = np.arange(len(scenarios)) # the label locations + width = 0.25 # the width of the bars + multiplier = 0 + + + plt.clf() + fig, ax = plt.subplots(layout='constrained') + + labels = { + 'err_uncalibrated': "Uncalibrated", + 'err_factory': "Factory", + 'err_calibrated': PROTOCOL_NAME + } + + for attribute, measurement in errs.items(): + offset = width * multiplier + rects = ax.bar(x + offset, measurement, width, label=labels[attribute]) + ax.bar_label(rects, fmt="{:.2f}", padding=3) + multiplier += 1 + + ax.set_ylabel('RMSE [cm]') + ax.set_xlabel('Scenario') + ax.set_xticks(x + width, scenarios) + ax.legend() + plt.gca().yaxis.grid(True, color='lightgray', linestyle='dashed') + ax.set_ylim(0, 20) + + plt.tight_layout() + + plt.savefig("{}/rmse_comparison.pdf".format(export_dir), bbox_inches='tight', pad_inches=0) + + +def export_overall_mae_reduction(config, export_dir): + + + # we extract values for: + # uncalibrated, factory, calibrated_unfiltered, calibrated_filtered + + errs = {} + ts = [trento_a, trento_b, lille] + + for (c, t) in enumerate(ts): + def proc(): + est_df = pd.DataFrame.from_records(gen_estimations_from_testbed_run(t, runs[t.name], src_dev=src_devs[t.name])) + + round = est_df['round'].max() + + + est_df = est_df[(est_df['round'] == round) & (est_df['mean_measurement'].notna())] + est_df['err_uncalibrated'] = est_df['est_distance_uncalibrated'] - est_df['dist'] + est_df['err_factory'] = est_df['est_distance_factory'] - est_df['dist'] + est_df['err_calibrated'] = est_df['est_distance_calibrated'] - est_df['dist'] + est_df['err_calibrated_filtered'] = est_df['est_distance_calibrated_filtered'] - est_df['dist'] + + #est_df['std'] = est_df['var_measurement'].apply(np.sqrt) * 100.0 + + return { + 'err_uncalibrated': est_df['err_uncalibrated'].to_numpy(), + 'err_factory': est_df['err_factory'].to_numpy(), + 'err_calibrated': est_df['err_calibrated'].to_numpy(), + 'err_calibrated_filtered': est_df['err_calibrated_filtered'].to_numpy() + } + + # est_df['abs_err_uncalibrated'] = est_df['err_uncalibrated'].apply(np.abs) + # est_df['abs_err_factory'] = est_df['err_factory'].apply(np.abs) + # est_df['abs_err_calibrated'] = est_df['err_calibrated'].apply(np.abs) + # + # est_df['squared_err_uncalibrated'] = est_df['err_uncalibrated'].apply(np.square) + # est_df['squared_err_factory'] = est_df['err_factory'].apply(np.square) + # est_df['squared_err_calibrated'] = est_df['err_calibrated'].apply(np.square) + # + # # we determine the mean squared error of all pairs + # + # est_df = est_df[['pair', 'squared_err_uncalibrated', 'squared_err_factory', 'squared_err_calibrated']] + # res = est_df.aggregate(func=['mean']) + # + # return { + # 'rmse_uncalibrated': np.sqrt(res['squared_err_uncalibrated'][0]), + # 'rmse_factory': np.sqrt(res['squared_err_factory'][0]), + # 'rmse_calibrated': np.sqrt(res['squared_err_calibrated'][0]) + # } + + data = cached(('overall_mae', t.name, runs[t.name], src_devs[t.name], 17), proc) + for k in data: + if k not in errs: + errs[k] = [] + errs[k].append(data[k]*100) + + def ae(xs): + return np.abs(np.array(xs)).mean(), np.abs(np.array(xs)).std() + + + stds = {} + + for k in errs: + stds[k] = [] + for (i, xs) in enumerate(errs[k]): + e, sd = ae(xs) + errs[k][i] = e*100 + stds[k].append(sd*100) + + scenarios = ["CLOVES-7", "CLOVES-14", "IoT-LAB-14"] + + x = np.arange(len(scenarios)) # the label locations + width = 0.25 # the width of the bars + multiplier = 0 + plt.clf() + fig, ax = plt.subplots(layout='constrained') + + labels = { + 'err_uncalibrated': "Uncalibrated", + 'err_factory': "Factory", + 'err_calibrated': PROTOCOL_NAME, + #'err_calibrated_filtered': "Calibrated Filtered", + } + + for attribute, measurement in errs.items(): + if attribute in labels: + offset = width * multiplier + rects = ax.bar(x + offset, measurement, width, label=labels[attribute], yerr=stds[attribute]) + + ax.bar_label(rects, padding=0, fontsize=9, label_type='edge', labels=["{:.1f}\n[{:.1f}]".format(round(measurement[i],1), round(stds[attribute][i],1)) for i in range(len(measurement))]) + + # for (i, r) in enumerate(rects): + # print(r) + # ax.text(r.xy[0] + r._width / 2, -1.5, '{:.2f}'.format(r._height), fontsize=10, color='black', + # ha='center', va='bottom') + + + multiplier += 1 + + ax.set_ylabel('MAE [cm]') + ax.set_xlabel('Scenario') + ax.set_xticks(x + width, scenarios) + ax.legend(loc='upper center') + ax.set_ylim(None, 30) + + plt.gca().yaxis.grid(True, color='lightgray', linestyle='dashed') + fig.set_size_inches(5.5, 4.5) + + plt.tight_layout() + plt.savefig("{}/mae_comparison.pdf".format(export_dir), bbox_inches='tight', pad_inches=0) + + +def export_trento_a_pairs(config, export_dir): + + # we extract values for: + # uncalibrated, factory, calibrated_unfiltered, calibrated_filtered + + errs = {} + ts = [trento_a] + + for (c, t) in enumerate(ts): + est_df = pd.DataFrame.from_records(gen_estimations_from_testbed_run(t, runs[t.name], src_dev=src_devs[t.name])) + + round = est_df['round'].max() + + est_df = est_df[(est_df['round'] == round) & (est_df['mean_measurement'].notna())] + + est_df['Uncalibrated'] = (est_df['est_distance_uncalibrated'] - est_df['dist'])*100 + est_df['Factory'] = (est_df['est_distance_factory'] - est_df['dist'])*100 + est_df[PROTOCOL_NAME] = (est_df['est_distance_calibrated'] - est_df['dist'])*100 + + est_df = est_df.sort_values(by='Uncalibrated') + + def rename_pairs(x=None): + return '{}-{}'.format(x['initiator']+1, x['responder']+1) + + est_df['pair'] = est_df.apply(rename_pairs, axis=1) + + # df.plot.bar(x='pair',y=['dist', 'est_distance_uncalibrated', 'est_distance_factory', 'est_distance_calibrated']) + est_df.plot.bar(x='pair', y=['Uncalibrated', 'Factory', PROTOCOL_NAME], width=0.75) + + plt.legend() + plt.xlabel("Pair") + plt.ylabel("Error [cm]") + + plt.gcf().set_size_inches(5.0, 4.0) + plt.tight_layout() + + plt.gca().yaxis.grid(True, color='lightgray', linestyle='dashed') + + plt.savefig("{}/error_sorted.pdf".format(export_dir), bbox_inches = 'tight', pad_inches = 0) + plt.close() + + + +import testbed_to_c_vals + +def export_filtered_mae_reduction(config, export_dir): + + + # we extract values for: + # uncalibrated, factory, calibrated_unfiltered, calibrated_filtered + + errs = {} + ts = [trento_a, lille] + + # their actual ids not their indices + ignored_pair = { + 'trento_a': [(6,3)], + 'trento_b': [], + 'lille': [(7,1), (4, 2)], + } + + for (c, t) in enumerate(ts): + def proc(): + est_df = pd.DataFrame.from_records(gen_estimations_from_testbed_run(t, runs[t.name], src_dev=src_devs[t.name])) + + est_df = est_df[(est_df['round'] == est_df['round'].max()) & (est_df['mean_measurement'].notna())] + + + # we manually calculate the errors for the filtered case and exclude pair 7-4 (or 6,3) + M = testbed_to_c_vals.create_inference_matrix(len(t.devs), ignored_pairs=ignored_pair[t.name]) + + num_pairs = round(len(t.devs) * (len(t.devs) - 1) / 2) + actual = np.zeros(shape=num_pairs) + measured = np.zeros(shape=num_pairs) + + for (a, da) in enumerate(t.devs): + for (b, db) in enumerate(t.devs): + if a > b: + actual[pair_index(a, b)] = get_dist(t.dev_positions[da], t.dev_positions[db]) + measured[pair_index(a, b)] = est_df[est_df['pair']== '{}-{}'.format(a,b)]['mean_measurement'] + + diffs = measured - actual + delays_in_m = np.matmul(M, 2 * diffs) + + filtered_res_in_m = np.zeros(shape=round(len(t.devs) * (len(t.devs) - 1) / 2)) + + for (a, da) in enumerate(t.devs): + for (b, db) in enumerate(t.devs): + if a > b: + filtered_res_in_m[pair_index(a, b)] = measured[pair_index(a, b)]-0.5*delays_in_m[a]-0.5*delays_in_m[b] + + est_df['err_calibrated_filtered'] = filtered_res_in_m - actual + est_df['err_uncalibrated'] = est_df['est_distance_uncalibrated'] - est_df['dist'] + est_df['err_factory'] = est_df['est_distance_factory'] - est_df['dist'] + est_df['err_calibrated'] = est_df['est_distance_calibrated'] - est_df['dist'] + + for (a,b) in ignored_pair[t.name]: + est_df = est_df[est_df['pair'] != '{}-{}'.format(a,b)] + + return { + 'err_uncalibrated': est_df['err_uncalibrated'].to_numpy(), + 'err_factory': est_df['err_factory'].to_numpy(), + 'err_calibrated': est_df['err_calibrated'].to_numpy(), + 'err_calibrated_filtered': est_df['err_calibrated_filtered'].to_numpy() + } + + + data = cached(('export_filtered_mae_reduction', t.name, runs[t.name], src_devs[t.name], hash(json.dumps(ignored_pair)), 1), proc) + print(data) + for k in data: + if k not in errs: + errs[k] = [] + errs[k].append(data[k]*100) + + def ae(xs): + return np.abs(np.array(xs)).mean(), np.abs(np.array(xs)).std() + + stds = {} + + for k in errs: + stds[k] = [] + for (i, xs) in enumerate(errs[k]): + e, sd = ae(xs) + errs[k][i] = e*100 + stds[k].append(sd*100) + + scenarios = ["CLOVES-7", "IoT-Lab-14"] + + x = np.arange(len(scenarios)) # the label locations + width = 0.2 # the width of the bars + multiplier = 0.0 + + + plt.clf() + fig, ax = plt.subplots(layout='constrained') + + labels = { + 'err_uncalibrated': "Uncalibrated", + 'err_factory': "Factory", + 'err_calibrated': PROTOCOL_NAME +" Unfiltered", + 'err_calibrated_filtered': PROTOCOL_NAME + " Filtered", + } + + colors = ['C0', 'C1', 'C2', 'C3'] + color = 0 + for attribute, measurement in errs.items(): + if attribute in labels: + offset = width * multiplier + rects = ax.bar(x + offset - width/2, measurement, width, label=labels[attribute], yerr=stds[attribute], color=colors[color]) + + ax.bar_label(rects, padding=0, fontsize=9, label_type='edge', + labels=["{:.1f}\n[{:.1f}]".format(round(measurement[i], 1), round(stds[attribute][i], 1)) for i + in range(len(measurement))]) + + multiplier += 1 + color+=1 + + ax.set_ylabel('MAE over Pairs [cm]') + ax.set_xlabel('Scenario') + ax.set_xticks(x + width, scenarios) + ax.legend(loc="upper left") + plt.gca().yaxis.grid(True, color='lightgray', linestyle='dashed') + + ax.set_ylim(None, 30) + + fig.set_size_inches(5.5, 4.5) + + plt.tight_layout() + plt.savefig("{}/mae_reduction_filtered.pdf".format(export_dir), bbox_inches='tight', pad_inches=0) + + + +def export_scatter_graph_trento_a(config, export_dir): + + t = trento_a + src_dev = trento_a.devs[0] + + meas_df = pd.DataFrame.from_records(gen_measurements_from_testbed_run(t, runs[t.name], src_dev=src_dev, include_dummy=True)) + # we first plot the number of measurements for all pairs + meas_df = meas_df[(meas_df['device'] == src_dev)] + + meas_df = meas_df[(meas_df['round'] <= 1200) & (meas_df['round'] > 200)] + + meas_df['round'] = meas_df['round'] - 200 + + meas_df['offset'] = (meas_df['estimated_tof'] - meas_df['dist'])*100 + + initiator = 6 + responder_a = 2 + responder_b = 3 + + df = meas_df + df = df[(df['initiator'] == initiator) & ((df['responder'] == responder_a))] + + df_b = meas_df[(meas_df['initiator'] == initiator) & ((meas_df['responder'] == responder_b))] + + ax = df_b.plot(kind='scatter', x='round', y='offset', color='C0', label='{}-{}'.format( initiator+1, responder_b+1), alpha=0.5, figsize=(5, 4), edgecolors='none') + ax = df.plot(ax=ax, kind='scatter', x='round', y='offset', color='C1', label='{}-{}'.format(initiator+1, responder_a+1), alpha=0.5, edgecolors='none') + + #plt.axhline(y=get_dist(dev_positions[d0], dev_positions[devs[other]]), color='b', linestyle='-') + + #ax = df.plot(ax=ax, kind='scatter', x='round', y='calculated_tof_single', color='b', label='C (32 bit)') + #ax = df.plot(ax=ax, kind='scatter', x='round', y='calculated_tof_int_10', color='b', label='Python (Integer)') + + plt.grid(color='lightgray', linestyle='dashed') + + plt.legend(loc='upper left') + + ax.set_ylabel('Offset [cm]') + ax.set_xlabel('Round') + + plt.gcf().set_size_inches(5.0, 4.5) + plt.tight_layout() + + #plt.title("Scatter {}-{}".format(i, other)) + plt.savefig("{}/measurements_scatter_trento_a.pdf".format(export_dir), bbox_inches='tight', pad_inches=0) + plt.close() + +if __name__ == '__main__': + + config = load_env_config() + + load_plot_defaults() + + assert 'EXPORT_DIR' in config and config['EXPORT_DIR'] + + if 'CACHE_DIR' in config and config['CACHE_DIR']: + init_cache(config['CACHE_DIR']) + + steps = [ + export_testbed_layouts, + export_scatter_graph_trento_a, + export_trento_a_pairs, + export_simulation_performance, + export_filtered_mae_reduction, + export_testbed_variance, + export_testbed_variance_from_device, + export_overall_mae_reduction, + export_overall_rmse_reduction, + ] + + for step in progressbar.progressbar(steps, redirect_stdout=True): + name = step.__name__.removeprefix(METHOD_PREFIX) + print("Handling {}".format(name)) + export_dir = os.path.join(config['EXPORT_DIR']) + '/' + os.makedirs(export_dir, exist_ok=True) + step(config, export_dir) diff --git a/scripts/logs.py b/scripts/logs.py new file mode 100644 index 0000000..3c501e3 --- /dev/null +++ b/scripts/logs.py @@ -0,0 +1,284 @@ +import numpy as np + +from base import get_dist, pair_index, convert_ts_to_sec, convert_sec_to_ts, convert_ts_to_m, convert_m_to_ts, ci_to_rd +import ctypes + +from testbed_to_c_vals import create_inference_matrix + +def convert_logged_measurement(val): + + if val > 2**62: + #this value is likely overflown... + number = val & 0xFFFFFFFFFFFFFFFF + signed_number = ctypes.c_longlong(number).value + val = signed_number + + return val / 1000.0 + +def extract_types(msg_iter, types): + rounds = set() + + by_round_and_dev = {} + + for t in types: + by_round_and_dev[t] = {} + + for (ts, d, msg) in msg_iter: + + if 'type' not in msg: + continue + + if msg['type'] not in types: + continue + + round = msg['round'] + rounds.add(round) + + by_round_and_dev[msg['type']][(d, round)] = msg + + rounds = list(rounds) + rounds.sort() + + return tuple([rounds] + [by_round_and_dev[t] for t in types]) + + + +def extract_measurements(msg_iter, testbed, src_dev, include_dummy=False): + rounds, raw_measurements, drift_estimations = extract_types(msg_iter, ['raw_measurements', 'drift_estimation']) + + # put all messages into (d, round) sets + + for r in rounds: + for d in testbed.devs: + + if src_dev and d != src_dev: + continue # we skip as we do not have this data anyway! + + for (a, da) in enumerate(testbed.devs): + for (b, db) in enumerate(testbed.devs): + if a <= b: + continue + + + + pi = pair_index(a,b) + + record = {} + + record['round'] = int(r) + record['device'] = d + record['initiator'] = a + record['responder'] = b + record['pair'] = "{}-{}".format(a,b) + record['dist'] = get_dist(testbed.dev_positions[da], testbed.dev_positions[db]) + + msg = raw_measurements.get((d, r), None) + + if msg is not None and include_dummy == False and 'dummy' in msg and msg['dummy'] == True: + continue + + record['estimated_tof'] = None + record['round_dur'] = None + record['response_dur'] = None + + if msg is not None and pi < len(msg['measurements']) and msg['measurements'][pi] is not None: + record['round_dur'] = msg['measurements'][pi][0] + record['response_dur'] = msg['measurements'][pi][1] + + if msg['measurements'][pi][2] is not None: + record['estimated_tof'] = convert_ts_to_m(convert_logged_measurement(msg['measurements'][pi][2])) + + msg = drift_estimations.get((d, r), None) + record['own_dur_a'] = None + record['other_dur_a'] = None + record['relative_drift_a'] = None + record['relative_drift_a_ci'] = None + record['own_dur_b'] = None + record['other_dur_b'] = None + record['relative_drift_b'] = None + record['relative_drift_b_ci'] = None + + if msg is not None: + + if 'carrierintegrators' in msg: + if msg['carrierintegrators'][a] != 0: + record['relative_drift_a_ci'] = ci_to_rd(msg['carrierintegrators'][a]) + if msg['carrierintegrators'][b] != 0: + record['relative_drift_b_ci'] = ci_to_rd(msg['carrierintegrators'][b]) + + if msg['durations'][a] is not None: + record['own_dur_a'] = msg['durations'][a][0] + record['other_dur_a'] = msg['durations'][a][1] + if record['own_dur_a'] != 0 and record['other_dur_a'] != 0: + record['relative_drift_a'] = float(record['own_dur_a']) / float(record['other_dur_a']) + + if msg['durations'][b] is not None: + record['own_dur_b'] = msg['durations'][b][0] + record['other_dur_b'] = msg['durations'][b][1] + if record['own_dur_b'] != 0 and record['other_dur_b'] != 0: + record['relative_drift_b'] = float(record['own_dur_b']) / float(record['other_dur_b']) + + record['calculated_tof'] = None + if None not in [record['relative_drift_a'], record['relative_drift_b'], record['round_dur'], record['response_dur']]: + record['calculated_tof'] = convert_ts_to_m(record['relative_drift_a'] *record['round_dur'] - record['relative_drift_b'] * record['response_dur'])*0.5 + + yield record + + +def extract_estimations(msg_iter, testbed, src_dev): + rounds, estimations = extract_types(msg_iter, ['estimation']) + + records = [] + for r in rounds: + for d in testbed.devs: + if src_dev and d != src_dev: + continue # we skip as we do not have this data anyway! + + for (a, da) in enumerate(testbed.devs): + for (b, db) in enumerate(testbed.devs): + if a <= b: + continue + + pi = pair_index(a,b) + + record = {} + + record['round'] = int(r) + record['device'] = d + record['initiator'] = a + record['responder'] = b + record['pair'] = "{}-{}".format(a,b) + record['dist'] = get_dist(testbed.dev_positions[da], testbed.dev_positions[db]) + + msg = estimations.get((d, r), None) + + #print(pi, len(msg['mean_measurements'])) + if msg is not None and pi < len(msg['mean_measurements']): + record['mean_measurement'] = convert_ts_to_m(convert_logged_measurement(msg['mean_measurements'][pi])) + + if 'var_measurements' in msg: + record['var_measurement'] = np.square(convert_ts_to_m(np.sqrt(convert_logged_measurement(msg['var_measurements'][pi])))) + else: + record['var_measurement'] = None + + record['est_distance_uncalibrated'] = convert_ts_to_m(convert_logged_measurement(msg['tofs_uncalibrated'][pi])) + record['est_distance_factory'] = convert_ts_to_m(convert_logged_measurement(msg['tofs_from_factory_delays'][pi])) + record['est_distance_calibrated'] = convert_ts_to_m(convert_logged_measurement(msg['tofs_from_estimated_delays'][pi])) + + if 'tofs_from_filtered_estimated_delays' in msg: + record['est_distance_calibrated_filtered'] = convert_ts_to_m(convert_logged_measurement(msg['tofs_from_filtered_estimated_delays'][pi])) + else: + record['est_distance_calibrated_filtered'] = None + + else: + record['mean_measurement'] = None + record['var_measurement'] = None + record['est_distance_uncalibrated'] = None + record['est_distance_factory'] = None + record['est_distance_calibrated'] = None + record['est_distance_calibrated_filtered'] = None + + yield record + + +def extract_delay_estimates(msg_iter, testbed, src_dev, ignore_pairs=[]): + rounds, estimations = extract_types(msg_iter, ['estimation']) + for r in rounds: + for d in testbed.devs: + if src_dev and d != src_dev: + continue # we skip as we do not have this data anyway! + + msg = estimations.get((d, r), None) + + if msg: + record = {} + record['delays_from_measurements'] = [] + record['delays_from_measurements_rounded'] = [] + record['factory_delays'] = [] + record['factory_delays_diff'] = [] + + for dm in msg['delays_from_measurements']: + record['delays_from_measurements'].append(convert_logged_measurement(dm)) + record['delays_from_measurements_rounded'].append(round(convert_logged_measurement(dm))) + + for (i,d) in enumerate(testbed.devs): + record['factory_delays'].append(testbed.factory_delays[d]-16450) + record['factory_delays_diff'].append(record['delays_from_measurements_rounded'][i]-record['factory_delays'][i]) + + M = create_inference_matrix(len(testbed.devs), ignored_pairs=ignore_pairs) + + measured = np.array([convert_logged_measurement(x) for x in msg['mean_measurements']]) + + actual = np.zeros(shape=round(len(testbed.devs)*(len(testbed.devs)-1)/2)) + + for (a, da) in enumerate(testbed.devs): + for (b, db) in enumerate(testbed.devs): + if a > b: + actual[pair_index(a,b)] = convert_m_to_ts(get_dist(testbed.dev_positions[da], testbed.dev_positions[db])) + + diffs = measured - actual + + record['python_delays'] = np.matmul(M, 2*diffs) + record['python_delays_rounded'] = [round(x) for x in record['python_delays']] + + record['python_delays_diff'] = record['python_delays_rounded']-np.array(record['factory_delays']) + record['mse'] = np.sqrt(np.mean(np.square(record['python_delays_diff']))) + record['mae'] = np.mean(np.abs(record['python_delays_diff'])) + + yield record + + +def gen_measurements_from_testbed_run(testbed, run, src_dev=None, include_dummy=False): + logfile = "data/{}/{}.log".format(testbed.name, run) + + with open(logfile) as f: + yield from extract_measurements(testbed.parse_messages_from_lines(f, src_dev=src_dev), testbed=testbed, src_dev=src_dev, include_dummy=include_dummy) + + +def gen_estimations_from_testbed_run(testbed, run, src_dev=None): + logfile = "data/{}/{}.log".format(testbed.name, run) + with open(logfile) as f: + yield from extract_estimations(testbed.parse_messages_from_lines(f, src_dev=src_dev), testbed=testbed, src_dev=src_dev) + +def gen_delay_estimates_from_testbed_run(testbed, run, src_dev=None, ignore_pairs=[]): + logfile = "data/{}/{}.log".format(testbed.name, run) + with open(logfile) as f: + yield from extract_delay_estimates(testbed.parse_messages_from_lines(f, src_dev=src_dev), testbed=testbed, src_dev=src_dev, ignore_pairs=ignore_pairs) + +from testbed import trento_a, trento_b, lille + +print("TRENTO A All Pairs") +res = list(gen_delay_estimates_from_testbed_run(trento_a, run='job_fixed', src_dev=None, ignore_pairs=[]))[0] +print(res['mae']) +trento_a_unfiltered = res + +print("TRENTO B All Pairs") +res = list(gen_delay_estimates_from_testbed_run(trento_b, run='job_fixed', src_dev=None, ignore_pairs=[]))[0] +print(res['mae']) + +print("LILLE All Pairs") +res = list(gen_delay_estimates_from_testbed_run(lille, run='job_fixed', src_dev=None, ignore_pairs=[]))[0] +print(res['mae']) + +print("TRENTO A Filtered") +res = list(gen_delay_estimates_from_testbed_run(trento_a, run='job_fixed', src_dev=None, ignore_pairs=[(6,3)]))[0] +print(res['mae']) +trento_a_filtered = res + +print("LILLE Filtered") +res = list(gen_delay_estimates_from_testbed_run(lille, run='job_fixed', src_dev=None, ignore_pairs=[(7,1), (4, 2)]))[0] +print(res['mae']) + + + +exp = trento_a_unfiltered + +for i in range(0, 7): + est = exp['delays_from_measurements_rounded'][i]*0.47 + fact = exp['factory_delays'][i]*0.47 + diff = est-fact + print("{} & {:2.2f} & {:2.2f} & {:2.2f} \\\\ \\hline".format(i+1, est, fact, diff)) + + + +#1 & XX & 10.34 & XX \\ \hline \ No newline at end of file diff --git a/scripts/sim.py b/scripts/sim.py new file mode 100644 index 0000000..79d8798 --- /dev/null +++ b/scripts/sim.py @@ -0,0 +1,509 @@ +import numpy as np +import pandas as pd +import matplotlib.pyplot as plt +import seaborn as sns +from base import pair_index + +NODES_POSITIONS = [ + (0,0), + (11,0), + (22,0), + (22,7.5), + (22,15), + (11,15), + (0,15), + (0,7.5) +] + +PASSIVE_NODE_POSITION=(5,5) + +# Speed of light in air +c_in_air = 299702547.236 +RESP_DELAY_S = 0.005 + +NODE_DRIFT_STD = 10.0/1000000.0 + +TX_DELAY_MEAN = 0.516e-09 +TX_DELAY_STD = 0.06e-09 +RX_DELAY_MEAN = TX_DELAY_MEAN +RX_DELAY_STD = TX_DELAY_STD +RX_NOISE_STD = 1.0e-09 + +# Enable perfect measurements but with drift! +# TX_DELAY_MEAN = 0 +# TX_DELAY_STD = 0.01e-09 +# RX_DELAY_MEAN = 0 +# RX_DELAY_STD = 0.01e-09 +# RX_NOISE_STD = 0 +# NODE_DRIFT_STD = 0.0 + + +import time + + +def dist(a, b): + return np.linalg.norm(np.array(NODES_POSITIONS[a]) - np.array(NODES_POSITIONS[b])) + +def tof(a, b): + return dist(a, b) / c_in_air + + +def tof_to_passive(a): + return np.linalg.norm(np.array(NODES_POSITIONS[a]) - np.array(PASSIVE_NODE_POSITION)) / c_in_air + + +def create_inference_matrix(n): + X = np.zeros((int(n * (n - 1) / 2), n)) + + row = 0 + for a in range(n): + for b in range(n): + if b < a: + X[row][a] = 1 + X[row][b] = 1 + row += 1 + + B = np.matmul(np.linalg.inv(np.matmul(np.transpose(X), X)), np.transpose(X)) + return B + +INF_MAT = create_inference_matrix(len(NODES_POSITIONS)) + + +def create_inference_matrix_rep(n, rep=1): + X = np.zeros((int(n * (n - 1) / 2)*rep, n)) + + row = 0 + for a in range(n): + for b in range(n): + if b < a: + for i in range(rep): + X[row][a] = 1 + X[row][b] = 1 + row += 1 + + B = np.matmul(np.linalg.inv(np.matmul(np.transpose(X), X)), np.transpose(X)) + return B + + + +def sim_exchange(num_total_exchanges): + n = len(NODES_POSITIONS) + # we fix the node drifts + node_drifts = np.random.normal(loc=1.0, scale=NODE_DRIFT_STD, size=n) + passive_node_drift = np.random.normal(loc=1.0, scale=NODE_DRIFT_STD, size=1) + + tx_delays = np.random.normal(loc=TX_DELAY_MEAN, scale=TX_DELAY_STD, size=n) + rx_delays = np.random.normal(loc=RX_DELAY_MEAN, scale=RX_DELAY_STD, size=n) + + rounds = [] + for i in range(num_total_exchanges): + round = [] + + for a in range(len(NODES_POSITIONS)): + for b in range(len(NODES_POSITIONS)): + if b < a: + # a initates the ranging + t = tof(a, b) + + a_actual_poll_tx = 0 + b_actual_poll_rx = a_actual_poll_tx + np.random.normal(loc=t, scale=RX_NOISE_STD) + b_actual_response_tx = b_actual_poll_rx + RESP_DELAY_S + a_actual_response_rx = b_actual_response_tx + np.random.normal(loc=t, scale=RX_NOISE_STD) + a_actual_final_tx = a_actual_response_rx + RESP_DELAY_S + b_actual_final_rx = a_actual_final_tx + np.random.normal(loc=t, scale=RX_NOISE_STD) + + # tx timestamps are skewed in a negative way -> i.e. increase the measured_rtt + a_delayed_poll_tx = a_actual_poll_tx - tx_delays[a] + b_delayed_response_tx = b_actual_response_tx - tx_delays[b] + a_delayed_final_tx = a_actual_final_tx - tx_delays[a] + + # rx timestamps are skewed in a positive way + b_delayed_poll_rx = b_actual_poll_rx + rx_delays[b] + a_delayed_response_rx = a_actual_response_rx + rx_delays[a] + b_delayed_final_rx = b_actual_final_rx + rx_delays[b] + + a_measured_round_undrifted = a_delayed_response_rx - a_delayed_poll_tx + b_measured_round_undrifted = b_delayed_final_rx - b_delayed_response_tx + a_measured_delay_undrifted = a_delayed_final_tx - a_delayed_response_rx + b_measured_delay_undrifted = b_delayed_response_tx - b_delayed_poll_rx + + # we compute times for TDoA using an additional passive node, note that we do not need delays here + p_actual_poll_rx = a_actual_poll_tx + np.random.normal(loc=tof_to_passive(a), scale=RX_NOISE_STD) + p_actual_response_rx = b_actual_response_tx + np.random.normal(loc=tof_to_passive(b), scale=RX_NOISE_STD) + p_actual_final_rx = a_actual_final_tx + np.random.normal(loc=tof_to_passive(a), scale=RX_NOISE_STD) + + round.append({ + "device_a": a, + "device_b": b, + "drift_a": node_drifts[a], + "drift_b": node_drifts[b], + "drift_p": passive_node_drift, + "round_a": a_measured_round_undrifted * node_drifts[a], + "delay_a": a_measured_delay_undrifted * node_drifts[a], + "round_b": b_measured_round_undrifted * node_drifts[b], + "delay_b": b_measured_delay_undrifted * node_drifts[b], + "passive_tdoa": (p_actual_response_rx-p_actual_poll_rx) * passive_node_drift, + "passive_overall": (p_actual_final_rx-p_actual_poll_rx) * passive_node_drift, + }) + + rounds.append(round) + return (node_drifts, tx_delays, rx_delays, rounds) + + +def calc_simple(ex, comb_delay=0.0): + (round_a, delay_a, round_b, delay_b) = (ex['round_a'], ex['delay_a'], ex['round_b'], ex['delay_b']) + relative_drift = (round_a+delay_a)/(round_b+delay_b) + return (round_a-delay_b*relative_drift-comb_delay) * 0.5 + + + + + + +#def calc_simple_2(ex, comb_delay=0.0): +# (round_a, delay_a, round_b, delay_b) = (ex['round_a'], ex['delay_a'], ex['round_b'], ex['delay_b']) +# #return ((round_b + delay_b)*round_a - delay_b * (round_a + delay_a) - (round_b + delay_b)*comb_delay) / (2.0*(round_b + delay_b)) +# return ((round_b * round_a - delay_b * delay_a) - (round_b + delay_b)*comb_delay) / (2.0*(round_b + delay_b)) + + +def calc_complex_tof(ex, comb_delay=0.0): + (round_a, delay_a, round_b, delay_b) = (ex['round_a'], ex['delay_a'], ex['round_b'], ex['delay_b']) + (a, x, b, y) = (ex['round_a'], ex['delay_a'], ex['round_b'], ex['delay_b']) + + #print ((a*b-x*y-(x+y)*c-c*c) / (x+y+a+b+2*c)-(round_a*round_b-delay_a*delay_b-(delay_a+delay_b)*comb_delay-comb_delay*comb_delay) / (delay_a+delay_b+round_a+round_b+2*comb_delay)) + return (round_a*round_b-delay_a*delay_b-(delay_a+delay_b)*comb_delay-comb_delay*comb_delay) / (delay_a+delay_b+round_a+round_b+2*comb_delay) + +def calc_complex_tof_d_comb(ex, comb_delay=0.0): + (a, x, b, y) = (ex['round_a'], ex['delay_a'], ex['round_b'], ex['delay_b']) + c = comb_delay + #d/dc((a b - x y - (x + y) c - c c)/(x + y + a + b + 2 c)) = + return (-(2*c + x + y)*(a + b + 2*c + x + y) - 2*a*b + 2*pow(c,2) + 2*c*(x + y) + 2*x*y) / pow(a + b + 2*c + x + y, 2) + + + +def calibrate_delays_gn(rounds, num_iterations=100): + actual = [] + + for a in range(len(NODES_POSITIONS)): + for b in range(len(NODES_POSITIONS)): + if b < a: + actual.append(tof(a, b)) + actual = np.array(actual) + + est_combined_delays = [] + + for a in range(len(NODES_POSITIONS)): + for b in range(len(NODES_POSITIONS)): + if b < a: + pi = pair_index(a,b) + est_combined_delay = RX_DELAY_MEAN+TX_DELAY_MEAN + exchanges = [] + for r in rounds: + exchanges.append(r[pi]) + + def compute_h(delay): + xs = [] + for e in exchanges: + xs.append(calc_complex_tof_d_comb(e, delay)) + return np.array(xs).transpose() + + def compute_T(delay): + xs = [] + for e in exchanges: + xs.append(calc_complex_tof(e, delay)) + return actual[pi] - np.array(xs) + + + for i in range(num_iterations): + h = compute_h(est_combined_delay) + T = compute_T(est_combined_delay) + + # we normalize h + h_normed = h / np.linalg.norm(h) + + #old_est_combined_delay = est_combined_delay + est_combined_delay = est_combined_delay + np.matmul(h_normed, T) * 0.1 # we adjust the learning rate + #est_combined_delay_new = est_combined_delay - np.mean(T) + #print(len(exchanges), old_est_combined_delay, est_combined_delay) + #print(old_est_combined_delay, np.matmul(h_normed, T), est_combined_delay) + + if abs(est_combined_delay) > RX_NOISE_STD*10.0: + print("GN might not converge!", est_combined_delay) + # print(a,b,i) + # print(h) + # print(h_normed) + # print(T) + # print(T.dtype, h.dtype) + # print("matmul_normed", np.matmul(h_normed, T)) + # print("matmul", np.matmul(h, T)) + # print("mean", np.sum(T)*h_normed[0]) + + #print(old_est_combined_delay, est_combined_delay, est_combined_delay_new) + est_combined_delays.append(est_combined_delay) + est_combined_delays = np.array(est_combined_delays) + + delays = np.matmul(INF_MAT, np.transpose(est_combined_delays)) + + return np.transpose(delays) + + + + +def calibrate_delays_our_approach(rounds, device_drifts, source_device=0): + + sums = [] + actual = [] + + for a in range(len(NODES_POSITIONS)): + for b in range(len(NODES_POSITIONS)): + if b < a: + sums.append(0) + actual.append(tof(a,b)) + + individual_measurements = [] + + for a in range(len(NODES_POSITIONS)): + for b in range(len(NODES_POSITIONS)): + if b < a: + pi = pair_index(a,b) + for r in rounds: + ex = r[pi] + tof_calc_in_time_a = calc_simple(ex) + tof_calc_in_source_time = device_drifts[source_device] * tof_calc_in_time_a / device_drifts[a] + sums[pi] += tof_calc_in_source_time + individual_measurements.append(tof_calc_in_source_time) + + sums = np.array(sums) + actual = np.array(actual) + + sums /= len(rounds) + + diffs = sums - actual + diffs *= 2.0 + + delays = np.matmul(INF_MAT, np.transpose(diffs)) + + return np.transpose(delays) # list of antenna delays + + +def calibrate_delays_our_approach_via_source_device(rounds, source_device=0): + sums = [] + actual = [] + assert (source_device==0) + for a in range(len(NODES_POSITIONS)): + for b in range(len(NODES_POSITIONS)): + if b < a: + sums.append(0) + actual.append(tof(a, b)) + + individual_diffs = [] + for a in range(len(NODES_POSITIONS)): + for b in range(len(NODES_POSITIONS)): + if b < a: + pi = pair_index(a, b) + for r in rounds: + ex_a_b = r[pi] + + # we calculate the simple one from a to b + t = calc_simple(ex_a_b, comb_delay=0.0) + + if source_device != a: + ex_s_a = r[pair_index(source_device, a)] + # then we convert it to the source time + # TODO: we only support source device 0 for now (because of the ranging sides...) + (round_s, delay_s, round_a, delay_a) = (ex_s_a['round_a'], ex_s_a['delay_a'], ex_s_a['round_b'], ex_s_a['delay_b']) + t = (t * (round_s + delay_s)) / (round_a + delay_a) + + sums[pi] += t + #individual_diffs.append((t-tof(a, b))*2.0) + + sums = np.array(sums) + actual = np.array(actual) + + sums /= len(rounds) + + diffs = sums - actual + diffs *= 2.0 + + delays = np.matmul(INF_MAT, np.transpose(diffs)) + + # print("TEST", len(rounds)) + # print(delays) + # + # individual_diffs = np.array(individual_diffs) + # delays = np.matmul(create_inference_matrix_rep(len(NODES_POSITIONS), len(rounds)), + # np.transpose(individual_diffs)) + # print(delays) + + return np.transpose(delays) # list of antenna delays + + + +def calibrate_delays_tdoa(rounds): + # we use the last device since it initiates all rangings + # we will calculate the master node independently + m = len(NODES_POSITIONS) - 1 + + + delays = [] + + # Calibrate devices independently + for a in range(len(NODES_POSITIONS)-1): + pi = pair_index(m, a) + + delays_a = [] + delays_m = [] + + for r in rounds: + ex_m_a = r[pi] + + assert ex_m_a['device_a'] == m + assert ex_m_a['device_b'] == a + + (round_m, delay_m, round_a, delay_a) = (ex_m_a['round_a'], ex_m_a['delay_a'], ex_m_a['round_b'], ex_m_a['delay_b']) + (passive_tdoa, passive_overall) = (ex_m_a['passive_tdoa'], ex_m_a['passive_overall']) + + + #tof(m, a), tof_to_passive(m), tof_to_passive(a) + + est_delay_m = round_m - (passive_tdoa * ((round_m+delay_m)/passive_overall)) + tof_to_passive(a) - tof(m, a) - tof_to_passive(m) + est_delay_a = (passive_tdoa * (round_m+delay_m)/passive_overall) - delay_a * ((round_m+delay_m)/(round_a+delay_a)) - tof_to_passive(a) - tof(m, a) + tof_to_passive(m) + + delays_a.append(est_delay_a) + delays_m.append(est_delay_m) + + delays.append(np.mean(delays_a)) + + # we add the master delay in the end + if a == m - 1: + delays.append(np.mean(delays_m)) + return np.transpose(delays) # list of antenna delays + +# we get a list of dictionaries, containing the measurement pairs + +xs = [16, 64, 256, 1024] + + + + + # (node_drifts, tx_delays, rx_delays, rounds) = exchanges + # + # compl_errors = [] + # simple_errors = [] + # for r in rounds: + # i = 0 + # for a in range(len(NODES_POSITIONS)): + # for b in range(len(NODES_POSITIONS)): + # if b < a: + # actual_dist = tof(a,b) + # compl = calc_complex_tof(r[i]) + # simple = calc_simple(r[i]) + # compl_errors.append(compl-actual_dist) + # simple_errors.append(simple-actual_dist) + # i += 1 + # + # compl_errors = np.array(compl_errors) + # simple_errors = np.array(simple_errors) + # + # compl_error_rmse = np.sqrt(((compl_errors) ** 2).mean()) + # simple_error_rmse = np.sqrt(((simple_errors) ** 2).mean()) + # print(compl_errors.mean(), simple_errors.mean()) + # print(compl_error_rmse, simple_error_rmse) + # exit() + + +def get_sim_data_rows(xs=[16, 64, 256, 1024], num_repetitiosn=1): + + max_exchanges = max(xs) + repetitions = [] + for i in range(num_repetitiosn): + exchanges = sim_exchange(num_total_exchanges=max_exchanges) + repetitions.append(exchanges) + + data_rows = [] + + for x in xs: + + rmses_tdoa = [] + rmses_gn = [] + rmses_our = [] + tdoa_times = [] + gn_times = [] + our_times = [] + + for i in range(num_repetitiosn): + + (node_drifts, tx_delays, rx_delays, rounds) = repetitions[i] + + rounds = rounds[0:x] + + actual_delays = tx_delays + rx_delays + + ts = time.time() + gn_est_delays = calibrate_delays_gn(rounds=rounds) + te = time.time() + gn_times.append(te-ts) + + ts = time.time() + tdoa_est_delays = calibrate_delays_tdoa(rounds=rounds) + te = time.time() + tdoa_times.append(te - ts) + + ts = time.time() + our_est_delays = calibrate_delays_our_approach_via_source_device(rounds=rounds, source_device = 0) + te = time.time() + our_times.append(te - ts) + + tdoa_err = np.sqrt(((tdoa_est_delays-actual_delays)** 2).mean()) + gn_err = np.sqrt(((gn_est_delays-actual_delays)** 2).mean()) + our_err = np.sqrt(((our_est_delays-actual_delays)** 2).mean()) + + rmses_tdoa.append(tdoa_err) + rmses_gn.append(gn_err) + rmses_our.append(our_err) + + rmses_tdoa = np.array(rmses_tdoa) + rmses_gn = np.array(rmses_gn) + rmses_our = np.array(rmses_our) + + gn_times = np.array(gn_times) + our_times = np.array(our_times) + + + data_rows.append({ + 'num_measurements': x, + 'tdoa_mean': rmses_tdoa.mean() * c_in_air * 100, + 'tdoa_std': rmses_tdoa.std() * c_in_air * 100, + 'gn_mean': rmses_gn.mean()* c_in_air*100, + 'gn_std': rmses_gn.std()* c_in_air*100, + 'our_mean': rmses_our.mean() * c_in_air * 100, + 'our_std': rmses_our.std() * c_in_air * 100, + 'gn_time_mean': gn_times.mean(), + 'our_time_mean': our_times.mean(), + 'speedup': gn_times.mean() / our_times.mean(), + 'speedup_err': 0, + }) + return data_rows + + + +# print(x, rmses_gn.mean() * c_in_air*100, rmses_our.mean()* c_in_air*100, rmses_gn.std() * c_in_air*100, rmses_our.std() * c_in_air*100) +# #print(rmses_tdoa) +# #print(rmses_gn) +# +# +# +# #print(exchanges) +# +# first = rounds[0][0] +# +# actual = dist(1, 0) +# simple = calc_simple(first)*c_in_air +# com = calc_complex_tof(first)*c_in_air +# +# #print(actual, simple, com) +# +# actual = dist(1, 0) +# simple = calc_simple(first, comb_delay=rx_delays[0]+rx_delays[1]+tx_delays[0]+tx_delays[1]) * c_in_air +# com = calc_complex_tof(first, comb_delay=rx_delays[0]+rx_delays[1]+tx_delays[0]+tx_delays[1]) * c_in_air +# #print(actual, simple, com) diff --git a/scripts/testbed/lille.py b/scripts/testbed/lille.py new file mode 100644 index 0000000..b594125 --- /dev/null +++ b/scripts/testbed/lille.py @@ -0,0 +1,164 @@ + +name = 'lille' + +devs = [ + 'dwm1001-1', + 'dwm1001-2', + 'dwm1001-3', + 'dwm1001-4', + 'dwm1001-5', + 'dwm1001-6', + 'dwm1001-7', + 'dwm1001-8', + 'dwm1001-9', + 'dwm1001-10', + 'dwm1001-11', + 'dwm1001-12', + 'dwm1001-13', + 'dwm1001-14' +] + +dev_ids = { + 'dwm1001-1': '0x6b93', + 'dwm1001-2': '0xf1c3', + 'dwm1001-3': '0xc240', + 'dwm1001-4': '0x013f', + 'dwm1001-5': '0xb227', + 'dwm1001-6': '0x033b', + 'dwm1001-7': '0xf524', + 'dwm1001-8': '0x37b2', + 'dwm1001-9': '0x15ef', + 'dwm1001-10': '0x7e02', + 'dwm1001-11': '0xad36', + 'dwm1001-12': '0x3598', + 'dwm1001-13': '0x47e0', + 'dwm1001-14': '0x0e92' +} +# +# 0x6b93, // c1 05 0a d2 ca 32 +# 0xf1c3, // 2c 8e 38 0e 2c 2f +# 0xc240, // a6 e9 82 fe b9 f2 +# 0x013f, // b2 04 f6 33 7c 79 +# 0xb227, // e1 bc c5 19 92 66 +# 0x033b, // 5e 2a 24 f7 5d 92 +# 0xf524, // d8 8d 45 48 12 b9 +# 0x37b2, // 12 6a 1d 03 69 47 +# 0x15ef, // 2e fb cb ec 8d af +# 0x7e02, // c2 7f f2 eb 6d e0 +# 0xad36, // fa 0e a0 b9 19 b7 +# 0x3598, // 3c 33 73 36 cb 47 +# 0x47e0, // c9 65 a0 9b 5b 9f +# 0x0e92 // 06 79 a6 59 fe 98 + +dev_positions = { + 'dwm1001-1': (0.26, 23.31, 7.55), + 'dwm1001-2': ( 0.26, 24.51, 8.96), + 'dwm1001-3': (0.26, 26.91, 8.96), + 'dwm1001-4': ( 0.26, 28.11, 7.55), + 'dwm1001-5': (1.1, 25.11, 9.51), + 'dwm1001-6': (1.1, 27.51, 9.51), + 'dwm1001-7': (3.5, 25.11, 9.51), + 'dwm1001-8': (3.5, 27.51, 9.51), + 'dwm1001-9': (5.9, 26.31, 9.51), + 'dwm1001-10': (7.1, 25.11, 9.51), + 'dwm1001-11': (7.1, 27.51, 9.51), + 'dwm1001-12': (9.39, 24.51,7.52), + 'dwm1001-13': (9.39, 25.71, 9.22), + 'dwm1001-14': ( 9.39, 26.91,7.52) +} + +factory_delays = { + 'dwm1001-1': 16450+8, + 'dwm1001-2': 16450+9, + 'dwm1001-3': 16450+7, + 'dwm1001-4': 16450+22, + 'dwm1001-5': 16450+11, + 'dwm1001-6': 16450+12, + 'dwm1001-7': 16450+7, + 'dwm1001-8': 16450+22, + 'dwm1001-9': 16450+22, + 'dwm1001-10': 16450+22, + 'dwm1001-11': 16450+-14, + 'dwm1001-12': 16450+-9, + 'dwm1001-13': 16450+9, + 'dwm1001-14': 16450+-13 +} + +def parse_messages_from_lines(line_it, src_dev=None): + import json + + dev_set = set(devs) + + if src_dev is not None: + dev_set = {src_dev} + + for line in line_it: + if line.strip() == "": + continue + try: + # 1670158055.359572;dwm1001-2;{ "type": "rx", "carrierintegrator": 1434, "rssi": -81, "tx": {"addr": "0xBC48", "sn": 0, "ts": 186691872256}, "rx": [{"addr": "0x471A", "sn": 0, "ts": 185512854420}]} + log_ts, dev, json_str = line.split(';', 3) + log_ts = float(log_ts) + + if dev not in dev_set: + continue + + try: + msg = json.loads(json_str) + msg['_log_ts'] = log_ts + yield (log_ts, dev, msg) + except json.decoder.JSONDecodeError: + #print(json_str) + pass + except ValueError: + pass + + +import matplotlib as mpl +import matplotlib.pyplot as plt +from matplotlib.patches import Rectangle +from matplotlib.offsetbox import (OffsetImage, AnnotationBbox) + +import numpy as np +from PIL import Image + + +def draw_layout(plt): + + + + lineargs = { + "color": "black", + #"alpha": 0.5, + "lw": 1.5, + "zorder": -1, + } + + rectargs = { + "facecolor": "white", + "edgecolor": "black", + #"alpha": 0.5, + "lw": 1.5, + "zorder": -1, + } + + pillar_size = 0.25 + # npimage = np.flip(np.asarray(Image.open('img/lille.png')), axis=0) + # scalingx = 0.0325 + # scalingy = 0.027 + # tx = dev_positions['dwm1001-1'][0]-0.02 + # ty = dev_positions['dwm1001-1'][1]-0.02 + + # the axes might be wrong here! + # plt.gca().imshow(npimage, origin="upper", extent=(tx, tx + npimage.shape[0]*scalingx, ty, ty + npimage.shape[1]*scalingy), zorder=-1) + + plt.plot([dev_positions['dwm1001-1'][0], dev_positions['dwm1001-1'][0]], [22, 32], **lineargs) + plt.plot([dev_positions['dwm1001-12'][0], dev_positions['dwm1001-12'][0]], [22, dev_positions['dwm1001-14'][1]+0.25], **lineargs) + plt.plot([dev_positions['dwm1001-14'][0], 12.0], [dev_positions['dwm1001-14'][1]+0.25, dev_positions['dwm1001-14'][1]+0.25], **lineargs) + + plt.gca().add_patch(Rectangle((dev_positions['dwm1001-1'][0]-pillar_size*0.5, dev_positions['dwm1001-1'][1]-pillar_size), pillar_size, pillar_size, **rectargs)) + plt.gca().add_patch(Rectangle((0.375-pillar_size, 28.437-pillar_size), pillar_size, pillar_size, **rectargs)) + plt.gca().add_patch(Rectangle((5.50, 28.17), pillar_size, pillar_size, **rectargs)) + plt.gca().add_patch(Rectangle((5.50, dev_positions['dwm1001-1'][1]-pillar_size), pillar_size, pillar_size, **rectargs)) + + diff --git a/scripts/testbed/trento_a.py b/scripts/testbed/trento_a.py new file mode 100644 index 0000000..aecb1a7 --- /dev/null +++ b/scripts/testbed/trento_a.py @@ -0,0 +1,157 @@ + + +name = 'trento_a' + +devs = [ + 'dwm1001.1', + 'dwm1001.2', + 'dwm1001.3', + 'dwm1001.4', + 'dwm1001.5', + 'dwm1001.6', + 'dwm1001.7' +] + +dev_ids = { + 'dwm1001.1': '0x5b9a', + 'dwm1001.2': '0x56d4', + 'dwm1001.3': '0x0345', + 'dwm1001.4': '0x9535', + 'dwm1001.5': '0x87e8', + 'dwm1001.6': '0xa7d8', + 'dwm1001.7': '0x24f1', +} + +dev_positions = { + 'dwm1001.1': (76, 3.97, 0.0), + 'dwm1001.2': (72.74, 6.6, 0.0), + 'dwm1001.3': (75.97, 6.86, 0.0), + 'dwm1001.4': (78.98, 6.85, 0.0), + 'dwm1001.5': (78.89, 0.44, 0.0), + 'dwm1001.6': (75.89, 0.43, 0.0), + 'dwm1001.7': (72.91, 0.37, 0.0) +} + +factory_delays = { + 'dwm1001.1': 16472, + 'dwm1001.2': 16459, + 'dwm1001.3': 16472, + 'dwm1001.4': 16472, + 'dwm1001.5': 16458, + 'dwm1001.6': 16460, + 'dwm1001.7': 16460 +} + +def parse_messages_from_lines(line_it, src_dev=None): + import json + + dev_set = set(devs) + + + if src_dev is not None: + dev_set = {src_dev} + + for line in line_it: + if line.strip() == "": + continue + try: + + #[2023-02-03 17:48:15,819] INFO:dwm1001.169: 169.dwm1001 < b'{"type": "drift_estimation", "round": 1, "durations": [[0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [1, 1], [0, 0], [0, 0], [0, 0], [0, 0]]}' + + ts_str = line[1:24] + + dev = line[24:].split(':', 2)[1] + + if dev not in dev_set: + continue + + json_str = line[24:].split(" < b\'", 2)[1][:-2] + log_ts = ts_str + + try: + msg = json.loads(json_str) + msg['_log_ts'] = ts_str + yield (log_ts, dev, msg) + except json.decoder.JSONDecodeError: + #print(json_str) + pass + except ValueError: + pass + + +import matplotlib as mpl +import matplotlib.pyplot as plt +from matplotlib.patches import Rectangle +from matplotlib.offsetbox import (OffsetImage, AnnotationBbox) + +import numpy as np +from PIL import Image + +def draw_layout(plt): + + + + lineargs = { + "color": "black", + #"alpha": 0.5, + "lw": 1.5, + "zorder": -1, + } + + rectargs = { + "facecolor": "white", + "edgecolor": "black", + #"alpha": 0.5, + "lw": 1.5, + "zorder": -1, + } + + # npimage = np.flip(np.asarray(Image.open('img/trento_a.png')), axis=0) + # scalingx = 0.01295 + # scalingy = 0.0134 + # tx = 72-0.561 + # ty = -1.5+1.024-0.535 + #plt.gca().imshow(npimage, origin="lower", extent=(tx, tx + npimage.shape[0]*scalingx, ty, ty + npimage.shape[1]*scalingy), zorder=-1) + + + lines = [ + ((70.0, 2.45), (72.16, 2.45)), + ((72.16, 2.45), (72.16, 8.5)), + ((72.16, 6.37), (70.0, 6.37)), + ((72.16, 7.41), (70.0, 7.41)), + ((74.72, 7.41), (74.72, 8.5)), + ((74.72, 7.41), (78.27, 7.41)), + ((78.27, 7.41), (78.27, 8.5)), + ((80.0, 8.5), (80.0, 6.96)), + ((80.0, 5.69), (80.0, 3.82)), + ((80.0, 2.578), (80.0, 1.119)), + ((80.0, -0.07), (80.0, -0.65)), + ((80.0, -0.65), (82.0, -0.65)), + ((80.0, 1.86), (82.0, 1.86)), + ((80.0, 4.52), (82.0, 4.52)), + ((80.0, 7.66), (82.0, 7.66)), + ((70.0, -0.07), (72.43, -0.07)), + ((74.48, -0.07), (74.72, -0.07)), + ((74.72, -0.07), (74.72, -1.5)), + ] + + rects = [ + ((71.73, 8.0), (72.16, 7.41)), + ((74.72, 8.0), (75.35, 7.41)), + ((74.72, -0.07), (75.35, -0.7)) + ] + + + for (f, t) in lines: + plt.plot([f[0], t[0]], [f[1], t[1]], **lineargs) + + for (ul, lr) in rects: + width = lr[0]-ul[0] + height = lr[1]-ul[1] + plt.gca().add_patch( + Rectangle((ul[0], ul[1]), width, height, **rectargs) + ) + + + + diff --git a/scripts/testbed/trento_b.py b/scripts/testbed/trento_b.py new file mode 100644 index 0000000..8d90c99 --- /dev/null +++ b/scripts/testbed/trento_b.py @@ -0,0 +1,169 @@ + +name = 'trento_b' + +devs = [ + 'dwm1001.160', + 'dwm1001.161', + 'dwm1001.162', + 'dwm1001.163', + 'dwm1001.164', + 'dwm1001.165', + 'dwm1001.166', + 'dwm1001.167', + 'dwm1001.168', + 'dwm1001.169', + 'dwm1001.170', + 'dwm1001.171', + 'dwm1001.172', + 'dwm1001.173' +] + +dev_ids = { + 'dwm1001.160': '0x330b', + 'dwm1001.161': '0xbee3', + 'dwm1001.162': '0x17e3', + 'dwm1001.163': '0x427e', + 'dwm1001.164': '0xdd4f', + 'dwm1001.165': '0x292b', + 'dwm1001.166': '0x69f6', + 'dwm1001.167': '0x3843', + 'dwm1001.168': '0x3aea', + 'dwm1001.169': '0x2df2', + 'dwm1001.170': '0x3392', + 'dwm1001.171': '0x5418', + 'dwm1001.172': '0x869a', + 'dwm1001.173': '0x1582' +} + +dev_positions = { + 'dwm1001.160': (130.29, 4.7, 0.0), + 'dwm1001.161': (130.29, 6.56, 0.0), + 'dwm1001.162': (126.95, 6.84, 0.0), + 'dwm1001.163': (124.52, 6.82, 0.0), + 'dwm1001.164': (122.79, 6.8, 0.0), + 'dwm1001.165': (120.99, 6.78, 0.0), + 'dwm1001.166': (120.07, 4.64, 0.0), + 'dwm1001.167': (120.07, 2.28, 0.0), + 'dwm1001.168': (120.98, 0.19, 0.0), + 'dwm1001.169': (122.76, 0.2, 0.0), + 'dwm1001.170': (124.62, 0.19, 0.0), + 'dwm1001.171': (127.05, 0.18, 0.0), + 'dwm1001.172': (129.99, 0.19, 0.0), + 'dwm1001.173': (130.27, 2.31, 0.0) +} + +factory_delays = { + 'dwm1001.160': 16458, + 'dwm1001.161': 16460, + 'dwm1001.162': 16472, + 'dwm1001.163': 16481, + 'dwm1001.164': 16459, + 'dwm1001.165': 16472, + 'dwm1001.166': 16460, + 'dwm1001.167': 16472, + 'dwm1001.168': 16461, + 'dwm1001.169': 16472, + 'dwm1001.170': 16458, + 'dwm1001.171': 16472, + 'dwm1001.172': 16472, + 'dwm1001.173': 16472 +} + + +def parse_messages_from_lines(line_it, src_dev=None): + import json + + dev_set = set(devs) + + if src_dev is not None: + dev_set = {src_dev} + + for line in line_it: + + + if line.strip() == "": + continue + try: + + #[2023-02-03 17:48:15,819] INFO:dwm1001.169: 169.dwm1001 < b'{"type": "drift_estimation", "round": 1, "durations": [[0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [1, 1], [0, 0], [0, 0], [0, 0], [0, 0]]}' + + ts_str = line[1:24] + + dev = line[24:].split(':', 2)[1] + + if dev not in dev_set: + continue + + json_str = line[24:].split(" < b\'", 2)[1][:-2] + log_ts = ts_str + + try: + msg = json.loads(json_str) + msg['_log_ts'] = ts_str + yield (log_ts, dev, msg) + except json.decoder.JSONDecodeError: + #print(json_str) + pass + except ValueError: + pass + + + + +import matplotlib as mpl +import matplotlib.pyplot as plt +from matplotlib.patches import Rectangle +from matplotlib.offsetbox import (OffsetImage, AnnotationBbox) + +import numpy as np +from PIL import Image + +def draw_layout(plt): + + lineargs = { + "color": "black", + #"alpha": 0.5, + "lw": 1.5, + "zorder": -1, + } + + rectargs = { + "facecolor": "white", + "edgecolor": "black", + #"alpha": 0.5, + "lw": 1.5, + "zorder": -1, + } + + # npimage = np.flip(np.asarray(Image.open('img/trento_b.png')), axis=0) + # scalingx = 0.013 + # scalingy = scalingx + # tx = 119.11-0.561-0.824 + # ty = -0.535+0.35-0.09 + #plt.gca().imshow(npimage, origin="lower", extent=(tx, tx + npimage.shape[1]*scalingx, ty, ty + npimage.shape[0]*scalingy), zorder=-1) + + + lines = [ + ((119.647, 7.19), (130.69, 7.19)), + ((130.69, 7.19), (130.69, 4.09)), + ((130.69, 4.7), (132.0, 4.7)), + ((130.69, 2.87), (130.69, -0.3)), + ((130.69, 2.24), (132.0, 2.24)), + ((130.69, -0.3), (119.647, -0.3)) + ] + + rects = [ + ((119.0, 0.0), (119.647, -0.55)), + ((119.0, 7.5), (119.647, 6.947)) + ] + + + for (f, t) in lines: + plt.plot([f[0], t[0]], [f[1], t[1]], **lineargs) + + for (ul, lr) in rects: + width = lr[0]-ul[0] + height = lr[1]-ul[1] + plt.gca().add_patch( + Rectangle((ul[0], ul[1]), width, height, **rectargs) + ) \ No newline at end of file diff --git a/scripts/testbed_to_c_vals.py b/scripts/testbed_to_c_vals.py new file mode 100644 index 0000000..6f70a88 --- /dev/null +++ b/scripts/testbed_to_c_vals.py @@ -0,0 +1,122 @@ +from base import get_dist + +import testbed.lille as lille +import testbed.trento_a as trento_a +import testbed.trento_b as trento_b + +import sys +import numpy as np + +DEFAULT_DELAY = 16450 + +TESTBEDS = [ + lille, trento_a, trento_b +] + + +def create_inference_matrix(n, ignored_pairs=None): + X = np.zeros((int(n * (n - 1) / 2), n)) + + row = 0 + for a in range(n): + for b in range(n): + if b < a: + if ignored_pairs and ((a,b) in ignored_pairs or (b,a) in ignored_pairs): + pass + else: + X[row][a] = 1 + X[row][b] = 1 + row += 1 + B = np.matmul(np.linalg.inv(np.matmul(np.transpose(X), X)), np.transpose(X)) + return B + +def print_est_matrix(M): + print("{") + for r in range(M.shape[0]): + s = "{" + + for c in range(M.shape[1]): + s += str(M[r][c]) + + if c < M.shape[1] - 1: + s += ", " + s += "}" + if r < M.shape[0] - 1: + s += ", " + print(s) + print("}") + + + + +def print_testbed(t): + n = len(t.dev_positions.keys()) + + print("// ----------------------") + print("// Definition of Testbed {}".format(t.name)) + + print("#if NUM_NODES>{}".format(n)) + print("#error Testbed {} only has {} nodes".format(t.name.upper(), n)) + print("#elif NUM_NODES<{}".format(n)) + print("#warning Testbed {} has more nodes available ({} in total)".format(t.name.upper(), n)) + print("#endif") + + print("uint16_t node_ids[NUM_NODES] = {") + for d in t.devs: + print("\t{}, // {}".format(t.dev_ids[d], d)) + print("};") + + print("int16_t node_factory_antenna_delay_offsets[NUM_NODES] = {") + for d in t.devs: + print("\t{}, // {}".format(t.factory_delays[d] - DEFAULT_DELAY, d)) + print("};") + + print("float32_t node_distances[NUM_PAIRS] = {") + + for (a, da) in enumerate(t.devs): + for (b, db) in enumerate(t.devs): + if b < a: + print("\t {}, // {} to {}".format(round(get_dist(t.dev_positions[da], t.dev_positions[db]), 4), da, db)) + print("};") + + + # print('//TODO: we might want to enable the robust estimation all the time?') + # print('#if 1') + # print("float32_t estimation_mat_full[NUM_NODES][(NUM_PAIRS/2)] =") + # print_est_matrix(create_inference_matrix(len(t.devs))) + # print(";") + # print('#else') + # print("float32_t estimation_mat_robus[NUM_NODES-1][(NUM_NODES-1)*(NUM_NODES-2)/2)] =") + # print_est_matrix(create_inference_matrix(len(t.devs)-1)) + # print(";") + # print('#endif') + + + + + +def print_all_testbeds(): + for (i, t) in enumerate(TESTBEDS): + if i == 0: + print("#if TESTBED==TESTBED_{}".format(t.name.upper())) + else: + print("#elif TESTBED==TESTBED_{}".format(t.name.upper())) + print_testbed(t) + print("#endif") + + + +if __name__ == "__main__": + + if len(sys.argv) > 1: + found = False + for t in TESTBEDS: + if t.name == sys.argv[1]: + print("#include \"nodes.h\"") + print_testbed(t) + found = True + if not found: + print("Could not find testbed {}".format(sys.argv[1])) + else: + print("#include \"nodes.h\"") + print_all_testbeds() \ No newline at end of file diff --git a/scripts/utility.py b/scripts/utility.py new file mode 100644 index 0000000..be59c25 --- /dev/null +++ b/scripts/utility.py @@ -0,0 +1,55 @@ +import re +import gzip +import json +import os +import numpy as np + +# https://stackoverflow.com/a/46801075/6669161 +def slugify(s): + if isinstance(s, list) or isinstance(s, tuple): + s = " ".join(slugify(x) for x in s) + s = str(s).strip().replace(' ', '_') + return re.sub(r'(?u)[^-\w.]', '', s) + +def load_env_config(): + import dotenv + return { + **dotenv.dotenv_values(".env"), # load shared development variables + **dotenv.dotenv_values(".env.local"), # load sensitive variables + **os.environ, # override loaded values with environment variables + } + +# Source: https://github.com/mpld3/mpld3/issues/434#issuecomment-340255689 +class NumpyEncoder(json.JSONEncoder): + """ Special json encoder for numpy types """ + def default(self, obj): + if isinstance(obj, (np.int_, np.intc, np.intp, np.int8, + np.int16, np.int32, np.int64, np.uint8, + np.uint16, np.uint32, np.uint64)): + return int(obj) + elif isinstance(obj, (np.float_, np.float16, np.float32, + np.float64)): + return float(obj) + elif isinstance(obj,(np.ndarray,)): + return obj.tolist() + return json.JSONEncoder.default(self, obj) + + +export_cache_dir = None + +def cached(id, proc_cb=None): + if export_cache_dir: + cache_file = os.path.join(export_cache_dir, slugify(id) + '.json.gz') + if not os.path.isfile(cache_file): + data = proc_cb() + with gzip.open(cache_file, 'wt', encoding='UTF-8') as zipfile: + json.dump(data, zipfile, cls=NumpyEncoder) + with gzip.open(cache_file, 'rt', encoding='UTF-8') as json_file: + return json.load(json_file) + else: + return proc_cb() + +def init_cache(path): + global export_cache_dir + export_cache_dir = path + os.makedirs(export_cache_dir, exist_ok=True) diff --git a/src/estimation.c b/src/estimation.c new file mode 100644 index 0000000..1271a9f --- /dev/null +++ b/src/estimation.c @@ -0,0 +1,667 @@ + +#include +#include +#include +#include +#include + +#include "estimation.h" +#include "nodes.h" +#include "uart.h" +#include "measurements.h" + +LOG_MODULE_REGISTER(estimation); + +#if 1 + typedef float16_t matrix_entry_t; + #define MATRIX arm_matrix_instance_f16 + #define OP_MULT arm_mat_mult_f16 + #define OP_INV arm_mat_inverse_f16 + #define OP_TRANS arm_mat_trans_f16 + #define OP_MEAN arm_mean_f16 +#else + typedef float32_t matrix_entry_t; + #define MATRIX arm_matrix_instance_f32 + #define OP_MULT arm_mat_mult_f32 + #define OP_INV arm_mat_inverse_f32 + #define OP_TRANS arm_mat_trans_f32 + #define OP_MEAN arm_mean_f32 +#endif + + + +#define MAX_SD_IN_M (0.1) +#define MAX_SD_IN_UWB_TS (MAX_SD_IN_M/SPEED_OF_LIGHT_M_PER_UWB_TU) + + +#define MAX_VAR_IN_M (MAX_SD_IN_UWB_TS*MAX_SD_IN_UWB_TS) + + + + +// the maximum of supported nodes for estimation (limited by memory sadly) +// TODO: They use up a LOT of memory! We could later allocate them dynamically (if we would need that extra space!) + +void print_matrix(MATRIX *m) { +return; + char buf[32]; + for (int r = 0; r < m->numRows; r++) { + for (int c = 0; c < m->numCols; c++) { + int32_t val = m->pData[r*m->numCols + c]; + snprintf(buf, sizeof(buf), "%d ", val); + uart_out(buf); + } + uart_out("\n"); + } +} + + +static matrix_entry_t matrix_data_a[EST_MAX_INPUTS * EST_MAX_INPUTS]; +static matrix_entry_t matrix_data_b[EST_MAX_PARAMS * EST_MAX_INPUTS]; +static matrix_entry_t matrix_data_c[EST_MAX_INPUTS * EST_MAX_INPUTS]; + +static void estimate( + uint8_t num_nodes, + float32_t delays_out[EST_MAX_NODES], + float32_t tofs_out[EST_MAX_PAIRS], + float32_t mean_measurements[EST_MAX_PAIRS], + float32_t var_measurements[EST_MAX_PAIRS], + bool delay_known[EST_MAX_NODES], + float32_t known_delays[EST_MAX_NODES], + bool tof_known[EST_MAX_PAIRS], + float32_t known_tofs[EST_MAX_PAIRS], + float32_t max_pair_variance +) { + + if (num_nodes > EST_MAX_NODES) { + LOG_ERR("Num Nodes too high!"); + return; + } + + int ret = 0; + + uint32_t num_pairs = PAIRS(num_nodes); // we use the actual number of input pairs + uint32_t num_input_rows = PAIRS(num_nodes); // we use the actual number of input pairs + uint32_t num_input_cols = 0; + + // we add all unknown delays and distances as columns + for(int i = 0; i < num_nodes; i++) { + if (!delay_known[i]) { + num_input_cols++; + } + } + + if (num_input_cols > EST_MAX_NODES) { + LOG_ERR("Inference of TOFs currently not supported! Only antenna delays as of now"); + return; + } + + + for(int i = 0; i < num_pairs; i++) { + if (!tof_known[i]) { + num_input_cols++; + } + // TDDO: We could also filter by variance here + } + + // make sure that matrices are empty! we only need to reset matrix A since the others are overriden anyway + // this zero matrix a + memset(&matrix_data_a, 0, sizeof(matrix_data_a)); + + MATRIX mat_a; + MATRIX mat_b; + MATRIX mat_c; + + // assign memory regions + { + mat_a.pData = matrix_data_a; //malloc(EST_MAX_PARAMS * EST_MAX_PARAMS * sizeof(float16_t)); + mat_b.pData = matrix_data_b; //malloc(EST_MAX_PARAMS * EST_MAX_PARAMS * sizeof(float16_t)); + mat_c.pData = matrix_data_c; //malloc(EST_MAX_PARAMS * EST_MAX_PARAMS * sizeof(float16_t)); + + // TODO: This cannot happen as we statically defined them. +// if (mat_a.pData == NULL || mat_b.pData == NULL || mat_c.pData == NULL ) { +// LOG_INF("Allocation failed..."); +// return false; +// } + } + + // Store X in mat_a + { + // TODO: we do not these this many rows usually! + mat_a.numRows = num_input_rows; + mat_a.numCols = num_input_cols; + + // as the parameter indices + + int param_index = 0; + + // first add all the delays + for(int a = 0; a < num_nodes; a++) { + if (delay_known[a]) { + continue; // this one is not relevant here + } + + // we have to loop through all nodes to get also the reverse combination + for(size_t b = 0; b < num_nodes; b++) { + if (a == b) { + continue; + } + size_t pi = pair_index(a, b); + if (max_pair_variance > 0.0 && var_measurements != NULL && var_measurements[pi] >= max_pair_variance) { + continue; // we skip this entry as it has too high variance, note that we do not change parameter indices because of it + } + matrix_data_a[(pi * num_input_cols) + param_index] = 1.0; + } + param_index++; + } + + for(int a = 0; a < num_nodes; a++) { + for(int b = 0; b < a; b++) { + size_t pi = pair_index(a, b); + + if (tof_known[pi]) { + continue; // this one is not relevant here + } + + matrix_data_a[(pi * num_input_cols) + param_index] = 2.0; // the distance is of factor two + param_index++; + } + } + + if (param_index != num_input_cols) { + LOG_ERR("aram_index != num_input_cols, %d, %d", param_index, num_input_cols); + return; + } + + //LOG_DBG("Matrix X:"); + //print_matrix(&mat_a); + } + + // transpose X to mat_b + { + mat_b.numRows = num_input_cols; + mat_b.numCols = num_input_rows; + + ret = OP_TRANS(&mat_a, &mat_b); + if (ret == ARM_MATH_SIZE_MISMATCH ) { + LOG_INF("SIZE MISMATCH... A"); + return false; + } + } + + + // multiply X^T (mat_b) with X (mat_a) and store in mat_c + { + mat_c.numRows = num_input_cols; + mat_c.numCols = num_input_cols; + + ret = OP_MULT(&mat_b, &mat_a, &mat_c); + + if (ret == ARM_MATH_SIZE_MISMATCH ) { + LOG_INF("SIZE MISMATCH... B"); + return false; + } + } + + // invert (X^T X) (mat_c) and store in mat_a + // we keep mat_b as we need the transpose in the next step + { + mat_a.numRows = num_input_cols; + mat_a.numCols = num_input_cols; + + ret = OP_INV(&mat_c, &mat_a); + + if (ret == ARM_MATH_SIZE_MISMATCH ) { + LOG_INF("SIZE MISMATCH... C"); + return false; + } + } + + // multiply (X^T X)^{-1} (mat_a) with X^T (mat_b) and store in mat_c + { + mat_c.numRows = num_input_cols; + mat_c.numCols = num_input_rows; + + ret = OP_MULT(&mat_a, &mat_b, &mat_c); + + if (ret == ARM_MATH_SIZE_MISMATCH ) { + LOG_INF("SIZE MISMATCH... D"); + return false; + } + } + + + // load Y into mat_b + { + mat_b.numRows = num_input_rows; + mat_b.numCols = 1; + //TODO: implement this + + // we first store all of the measurements + for(size_t a = 0; a < num_nodes; a++) { + for(size_t b = 0; b < a; b++) { + size_t pi = pair_index(a, b); + float32_t val; + if (delay_known[a] && delay_known[b] && tof_known[pi]) { + val = 0.0; // nothing todo here! this is a null row... + } + else if (delay_known[a] && max_pair_variance > 0.0 && var_measurements != NULL && var_measurements[pi] >= max_pair_variance) { + val = 0.0; // we remove this entire row BUT just for antenna delay estimation! + } + else { + val = mean_measurements[pi] * 2.0; // we need to scale this accordingly + + if (delay_known[a]) { + val -= known_delays[a]; + } + if (delay_known[b]) { + val -= known_delays[b]; + } + if(tof_known[pi]) { + val -= known_tofs[pi]*2.0; //tof has factor two! + } + + } + matrix_data_b[pi] = val; + } + } + +// LOG_DBG("Matrix Y:"); +// print_matrix(&mat_b); + } + + // multiply (X^T X)^{-1} X^T (mat_c) with Y (mat_b) and store in mat_a + { + mat_a.numRows = num_input_cols; + mat_a.numCols = 1; + + ret = OP_MULT(&mat_c, &mat_b, &mat_a); + + if (ret == ARM_MATH_SIZE_MISMATCH ) { + LOG_INF("SIZE MISMATCH... E"); + return; + } + } + + +// LOG_DBG("Estimate:"); +// print_matrix(&mat_a); + + { + // copy stuff to out buffers + int param_index = 0; + + // first copy all the delays + for(int a = 0; a < num_nodes; a++) { + if (delay_known[a]) { + if (delays_out != NULL){ + delays_out[a] = known_delays[a]; + } + } else { + if (delays_out != NULL){ + delays_out[a] = matrix_data_a[param_index]; + } + param_index++; + } + } + + + // TODO: we cannot infer tofs because of memory constraints... + // so we simply calculate them manually here... + + for(int a = 0; a < num_nodes; a++) { + for(int b = 0; b < a; b++) { + size_t pi = pair_index(a, b); + + if (tof_known[pi]) { + if (tofs_out != NULL) { + tofs_out[pi] = known_tofs[pi]; + } + } else { + if (tofs_out != NULL) { + // TODO: This might not work as planned? + tofs_out[pi] = mean_measurements[pi] - 0.5*delays_out[a] - 0.5*delays_out[b]; + } + param_index++; // we only have that here so it + } + } + } + + +// for(int a = 0; a < num_nodes; a++) { +// for(int b = 0; b < a; b++) { +// +// size_t pi = pair_index(a, b); +// +// if (tof_known[pi]) { +// if (tofs_out != NULL) { +// tofs_out[pi] = known_tofs[pi]; +// } +// } else { +// if (tofs_out != NULL) { +// tofs_out[pi] = matrix_data_a[param_index]; +// } +// param_index++; +// } +// } +// } + + if (param_index != num_input_cols) { + LOG_ERR("aram_index != num_input_cols, %d, %d", param_index, num_input_cols); + return; + } + } +} + + +static inline void output_measurement(measurement_t val) { + char buf[32]; + int64_t int_val = val * 1000.0f; + snprintf(buf, sizeof(buf), "%lld", int_val); + uart_out(buf); +} + +static void output_measurements(measurement_t *out, size_t num, bool *known) { + + uart_out("["); + for(size_t i = 0; i < num; i ++) { + if (known == NULL || known[i]) { + output_measurement(out[i]); + } else { + uart_out("null"); + } + + if (i < num-1) { + uart_out(", "); + } + } + uart_out("]"); +} + + +static void log_estimation( + uint8_t num_nodes, + float32_t delays_out[EST_MAX_NODES], + float32_t tofs_out[EST_MAX_PAIRS], + float32_t mean_measurements[EST_MAX_PAIRS], + float32_t var_measurements[EST_MAX_PAIRS], + bool delay_known[EST_MAX_NODES], + float32_t known_delays[EST_MAX_NODES], + bool tof_known[EST_MAX_PAIRS], + float32_t known_tofs[EST_MAX_PAIRS] +) { + uart_out("{ \"mean_measurements\": "); + output_measurements(mean_measurements, EST_MAX_PAIRS, NULL); + uart_out("{ \"var_measurements\": "); + output_measurements(var_measurements, EST_MAX_PAIRS, NULL); + + // we further log the standard deviations + float32_t std_measurements[EST_MAX_PAIRS]; + + for(int i = 0; i < EST_MAX_PAIRS; i++) { + std_measurements[i] = sqrt(var_measurements[i]); + } + + uart_out("{ \"std_measurements\": "); + output_measurements(std_measurements, EST_MAX_PAIRS, NULL); + + uart_out(", \"delays_out\": "); + output_measurements(delays_out, EST_MAX_NODES, NULL); + uart_out(", \"tofs_out\": "); + output_measurements(tofs_out, EST_MAX_PAIRS, NULL); + uart_out(", \"known_delays\": "); + output_measurements(known_delays, EST_MAX_NODES, NULL); // we just assume we know everything for now + uart_out(", \"known_tofs\": "); + output_measurements(known_tofs, EST_MAX_PAIRS, NULL); // we just assume we know everything for now + uart_out("}"); +} + +static void infer_tofs_out( + uint8_t num_nodes, + float32_t delays_out[EST_MAX_NODES], + float32_t tofs_out[EST_MAX_PAIRS], + float32_t mean_measurements[EST_MAX_PAIRS] +) { + for(int a = 0; a < num_nodes; a++) { + for(int b = 0; b < a; b++) { + size_t pi = pair_index(a, b); + tofs_out[pi] = mean_measurements[pi] - 0.5*delays_out[a] - 0.5*delays_out[b]; + } + } +} + +void estimate_all(uint16_t round) { + + static measurement_t delays_out[EST_MAX_NODES] = {0.0}; + static measurement_t tofs_out[EST_MAX_PAIRS] = {0.0}; + static measurement_t mean_measurements[EST_MAX_PAIRS] = {0.0}; + static measurement_t var_measurements[EST_MAX_PAIRS] = {0.0}; + + static bool delay_known[EST_MAX_NODES]; + static measurement_t known_delays[EST_MAX_NODES]; + static bool tof_known[EST_MAX_PAIRS]; + static measurement_t known_tofs[EST_MAX_PAIRS]; + + int64_t estimate_start = 0; + int64_t estimate_duration = 0; + + //for(int n = EST_MAX_NODES; n <= EST_MAX_NODES; n++) + int n = EST_MAX_NODES; // we just execute it for a single node right now + { + uart_out("{\"type\": \"estimation\", "); + char buf[32]; + snprintf(buf, sizeof(buf), "\"round\": %hu", round); + uart_out(buf); + + //initialize known values (measurements, delays, distances + for(int i = 0; i < n; i++) { + known_delays[i] = (float32_t) node_factory_antenna_delay_offsets[i]; + } + + for(int p = 0; p < PAIRS(n); p++) { + known_tofs[p] = node_distances[p] / SPEED_OF_LIGHT_M_PER_UWB_TU; + // TODO: If we reorder, we have to be careful with those distances! + mean_measurements[p] = get_mean_measurement(p); + var_measurements[p] = get_var_measurement(p); + } + + uart_out(", \"mean_measurements\": "); + output_measurements(mean_measurements, PAIRS(n), NULL); + + uart_out(", \"var_measurements\": "); + output_measurements(var_measurements, PAIRS(n), NULL); + + // first estimate antenna delays based on all known distances WITHOUT filtering + { + memset(tof_known, 1, sizeof(tof_known)); + memset(delay_known, 0, sizeof(delay_known)); + + estimate_start = k_uptime_get(); + estimate( + n, + delays_out, + tofs_out, + mean_measurements, + var_measurements, + delay_known, + known_delays, + tof_known, + known_tofs, + 0.0 + ); + estimate_duration = k_uptime_delta(&estimate_start); + + uart_out(", \"delays_from_measurements\": "); + output_measurements(delays_out, n, NULL); + + uart_out(", \"delays_from_measurements_time\": "); + snprintf(buf, sizeof(buf), "%lld", estimate_duration); + uart_out(buf); + } + + // now use the new delays to infer the distances again -> should result in the same distances basically... + { + // we now use the new delay values + for(int i = 0; i < n; i++) { + known_delays[i] = delays_out[i]; + } + for(int p = 0; p < PAIRS(n); p++) { + // TODO: If we reorder, we have to be careful with those distances! + mean_measurements[p] = get_mean_measurement(p); + known_tofs[p] = node_distances[p] / SPEED_OF_LIGHT_M_PER_UWB_TU; + } + + memset(tof_known, 0, sizeof(tof_known)); + memset(delay_known, 1, sizeof(delay_known)); + + estimate_start = k_uptime_get(); + + infer_tofs_out( + n, + delays_out, + tofs_out, + mean_measurements + ); + + estimate_duration = k_uptime_delta(&estimate_start); + + uart_out(", \"tofs_from_estimated_delays\": "); + output_measurements(tofs_out, PAIRS(n), NULL); + + uart_out(", \"tofs_from_estimated_delays_time\": "); + snprintf(buf, sizeof(buf), "%lld", estimate_duration); + uart_out(buf); + } + + // estimate antenna delays on all known distances WITH filtering + { + // reset values just to be sure + memset(delays_out, 0, sizeof(delays_out)); + memset(tofs_out, 0, sizeof(tofs_out)); + + memset(tof_known, 1, sizeof(tof_known)); + memset(delay_known, 0, sizeof(delay_known)); + + estimate_start = k_uptime_get(); + estimate( + n, + delays_out, + tofs_out, + mean_measurements, + var_measurements, + delay_known, + known_delays, + tof_known, + known_tofs, + MAX_VAR_IN_M + ); + estimate_duration = k_uptime_delta(&estimate_start); + + uart_out(", \"delays_from_measurements_filtered\": "); + output_measurements(delays_out, n, NULL); + + uart_out(", \"delays_from_measurements_filtered_time\": "); + snprintf(buf, sizeof(buf), "%lld", estimate_duration); + uart_out(buf); + } + + // now use the Filtered delays to infer the distances again -> should result in the same distances basically... + { + // we now use the new delay values + for(int i = 0; i < n; i++) { + known_delays[i] = delays_out[i]; + } + for(int p = 0; p < PAIRS(n); p++) { + // TODO: If we reorder, we have to be careful with those distances! + mean_measurements[p] = get_mean_measurement(p); + known_tofs[p] = node_distances[p] / SPEED_OF_LIGHT_M_PER_UWB_TU; + } + + memset(tof_known, 0, sizeof(tof_known)); + memset(delay_known, 1, sizeof(delay_known)); + + estimate_start = k_uptime_get(); + infer_tofs_out( + n, + delays_out, + tofs_out, + mean_measurements + ); + estimate_duration = k_uptime_delta(&estimate_start); + + uart_out(", \"tofs_from_filtered_estimated_delays\": "); + output_measurements(tofs_out, PAIRS(n), NULL); + + uart_out(", \"tofs_from_filtered_estimated_delays_time\": "); + snprintf(buf, sizeof(buf), "%lld", estimate_duration); + uart_out(buf); + } + + + // now use FACTORY delays to infer the distances again -> should result in the same distances basically... + { + // we now use the new delay values + for(int i = 0; i < n; i++) { + delays_out[i] = (float32_t) node_factory_antenna_delay_offsets[i]; + } + for(int p = 0; p < PAIRS(n); p++) { + // TODO: If we reorder, we have to be careful with those distances! + mean_measurements[p] = get_mean_measurement(p); + known_tofs[p] = node_distances[p] / SPEED_OF_LIGHT_M_PER_UWB_TU; + } + + memset(tof_known, 0, sizeof(tof_known)); + memset(delay_known, 1, sizeof(delay_known)); + + estimate_start = k_uptime_get(); + infer_tofs_out( + n, + delays_out, + tofs_out, + mean_measurements + ); + estimate_duration = k_uptime_delta(&estimate_start); + + uart_out(", \"tofs_from_factory_delays\": "); + output_measurements(tofs_out, PAIRS(n), NULL); + + uart_out(", \"tofs_from_factory_delays_time\": "); + snprintf(buf, sizeof(buf), "%lld", estimate_duration); + uart_out(buf); + + } + + // now use the new delays to infer the distances again -> should result in the same distances basically... + { + // we now use the new delay values + for(int i = 0; i < n; i++) { + delays_out[i] = 0; // no differences in the delays here + } + for(int p = 0; p < PAIRS(n); p++) { + // TODO: If we reorder, we have to be careful with those distances! + mean_measurements[p] = get_mean_measurement(p); + known_tofs[p] = node_distances[p] / SPEED_OF_LIGHT_M_PER_UWB_TU; + } + + memset(tof_known, 0, sizeof(tof_known)); + memset(delay_known, 1, sizeof(delay_known)); + + estimate_start = k_uptime_get(); + infer_tofs_out( + n, + delays_out, + tofs_out, + mean_measurements + ); + estimate_duration = k_uptime_delta(&estimate_start); + + uart_out(", \"tofs_uncalibrated\": "); + output_measurements(tofs_out, PAIRS(n), NULL); + + uart_out(", \"tofs_uncalibrated_time\": "); + snprintf(buf, sizeof(buf), "%lld", estimate_duration); + uart_out(buf); + } + uart_out("}\n"); + } +} \ No newline at end of file diff --git a/src/estimation.h b/src/estimation.h new file mode 100644 index 0000000..dabc78d --- /dev/null +++ b/src/estimation.h @@ -0,0 +1,19 @@ +#ifndef ESTIMATION_H +#define ESTIMATION_H + +#include +#include +#include + +#define EST_MAX_NODES (NUM_NODES) +#define EST_MAX_PAIRS (PAIRS(EST_MAX_NODES)) +#define EST_MAX_PARAMS (EST_MAX_NODES) +#define EST_MAX_INPUTS (EST_MAX_PAIRS) + +//#define SPEED_OF_LIGHT_M_PER_S 299792458.0 +#define SPEED_OF_LIGHT_M_PER_S 299702547.236 +#define SPEED_OF_LIGHT_M_PER_UWB_TU ((SPEED_OF_LIGHT_M_PER_S * 1.0E-15) * 15650.0) // around 0.00469175196 + +void estimate_all(uint16_t round); + +#endif \ No newline at end of file diff --git a/src/history.c b/src/history.c new file mode 100644 index 0000000..9b8e898 --- /dev/null +++ b/src/history.c @@ -0,0 +1,154 @@ +#include +#include + +#include +#include +#include +#include + +#include "history.h" +#include "uart.h" + +#if HISTORY_LENGTH > 0 + +struct __attribute__((__packed__)) history_record { + struct msg msg; + int8_t rssi; + int8_t applied_bias_correction; + int carrierintegrator; +}; + +static int num_stored = 0; + +static struct history_record history[HISTORY_NUM_ROUNDS*NUM_NODES]; + +void history_reset() { + memset(&history, 0, sizeof(history)); + num_stored = 0; +} + +void history_save(struct msg *msg, int8_t rssi, int8_t applied_bias_correction, int carrierintegrator) { + + if (num_stored == HISTORY_LENGTH -1){ + return; + } + + num_stored += 1; + + struct history_record *h = &history[num_stored]; + h->msg = *msg; //copy everything! + h->rssi = rssi; + h->applied_bias_correction = applied_bias_correction; + h->carrierintegrator = carrierintegrator; +} + +static inline uint64_t ts_to_uint(const ts_t *ts) { + uint8_t buf[sizeof(uint64_t)] = {0}; + memcpy(&buf, ts, sizeof(ts_t)); + return sys_get_le64(buf); +} + +static void output_msg_to_uart(struct msg* m) { + + char buf[256]; + + // write open parentheses + uart_out("{"); + + // write round + snprintf(buf, sizeof(buf), "\"round\": %hu", m->round); + uart_out(buf); + uart_out(", "); + + // write number + snprintf(buf, sizeof(buf), "\"number\": %hhu", m->number); + uart_out(buf); + uart_out(", "); + + // write tx ts + uint64_t ts = ts_to_uint(&m->tx_ts); + snprintf(buf, sizeof(buf), "\"tx_ts\": %llu", ts); + uart_out(buf); + uart_out(", "); + + + // write all rx ts: + uart_out("\"rx_ts: \": ["); + for(size_t i = 0; i < NUM_NODES; i ++) { + ts = ts_to_uint(&m->rx_ts[i]); + snprintf(buf, sizeof(buf), "%llu", ts); + uart_out(buf); + + if (i < NUM_NODES-1) { + uart_out(", "); + } + } + uart_out("]"); + + // end msg + uart_out("}"); +} + +static void output_record_to_uart(struct history_record* m) { + + char buf[256]; + + // write open parentheses + uart_out("{"); + + uart_out("\"msg\":"); + output_msg_to_uart(&m->msg); + uart_out(", "); + + // write rssi + snprintf(buf, sizeof(buf), "\"rssi\": %hhd", m->rssi); + uart_out(buf); + uart_out(", "); + + // write applied_bias_correction + snprintf(buf, sizeof(buf), "\"bias\": %hhd", m->applied_bias_correction); + uart_out(buf); + uart_out(", "); + + // write carrierintegrator + snprintf(buf, sizeof(buf), "\"ci\": %d", m->carrierintegrator); + uart_out(buf); + + // end msg + uart_out("}"); +} + + +void history_print_entries() { + + // we simply dump all recorded records! + char buf[256]; + + // write open parentheses + uart_out("["); + + for(int i = 0; i < num_stored; i++) { + output_record_to_uart(&history[i]); + if (i < num_stored-1) { + uart_out(", "); + } + } + + // write close parentheses + uart_out("]"); +} + +void history_print() { + uart_out("{\"type\": \"history\", \"entries\": "); + history_print_entries(); + uart_out("}"); +} + +#else +// no history is saved! -> NOOP! + +void history_reset(); +void history_save(struct msg *msg, int8_t rssi, int8_t applied_bias_correction, int carrierintegrator) {} +void history_print() {} + +#endif \ No newline at end of file diff --git a/src/history.h b/src/history.h new file mode 100644 index 0000000..ea4f004 --- /dev/null +++ b/src/history.h @@ -0,0 +1,22 @@ +#ifndef HISTORY_H +#define HISTORY_H + +#include "nodes.h" + +#define HISTORY_NUM_ROUNDS (0) +#define HISTORY_LENGTH (HISTORY_NUM_ROUNDS*NUM_NODES) + +typedef uint8_t ts_t[5]; + +struct __attribute__((__packed__)) msg { + uint16_t round; + uint8_t number; + ts_t tx_ts; + ts_t rx_ts[NUM_NODES]; // we keep the slot for our own nodes (wasting a bit of space in transmissions but making it a lot easier to handle everywhere...) +}; + +void history_reset(); +void history_save(struct msg *msg, int8_t rssi, int8_t applied_bias_correction, int carrierintegrator); +void history_print(); + +#endif \ No newline at end of file diff --git a/src/main.c b/src/main.c new file mode 100644 index 0000000..9f50c7f --- /dev/null +++ b/src/main.c @@ -0,0 +1,671 @@ +#include +#include + +#include +#include +#include +#include + + +#include "nodes.h" +#include "estimation.h" +#include "uart.h" +#include "measurements.h" +#include "history.h" + +LOG_MODULE_REGISTER(main); + +#define NUM_MEASUREMENTS 1000 +#define NUM_DUMMY_ROUNDS 200 +#define INITIAL_DELAY_MS 5000 +#define LOG_RAW 1 + +#define NUM_ROUNDS ((NUM_DUMMY_ROUNDS+NUM_MEASUREMENTS)+1) +#define SLOT_DELAY_MS 2 +#define ROUND_TIMEOUT_MS (NUM_NODES*SLOT_DELAY_MS*2*2) +#define POST_ROUND_DELAY_MS 50 + +#if LOG_RAW +#define LOG_RAW_CMD uart_out +#else +#define LOG_RAW_CMD uart_disabled +#endif + +#define FINAL_ESTIMATION_TIMEOUT_MS (ROUND_TIMEOUT_MS*10) + +/* ieee802.15.4 device */ +static struct ieee802154_radio_api *radio_api; +static const struct device *ieee802154_dev; + + +static int sent_packets = 0; + +// measurement +// 40 bit measurements +// we save each measurement + +// TODO: This is not standard compliant +static uint8_t msg_header[] = {0xDE, 0xCA}; + +static struct msg msg_tx_buf; + +K_SEM_DEFINE(tx_sem, 0, 1); +K_SEM_DEFINE(msg_tx_buf_sem, 0, 1); + +static void init_tx_queue(); + +extern void matrix_test(); + + +static uint16_t own_number = 0; +#define INITIATOR_ID 0 +#define IS_INITIATOR (own_number == INITIATOR_ID) + +static void output_msg_to_uart(struct msg* m); + +static int64_t round_start = 0; +static int64_t round_end = 0; +static int64_t last_msg_ms = 0; + +static bool schedule_next_round = 0; + +// we save the last & current msgs of every node +static struct msg last_msg[NUM_NODES]; +static struct msg cur_msg[NUM_NODES]; + +// save the last carrierintegrator value for each received message +static int carrierintegrators[NUM_NODES]; + +static inline void ts_from_uint(ts_t *ts_out, uint64_t val) { + uint8_t dst[sizeof(uint64_t)]; + sys_put_le64(val, dst); + memcpy(ts_out, dst, sizeof(ts_t)); +} + +static inline uint64_t ts_to_uint(const ts_t *ts) { + uint8_t buf[sizeof(uint64_t)] = {0}; + memcpy(&buf, ts, sizeof(ts_t)); + return sys_get_le64(buf); +} + +static inline uint64_t get_uint_duration(const ts_t *end_ts, const ts_t *start_ts){ + + uint64_t end = ts_to_uint(end_ts); + uint64_t start = ts_to_uint(start_ts); + + if (end == 0 || start == 0) { + return 0; // TODO: we might want to change this behavior + } + + if (end < start) { + + // handle overflow! + end += 0xFFFFFFFFFF+1; + + if (end < start) { + return 0; // it is still wrong... + } + } + + return end - start; +} + +//void on_round_end(uint16_t round_number) { +// if (round_number > HISTORY_NUM_ROUNDS){ +// history_print(); +// history_reset(); +// } +//} + +void on_round_end(uint16_t round_number) { + + bool dummy = round_number <= NUM_DUMMY_ROUNDS; + + // cur_msg contains all messages of the this round + // last_msg contains all messages of the previous round + + // we cleanup invalid values first + for(int i = 0; i < NUM_NODES; i++) { + if(last_msg[i].round != round_number - 1) { + memset(&last_msg[i], 0, sizeof(last_msg[i])); + //LOG_DBG("LastMsg invalid value of %d, expected %hu, got %hu", i, round_number - 1, last_msg[i].round); + } + } + + for(int i = 0; i < NUM_NODES; i++) { + if(cur_msg[i].round != round_number) { + memset(&cur_msg[i], 0, sizeof(cur_msg[i])); + //LOG_DBG("CurMsg invalid value of %d, expected %hu, got %hu", i, round_number, cur_msg[i].round); + } + } + + // determine all relative drift rates for now: + + static uint64_t own_dur[NUM_NODES]; + static uint64_t other_dur[NUM_NODES]; + static float32_t relative_drift_offsets[NUM_NODES]; + + if (dummy){ + LOG_RAW_CMD("{\"type\": \"drift_estimation\", \"dummy\": true, "); + } + else { + LOG_RAW_CMD("{\"type\": \"drift_estimation\", \"dummy\": false, "); + } + + char buf[256]; + snprintf(buf, sizeof(buf), "\"round\": %hu, \"durations\": [", round_number); + LOG_RAW_CMD(buf); + + for(int i = 0; i < NUM_NODES; i++) { + + uint64_t rd_other_dur, rd_own_dur = 0; + + if (i == own_number) { + rd_other_dur = 1; + rd_own_dur = 1; + } else { + // we can extract last and current timestamps + rd_other_dur = get_uint_duration(&cur_msg[i].tx_ts, &last_msg[i].tx_ts); + + if (i < own_number) { + // in this case, the other node transmitted before us + // so we have to extract rx timestamps from our own cur and last msg + rd_own_dur = get_uint_duration(&cur_msg[own_number].rx_ts[i], &last_msg[own_number].rx_ts[i]); + } else { + // i > own_number: in this case, the rx timestamp from the current round is actually in our current tx_buf and the last one in cur_msg + rd_own_dur = get_uint_duration(&msg_tx_buf.rx_ts[i], &cur_msg[own_number].rx_ts[i]); + } + } + + // log values! + snprintf(buf, sizeof(buf), "[%llu, %llu]", rd_own_dur, rd_other_dur); + LOG_RAW_CMD(buf); + + if (i < NUM_NODES-1) { + LOG_RAW_CMD(", "); + } + + if ( rd_other_dur != 0 && rd_own_dur != 0) { + relative_drift_offsets[i] = (float32_t)((int64_t)rd_own_dur-(int64_t)rd_other_dur) / (float32_t)(rd_other_dur); + } else { + relative_drift_offsets[i] = NAN; + } + + own_dur[i] = rd_own_dur; + other_dur[i] = rd_other_dur; + } + + LOG_RAW_CMD("], \"carrierintegrators\": ["); + for(int i = 0; i < NUM_NODES; i++) { + // log values! + snprintf(buf, sizeof(buf), " %d", carrierintegrators[i]); + LOG_RAW_CMD(buf); + if (i < NUM_NODES-1) { + LOG_RAW_CMD(", "); + } + } + + LOG_RAW_CMD("]}\n"); + + if (dummy){ + LOG_RAW_CMD("{\"type\": \"raw_measurements\", \"dummy\": true, "); + } + else { + LOG_RAW_CMD("{\"type\": \"raw_measurements\", \"dummy\": false, "); + } + snprintf(buf, sizeof(buf), "\"round\": %hu, \"measurements\": [", round_number); + LOG_RAW_CMD(buf); + + // we now check every combination + // TODO: we might also just want to check for ourselves + for(int b = 0; b < NUM_NODES; b++) { // TODO: update again some time + for (int a = 0; a < b; a++) { + + // we extract the ranging as initiated by A: + uint64_t round_dur_a = 0; + uint64_t delay_dur_b = 0; + + round_dur_a = get_uint_duration(&cur_msg[a].rx_ts[b], &last_msg[a].tx_ts); + + if (a < b) {// TODO: this is automatically given as of right now + // the response delay of b should be in the last_msg as well + delay_dur_b = get_uint_duration(&last_msg[b].tx_ts, &last_msg[b].rx_ts[a]); + } else { + // otherwise b transmitted before a, so it should reside in the current round + // TODO: Since we have a lot of delay between rounds, this round and delay values are too big to be handled with the necessary precision! + //delay_dur_b = get_uint_duration(&cur_msg[b].tx_ts, &cur_msg[b].rx_ts[a]); + } + + if (round_dur_a != 0 && delay_dur_b != 0 && !isnan(relative_drift_offsets[a]) && !isnan(relative_drift_offsets[b])) { + int64_t drift_offset_int = round(relative_drift_offsets[a]*(float32_t)round_dur_a - relative_drift_offsets[b]*(float32_t)delay_dur_b); + int64_t two_tof_int = (int64_t)round_dur_a - (int64_t)delay_dur_b + drift_offset_int; + + measurement_t tof_in_uwb_us = ((float) two_tof_int) * 0.5; + + if (!dummy){ + estimation_add_measurement(a, b, tof_in_uwb_us); + } + + int64_t int_val = tof_in_uwb_us * 1000.0f; + snprintf(buf, sizeof(buf), "[%llu, %llu, %lld, %lld]", round_dur_a, delay_dur_b, int_val, two_tof_int); + LOG_RAW_CMD(buf); + + float est_distance_in_m = tof_in_uwb_us*SPEED_OF_LIGHT_M_PER_UWB_TU; + + int64_t est_cm = est_distance_in_m*100; + //LOG_DBG("Round est cm: %hhu, %hhu, %lld, r:%lld, d: %lld", a, b, est_cm, round_dur_a, delay_dur_b); + } else { + LOG_RAW_CMD("null"); + } + + if (b != NUM_NODES - 1 || a < b - 1) { + LOG_RAW_CMD(", "); + } + + } + } + LOG_RAW_CMD("]}\n"); + + // copy all of the current messages to the last round + memcpy(&last_msg, &cur_msg, sizeof(last_msg)); + + // reset cur_msg! + memset(&cur_msg, 0, sizeof(cur_msg)); +} + +int main(void) { + + LOG_INF("Getting node id"); + int16_t signed_node_id = get_node_number(get_own_node_id()); + + if (signed_node_id < 0) { + LOG_INF("Node number NOT FOUND! Shutting down :( I am: 0x%04hx", get_own_node_id()); + return; + } + + own_number = signed_node_id; + LOG_INF("GOT node id: %hhu", own_number); + + //LOG_INF("Testing ..."); + //matrix_test(); + //return; + + int ret = 0; + LOG_INF("Starting ..."); + + LOG_INF("Initialize ieee802.15.4"); + ieee802154_dev = device_get_binding(CONFIG_NET_CONFIG_IEEE802154_DEV_NAME); + + if (!ieee802154_dev) { + LOG_ERR("Cannot get ieee 802.15.4 device"); + return false; + } + + // prepare msg buffer + { + (void)memset(&msg_tx_buf, 0, sizeof(msg_tx_buf)); + (void)memset(&last_msg, 0, sizeof(last_msg)); + (void)memset(&cur_msg, 0, sizeof(cur_msg)); + + // TODO: Add own addr! + msg_tx_buf.number = own_number&0xFF; + msg_tx_buf.round = 0; + (void)memset(&msg_tx_buf.rx_ts, 0, sizeof(msg_tx_buf.rx_ts)); + k_sem_give(&msg_tx_buf_sem); + } + + /* Setup antenna delay values to 0 to get raw tx values */ + dwt_set_antenna_delay_rx(ieee802154_dev, 16450); + dwt_set_antenna_delay_tx(ieee802154_dev, 16450); + + // we disable the frame filter, otherwise the packets are not received! + dwt_set_frame_filter(ieee802154_dev, 0, 0); + + radio_api = (struct ieee802154_radio_api *)ieee802154_dev->api; + + LOG_INF("Start IEEE 802.15.4 device"); + ret = radio_api->start(ieee802154_dev); + + if(ret) { + LOG_ERR("Could not start ieee 802.15.4 device"); + return false; + } + + k_msleep(INITIAL_DELAY_MS); + + // Create TX thread and queue + init_tx_queue(); + + while (1) { + + if (sent_packets > 0 && sent_packets % 1000 == 0) { + LOG_DBG("Sent %d packets", sent_packets); + } + + int64_t ms_since_last_msg = k_uptime_get() - last_msg_ms; + + if (IS_INITIATOR && msg_tx_buf.round <= NUM_ROUNDS) { + if (schedule_next_round || ms_since_last_msg >= ROUND_TIMEOUT_MS) { + schedule_next_round = 0; + //LOG_INF("Advancing to new round! (%hu)", msg_tx_buf.round+1); + + k_sem_take(&msg_tx_buf_sem, K_FOREVER); // we take this to be sure that on_round_end finished! + + // we then add more delay! + k_msleep(POST_ROUND_DELAY_MS); + + msg_tx_buf.round += 1; + k_sem_give(&tx_sem); + + k_sem_give(&msg_tx_buf_sem); + + round_start = k_uptime_get(); + round_end = 0; + } + } + + if (msg_tx_buf.round >= 2 && ms_since_last_msg >= FINAL_ESTIMATION_TIMEOUT_MS) { + k_sem_take(&msg_tx_buf_sem, K_FOREVER); + estimate_all(msg_tx_buf.round); + k_sem_give(&msg_tx_buf_sem); +// // we reset all messages! since the drift is kind of high +// (void)memset(&last_msg, 0, sizeof(last_msg)); +// (void)memset(&cur_msg, 0, sizeof(cur_msg)); +// (void)memset(&msg_tx_buf.rx_ts, 0, sizeof(msg_tx_buf.rx_ts)); +// (void)memset(&carrierintegrators, 0, sizeof(carrierintegrators)); + return 0; + } + + k_msleep(1); + k_yield(); + } + return 0; +} + +static void net_pkt_hexdump(struct net_pkt *pkt, const char *str) +{ + struct net_buf *buf = pkt->buffer; + + while (buf) { + LOG_HEXDUMP_DBG(buf->data, buf->len, str); + buf = buf->frags; + } +} + +static void output_msg_to_uart(struct msg* m) { + + char buf[256]; + + // write open parentheses + uart_out("{"); + + // write round + snprintf(buf, sizeof(buf), "\"round\": %hu", m->round); + uart_out(buf); + uart_out(", "); + + // write number + snprintf(buf, sizeof(buf), "\"number\": %hhu", m->number); + uart_out(buf); + uart_out(", "); + + // write tx ts + uint64_t ts = ts_to_uint(&m->tx_ts); + snprintf(buf, sizeof(buf), "\"tx_ts\": %llu", ts); + uart_out(buf); + uart_out(", "); + + + // write all rx ts: + uart_out("\"rx_ts: \": ["); + for(size_t i = 0; i < NUM_NODES; i ++) { + ts = ts_to_uint(&m->rx_ts[i]); + snprintf(buf, sizeof(buf), "%llu", ts); + uart_out(buf); + + if (i < NUM_NODES-1) { + uart_out(", "); + } + } + uart_out("]"); + + // end msg + uart_out("}"); +} + + +enum net_verdict ieee802154_radio_handle_ack(struct net_if *iface, struct net_pkt *pkt) +{ + return NET_CONTINUE; +} + +/** + * Interface to the network stack, will be called when the packet is + * received + */ +int net_recv_data(struct net_if *iface, struct net_pkt *pkt) +{ + size_t len = net_pkt_get_len(pkt); + struct net_buf *buf = pkt->buffer; + int ret = 0; + + //LOG_WRN("Got data of length %d", len); + + if (len > sizeof(msg_header) + 2 && !memcmp(msg_header, net_buf_pull_mem(buf, sizeof(msg_header)), sizeof(msg_header))) { + len -= sizeof(msg_header) + 2; // 2 bytes crc? + struct msg *rx_msg = net_buf_pull_mem(buf, len); + + // TODO: Use these? + net_buf_pull_u8(buf); + net_buf_pull_u8(buf); + + if (len > 0 && len % sizeof (struct msg) == 0) { + //size_t num_msg = len / sizeof (struct msg_ts); + + // TODO: Check that rx_number is actually valid... -> buffer overflow! + // TODO: CHECK IF WE would need to ignore this msg + uint8_t rx_number = rx_msg->number; + uint16_t rx_round = rx_msg->round; + + //LOG_DBG("Received (n: %hhu, r: %hu)", rx_number, rx_round); + + uint64_t rx_ts = dwt_rx_ts(ieee802154_dev); + int carrierintegrator = dwt_readcarrierintegrator(ieee802154_dev); + + carrierintegrators[rx_number] = carrierintegrator; + + int8_t rssi = (int8_t)net_pkt_ieee802154_rssi(pkt); + + int8_t bias_correction = get_range_bias_by_rssi(rssi); + rx_ts -= bias_correction; + + // save this message for later processing + memcpy(&cur_msg[rx_number], rx_msg, sizeof(struct msg)); + + // and save it for potential printing + history_save(rx_msg, rssi, bias_correction, carrierintegrator); + + last_msg_ms = k_uptime_get(); // we save this value to restart the round when we are the initiator + + k_sem_take(&msg_tx_buf_sem, K_FOREVER); + { + // save this ts in our tx buf! + ts_from_uint(&msg_tx_buf.rx_ts[rx_number], rx_ts); + + // we wait for the packet of our predecessor + if (!IS_INITIATOR && rx_number == msg_tx_buf.number-1) { + if (rx_round == msg_tx_buf.round + 1) { + // we can advance to the new round without problems + } else { + // this is problematic! + if (rx_round <= msg_tx_buf.round) { + LOG_WRN("Received outdated round from predecessor!!!"); // this is NOT good + // TODO: how to handle this case? + } else { + LOG_INF("Values seem outdated... Resetting...my: %d, rx: %d", msg_tx_buf.round, rx_round); // note that we can waste time in this case since we are the next to transmit anyway! + // the received round is a lot more progressed than we are + // we hence cannot be sure that our received timestamps are still valid and reset them! + // we reset the tx_buf Note that we still hold msg_tx_buf_sem + (void)memset(&msg_tx_buf.rx_ts, 0, sizeof(msg_tx_buf.rx_ts)); + } + } + + round_start = k_uptime_get(); + round_end = 0; + + msg_tx_buf.round = MAX(msg_tx_buf.round, rx_round); // use new updated round number in any case + k_sem_give(&tx_sem); + + //LOG_DBG("Starting new round! (n: %hhu, r: %hu)", msg_tx_buf.number, msg_tx_buf.round); + } else if (rx_number == NUM_NODES-1) { + // oh wow, this was the last one! + int64_t milliseconds_spent = k_uptime_delta(&round_start); + //LOG_INF("ROUND FINISHED! ms: %lld", milliseconds_spent); + on_round_end(msg_tx_buf.round); + + if (IS_INITIATOR){ + schedule_next_round = 1; + } + } + } + k_sem_give(&msg_tx_buf_sem); + + //num_receptions++; + //int carrierintegrator = dwt_readcarrierintegrator(ieee802154_dev); + // and simply dump this whole message to the output + //output_msg_to_uart("rx", rx_msg, num_msg, &carrierintegrator, &rssi); + + } else { + LOG_WRN("Got weird data of length %d", len); + net_pkt_hexdump(pkt, "<"); + } + } else { + LOG_WRN("Got WRONG data, pkt %p, len %d", pkt, len); + } + + net_pkt_unref(pkt); + + return ret; +} + +static int transmit() { + + struct net_pkt *pkt = NULL; + struct net_buf *buf = NULL; + + size_t len = sizeof (msg_header)+sizeof(msg_tx_buf); + + /* Maximum 2 bytes are added to the len */ + + while(pkt == NULL) { + pkt = net_pkt_alloc_with_buffer(NULL, len, AF_UNSPEC, 0, K_MSEC(100));//K_NO_WAIT); + if (!pkt) { + LOG_WRN("COULD NOT ALLOCATE MEMORY FOR PACKET!"); + } + } + + buf = net_buf_frag_last(pkt->buffer); + len = net_pkt_get_len(pkt); + + //LOG_DBG("Send pkt %p buf %p len %d", pkt, buf, len); + + //LOG_HEXDUMP_DBG(buf->data, buf->len, "TX Data"); + + // transmit the packet + int ret; + { + + uint64_t uus_delay = 900; // what value to choose here? Depends on the processor etc! + uint64_t estimated_ts = 0; + + struct net_ptp_time ts; + ts.second = 0; + ts.nanosecond = 0; + net_pkt_set_timestamp(pkt, &ts); + + net_pkt_write(pkt, msg_header, sizeof(msg_header)); + + k_sem_take(&msg_tx_buf_sem, K_FOREVER); + // START OF TIMING SENSITIVE + { + estimated_ts = dwt_plan_delayed_tx(ieee802154_dev, uus_delay); + + // put planned ts into the packet! + + ts_from_uint(&msg_tx_buf.tx_ts, estimated_ts); + + // all other entries are updated in the rx event! + net_pkt_write(pkt, &msg_tx_buf, sizeof(msg_tx_buf)); + ret = radio_api->tx(ieee802154_dev, IEEE802154_TX_MODE_TXTIME, pkt, buf); + } + // END OF TIMING SENSITIVE + + if (ret) { + LOG_ERR("TX: Error transmitting data!"); + } else { + + //output_msg_to_uart("tx", msg_tx_buf, MIN(NUM_MSG_TS, num_receptions+1), NULL, NULL); + + //msg_tx_buf[0].sn++; + //sent_packets++; + + //uint64_t estimated_ns = dwt_ts_to_fs(estimated_ts) / 1000000U; + //struct net_ptp_time *actual_ts = net_pkt_timestamp(pkt); + //uint64_t actual_ns = actual_ts->second * 1000000000U + actual_ts->nanosecond; + //LOG_DBG("TX: Estimated %llu Actual %llu", estimated_ns, actual_ns); + + // we also save our own message before resetting it + memcpy(&cur_msg[own_number], &msg_tx_buf, sizeof(struct msg)); + history_save(&msg_tx_buf, 0, 0, 0); + + // we reset the tx_buf Note that we still hold msg_tx_buf_sem + (void)memset(&msg_tx_buf.rx_ts, 0, sizeof(msg_tx_buf.rx_ts)); + + if (own_number == NUM_NODES-1) { + // oh wow, this was the last one! -> we end the round now + int64_t milliseconds_spent = k_uptime_delta(&round_start); + //LOG_INF("ROUND FINISHED! ms: %lld", milliseconds_spent); + on_round_end(msg_tx_buf.round); + } + } + k_sem_give(&msg_tx_buf_sem); + } + + net_pkt_unref(pkt); + return ret; +} + +/** + * Stack for the tx thread. + */ +static K_THREAD_STACK_DEFINE(tx_stack, 2048); +static struct k_thread tx_thread_data; +static void tx_thread(void); +static struct k_fifo tx_queue; + +#define TX_THREAD_PRIORITY K_PRIO_COOP(CONFIG_NUM_COOP_PRIORITIES - 1) +static void init_tx_queue(void) +{ + /* Transmit queue init */ + k_fifo_init(&tx_queue); + + k_thread_create(&tx_thread_data, tx_stack, + K_THREAD_STACK_SIZEOF(tx_stack), + (k_thread_entry_t)tx_thread, + NULL, NULL, NULL, TX_THREAD_PRIORITY, 0, K_NO_WAIT); +} + + + + +static void tx_thread(void) +{ + LOG_DBG("TX thread started"); + + while (true) { + k_sem_take(&tx_sem, K_FOREVER); + last_msg_ms = k_uptime_get(); + k_msleep(SLOT_DELAY_MS); // we sleep a bit to make sure that everyone receives our messages. + transmit(); + } +} \ No newline at end of file diff --git a/src/measurements.c b/src/measurements.c new file mode 100644 index 0000000..bbc6967 --- /dev/null +++ b/src/measurements.c @@ -0,0 +1,100 @@ +#include +#include +#include +#include +#include + +#include "measurements.h" +#include "nodes.h" + + +LOG_MODULE_REGISTER(measurements); + +#if 1 + +static struct { + size_t count; + float32_t mean; + float32_t M2; +} aggr[NUM_PAIRS]; + +// For a new value newValue, compute the new count, new mean, the new M2. +// mean accumulates the mean of the entire dataset +// M2 aggregates the squared distance from the mean +// count aggregates the number of samples seen so far +void aggr_add(int index, float32_t newValue) { + aggr[index].count += 1; + float32_t delta = newValue - aggr[index].mean; + aggr[index].mean += delta / (float32_t) aggr[index].count; + float32_t delta2 = newValue - aggr[index].mean; + aggr[index].M2 += delta * delta2; +} + +void aggr_reset(int index) { + aggr[index].count = 0; + aggr[index].mean = 0.0; + aggr[index].M2 = 0.0; +} + +float32_t aggr_mean(int index) { + return aggr[index].mean; +} + +float32_t aggr_variance(int index) { + return aggr[index].M2 / (float32_t) (aggr[index].count); +} + +void estimation_add_measurement(uint8_t a, uint8_t b, measurement_t val) { + if (a == b) { + LOG_WRN("Tried to add a measurement to itself!"); + return; + } else if (val == 0.0) { + LOG_WRN("Tried to add a zero measurement!"); + return; + } + + size_t pi = pair_index(a, b); + aggr_add(pi, val); +} + +measurement_t get_mean_measurement(size_t pi) { + return aggr_mean(pi); +} + +measurement_t get_var_measurement(size_t pi) { + return aggr_variance(pi); +} + +#else + +// note that we collect measurements of all nodes for now +static float32_t measurements_sum[NUM_PAIRS] = {0.0}; +static float32_t measurements_sum_of_squares[NUM_PAIRS] = {0.0}; + +static size_t num_measurements[NUM_PAIRS] = {0}; // note that this might overflow and wrap around if >= MAX_NUM_MEASUREMENTS + +void estimation_add_measurement(uint8_t a, uint8_t b, measurement_t val) { + if (a == b) { + //LOG_WRN("Tried to add a measurement to itself!"); + return; + } else if (val == 0.0) { + //LOG_WRN("Tried to add a zero measurement!"); + return; + } + + size_t pi = pair_index(a, b); + + measurements_sum[pi] += val; // we save it in a circular buffer + measurements_sum_of_squares[pi] += val*val; // we save it in a circular buffer + num_measurements[pi]++; // we also save the amount of measurements +} + +measurement_t get_mean_measurement(size_t pi) { + if (num_measurements[pi] == 0) { + return 0.0; + } else { + return measurements_sum[pi] / ((float32_t)num_measurements[pi]); + } +} + +#endif \ No newline at end of file diff --git a/src/measurements.h b/src/measurements.h new file mode 100644 index 0000000..9bd74c4 --- /dev/null +++ b/src/measurements.h @@ -0,0 +1,15 @@ +#ifndef MEASUREMENTS_H +#define MEASUREMENTS_H + +#include +#include +#include + + +typedef float32_t measurement_t; + +void estimation_add_measurement(uint8_t a, uint8_t b, measurement_t val); +measurement_t get_mean_measurement(size_t pi); +measurement_t get_var_measurement(size_t pi); + +#endif \ No newline at end of file diff --git a/src/nodes.c b/src/nodes.c new file mode 100644 index 0000000..ba63be1 --- /dev/null +++ b/src/nodes.c @@ -0,0 +1,77 @@ +#include +#include +#include "nodes.h" + + +// testbed defined in testbed.c + +inline size_t pair_index(uint16_t a, uint16_t b) { + if (a > b) { + size_t num_pairs_before = (a*(a-1))/2; // we have exactly that sum of measurements before + return num_pairs_before+b; + } else { + return pair_index(b, a); + } +} + + +uint16_t get_own_node_id() { + uint8_t id_buf[2] = {0}; + ssize_t copied_bytes = hwinfo_get_device_id(id_buf, sizeof(id_buf)); + return id_buf[0] << 8 | id_buf[1]; +} + + +int8_t get_node_number(uint16_t node_id) { + for (int i = 0; i < NUM_NODES; i++) { + if (node_id == node_ids[i]) { + return i; + } + } + + return -1; //node number not found! +} + +#define RANGE_CORR_MAX_RSSI (-61) +#define RANGE_CORR_MIN_RSSI (-93) + +int8_t range_bias_by_rssi[RANGE_CORR_MAX_RSSI-RANGE_CORR_MIN_RSSI+1] = { + -23, // -61dBm (-11 cm) + -23, // -62dBm (-10.75 cm) + -22, // -63dBm (-10.5 cm) + -22, // -64dBm (-10.25 cm) + -21, // -65dBm (-10.0 cm) + -21, // -66dBm (-9.65 cm) + -20, // -67dBm (-9.3 cm) + -19, // -68dBm (-8.75 cm) + -17, // -69dBm (-8.2 cm) + -16, // -70dBm (-7.55 cm) + -15, // -71dBm (-6.9 cm) + -13, // -72dBm (-6.0 cm) + -11, // -73dBm (-5.1 cm) + -8, // -74dBm (-3.9 cm) + -6, // -75dBm (-2.7 cm) + -3, // -76dBm (-1.35 cm) + 0, // -77dBm (0.0 cm) + 2, // -78dBm (1.05 cm) + 4, // -79dBm (2.1 cm) + 6, // -80dBm (2.8 cm) + 7, // -81dBm (3.5 cm) + 8, // -82dBm (3.85 cm) + 9, // -83dBm (4.2 cm) + 10, // -84dBm (4.55 cm) + 10, // -85dBm (4.9 cm) + 12, // -86dBm (5.55 cm) + 13, // -87dBm (6.2 cm) + 14, // -88dBm (6.65 cm) + 15, // -89dBm (7.1 cm) + 16, // -90dBm (7.35 cm) + 16, // -91dBm (7.6 cm) + 17, // -92dBm (7.85 cm) + 17, // -93dBm (8.1 cm) +}; + +int8_t get_range_bias_by_rssi(int8_t rssi) { + rssi = MAX(MIN(RANGE_CORR_MAX_RSSI, rssi), RANGE_CORR_MIN_RSSI); + return range_bias_by_rssi[-(rssi-RANGE_CORR_MAX_RSSI)]; +} \ No newline at end of file diff --git a/src/nodes.h b/src/nodes.h new file mode 100644 index 0000000..8084342 --- /dev/null +++ b/src/nodes.h @@ -0,0 +1,32 @@ +#ifndef NODE_NUMBERS_H +#define NODE_NUMBERS_H + +#include +#include +#include + +#define NUM_NODES 7 + + + +#define PAIRS(X) (((X)*((X)-1))/2) +#define NUM_PAIRS (PAIRS(NUM_NODES)) + +extern int16_t node_factory_antenna_delay_offsets[NUM_NODES]; +extern uint16_t node_ids[NUM_NODES]; +extern float32_t node_distances[NUM_PAIRS]; +#if 1 +extern float32_t estimation_mat_full[NUM_NODES][(NUM_PAIRS/2)]; +#else +extern float32_t estimation_mat_robust[NUM_NODES-1][(NUM_NODES-1)*(NUM_NODES-2)/2)]; +#endif + + +size_t pair_index(uint16_t a, uint16_t b); +uint16_t get_own_node_id(); +int8_t get_node_number(uint16_t node_id); + +int8_t get_range_bias_by_rssi(int8_t rssi); + + +#endif \ No newline at end of file diff --git a/src/positioning.c b/src/positioning.c new file mode 100644 index 0000000..56db00d --- /dev/null +++ b/src/positioning.c @@ -0,0 +1,50 @@ + +#include "nodes.h" +#include "positioning.h" + +static uint16_t own_node_id; +static float32_t estimates[NUM_NODES][3]; +static float32_t distances[NUM_NODES]; + +void positioning_init() { + own_node_id = get_own_node_id(); + positioning_reset(); +} + +void positioning_reset() { + // reset all position and distances + memset(node_pos, 0, sizeof(node_pos)); + memset(distances, 0, sizeof(distances)); +} + +void positioning_set_distances(float32_t new_distances[NUM_NODES]) { + memcpy(distances, new_distances, sizeof(distances)); +} + +void positioning_set_estimate(uint16_t other_number, pos_t &new_pos) { + node_pos[other_number] = *new_pos; +} + +void positioning_update_own_estimate(pos_t &out) { + + estimates + + if own_number == 0 -> skip everything + if own_number == 1 -> skip two dimensions (e.g. set to the actual distance) + if own_number == 2 -> skip one dimensions (e.g. set to the actual distance) + if own_number == 3 -> force one dimension > 0 + + + // iterate over each dimension + for(size_t d = 0; d < 3; d++) { + + } + + + + node_pos[own_node_id] + + + // copy estimate back + *out = node_pos[own_node_id]; +} \ No newline at end of file diff --git a/src/positioning.h b/src/positioning.h new file mode 100644 index 0000000..d2cf278 --- /dev/null +++ b/src/positioning.h @@ -0,0 +1,15 @@ +#if 0 +#ifndef POSITIONING_H +#define POSITIONING_H + +typedef float32_t pos_t[3]: + +void positioning_init(); +void positioning_reset(); + +void positioning_set_distances(float32_t distances[NUM_NODES]); +void positioning_set_estimate(uint16_t other_number, pos_t &new_pos); +void positioning_update_own_estimate(pos_t &out); + +#endif +#endif \ No newline at end of file diff --git a/src/testbed.c b/src/testbed.c new file mode 100644 index 0000000..1d9f13c --- /dev/null +++ b/src/testbed.c @@ -0,0 +1,49 @@ +#include "nodes.h" +// ---------------------- +// Definition of Testbed trento_a +#if NUM_NODES>7 +#error Testbed TRENTO_A only has 7 nodes +#elif NUM_NODES<7 +#warning Testbed TRENTO_A has more nodes available (7 in total) +#endif +uint16_t node_ids[NUM_NODES] = { + 0x5b9a, // dwm1001-1 + 0x56d4, // dwm1001-2 + 0x0345, // dwm1001-3 + 0x9535, // dwm1001-4 + 0x87e8, // dwm1001-5 + 0xa7d8, // dwm1001-6 + 0x24f1, // dwm1001-7 +}; +int16_t node_factory_antenna_delay_offsets[NUM_NODES] = { + 22, // dwm1001-1 + 9, // dwm1001-2 + 22, // dwm1001-3 + 22, // dwm1001-4 + 8, // dwm1001-5 + 10, // dwm1001-6 + 10, // dwm1001-7 +}; +float32_t node_distances[NUM_PAIRS] = { + 4.1886, // dwm1001-2 to dwm1001-1 + 2.8902, // dwm1001-3 to dwm1001-1 + 3.2404, // dwm1001-3 to dwm1001-2 + 4.1442, // dwm1001-4 to dwm1001-1 + 6.245, // dwm1001-4 to dwm1001-2 + 3.01, // dwm1001-4 to dwm1001-3 + 4.5621, // dwm1001-5 to dwm1001-1 + 8.7045, // dwm1001-5 to dwm1001-2 + 7.0529, // dwm1001-5 to dwm1001-3 + 6.4106, // dwm1001-5 to dwm1001-4 + 3.5417, // dwm1001-6 to dwm1001-1 + 6.9276, // dwm1001-6 to dwm1001-2 + 6.4305, // dwm1001-6 to dwm1001-3 + 7.1249, // dwm1001-6 to dwm1001-4 + 3.0, // dwm1001-6 to dwm1001-5 + 4.7443, // dwm1001-7 to dwm1001-1 + 6.2323, // dwm1001-7 to dwm1001-2 + 7.1752, // dwm1001-7 to dwm1001-3 + 8.8789, // dwm1001-7 to dwm1001-4 + 5.9804, // dwm1001-7 to dwm1001-5 + 2.9806, // dwm1001-7 to dwm1001-6 +}; \ No newline at end of file diff --git a/src/testbed_lille.c b/src/testbed_lille.c new file mode 100644 index 0000000..23da30f --- /dev/null +++ b/src/testbed_lille.c @@ -0,0 +1,133 @@ +#include "nodes.h" +// ---------------------- +// Definition of Testbed lille +#if NUM_NODES>14 +#error Testbed LILLE only has 14 nodes +#elif NUM_NODES<14 +#warning Testbed LILLE has more nodes available (14 in total) +#endif +uint16_t node_ids[NUM_NODES] = { + 0x6b93, // dwm1001-1 + 0xf1c3, // dwm1001-2 + 0xc240, // dwm1001-3 + 0x013f, // dwm1001-4 + 0xb227, // dwm1001-5 + 0x033b, // dwm1001-6 + 0xf524, // dwm1001-7 + 0x37b2, // dwm1001-8 + 0x15ef, // dwm1001-9 + 0x7e02, // dwm1001-10 + 0xad36, // dwm1001-11 + 0x3598, // dwm1001-12 + 0x47e0, // dwm1001-13 + 0x0e92, // dwm1001-14 +}; +int16_t node_factory_antenna_delay_offsets[NUM_NODES] = { + 8, // dwm1001-1 + 9, // dwm1001-2 + 7, // dwm1001-3 + 22, // dwm1001-4 + 11, // dwm1001-5 + 12, // dwm1001-6 + 7, // dwm1001-7 + 22, // dwm1001-8 + 22, // dwm1001-9 + 22, // dwm1001-10 + -14, // dwm1001-11 + -9, // dwm1001-12 + 9, // dwm1001-13 + -13, // dwm1001-14 +}; +float32_t node_distances[NUM_PAIRS] = { + 1.8515, // dwm1001-2 to dwm1001-1 + 3.8663, // dwm1001-3 to dwm1001-1 + 2.4, // dwm1001-3 to dwm1001-2 + 4.8, // dwm1001-4 to dwm1001-1 + 3.8663, // dwm1001-4 to dwm1001-2 + 1.8515, // dwm1001-4 to dwm1001-3 + 2.7906, // dwm1001-5 to dwm1001-1 + 1.1697, // dwm1001-5 to dwm1001-2 + 2.0611, // dwm1001-5 to dwm1001-3 + 3.6807, // dwm1001-5 to dwm1001-4 + 4.7103, // dwm1001-6 to dwm1001-1 + 3.1636, // dwm1001-6 to dwm1001-2 + 1.1697, // dwm1001-6 to dwm1001-3 + 2.2152, // dwm1001-6 to dwm1001-4 + 2.4, // dwm1001-6 to dwm1001-5 + 4.1928, // dwm1001-7 to dwm1001-1 + 3.3407, // dwm1001-7 to dwm1001-2 + 3.747, // dwm1001-7 to dwm1001-3 + 4.8311, // dwm1001-7 to dwm1001-4 + 2.4, // dwm1001-7 to dwm1001-5 + 3.3941, // dwm1001-7 to dwm1001-6 + 5.655, // dwm1001-8 to dwm1001-1 + 4.4497, // dwm1001-8 to dwm1001-2 + 3.3407, // dwm1001-8 to dwm1001-3 + 3.834, // dwm1001-8 to dwm1001-4 + 3.3941, // dwm1001-8 to dwm1001-5 + 2.4, // dwm1001-8 to dwm1001-6 + 2.4, // dwm1001-8 to dwm1001-7 + 6.6822, // dwm1001-9 to dwm1001-1 + 5.9458, // dwm1001-9 to dwm1001-2 + 5.6984, // dwm1001-9 to dwm1001-3 + 6.2363, // dwm1001-9 to dwm1001-4 + 4.9477, // dwm1001-9 to dwm1001-5 + 4.9477, // dwm1001-9 to dwm1001-6 + 2.6833, // dwm1001-9 to dwm1001-7 + 2.6833, // dwm1001-9 to dwm1001-8 + 7.3394, // dwm1001-10 to dwm1001-1 + 6.8883, // dwm1001-10 to dwm1001-2 + 7.0942, // dwm1001-10 to dwm1001-3 + 7.7219, // dwm1001-10 to dwm1001-4 + 6.0, // dwm1001-10 to dwm1001-5 + 6.4622, // dwm1001-10 to dwm1001-6 + 3.6, // dwm1001-10 to dwm1001-7 + 4.3267, // dwm1001-10 to dwm1001-8 + 1.6971, // dwm1001-10 to dwm1001-9 + 8.2624, // dwm1001-11 to dwm1001-1 + 7.4892, // dwm1001-11 to dwm1001-2 + 6.8883, // dwm1001-11 to dwm1001-3 + 7.1405, // dwm1001-11 to dwm1001-4 + 6.4622, // dwm1001-11 to dwm1001-5 + 6.0, // dwm1001-11 to dwm1001-6 + 4.3267, // dwm1001-11 to dwm1001-7 + 3.6, // dwm1001-11 to dwm1001-8 + 1.6971, // dwm1001-11 to dwm1001-9 + 2.4, // dwm1001-11 to dwm1001-10 + 9.2086, // dwm1001-12 to dwm1001-1 + 9.2429, // dwm1001-12 to dwm1001-2 + 9.5494, // dwm1001-12 to dwm1001-3 + 9.8142, // dwm1001-12 to dwm1001-4 + 8.5466, // dwm1001-12 to dwm1001-5 + 9.0379, // dwm1001-12 to dwm1001-6 + 6.246, // dwm1001-12 to dwm1001-7 + 6.9031, // dwm1001-12 to dwm1001-8 + 4.4023, // dwm1001-12 to dwm1001-9 + 3.0926, // dwm1001-12 to dwm1001-10 + 4.2666, // dwm1001-12 to dwm1001-11 + 9.5868, // dwm1001-13 to dwm1001-1 + 9.2122, // dwm1001-13 to dwm1001-2 + 9.2122, // dwm1001-13 to dwm1001-3 + 9.5868, // dwm1001-13 to dwm1001-4 + 8.3167, // dwm1001-13 to dwm1001-5 + 8.4881, // dwm1001-13 to dwm1001-6 + 5.9276, // dwm1001-13 to dwm1001-7 + 6.1657, // dwm1001-13 to dwm1001-8 + 3.5531, // dwm1001-13 to dwm1001-9 + 2.385, // dwm1001-13 to dwm1001-10 + 2.9271, // dwm1001-13 to dwm1001-11 + 2.0809, // dwm1001-13 to dwm1001-12 + 9.8142, // dwm1001-14 to dwm1001-1 + 9.5494, // dwm1001-14 to dwm1001-2 + 9.2429, // dwm1001-14 to dwm1001-3 + 9.2086, // dwm1001-14 to dwm1001-4 + 8.7134, // dwm1001-14 to dwm1001-5 + 8.5466, // dwm1001-14 to dwm1001-6 + 6.4724, // dwm1001-14 to dwm1001-7 + 6.246, // dwm1001-14 to dwm1001-8 + 4.062, // dwm1001-14 to dwm1001-9 + 3.5276, // dwm1001-14 to dwm1001-10 + 3.0926, // dwm1001-14 to dwm1001-11 + 2.4, // dwm1001-14 to dwm1001-12 + 2.0809, // dwm1001-14 to dwm1001-13 +}; \ No newline at end of file diff --git a/src/testbed_trento_a.c b/src/testbed_trento_a.c new file mode 100644 index 0000000..1d9f13c --- /dev/null +++ b/src/testbed_trento_a.c @@ -0,0 +1,49 @@ +#include "nodes.h" +// ---------------------- +// Definition of Testbed trento_a +#if NUM_NODES>7 +#error Testbed TRENTO_A only has 7 nodes +#elif NUM_NODES<7 +#warning Testbed TRENTO_A has more nodes available (7 in total) +#endif +uint16_t node_ids[NUM_NODES] = { + 0x5b9a, // dwm1001-1 + 0x56d4, // dwm1001-2 + 0x0345, // dwm1001-3 + 0x9535, // dwm1001-4 + 0x87e8, // dwm1001-5 + 0xa7d8, // dwm1001-6 + 0x24f1, // dwm1001-7 +}; +int16_t node_factory_antenna_delay_offsets[NUM_NODES] = { + 22, // dwm1001-1 + 9, // dwm1001-2 + 22, // dwm1001-3 + 22, // dwm1001-4 + 8, // dwm1001-5 + 10, // dwm1001-6 + 10, // dwm1001-7 +}; +float32_t node_distances[NUM_PAIRS] = { + 4.1886, // dwm1001-2 to dwm1001-1 + 2.8902, // dwm1001-3 to dwm1001-1 + 3.2404, // dwm1001-3 to dwm1001-2 + 4.1442, // dwm1001-4 to dwm1001-1 + 6.245, // dwm1001-4 to dwm1001-2 + 3.01, // dwm1001-4 to dwm1001-3 + 4.5621, // dwm1001-5 to dwm1001-1 + 8.7045, // dwm1001-5 to dwm1001-2 + 7.0529, // dwm1001-5 to dwm1001-3 + 6.4106, // dwm1001-5 to dwm1001-4 + 3.5417, // dwm1001-6 to dwm1001-1 + 6.9276, // dwm1001-6 to dwm1001-2 + 6.4305, // dwm1001-6 to dwm1001-3 + 7.1249, // dwm1001-6 to dwm1001-4 + 3.0, // dwm1001-6 to dwm1001-5 + 4.7443, // dwm1001-7 to dwm1001-1 + 6.2323, // dwm1001-7 to dwm1001-2 + 7.1752, // dwm1001-7 to dwm1001-3 + 8.8789, // dwm1001-7 to dwm1001-4 + 5.9804, // dwm1001-7 to dwm1001-5 + 2.9806, // dwm1001-7 to dwm1001-6 +}; \ No newline at end of file diff --git a/src/testbed_trento_b.c b/src/testbed_trento_b.c new file mode 100644 index 0000000..1eb9af7 --- /dev/null +++ b/src/testbed_trento_b.c @@ -0,0 +1,133 @@ +#include "nodes.h" +// ---------------------- +// Definition of Testbed trento_b +#if NUM_NODES>14 +#error Testbed TRENTO_B only has 14 nodes +#elif NUM_NODES<14 +#warning Testbed TRENTO_B has more nodes available (14 in total) +#endif +uint16_t node_ids[NUM_NODES] = { + 0x330b, // dwm1001-160 + 0xbee3, // dwm1001-161 + 0x17e3, // dwm1001-162 + 0x427e, // dwm1001-163 + 0xdd4f, // dwm1001-164 + 0x292b, // dwm1001-165 + 0x69f6, // dwm1001-166 + 0x3843, // dwm1001-167 + 0x3aea, // dwm1001-168 + 0x2df2, // dwm1001-169 + 0x3392, // dwm1001-170 + 0x5418, // dwm1001-171 + 0x869a, // dwm1001-172 + 0x1582, // dwm1001-173 +}; +int16_t node_factory_antenna_delay_offsets[NUM_NODES] = { + 8, // dwm1001-160 + 10, // dwm1001-161 + 22, // dwm1001-162 + 31, // dwm1001-163 + 9, // dwm1001-164 + 22, // dwm1001-165 + 10, // dwm1001-166 + 22, // dwm1001-167 + 11, // dwm1001-168 + 22, // dwm1001-169 + 8, // dwm1001-170 + 22, // dwm1001-171 + 22, // dwm1001-172 + 22, // dwm1001-173 +}; +float32_t node_distances[NUM_PAIRS] = { + 1.86, // dwm1001-161 to dwm1001-160 + 3.9668, // dwm1001-162 to dwm1001-160 + 3.3517, // dwm1001-162 to dwm1001-161 + 6.1471, // dwm1001-163 to dwm1001-160 + 5.7759, // dwm1001-163 to dwm1001-161 + 2.4301, // dwm1001-163 to dwm1001-162 + 7.7885, // dwm1001-164 to dwm1001-160 + 7.5038, // dwm1001-164 to dwm1001-161 + 4.1602, // dwm1001-164 to dwm1001-162 + 1.7301, // dwm1001-164 to dwm1001-163 + 9.5298, // dwm1001-165 to dwm1001-160 + 9.3026, // dwm1001-165 to dwm1001-161 + 5.9603, // dwm1001-165 to dwm1001-162 + 3.5302, // dwm1001-165 to dwm1001-163 + 1.8001, // dwm1001-165 to dwm1001-164 + 10.2202, // dwm1001-166 to dwm1001-160 + 10.3988, // dwm1001-166 to dwm1001-161 + 7.2232, // dwm1001-166 to dwm1001-162 + 4.9553, // dwm1001-166 to dwm1001-163 + 3.4733, // dwm1001-166 to dwm1001-164 + 2.3294, // dwm1001-166 to dwm1001-165 + 10.5026, // dwm1001-167 to dwm1001-160 + 11.08, // dwm1001-167 to dwm1001-161 + 8.254, // dwm1001-167 to dwm1001-162 + 6.3572, // dwm1001-167 to dwm1001-163 + 5.2753, // dwm1001-167 to dwm1001-164 + 4.5931, // dwm1001-167 to dwm1001-165 + 2.36, // dwm1001-167 to dwm1001-166 + 10.3449, // dwm1001-168 to dwm1001-160 + 11.2806, // dwm1001-168 to dwm1001-161 + 8.9366, // dwm1001-168 to dwm1001-162 + 7.5159, // dwm1001-168 to dwm1001-163 + 6.8533, // dwm1001-168 to dwm1001-164 + 6.59, // dwm1001-168 to dwm1001-165 + 4.5421, // dwm1001-168 to dwm1001-166 + 2.2795, // dwm1001-168 to dwm1001-167 + 8.7722, // dwm1001-169 to dwm1001-160 + 9.8565, // dwm1001-169 to dwm1001-161 + 7.8515, // dwm1001-169 to dwm1001-162 + 6.85, // dwm1001-169 to dwm1001-163 + 6.6001, // dwm1001-169 to dwm1001-164 + 6.8139, // dwm1001-169 to dwm1001-165 + 5.1913, // dwm1001-169 to dwm1001-166 + 3.4004, // dwm1001-169 to dwm1001-167 + 1.78, // dwm1001-169 to dwm1001-168 + 7.2449, // dwm1001-170 to dwm1001-160 + 8.5279, // dwm1001-170 to dwm1001-161 + 7.0464, // dwm1001-170 to dwm1001-162 + 6.6308, // dwm1001-170 to dwm1001-163 + 6.8586, // dwm1001-170 to dwm1001-164 + 7.5236, // dwm1001-170 to dwm1001-165 + 6.3644, // dwm1001-170 to dwm1001-166 + 5.0071, // dwm1001-170 to dwm1001-167 + 3.64, // dwm1001-170 to dwm1001-168 + 1.86, // dwm1001-170 to dwm1001-169 + 5.5613, // dwm1001-171 to dwm1001-160 + 7.1556, // dwm1001-171 to dwm1001-161 + 6.6608, // dwm1001-171 to dwm1001-162 + 7.1057, // dwm1001-171 to dwm1001-163 + 7.8722, // dwm1001-171 to dwm1001-164 + 8.9601, // dwm1001-171 to dwm1001-165 + 8.2832, // dwm1001-171 to dwm1001-166 + 7.2891, // dwm1001-171 to dwm1001-167 + 6.07, // dwm1001-171 to dwm1001-168 + 4.29, // dwm1001-171 to dwm1001-169 + 2.43, // dwm1001-171 to dwm1001-170 + 4.52, // dwm1001-172 to dwm1001-160 + 6.3771, // dwm1001-172 to dwm1001-161 + 7.3119, // dwm1001-172 to dwm1001-162 + 8.5952, // dwm1001-172 to dwm1001-163 + 9.7741, // dwm1001-172 to dwm1001-164 + 11.1547, // dwm1001-172 to dwm1001-165 + 10.8724, // dwm1001-172 to dwm1001-166 + 10.1378, // dwm1001-172 to dwm1001-167 + 9.01, // dwm1001-172 to dwm1001-168 + 7.23, // dwm1001-172 to dwm1001-169 + 5.37, // dwm1001-172 to dwm1001-170 + 2.94, // dwm1001-172 to dwm1001-171 + 2.3901, // dwm1001-173 to dwm1001-160 + 4.25, // dwm1001-173 to dwm1001-161 + 5.6163, // dwm1001-173 to dwm1001-162 + 7.3077, // dwm1001-173 to dwm1001-163 + 8.7241, // dwm1001-173 to dwm1001-164 + 10.3005, // dwm1001-173 to dwm1001-165 + 10.4627, // dwm1001-173 to dwm1001-166 + 10.2, // dwm1001-173 to dwm1001-167 + 9.5288, // dwm1001-173 to dwm1001-168 + 7.8008, // dwm1001-173 to dwm1001-169 + 6.0346, // dwm1001-173 to dwm1001-170 + 3.8607, // dwm1001-173 to dwm1001-171 + 2.1384, // dwm1001-173 to dwm1001-172 +}; \ No newline at end of file diff --git a/src/uart.c b/src/uart.c new file mode 100644 index 0000000..3993146 --- /dev/null +++ b/src/uart.c @@ -0,0 +1,16 @@ + +#include +#include "uart.h" + +static const struct device* uart_device = DEVICE_DT_GET(DT_CHOSEN(zephyr_console)); + +void uart_out(char* msg) { + while (*msg != '\0') { + uart_poll_out(uart_device, *msg); + msg++; + } +} + +void uart_disabled(char* msg) { + //NOOP +} \ No newline at end of file diff --git a/src/uart.h b/src/uart.h new file mode 100644 index 0000000..3f519df --- /dev/null +++ b/src/uart.h @@ -0,0 +1,9 @@ +#ifndef UART_H +#define UART_H + + +void uart_out(char* msg); +void uart_disabled(char* msg); + + +#endif \ No newline at end of file