diff --git a/README.md b/README.md index c3a903d..56a1650 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,7 @@ # Silicon Labs Bluetooth Applications # -[![Version Badge](https://img.shields.io/badge/-v2.0.3-green)](https://github.com/SiliconLabs/bluetooth_applications/releases) +[![Version Badge](https://img.shields.io/badge/-v2.1.0-green)](https://github.com/SiliconLabs/bluetooth_applications/releases) [![GSDK Badge](https://img.shields.io/badge/GSDK-v4.4.0-green)](https://github.com/SiliconLabs/gecko_sdk/releases) [![TPHD Version Badge](https://img.shields.io/badge/TPHD-v2.0.0+-green)](https://github.com/SiliconLabs/third_party_hw_drivers_extension/releases) ![License badge](https://img.shields.io/badge/License-Zlib-green) @@ -84,6 +84,7 @@ This repository provides both SLCP projects (as External Repositories) and SLS p | 56 | Bluetooth - ESL Tag with E-Paper display 1,54inch 200x200 dots from Mikroe | [Click Here](./bluetooth_esl_tag_mikroe_eink154_e_paper_display) | | 56 | Bluetooth - RSSI - based position estimation for PEPS | [Click Here](./bluetooth_rssi_positioning_for_peps) | | 57 | Bluetooth - BTHome v2 - PIR Alarm | [Click Here](./bluetooth_bthome_v2_pir_alarm/) | +| 57 | Bluetooth - DW3000 TWR demo | [Click Here](./bluetooth_uwb_dw3000_slotted_twr/) | ## Requirements ## diff --git a/bluetooth_uwb_dw3000_slotted_twr/README.md b/bluetooth_uwb_dw3000_slotted_twr/README.md new file mode 100644 index 0000000..8a2ddf5 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/README.md @@ -0,0 +1,164 @@ +# Bluetooth - DW3000 TWR demo + +![Type badge](https://img.shields.io/badge/dynamic/json?url=https://raw.githubusercontent.com/SiliconLabs/application_examples_ci/master/bluetooth_applications/bluetooth_uwb_dw3000_slotted_twr_common.json&label=Type&query=type&color=green) +![Technology badge](https://img.shields.io/badge/dynamic/json?url=https://raw.githubusercontent.com/SiliconLabs/application_examples_ci/master/bluetooth_applications/bluetooth_uwb_dw3000_slotted_twr_common.json&label=Technology&query=technology&color=green) +![License badge](https://img.shields.io/badge/dynamic/json?url=https://raw.githubusercontent.com/SiliconLabs/application_examples_ci/master/bluetooth_applications/bluetooth_uwb_dw3000_slotted_twr_common.json&label=License&query=license&color=green) +![SDK badge](https://img.shields.io/badge/dynamic/json?url=https://raw.githubusercontent.com/SiliconLabs/application_examples_ci/master/bluetooth_applications/bluetooth_uwb_dw3000_slotted_twr_common.json&label=SDK&query=sdk&color=green) +[![Required board](https://img.shields.io/badge/Mikroe-UWB%202%20Click%20board™-green)](https://www.mikroe.com/uwb-2-click) +![Build badge](https://img.shields.io/endpoint?url=https://raw.githubusercontent.com/SiliconLabs/application_examples_ci/master/bluetooth_applications/bluetooth_uwb_dw3000_slotted_twr_build_status.json) +![Flash badge](https://img.shields.io/badge/dynamic/json?url=https://raw.githubusercontent.com/SiliconLabs/application_examples_ci/master/bluetooth_applications/bluetooth_uwb_dw3000_slotted_twr_common.json&label=Flash&query=flash&color=blue) +![RAM badge](https://img.shields.io/badge/dynamic/json?url=https://raw.githubusercontent.com/SiliconLabs/application_examples_ci/master/bluetooth_applications/bluetooth_uwb_dw3000_slotted_twr_common.json&label=RAM&query=ram&color=blue) +## Summary + +Ultra-wideband (UWB) is a radio technology based on the IEEE 802.15.4a and 802.15.4z standards that can enable a more precise measure of the Time of Flight of the radio signal, leading to centimeter accuracy distance/location measurement. UWB technology can be implemented in different ways based on the target applications needs: Time Difference of Arrival (TDoA), Two Way Ranging, or Phase Difference of Arrival (PDoA). + +This example is based on **DW3000 TWR demo** example, which can be downloaded from the [DW3000 module page](https://www.qorvo.com/products/p/DWM3000). The demo consists of two Mikroe UWB 2 Click units with DW3000 IC, one running a NODE application and the other a TAG application. The node performs double-sided two-way ranging with a tag, and then calculates the range (and optionally PDoA with tag’s x, y location coordinates) and reports the results to an external application (e.g. PC GUI application). The PC GUI application then plots the position of the tags based on the reported values. + +## Gecko SDK Suite version + +- GSDK v4.4.3 +- [Third-Party Hardware Drivers v2.0.2](https://github.com/SiliconLabs/third_party_hw_drivers_extension) + +## Required Hardware + +- [EFR32xG24 Explorer Kit](https://www.silabs.com/development-tools/wireless/efr32xg24-explorer-kit?tab=overview) + +- [UWB 2 Click board™](https://www.mikroe.com/uwb-2-click) + +**NOTE:** +Tested boards for working with this example: + +| Board ID | Description | +| --- | --- | +| BRD2703A | [xG24-EK2703A - EFR32xG24 Explorer Kit](https://www.silabs.com/development-tools/wireless/efr32xg24-explorer-kit?tab=overview) | + +## Hardware Connection + +The DWM3000 - UWB 2 Click board™ supports MikroBus. Therefore, it can connect easily to the MikroBus header of the EFR32xG24 Explorer Kit. Be sure that the 45-degree corner of the board matches the 45-degree white line of the Explorer Kit. The hardware connection is shown in the image below: + +![board](images/hardware_connection.png) + +## Setup + +### Create a project based on an example project + +1. From the Launcher Home, add the BRD2703A to My Products, click on it, and click on the **EXAMPLE PROJECTS & DEMOS** tab. Find the example project filtering by "twr". + +2. Click **Create** button on the **Bluetooth - UWB Slotted TWR Demo** example. Example project creation dialog pops up -> click Create and Finish and Project should be generated. + +![create_example](images/create_example.png) + +3. Build and flash this example to the board. + +**Note:** + +- Make sure that the SDK extension has already been installed. If not, please follow [this documentation](https://github.com/SiliconLabs/third_party_hw_drivers_extension/blob/master/README.md#how-to-add-to-simplicity-studio-ide). + +- SDK Extension must be enabled for the project to install "DWM3000 - UWB 2 Click (Mikroe)" component. + +### Port to different board or custom hardware (EFR32xG24) + +1. Create a project based on an example project follow the [previous section](#create-a-project-based-on-an-example-project) +2. Open .slcp file, slect **OVERVIEW** section and change the target board or target MCU + + | | | + | --- | --- | + | ![change_target](images/change_target.png) | ![select_target](images/select_target.png) | + +3. Open .slcp file, slect **SOFTWARE COMPONENTS** section, find the component with name **DWM3000 - UWB 2 Click (Mikroe)** in **SDK Extension: Third Party Hardware Drivers** and modify the GPIO configuration. + + | | | | + | --- | --- | --- | + | ![edit_component](images/edit_component.png) | ![edit_component2](images/edit_component2.png) | | + + The current used pin configuration is as follows: + + | Functionality name | Macro name | GPIO port and pin | + | --- | --- | --- | + | DW3000 SPI Clock | DWM3000_CLK | gpioPortC1 | + | DW3000 SPI Master In, Slave Out | DWM3000_MISO |gpioPortC2 | + | DW3000 SPI Master Out, Slave In | DWM3000_MOSI | gpioPortC3 | + | DW3000 SPI Chip Select | DWM3000_CS | gpioPortC0 | + | DW3000 Interrupt | DWM3000_INT | gpioPortB1 | + | DW3000 Reset | DWM3000_RESET | gpioPortC8 | + +4. Open file: src/platform/port/port_uart.h and modify the GPIO configuration. The current used pin configuration is as follows: + | Functionality name | Macro name | GPIO port and pin | + | --- | --- | --- | + | Virtual COM UART enable (if exists, can be undefined) | HAL_UART_ENABLE_PORT, HAL_UART_ENABLE_PIN | undefined | + | Virtual COM UART Tx | HAL_UART_TX_PORT, HAL_UART_TX_PIN | gpioPortA5 | + | Virtual COM UART Rx | HAL_UART_RX_PORT, HAL_UART_RX_PIN | gpioPortA6 | + | Button | HAL_IO_PORT_BUTTON, HAL_IO_PIN_BUTTON | gpioPortB2 | + | Error LED | HAL_IO_PORT_LED_ERROR, HAL_IO_PIN_LED_ERROR | gpioPortA4 | + +5. Open file: linkerfile.ld and modify the memory configuration to match with the new target MCU. The current memory configuration of brd2703a board is as follows: + + ``` + FLASH_PAGE_SIZE = 0x2000; + FLASH_SIZE = 0xfe000; + RAM_SIZE = 0x40000; + ``` + +## How It Works + +The detailed documentation can be downloaded from [Qorvo website](https://www.qorvo.com/products/d/da007992). In that package you can find the example document with path: Software/Slotted_TWR_Demo_ARM/Docs/SW-DW3000-TWR-demo.pdf + +### Basic operation + +The basic operation of the system is as follows: The node performs double-sided two-way ranging with a tag, and then calculates the range (and optionally PDoA with tag’s x, y location coordinates) and reports the results to an external application (e.g. PC GUI application). The PC GUI application then plots the position of the tags based on the reported values. The image bellow describes the Discovery and Ranging phases + +![discovery_and_ranging_phases](images/discovery_and_ranging_phases.png) + +1. TAGs start operating in Discovery mode, periodically sending Blink messages. +2. The node listens for Blink messages from tags and when a Blink message is received, the node responds to the tag with a Ranging Config message. +3. The Ranging Config message provides information to the tag describing how to perform ranging with the node. This information includes the PAN ID, the short address of the node, a short address assignment for the tag, timing parameters for the ranging phase to be used at the start of the next ranging exchange. Upon receiving the Ranging Config message, the tag operation changes to the Ranging mode where it periodically initiates ranging exchanges with the node. +4. Each ranging exchange starts with the tag sending a Poll message. +5. When the node receives the Poll message, it replies with a Response message, and the tag completes the ranging exchange by sending a Final message. +6. The node then calculates the range to the tag which it reports via USB/UART for displaying by the PC GUI application. + +NOTE: when using configurations with STS modes, the STS timestamp should be validated before replying to the Poll or Response messages. + +### Concept of Discovered and Known tags lists + +Before the node starts ranging to a tag, the tag needs to be added to a list specifying tags to which the node is allowed to range with. This list of tags is called "known tags list" or KList. Every record in the KList has all necessary information about each tag including: its 64-bit address, assigned 16-bit (short) address, assigned slot number, etc. This information is supplied to the tag in the Ranging Config reply by the node following the reception of the tag’s blink message. The KList can be saved and it will then be available for use during autonomous working mode of the NODE (i.e. after start-up). + +### Testing + +The testing can be done with 2 devices, one runs as NODE, one runs as TAG. It also can work with one NODE and many TAGs. + +#### Testing with controlling of the embedded applications over uart console + +To test the ranging application with uart console, we need 2 devies with TWR Demo application running. Then using the teminal console tool like PUTY to connect with them over UART (JLINK USB-UART bridge) + +1. Setup operation mode: Setup one device as NODE by sending command: "NODE", one device as TAG by sending command: "TAG" as the picture bellow +![setup](images/setup.png) +After the operation mode of the TAG device is changed, it will periodically sending "blink messages". And when the TAG is discovered, the NODE will report the TAG's address as 64 bits hex. + +2. Add TAG with addr64 to known tags list (KList): Use ADDTAG command on the NODE device: + > ADDTAG \ \ \ \ \ + + - \ is address of the tag, hexadecimal, must be 16 characters; + - \ is request to assign this short address of the tag, hexadecimal; This address may be automatically changed by the node, as KList is protected from adding of identical addresses in it. + - \ is hexadecimal value which will be used by the tag if it considered it is moving. This is in number of superframes. For example, 1 means that the Tag, when its moving, will range to the Node every superframe, and “0A” \(decimal 10\) means tag will range to the node every 10-th superframe. + - \ is hexadecimal value which will be used by the tag if it considered it is not moving, i.e. stationary. This value usually specified to a big number, 64 hexadecimal means tag will range to the node every 100 superframes. See Example below. + - \ is a hexadecimal bitfield parameter to pass to the tag. Bit 0 indicates tag shall use IMU to detect if it stationary or moving. Bits 1-15 are not used. + + Example: + > ADDTAG 0000000050f20595 1000 2 64 1 + + This instructs the node to add the tag to the KList with long address “0x0000000050f20595”, try to assign to this tag a new short address “0x1000”, configure tag to use IMU, the tag should range to the node every 2 superframes if it is moving (giving superframe is 100 ms, this means tag will range 5 times a second), and when tag is stationary, range every 100 superframes, i.e. every 10 seconds. On success the command will return a "TagAdded" JSON object with actual parameters, assigned to the tag + +3. Ranging phase: If the TAG is successfull added, the NODE device will start ranging imediately and then report the distance beetween TAG and NODE periodically. + + ![ranging](images/ranging.png) + +#### Testing with controlling of the embedded applications over a PC GUI app + +When NODE top-level application is running, it can be controlled externally by UART and accepting specific commands, which belongs specifically to the NODE top-level application. + +1. Setup operation mode: Connect 2 devices to PC by using USB cable. Then setup one device as TAG by sending command: "TAG", use PDoA GUI PC application to connect to other device to make it as a NODE. + + - When the PDoA GUI connected to the NODE, it will show as the picture bellow. + ![DecaPDOARTLS](images/DecaPDOARTLS.png) + - When new TAG is discovered, it will be shown in the device list table. Select the joined checkbox to add new TAG, the ranging phase will start imediately after that. Then the PDoA GUI will show the TAG relative location to the NODE in green area. + ![DecaPDOARTLS_ranging](images/DecaPDOARTLS_ranging.png) \ No newline at end of file diff --git a/bluetooth_uwb_dw3000_slotted_twr/SimplicityStudio/bluetooth_uwb_dw3000_slotted_twr.slcp b/bluetooth_uwb_dw3000_slotted_twr/SimplicityStudio/bluetooth_uwb_dw3000_slotted_twr.slcp new file mode 100644 index 0000000..bb08d1f --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/SimplicityStudio/bluetooth_uwb_dw3000_slotted_twr.slcp @@ -0,0 +1,337 @@ +# Silicon Labs Project Configuration Tools: slcp, v0, Component selection file. +project_name: bluetooth_uwb_dw3000_slotted_twr +label: Bluetooth - UWB Slotted Two Way Ranging Demo +description: | + This project aims to implement operation of Ultra-wideband Two Way Ranging method on DW3000. +category: Bluetooth Examples + +quality: experimental + +filter: + - name: "Device Type" + value: ["SoC"] + - name: "MCU" + value: ["32-bit MCU"] + - name: "Project Difficulty" + value: ["Advanced"] + - name: "Wireless Technology" + value: ["Bluetooth"] + +package: Bluetooth + +readme: +- path: ../README.md +source: +- path: ../src/apps/listener/listener/listener.c + directory: src/apps/listener/listener +- path: ../src/apps/listener/task_listener/task_listener.c + directory: src/apps/listener/task_listener +- path: ../src/apps/node/node/node.c + directory: src/apps/node/node +- path: ../src/apps/node/task_node/task_node.c + directory: src/apps/node/task_node +- path: ../src/apps/tag/tag/tag.c + directory: src/apps/tag/tag +- path: ../src/apps/tag/task_tag/task_tag.c + directory: src/apps/tag/task_tag +- path: ../src/apps/tcfm/task_tcfm/task_tcfm.c + directory: src/apps/tcfm/task_tcfm +- path: ../src/apps/tcfm/tcfm/tcfm.c + directory: src/apps/tcfm/tcfm +- path: ../src/apps/tcwm/task_tcwm/task_tcwm.c + directory: src/apps/tcwm/task_tcwm +- path: ../src/apps/tcwm/tcwm/tcwm.c + directory: src/apps/tcwm/tcwm +- path: ../src/apps/trilat/dwm_le/dwm-math.c + directory: src/apps/trilat/dwm_le +- path: ../src/apps/trilat/dwm_le/le-trilat.c + directory: src/apps/trilat/dwm_le +- path: ../src/apps/trilat/task_trilat + directory: src/apps/trilat +- path: ../src/apps/trilat/task_trilat/task_trilat.c + directory: src/apps/trilat/task_trilat +- path: ../src/apps/usb2spi/task_usb2spi/task_usb2spi.c + directory: src/apps/usb2spi/task_usb2spi +- path: ../src/apps/usb2spi/usb2spi/usb2spi.c + directory: src/apps/usb2spi/usb2spi +- path: ../src/config/config/config.c + directory: src/config/config +- path: ../src/config/default_config/default_config.c + directory: src/config/default_config +- path: ../src/core/command/cmd/cmd.c + directory: src/core/command/cmd +- path: ../src/core/command/cmd_fn/cmd_fn.c + directory: src/core/command/cmd_fn +- path: ../src/core/task_ctrl/task_ctrl.c + directory: src/core/task_ctrl +- path: ../src/core/task_flush/task_flush.c + directory: src/core/task_flush +- path: ../src/core/usb_uart_rx/usb_uart_rx.c + directory: src/core/usb_uart_rx +- path: ../src/core/usb_uart_tx/usb_uart_tx.c + directory: src/core/usb_uart_tx +- path: ../src/main.c +- path: ../src/platform/port/deca_uart.c + directory: src/platform/port +- path: ../src/platform/port/deca_usb.c + directory: src/platform/port +- path: ../src/platform/port/port_error.c + directory: src/platform/port +- path: ../src/platform/port/port_memory.c + directory: src/platform/port +- path: ../src/platform/port/port_rtc.c + directory: src/platform/port +- path: ../src/platform/port/port_timer.c + directory: src/platform/port +- path: ../src/platform/port/port_wdt.c + directory: src/platform/port +- path: ../src/srv/common_n/common_n.c + directory: src/srv/common_n +- path: ../src/srv/crc16/crc16.c + directory: src/srv/crc16 +- path: ../src/srv/json/cJSON.c + directory: src/srv/json +- path: ../src/srv/json/json_2pc.c + directory: src/srv/json +- path: ../src/srv/msg_time/msg_time.c + directory: src/srv/msg_time +- path: ../src/srv/tag_list/tag_list.c + directory: src/srv/tag_list +- path: ../src/srv/translate/translate.c + directory: src/srv/translate +- path: ../src/srv/util/util.c + directory: src/srv/util + +include: +- path: ../src/inc + file_list: + - path: app.h + - path: common.h + - path: deca_dbg.h + - path: error.h + - path: rtls_interface.h + - path: uwb_frames.h + - path: version.h + directory: src/inc + +- path: ../src/apps/listener/listener + file_list: + - path: listener.h + directory: src/apps/listener/listener + +- path: ../src/apps/listener/task_listener + file_list: + - path: task_listener.h + directory: src/apps/listener/task_listener +- path: ../src/apps/node/node + file_list: + - path: node.h + directory: src/apps/node/node +- path: ../src/apps/node/task_node + file_list: + - path: task_node.h + directory: src/apps/node/task_node +- path: ../src/apps/tag/tag + file_list: + - path: tag.h + directory: src/apps/tag/tag +- path: ../src/apps/tag/task_tag + file_list: + - path: task_tag.h + directory: src/apps/tag/task_tag +- path: ../src/apps/tcfm/task_tcfm + file_list: + - path: task_tcfm.h + directory: src/apps/tcfm/task_tcfm +- path: ../src/apps/tcfm/tcfm + file_list: + - path: tcfm.h + directory: src/apps/tcfm/tcfm +- path: ../src/apps/tcwm/task_tcwm + file_list: + - path: task_tcwm.h + directory: src/apps/tcwm/task_tcwm +- path: ../src/apps/tcwm/tcwm + file_list: + - path: tcwm.h + directory: src/apps/tcwm/tcwm +- path: ../src/apps/trilat/dwm_le + file_list: + - path: dwm-math.h + - path: le-trilat.h + directory: src/apps/trilat/dwm_le +- path: ../src/apps/usb2spi/task_usb2spi + file_list: + - path: task_usb2spi.h + directory: src/apps/usb2spi/task_usb2spi +- path: ../src/apps/usb2spi/usb2spi + file_list: + - path: usb2spi.h + directory: src/apps/usb2spi/usb2spi +- path: ../src/config/config + file_list: + - path: config.h + directory: src/config/config +- path: ../src/config/default_config + file_list: + - path: default_config.h + directory: src/config/default_config +- path: ../src/core/command/cmd + file_list: + - path: cmd.h + directory: src/core/command/cmd +- path: ../src/core/command/cmd_fn + file_list: + - path: cmd_fn.h + directory: src/core/command/cmd_fn +- path: ../src/core/task_ctrl + file_list: + - path: task_ctrl.h + directory: src/core/task_ctrl +- path: ../src/core/task_flush + file_list: + - path: task_flush.h + directory: src/core/task_flush +- path: ../src/core/usb_uart_rx + file_list: + - path: usb_uart_rx.h + directory: src/core/usb_uart_rx +- path: ../src/core/usb_uart_tx + file_list: + - path: usb_uart_tx.h + directory: src/core/usb_uart_tx +- path: ../src/platform/port + file_list: + - path: port_common.h + - path: port_uart.h + directory: src/platform/port +- path: ../src/srv/common_n + file_list: + - path: common_n.h + directory: src/srv/common_n +- path: ../src/srv/crc16 + file_list: + - path: crc16.h + directory: src/srv/crc16 +- path: ../src/srv/json + file_list: + - path: cJSON.h + - path: json_interface.h + directory: src/srv/json +- path: ../src/srv/msg_time + file_list: + - path: msg_time.h + directory: src/srv/msg_time +- path: ../src/srv/tag_list + file_list: + - path: tag_list.h + directory: src/srv/tag_list +- path: ../src/srv/translate + file_list: + - path: translate.h + directory: src/srv/translate +- path: ../src/srv/util + file_list: + - path: util.h + directory: src/srv/util + +component: +- id: bluetooth_feature_gatt_server +- id: bluetooth_feature_sm +- id: mpu +- id: bluetooth_feature_legacy_advertiser +- id: gatt_configuration +- id: freertos +- id: bluetooth_stack +- id: bluetooth_feature_gatt +- id: uartdrv_core +- id: udelay +- id: bluetooth_feature_legacy_scanner +- id: bt_post_build +- id: bluetooth_feature_connection +- id: spidrv_core +- id: bluetooth_feature_system +- id: mikroe_uwb2_dwm3000 + from: third_party_hw_drivers +- id: component_catalog +- id: app_assert + +define: + - name: ACCUM_READ_SUPPORT + value: 1 + - name: DIAG_READ_SUPPORT + value: 0 + - name: PDOA_NODE + value: 1 + - name: PDOA_TAG + value: 1 + - name: CFG_LE_TRILAT + value: 1 + - name: CFG_LE_TRILAT_UTILS + value: 1 + - name: DEBUG_NO_GUI + value: 0 + +sdk_extension: +- id: third_party_hw_drivers + version: 2.0.2 + +other_file: +- path: ../config/brd2703a/linkerfile.ld + condition: [brd2703a] +- path: ../images/DecaPDOARTLS.png + directory: images +- path: ../images/change_target.png + directory: images +- path: ../images/create_example.png + directory: images +- path: ../images/DecaPDOARTLS.png + directory: images +- path: ../images/DecaPDOARTLS_ranging.png + directory: images +- path: ../images/discovery_and_ranging_phases.png + directory: images +- path: ../images/edit_component2.png + directory: images +- path: ../images/edit_component.png + directory: images +- path: ../images/hardware_connection.png + directory: images +- path: ../images/ranging.png + directory: images +- path: ../images/select_target.png + directory: images +- path: ../images/setup.png + directory: images + +tag: + - hardware:rf:band:2400 + +toolchain_settings: + - option: linkerfile + value: linkerfile.ld + + +configuration: + - name: SL_STACK_SIZE + value: "2752" + - name: SL_HEAP_SIZE + value: "9200" + - name: configTOTAL_HEAP_SIZE + value: 50176 + - name: SL_PSA_KEY_USER_SLOT_COUNT + value: "0" + condition: + - psa_crypto + - name: TDOA_ANCHOR + value: 1 + - name: UWB_BH_ENABLE + value: 1 + - name: UWB_BH_ENABLE + value: 1 + +ui_hints: + highlight: + - path: config/btconf/gatt_configuration.btconf + - path: README.md + focus: true \ No newline at end of file diff --git a/bluetooth_uwb_dw3000_slotted_twr/config/brd2703a/linkerfile.ld b/bluetooth_uwb_dw3000_slotted_twr/config/brd2703a/linkerfile.ld new file mode 100644 index 0000000..1fff77e --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/config/brd2703a/linkerfile.ld @@ -0,0 +1,273 @@ +/***************************************************************************//** + * GCC Linker script for Silicon Labs devices + ******************************************************************************* + * # License + * Copyright 2020 Silicon Laboratories Inc. www.silabs.com + ******************************************************************************* + * + * SPDX-License-Identifier: Zlib + * + * The licensor of this software is Silicon Laboratories Inc. + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + * + ******************************************************************************/ +FLASH_PAGE_SIZE = 0x2000; +FLASH_SIZE = 0xfe000; +RAM_SIZE = 0x40000; + + MEMORY + { + FLASH (rx) : ORIGIN = 0x8000000, LENGTH = (FLASH_SIZE - FLASH_PAGE_SIZE) /* No bootloader at this moment for easy use */ + DFLASH (rw) : ORIGIN = (ORIGIN(FLASH) + LENGTH(FLASH)), LENGTH = FLASH_PAGE_SIZE + RAM (rwx) : ORIGIN = 0x20000000, LENGTH = RAM_SIZE + } + +ENTRY(Reset_Handler) + +SECTIONS +{ + + .text : + { + linker_vectors_begin = .; + KEEP(*(.vectors)) + linker_vectors_end = .; + + __Vectors_End = .; + __Vectors_Size = __Vectors_End - __Vectors; + + linker_code_begin = .; + *(SORT_BY_ALIGNMENT(.text*)) + . = ALIGN(32); + linker_code_end = .; + + KEEP(*(.init)) + KEEP(*(.fini)) + + /* .ctors */ + *crtbegin.o(.ctors) + *crtbegin?.o(.ctors) + *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors) + *(SORT(.ctors.*)) + *(.ctors) + + /* .dtors */ + *crtbegin.o(.dtors) + *crtbegin?.o(.dtors) + *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors) + *(SORT(.dtors.*)) + *(.dtors) + + __code_classification_validator_start__ = .; + . = . + 0x20; + *(code_classification_validator) + . = ALIGN(32); + __code_classification_validator_end__ = .; + + *(.rodata*) + *(.eh_frame*) + } > FLASH + + .ARM.extab : + { + *(.ARM.extab* .gnu.linkonce.armextab.*) + } > FLASH + + __exidx_start = .; + .ARM.exidx : + { + *(.ARM.exidx* .gnu.linkonce.armexidx.*) + } > FLASH + __exidx_end = .; + + .copy.table : + { + . = ALIGN(4); + __copy_table_start__ = .; + + LONG (__etext) + LONG (__data_start__) + LONG ((__data_end__ - __data_start__) / 4) + + /* Add each additional data section here */ +/* + LONG (__etext2) + LONG (__data2_start__) + LONG ((__data2_end__ - __data2_start__) / 4) +*/ + __copy_table_end__ = .; + } > FLASH + + .zero.table : + { + . = ALIGN(4); + __zero_table_start__ = .; + /* Add each additional bss section here */ +/* + LONG (__bss2_start__) + LONG ((__bss2_end__ - __bss2_start__) / 4) +*/ + __zero_table_end__ = .; + } > FLASH + + .dw_drivers ALIGN(4): + { + __dw_drivers_start = . ; + KEEP(*(.dw_drivers*)) + __dw_drivers_end = . ; + } > FLASH + + __etext = .; + + /* Start placing output sections which are loaded into RAM */ + . = ORIGIN(RAM); + + .stack ALIGN(8) (NOLOAD): + { + __StackLimit = .; + KEEP(*(.stack*)) + . = ALIGN(4); + __StackTop = .; + PROVIDE(__stack = __StackTop); + } > RAM + + + .noinit . (NOLOAD): + { + *(.noinit*); + } > RAM + + .data . : AT (__etext) + { + . = ALIGN(4); + __data_start__ = .; + *(vtable) + *(SORT_BY_ALIGNMENT(.data*)) + . = ALIGN(4); + + PROVIDE(__ram_func_section_start = .); + *(.ram) + PROVIDE(__ram_func_section_end = .); + + . = ALIGN(4); + /* preinit data */ + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP(*(.preinit_array)) + PROVIDE_HIDDEN (__preinit_array_end = .); + + . = ALIGN(4); + /* init data */ + PROVIDE_HIDDEN (__init_array_start = .); + KEEP(*(SORT(.init_array.*))) + KEEP(*(.init_array)) + PROVIDE_HIDDEN (__init_array_end = .); + + . = ALIGN(4); + /* finit data */ + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP(*(SORT(.fini_array.*))) + KEEP(*(.fini_array)) + PROVIDE_HIDDEN (__fini_array_end = .); + + . = ALIGN(4); + /* All data end */ + __data_end__ = .; + + } > RAM + + .bss . : + { + . = ALIGN(4); + __bss_start__ = .; + *(SORT_BY_ALIGNMENT(.bss*)) + *(COMMON) + . = ALIGN(4); + __bss_end__ = .; + } > RAM + + __ramfuncs_start__ = .; + + __vma_ramfuncs_start__ = .; + __lma_ramfuncs_start__ = __etext + SIZEOF(.data); + + __text_application_ram_offset__ = . - __vma_ramfuncs_start__; + text_application_ram . : AT(__lma_ramfuncs_start__ + __text_application_ram_offset__) + { + . = ALIGN(4); + __text_application_ram_start__ = .; + *(text_application_ram) + . = ALIGN(4); + __text_application_ram_end__ = .; + } > RAM + + . = ALIGN(4); + __vma_ramfuncs_end__ = .; + __lma_ramfuncs_end__ = __lma_ramfuncs_start__ + __text_application_ram_offset__ + SIZEOF(text_application_ram); + + __ramfuncs_end__ = .; + + .heap (COPY): + { + __HeapBase = .; + __end__ = .; + end = __end__; + _end = __end__; + KEEP(*(.heap*)) + __HeapLimit = ORIGIN(RAM) + LENGTH(RAM); + } > RAM + + __heap_size = __HeapLimit - __HeapBase; + __ram_end__ = 0x20000000 + 0x40000; + __main_flash_end__ = 0x8000000 + 0x17e000; + + /* This is where we handle flash storage blocks. We use dummy sections for finding the configured + * block sizes and then "place" them at the end of flash when the size is known. */ + .internal_storage (DSECT) : { + KEEP(*(.internal_storage*)) + } > FLASH + + + .nvm (DSECT) : { + KEEP(*(.simee*)) + } > FLASH + + linker_nvm_end = __main_flash_end__; + linker_nvm_begin = linker_nvm_end - SIZEOF(.nvm); + linker_nvm_size = SIZEOF(.nvm); + linker_storage_end = linker_nvm_begin; + __nvm3Base = linker_nvm_begin; + + linker_storage_begin = linker_storage_end - SIZEOF(.internal_storage); + linker_storage_size = SIZEOF(.internal_storage); + ASSERT((linker_storage_begin >= (__etext + SIZEOF(.data))), "FLASH memory overflowed !") + + + app_flash_end = 0x8000000 + 0x17e000; + ASSERT( (linker_nvm_begin + SIZEOF(.nvm)) <= app_flash_end, "NVM3 is excessing the flash size !") + + .config_otp : { + __fconfig_start = . ; + KEEP(*(.fConfig)) + . = ALIGN(FLASH_PAGE_SIZE); + __fconfig_end = . ; + } > DFLASH + + ASSERT((ORIGIN(DFLASH) == __fconfig_start), "DATA FLASH start address is wrong!") + ASSERT((0 == (__fconfig_end % FLASH_PAGE_SIZE)), "DATA FLASH alignment issue!") + ASSERT(((ORIGIN(DFLASH) + LENGTH(DFLASH)) == __fconfig_end), "DATA FLASH memory allocation issue!") +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/config/btconf/gatt_configuration.btconf b/bluetooth_uwb_dw3000_slotted_twr/config/btconf/gatt_configuration.btconf new file mode 100644 index 0000000..58784d0 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/config/btconf/gatt_configuration.btconf @@ -0,0 +1,48 @@ + + + + + Abstract: The generic_access service contains generic information about the device. All available Characteristics are readonly. + + + Empty Example + + + + Abstract: The external appearance of this device. The values are composed of a category (10-bits) and sub-categories (6-bits). + 0000 + + + + + Abstract: The Device Information Service exposes manufacturer and/or vendor information about a device. Summary: This service exposes manufacturer information about a device. The Device Information Service is instantiated as a Primary Service. Only one instance of the Device Information Service is exposed on a device. + + Abstract: The value of this characteristic is a UTF-8 string representing the name of the manufacturer of the device. + Silicon Labs + + + + + + Abstract: The value of this characteristic is a UTF-8 string representing the model number assigned by the device vendor. + 00000000 + + + + Summary: The value of this characteristic is a UTF-8 string representing the hardware revision for the hardware within the device. + 000 + + + + Summary: The value of this characteristic is a UTF-8 string representing the firmware revision for the firmware within the device. + 0.0.0 + + + + Abstract: The SYSTEM ID characteristic consists of a structure with two fields. The first field are the LSOs and the second field contains the MSOs. This is a 64-bit structure which consists of a 40-bit manufacturer-defined identifier concatenated with a 24 bit unique Organizationally Unique Identifier (OUI). The OUI is issued by the IEEE Registration Authority (http://standards.ieee.org/regauth/index.html) and is required to be used in accordance with IEEE Standard 802-2001.6 while the least significant 40 bits are manufacturer defined. If System ID generated based on a Bluetooth Device Address, it is required to be done as follows. System ID and the Bluetooth Device Address have a very similar structure: a Bluetooth Device Address is 48 bits in length and consists of a 24 bit Company Assigned Identifier (manufacturer defined identifier) concatenated with a 24 bit Company Identifier (OUI). In order to encapsulate a Bluetooth Device Address as System ID, the Company Identifier is concatenated with 0xFFFE followed by the Company Assigned Identifier of the Bluetooth Address. For more guidelines related to EUI-64, refer to http://standards.ieee.org/develop/regauth/tut/eui64.pdf. Examples: If the system ID is based of a Bluetooth Device Address with a Company Identifier (OUI) is 0x123456 and the Company Assigned Identifier is 0x9ABCDE, then the System Identifier is required to be 0x123456FFFE9ABCDE. + + + + + + diff --git a/bluetooth_uwb_dw3000_slotted_twr/images/DecaPDOARTLS.png b/bluetooth_uwb_dw3000_slotted_twr/images/DecaPDOARTLS.png new file mode 100644 index 0000000..12a6895 Binary files /dev/null and b/bluetooth_uwb_dw3000_slotted_twr/images/DecaPDOARTLS.png differ diff --git a/bluetooth_uwb_dw3000_slotted_twr/images/DecaPDOARTLS_ranging.png b/bluetooth_uwb_dw3000_slotted_twr/images/DecaPDOARTLS_ranging.png new file mode 100644 index 0000000..a6f5feb Binary files /dev/null and b/bluetooth_uwb_dw3000_slotted_twr/images/DecaPDOARTLS_ranging.png differ diff --git a/bluetooth_uwb_dw3000_slotted_twr/images/change_target.png b/bluetooth_uwb_dw3000_slotted_twr/images/change_target.png new file mode 100644 index 0000000..e1ba910 Binary files /dev/null and b/bluetooth_uwb_dw3000_slotted_twr/images/change_target.png differ diff --git a/bluetooth_uwb_dw3000_slotted_twr/images/create_example.png b/bluetooth_uwb_dw3000_slotted_twr/images/create_example.png new file mode 100644 index 0000000..34f2e78 Binary files /dev/null and b/bluetooth_uwb_dw3000_slotted_twr/images/create_example.png differ diff --git a/bluetooth_uwb_dw3000_slotted_twr/images/discovery_and_ranging_phases.png b/bluetooth_uwb_dw3000_slotted_twr/images/discovery_and_ranging_phases.png new file mode 100644 index 0000000..fe26bca Binary files /dev/null and b/bluetooth_uwb_dw3000_slotted_twr/images/discovery_and_ranging_phases.png differ diff --git a/bluetooth_uwb_dw3000_slotted_twr/images/edit_component.png b/bluetooth_uwb_dw3000_slotted_twr/images/edit_component.png new file mode 100644 index 0000000..5f00cdd Binary files /dev/null and b/bluetooth_uwb_dw3000_slotted_twr/images/edit_component.png differ diff --git a/bluetooth_uwb_dw3000_slotted_twr/images/edit_component2.png b/bluetooth_uwb_dw3000_slotted_twr/images/edit_component2.png new file mode 100644 index 0000000..c68e623 Binary files /dev/null and b/bluetooth_uwb_dw3000_slotted_twr/images/edit_component2.png differ diff --git a/bluetooth_uwb_dw3000_slotted_twr/images/hardware_connection.png b/bluetooth_uwb_dw3000_slotted_twr/images/hardware_connection.png new file mode 100644 index 0000000..d4afecf Binary files /dev/null and b/bluetooth_uwb_dw3000_slotted_twr/images/hardware_connection.png differ diff --git a/bluetooth_uwb_dw3000_slotted_twr/images/ranging.png b/bluetooth_uwb_dw3000_slotted_twr/images/ranging.png new file mode 100644 index 0000000..8f018eb Binary files /dev/null and b/bluetooth_uwb_dw3000_slotted_twr/images/ranging.png differ diff --git a/bluetooth_uwb_dw3000_slotted_twr/images/select_target.png b/bluetooth_uwb_dw3000_slotted_twr/images/select_target.png new file mode 100644 index 0000000..8bec95a Binary files /dev/null and b/bluetooth_uwb_dw3000_slotted_twr/images/select_target.png differ diff --git a/bluetooth_uwb_dw3000_slotted_twr/images/setup.png b/bluetooth_uwb_dw3000_slotted_twr/images/setup.png new file mode 100644 index 0000000..ab6c859 Binary files /dev/null and b/bluetooth_uwb_dw3000_slotted_twr/images/setup.png differ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/listener/listener/listener.c b/bluetooth_uwb_dw3000_slotted_twr/src/apps/listener/listener/listener.c new file mode 100644 index 0000000..8539557 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/listener/listener/listener.c @@ -0,0 +1,372 @@ +/** + * @file listener.c + * @brief Decawave Application level + * collection of TWR bare-metal functions for a Node + * + * @author Decawave + * + * @attention Copyright 2017 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + * ============================================================================= + * = = + * = = + * = D E C A W A V E C O N F I D E N T I A L = + * = = + * = = + * ============================================================================= + * + * This software contains Decawave confidential information and techniques, + * subject to licence and non-disclosure agreements. No part of this software + * package may be revealed to any third-party without the express permission of + * Decawave Ltd. + * + * ============================================================================= + */ + +/* Includes */ +#include "listener.h" +#include "deca_device_api.h" +#include "deca_dbg.h" + +#pragma GCC diagnostic ignored "-Waddress-of-packed-member" + +// ---------------------------------------------------------------------------- +// implementation-specific: critical section protection +#ifndef TWR_ENTER_CRITICAL +#define TWR_ENTER_CRITICAL taskENTER_CRITICAL +#endif + +#ifndef TWR_EXIT_CRITICAL +#define TWR_EXIT_CRITICAL taskEXIT_CRITICAL +#endif + +#define LSTNR_MALLOC pvPortMalloc +#define LSTNR_FREE vPortFree + +// ----------------------------------------------------------------------------- +// The psListenerInfo structure holds all Listener's process parameters +static listener_info_t *psListenerInfo = NULL; + +/* + * @brief get pointer to the twrInfo structure + * */ +listener_info_t * getListenerInfoPtr(void) +{ + return (psListenerInfo); +} + +/* + * @brief ISR level (need to be protected if called from APP level) + * low-level configuration for DW3000 + * + * if called from app, shall be performed with DW IRQ off & + * TWR_ENTER_CRITICAL(); / TWR_EXIT_CRITICAL(); + * + * The SPI for selected chip shall already be chosen + * + * + * + * @note + * */ +static void +rxtx_listener_configure +( + dwt_config_t *pdwCfg, + uint16_t frameFilter, + uint16_t txAntDelay, + uint16_t rxAntDelay +) +{ + (void) frameFilter; + (void) txAntDelay; + (void) rxAntDelay; + + if (dwt_configure(pdwCfg)) { /**< Configure the Physical Channel parameters + * (PLEN, PRF, etc) */ + error_handler(1, _ERR_INIT); + } + dwt_setrxaftertxdelay(0); /**< no any delays set by default : part of + * config of receiver on Tx sending */ + dwt_setrxtimeout(0); /**< no any delays set by default : part of + * config of receiver on Tx sending */ + dwt_configureframefilter(DWT_FF_DISABLE, 0); +} + +/* @brief ISR level + * TWR application Rx callback + * to be called from dwt_isr() as an Rx call-back + * */ +void rx_listener_cb(const dwt_cb_data_t *rxd) +{ + listener_info_t *pListenerInfo = getListenerInfoPtr(); + + if (!pListenerInfo) { + return; + } + + int16_t stsQual; + + const int size = sizeof(pListenerInfo->rxPcktBuf.buf) + / sizeof(pListenerInfo->rxPcktBuf.buf[0]); + + int head = pListenerInfo->rxPcktBuf.head; + int tail = pListenerInfo->rxPcktBuf.tail; + + if (CIRC_SPACE(head, tail, size) > 0) { + rx_listener_pckt_t *p = &pListenerInfo->rxPcktBuf.buf[head]; + + dwt_readrxtimestamp(p->timeStamp); // Raw Rx TimeStamp (STS or + // IPATOV based on STS config) + + p->clock_offset = dwt_readclockoffset(); // Reading Clock offset for any + // Rx packets + + p->status = rxd->status; + + if (dwt_readstsquality(&stsQual) < 0) { // if < 0 then this is "bad" STS + app.event_counts_sts_bad++; + } else { + app.event_counts_sts_good++; + } + + app.event_counts_sfd_detect++; + + // check if this is an SP3 packet + if (rxd->rx_flags & DWT_CB_DATA_RX_FLAG_ND) { + p->rxDataLen = 0; + } else { + p->rxDataLen = MIN(rxd->datalength, sizeof(p->msg)); + + dwt_readrxdata((uint8_t *)&p->msg, p->rxDataLen, 0); // Raw message + + // p->msg.data[p->rxDataLen] = stsQual>>8; + // p->msg.data[p->rxDataLen+1] = stsQual; + // p->rxDataLen+=2; + } + + if (app.listenerTask.Handle) { // RTOS : listenerTask can be not + // started yet + head = (head + 1) & (size - 1); + pListenerInfo->rxPcktBuf.head = head; // ISR level : do not need + // to protect + } + } + + if (app.listenerTask.Handle) { // RTOS : listenerTask can be not + // started yet + // Sends the Signal to the application level via OS kernel. + // This will add a small delay of few us, but + // this method make sense from a program structure point of view. + if (osThreadFlagsSet(app.listenerTask.Handle, + app.listenerTask.Signal) & osFlagsError) { + error_handler(1, _ERR_Signal_Bad); + } + } + + if (app.pConfig->s.stsStatic) { // value 0 = dynamic STS, 1 = fixed STS) + // re-load the initial cp_iv value to keep STS the same for each frame + dwt_configurestsloadiv(); + } + + dwt_readeventcounters(&app.event_counts); // take a snapshot of event + // counters + + dwt_rxenable(DWT_START_RX_IMMEDIATE); // re-enable receiver again - no + // timeout + + /* ready to serve next raw reception */ +} + +void listener_timeout_cb(const dwt_cb_data_t *rxd) +{ + (void) rxd; + + if (app.pConfig->s.stsStatic) { // value 0 = dynamic STS, 1 = fixed STS + // re-load the initial cp_iv value to keep STS the same for each frame + dwt_configurestsloadiv(); + } + + dwt_readeventcounters(&app.event_counts); + + dwt_rxenable(DWT_START_RX_IMMEDIATE); +} + +void listener_error_cb(const dwt_cb_data_t *rxd) +{ + listener_timeout_cb(rxd); +} + +// ----------------------------------------------------------------------------- + +/* @brief app level + * RTOS-independent application level function. + * initializing of a TWR Node functionality. + * + * */ +error_e listener_process_init() +{ + if (!psListenerInfo) { + psListenerInfo = LSTNR_MALLOC(sizeof(listener_info_t)); + } + + listener_info_t *pListenerInfo = getListenerInfoPtr(); + + if (!pListenerInfo) { + return(_ERR_Cannot_Alloc_NodeMemory); + } + + /* switch off receiver's rxTimeOut, RxAfterTxDelay, delayedRxTime, + * autoRxEnable, dblBufferMode and autoACK, + * clear all initial counters, etc. + * */ + memset(pListenerInfo, 0, sizeof(listener_info_t)); + + memset(&app.event_counts, 0, sizeof(app.event_counts)); + + app.event_counts_sts_bad = 0; + app.event_counts_sts_good = 0; + app.event_counts_sfd_detect = 0; + + /* Configure non-zero initial variables.1 : from app parameters */ + + /* The Listener has its configuration in the app->pConfig, see DEFAULT_CONFIG. + * + * + * */ + pListenerInfo->pSfConfig = &app.pConfig->s.sfConfig; /**< Super Frame + * configuration */ + + /* dwt_xx calls in app level Must be in protected mode (DW3000 IRQ disabled) + */ + disable_dw3000_irq(); + + TWR_ENTER_CRITICAL(); + + if (dwt_initialise(DWT_DW_INIT) != DWT_SUCCESS) { /**< set callbacks to NULL + * inside + * dwt_initialise*/ + return (_ERR_INIT); + } + + set_dw_spi_fast_rate(); + + uint32_t dev_id = dwt_readdevid(); + + if ((dev_id == (uint32_t)DWT_DW3000_PDOA_DEV_ID) + || (dev_id == (uint32_t)DWT_QM33120_PDOA_DEV_ID)) { + pListenerInfo->dw3000ChipType = AOA; + + diag_printf("Found AOA DW3000 chip. PDoA is available.\r\n"); + } else if (dev_id == (uint32_t)DWT_DW3000_DEV_ID) { + pListenerInfo->dw3000ChipType = NON_AOA; + + app.pConfig->dwt_config.pdoaMode = DWT_PDOA_M0; + + diag_printf("Found non-AOA DW3000 chip. PDoA is not available.\r\n"); + } else { + diag_printf("Found unknown chip 0x%08lX. Stop.\r\n", + (unsigned long int)dev_id); + return _ERR_DEVID; + } + + /* Configure DW IC's UWB mode, sets power and antenna delays for TWR mode + * Configure SPI to fast rate */ + rxtx_listener_configure(&app.pConfig->dwt_config, + DWT_FF_DISABLE, /* No frame filtering for + * Listener */ + app.pConfig->s.antTx_a, + app.pConfig->s.antRx_a + ); + + dwt_setleds(DWT_LEDS_ENABLE | DWT_LEDS_INIT_BLINK); /**< DEBUG I/O 2&3 + * : configure + * the GPIOs + * which control + * the LEDs on HW + */ + dwt_setlnapamode(DWT_PA_ENABLE | DWT_LNA_ENABLE); /**< DEBUG I/O 5&6 + * : configure + * TX/RX states + * to output on + * GPIOs + */ + + dwt_setcallbacks(NULL, + rx_listener_cb, + listener_timeout_cb, + listener_error_cb, + NULL, + NULL, + NULL); + + dwt_setinterrupt(DWT_INT_TXFRS_BIT_MASK | DWT_INT_RXFCG_BIT_MASK + | (DWT_INT_ARFE_BIT_MASK | DWT_INT_RXFSL_BIT_MASK + | DWT_INT_RXSTO_BIT_MASK | DWT_INT_RXPHE_BIT_MASK + | DWT_INT_RXFCE_BIT_MASK | DWT_INT_RXFTO_BIT_MASK + + /*| SYS_STATUS_RXPTO_BIT_MASK */), + 0, + 2); + + dwt_configciadiag(DW_CIA_DIAG_LOG_ALL); /* DW3000 */ + + // configure STS KEY/IV + dwt_configurestskey(&app.pConfig->s.stsKey); + dwt_configurestsiv(&app.pConfig->s.stsIv); + // load the configured KEY/IV values + dwt_configurestsloadiv(); + + dwt_configeventcounters(1); + + /* + * The dwt_initialize will read the default XTAL TRIM from the OTP or use the + * DEFAULT_XTAL_TRIM. + * In this case we would apply the user-configured value. + * + * Bit 0x80 can be used to overwrite the OTP settings if any. + * */ + if ((dwt_getxtaltrim() == DEFAULT_XTAL_TRIM) + || (app.pConfig->s.xtalTrim & ~XTAL_TRIM_BIT_MASK)) { + dwt_setxtaltrim(app.pConfig->s.xtalTrim & XTAL_TRIM_BIT_MASK); + } + + /* End configuration of DW IC */ + rtc_disable_irq(); + + TWR_EXIT_CRITICAL(); + + return (_NO_ERR); +} + +/* + * @brief + * Enable DW3000 IRQ to start + * */ +void listener_process_start(void) +{ + enable_dw3000_irq(); + + // start the RTC timer + rtc_enable_irq(); + + diag_printf("Listener Top Application: Started\r\n"); +} + +/* @brief app level + * RTOS-independent application level function. + * deinitialize the pListenerInfo structure. + * This must be executed in protected mode. + * + * */ +void listener_process_terminate(void) +{ + rtc_disable_irq(); + + if (psListenerInfo) { + LSTNR_FREE(psListenerInfo); + psListenerInfo = NULL; + } +} + +// ----------------------------------------------------------------------------- diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/listener/listener/listener.h b/bluetooth_uwb_dw3000_slotted_twr/src/apps/listener/listener/listener.h new file mode 100644 index 0000000..102aa26 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/listener/listener/listener.h @@ -0,0 +1,101 @@ +/** + * @file listener.h + * + * @brief Decawave + * bare implementation layer + * + * @author Decawave + * + * @attention Copyright 2020 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef __LISTENER__H__ +#define __LISTENER__H__ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#include "uwb_frames.h" +#include "port.h" +#include "port_common.h" + +// ----------------------------------------------------------------------------- + +/* + * Rx Events circular buffer : used to transfer RxPckt from ISR to APP + * 0x02, 0x04, 0x08, 0x10, etc. + * As per design, the amount of RxPckt in the buffer at any given time shall not + * be more than 1. + * */ +#define EVENT_BUF_L_SIZE (0x10) + +// ----------------------------------------------------------------------------- + +/* RxPckt is the structure is for the current reception */ +struct rx_listener_pckt_s +{ + int16_t rxDataLen; + + union { + std_msg_t stdMsg; + twr_msg_t twrMsg; + blink_msg_t blinkMsg; + rng_cfg_msg_t rngCfgMsg; + poll_msg_t pollMsg; + resp_pdoa_msg_t respMsg; + final_msg_accel_t finalMsg; + uint8_t data[STANDARD_FRAME_SIZE]; + } msg; + + uint8_t timeStamp[TS_40B_SIZE]; /* Full TimeStamp */ + + /* Below is Decawave's diagnostics information */ + uint32_t status; + int16_t clock_offset; +}; + +typedef struct rx_listener_pckt_s rx_listener_pckt_t; + +/* This structure holds Listener's application parameters */ +struct listener_info_s +{ + /* circular Buffer of received Rx packets : + * uses in transferring of the data from ISR to APP level. + * */ + struct { + rx_listener_pckt_t buf[EVENT_BUF_L_SIZE]; + uint16_t head; + uint16_t tail; + } rxPcktBuf; + + sfConfig_t *pSfConfig; // superFrame configuration + dw3000type_e dw3000ChipType; +}; + +typedef struct listener_info_s listener_info_t; + +// ----------------------------------------------------------------------------- +// exported functions prototypes +// +extern listener_info_t * getListenerInfoPtr(void); + +// ----------------------------------------------------------------------------- +// exported functions prototypes +// + +/* responder (Listener) */ + +error_e listener_process_init(void); +void listener_process_start(void); +void listener_process_terminate(void); + +// ----------------------------------------------------------------------------- + +#ifdef __cplusplus +} +#endif + +#endif /* __LISTENER__H__ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/listener/task_listener/task_listener.c b/bluetooth_uwb_dw3000_slotted_twr/src/apps/listener/task_listener/task_listener.c new file mode 100644 index 0000000..27cda34 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/listener/task_listener/task_listener.c @@ -0,0 +1,159 @@ +/* + * @file task_listener.c + * @brief + * + * @author Decawave + * + * @attention Copyright 2019-2020 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + */ +#include +#include "deca_dbg.h" +#include "util.h" +#include "port.h" +#include "port_common.h" +#include "usb_uart_tx.h" +#include "listener.h" + +// ----------------------------------------------------------------------------- +// extern functions to report output data +extern error_e send_to_pc_listener_info(uint8_t *data, + uint8_t size, + uint8_t *ts, + int cfo); + +// ----------------------------------------------------------------------------- + +/* @brief DW3000 RX : Listener RTOS implementation + * this is a high-priority task, which will be executed immediately + * on reception of waiting Signal. Any task with lower priority will be + * interrupted. + * No other tasks in the system should have higher priority. + * */ +static void ListenerTask(void const *arg) +{ + int head, tail, size; + listener_info_t *pListenerInfo; + + while (!(pListenerInfo = getListenerInfoPtr())) + { + osDelay(5); + } + + size = sizeof(pListenerInfo->rxPcktBuf.buf) + / sizeof(pListenerInfo->rxPcktBuf.buf[0]); + + const osMutexAttr_t thread_mutex_attr = { .attr_bits = osMutexPrioInherit }; + app.listenerTask.MutexId = osMutexNew(&thread_mutex_attr); + + taskENTER_CRITICAL(); + dwt_rxenable(DWT_START_RX_IMMEDIATE); // Start reception on the Listener + taskEXIT_CRITICAL(); + + do + { + osMutexRelease(app.listenerTask.MutexId); + + /* ISR is delivering RxPckt via circ_buf & Signal. + * This is the fastest method. + * */ + osThreadFlagsWait(app.listenerTask.Signal, osFlagsWaitAny, 1); + osMutexAcquire(app.listenerTask.MutexId, 0); + + taskENTER_CRITICAL(); + head = pListenerInfo->rxPcktBuf.head; + tail = pListenerInfo->rxPcktBuf.tail; + taskEXIT_CRITICAL(); + + if (CIRC_CNT(head, tail, size) > 0) { + rx_listener_pckt_t *pRx_listener_Pckt = + &pListenerInfo->rxPcktBuf.buf[tail]; + + send_to_pc_listener_info(pRx_listener_Pckt->msg.data, + pRx_listener_Pckt->rxDataLen, + pRx_listener_Pckt->timeStamp, + pRx_listener_Pckt->clock_offset); + + taskENTER_CRITICAL(); + tail = (tail + 1) & (size - 1); + pListenerInfo->rxPcktBuf.tail = tail; + taskEXIT_CRITICAL(); + + if (app.flushTask.Handle) { + osThreadFlagsSet(app.flushTask.Handle, app.flushTask.Signal); + } + } + + osThreadYield(); + }while (1); + + UNUSED(arg); +} + +// ----------------------------------------------------------------------------- + +/* @brief Setup Listener task, this task will send to received data to UART. + * Only setup, do not start. + * */ +static void listener_setup_tasks(void) +{ + /* listenerTask is receive the signal from + * passing signal from RX IRQ to an actual two-way ranging algorithm. + * It awaiting of an Rx Signal from RX IRQ ISR and decides what to do next in + * TWR exchange process + * */ + CREATE_NEW_TASK(ListenerTask, + NULL, + "listenerTask", + 512, + PRIO_RxTask, + &app.listenerTask.Handle); + app.listenerTask.Signal = 1; + + if (app.listenerTask.Handle == NULL) { + error_handler(1, _ERR_Create_Task_Bad); + } +} + +/* @brief Terminate all tasks and timers related to Node functionality, if any + * DW3000's RX and IRQ shall be switched off before task termination, + * that IRQ will not produce unexpected Signal + * */ +void listener_terminate(void) +{ + TERMINATE_STD_TASK(app.listenerTask); + + listener_process_terminate(); +} + +/* @fn listener_helper + * @brief this is a service function which starts the + * TWR Node functionality + * Note: the previous instance of TWR shall be killed + * with node_terminate_tasks(); + * + * Note: the node_process_init() will allocate the memory of + * sizeof(node_info_t) + * from the caller's task stack, see _malloc_r() ! + * + * */ +void listener_helper(void const *argument) +{ + (void) argument; + error_e tmp; + + port_disable_dw_irq_and_reset(1); + + /* "RTOS-independent" part : initialization of two-way ranging process */ + tmp = listener_process_init(); /* allocate ListenerInfo */ + + if (tmp != _NO_ERR) { + error_handler(1, tmp); + } + + listener_setup_tasks(); /**< "RTOS-based" : setup (not start) all + * necessary tasks for the Node operation. */ + + listener_process_start(); /**< IRQ is enabled from MASTER chip and it may + * receive UWB immediately after this point */ +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/listener/task_listener/task_listener.h b/bluetooth_uwb_dw3000_slotted_twr/src/apps/listener/task_listener/task_listener.h new file mode 100644 index 0000000..9898d95 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/listener/task_listener/task_listener.h @@ -0,0 +1,26 @@ +/** + * @file listener_task.h + * @brief + * + * @author Decawave + * + * @attention Copyright 2019-2020 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef __LISTENER_TASK__H__ +#define __LISTENER_TASK__H__ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +void listener_helper(void const *argument); +void listener_terminate(void); + +#ifdef __cplusplus +} +#endif + +#endif /* __LISTENER_TASK__H__ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/node/node/node.c b/bluetooth_uwb_dw3000_slotted_twr/src/apps/node/node/node.c new file mode 100644 index 0000000..405d5c4 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/node/node/node.c @@ -0,0 +1,1533 @@ +/** + * @file node.c + * @brief Decawave Application level + * collection of TWR bare-metal functions for a Node + * + * @author Decawave + * + * @attention Copyright 2017 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + * ============================================================================= + * = = + * = = + * = D E C A W A V E C O N F I D E N T I A L = + * = = + * = = + * ============================================================================= + * + * This software contains Decawave confidential information and techniques, + * subject to licence and non-disclosure agreements. No part of this software + * package may be revealed to any third-party without the express permission of + * Decawave Ltd. + * + * ============================================================================= + */ + +/* Includes */ +#include +#include + +#include "node.h" +#include "util.h" +#include "errno.h" +#include "deca_device_api.h" + +#include "assert.h" +#include "deca_dbg.h" + +#pragma GCC diagnostic ignored "-Waddress-of-packed-member" + +// ---------------------------------------------------------------------------- +// implementation-specific: critical section protection +#ifndef TWR_ENTER_CRITICAL +#define TWR_ENTER_CRITICAL taskENTER_CRITICAL +#endif + +#ifndef TWR_EXIT_CRITICAL +#define TWR_EXIT_CRITICAL taskEXIT_CRITICAL +#endif + +#define NODE_MALLOC pvPortMalloc +#define NODE_FREE vPortFree + +// ---------------------------------------------------------------------------- +#ifndef M_PI +#define M_PI (3.141592654f) +#endif + +#ifndef M_PI_2 +#define M_PI_2 (1.570796327f) +#endif + +#ifndef TWO_PI +#define TWO_PI (2 * M_PI) +#endif + +#define FB (499.2e6f) /* Basis frequency */ +#define L_M_5 (SPEED_OF_LIGHT / FB / 13.0f) /* Lambda, m, + * CH5 */ +#define L_M_9 (SPEED_OF_LIGHT / FB / 16.0f) /* Lambda, m, + * CH9 */ +#define D_M_5 (0.022777110597040736f) /* Distance between + * centers of + * antennas, + * ~(L_M/2), m, CH5 + */ +#define D_M_9 (0.017883104683497245f) /* Distance between + * centers of + * antennas, + * ~(L_M/2), m, CH9 + */ + +// ----------------------------------------------------------------------------- + +/* Note : for slot period less than 10 ms, more sophisticated TagHW shall be + * used. + * STM32L1x & STM32F3x provide RTC with resolution of 61.035us, thus + * current project can support precise timings. + * See "default_config.h" + **/ + +#define RX_RELAX_TIMEOUT_SY (50) /**< relaxed RX Timeout in sequential + * TWR process exchange */ + +// ----------------------------------------------------------------------------- +// The psNodeInfo structure holds all Node's process parameters +static node_info_t * psNodeInfo = NULL; + +extern void trilat_SF_cb(void); +static void rtcWakeUpTimerEventCallback_node(void); +static double pdoa2path_diff_ch5(float x); +static double pdoa2path_diff_ch9(float x); + +// ----------------------------------------------------------------------------- +// Implementation + +/* + * @brief get pointer to the twrInfo structure + * */ +node_info_t * getNodeInfoPtr(void) +{ + return (psNodeInfo); +} + +#if (DIAG_READ_SUPPORT == 1) + +/** + * @brief ISR level (need to be protected if called from APP level) + * read full diagnostic data form the received frame from the two DW1000s + * + * */ +static int +read_full_diagnostics(rx_pckt_t *prxPckt, + uint32_t status) +{ +// TODO: rewrite for DW3000 + uint16_t fpIndex = 0; + uint16_t fpIndex; + diag_v5_t *p = &prxPckt->diagnostics; + + p->header = DWT_DIAGNOSTIC_LOG_REV_5; + + memcpy(p->r0F, (uint8_t *) &status, 4); // copy 4bytes + // of status + // (saved on + // entry to + // ISR) + dwt_readfromdevice(RX_FINFO_ID, 4, 5, (uint8_t *)(p + 5)); // read MSB from + // status and + // 4byte frame + // info + dwt_readfromdevice(RX_FQUAL_ID, 0, 17, (uint8_t *)(p->r12)); // read 17 bytes + // of + // diagnostic + // data from + // 0x12,13,14 + + memcpy((uint8_t *)p->r15, prxPckt->rxTimeStamp, TS_40B_SIZE); // copy TS + dwt_readfromdevice(RX_TIME_ID, RX_TIME_FP_INDEX_OFFSET, 9, + (uint8_t *)(p->r15 + 5)); // 2FP, 2Diag, 5TSraw + + // Calculate the First Path Index ((LDE0 + LDE1 << 8) / 64) + fpIndex = (*((uint8_t *)(p + 32)) >> 6) + (*((uint8_t *)(p + 33)) << 2); + + fpIndex = fpIndex * 4 + 1; // get location in the accumulator + + // printf("%d FP index %02x %02x %i %i\n", offset, *((uint8_t*)(p+32)), + // *((uint8_t*)(p+33)), fpIndex, (fpIndex-1)>>2); + // Read CIR for the First Path + 3 More samples (4*4 = 16) + dwt_readaccdata(p->r25, 17, fpIndex - 1); // read 1 extra as first will be + // dummy byte + dwt_readfromdevice(LDE_IF_ID, LDE_PPINDX_OFFSET, 2, p->r2E); + dwt_readfromdevice(DRX_CONF_ID, 0x28, 4, p->r27); + dwt_readfromdevice(LDE_IF_ID, LDE_PPAMPL_OFFSET, 2, p->r2E2); + + return (int) fpIndex; +} + +#endif + +// ----------------------------------------------------------------------------- + +// ----------------------------------------------------------------------------- + +/** + * @brief function to convert the ToF to range/distance to be output over + * UART/USB. + * if RBC(RANGE_BIAS_CORRECTION) is used to correct the range (then the output + * value will contain corrected range) + * otherwise the it will contain raw range + * + * @param [in] + * *p : pointer to run-time param_block_t; + * tofi: time of flight used to calculate the output distance; + * @param [out] + * *pdist_cm : pointer to output distance result. This also corrected with + * p->s.rngOffset_mm offset value. + * (float)(0XDEADBEEF) if error + * + */ +error_e +tof2range(param_block_t *p, + float *r_m, + int32_t tofi) +{ + error_e ret = _NO_ERR; + + *r_m = (float)tofi * (SPEED_OF_LIGHT / 499.2e6f / 128.0f); + + if (*r_m > 2000.0f) { + ret = _ERR_Range_Calculation; + } else { + *r_m = (*r_m - ((float)p->s.rngOffset_mm / 1000.0f)); + } + + return ret; +} + +void +pdoa2XY(result_t *pRes, uint8_t channel) +{ + static const float pdoa_interval_shift_ch5 = 15.0f; + static const float pdoa_interval_shift_ch9 = -22.0f; + float pdoa_deg, r_m, x_m, y_m; /* PDOA (deg), range (m), x (m), y (m) */ + double p_diff_m; /* Path difference between the ports (m). */ + float pdoa_poll_deg, pdoa_final_deg; /* PDOAs poll and final (deg) */ + float pdoa_interval_shift, l_m, d_m; + // static float (*pdoa2path_diff)(float); + + if (channel == 5) { + pdoa_interval_shift = pdoa_interval_shift_ch5; + l_m = L_M_5; + d_m = D_M_5; + // pdoa2path_diff = &pdoa2path_diff_ch5; + } else { + pdoa_interval_shift = pdoa_interval_shift_ch9; + l_m = L_M_9; + d_m = D_M_9; + // pdoa2path_diff = &pdoa2path_diff_ch9; + } + + r_m = pRes->dist_cm / 100.0f; + pdoa_final_deg = pRes->pdoa_raw_deg; + pdoa_poll_deg = pRes->pdoa_raw_degP; + + /* Shift the range of PDOAs out of the board */ + pdoa_final_deg = + fmod(pdoa_final_deg - pdoa_interval_shift + 540.0f, + 360.0f) + pdoa_interval_shift - 180.0f; + pdoa_poll_deg = + fmod(pdoa_poll_deg - pdoa_interval_shift + 540.0f, + 360.0f) + pdoa_interval_shift - 180.0f; + + /* If jumping detected do not average. */ + if ((fabs(pdoa_final_deg) > 130.0f) + && (pdoa_final_deg * pdoa_poll_deg < 0.0f)) { + pdoa_deg = pdoa_final_deg; + } else { /*Average PDOA value using poll and final PDOA*/ + pdoa_deg = (pdoa_poll_deg + pdoa_final_deg) / 2.0f; + } + + /* Path difference (either LUT or just wave propagation theory). */ + if (channel == 5) { + p_diff_m = + (app.pConfig->s.phaseCorrEn) ? pdoa2path_diff_ch5(pdoa_deg) : (pdoa_deg + / 360.0f + * l_m); + } else { + p_diff_m = + (app.pConfig->s.phaseCorrEn) ? pdoa2path_diff_ch9(pdoa_deg) : (pdoa_deg + / 360.0f + * l_m); + } + pRes->path_diff = p_diff_m * 1e9; + + /* x and y from path difference and range */ + x_m = p_diff_m / d_m * r_m; + y_m = (fabs(x_m) < r_m) ? sqrt(r_m * r_m - x_m * x_m) : 0.0f; + + /* m -> cm */ + pRes->x_cm = x_m * 100.0f; + pRes->y_cm = y_m * 100.0f; +} + +/* + * @brief This is a special function, which starts the SAR sampling. + * This shall be started in order to update temperature of the chip. + * Note: the reading of temperature will be available ~1mS after start of + * this function. + * */ +// static void +// start_tempvbat_sar(void) +// { +// TODO: DW3000 +// uint8_t wr_buf[1]; +// // These writes should be single writes and in sequence +// wr_buf[0] = 0x80; // Enable TLD Bias +// dwt_writetodevice(RF_CONF_ID,0x11,1,wr_buf); +// +// wr_buf[0] = 0x0A; // Enable TLD Bias and ADC Bias +// dwt_writetodevice(RF_CONF_ID,0x12,1,wr_buf); +// +// wr_buf[0] = 0x0f; // Enable Outputs (only after Biases are up and running) +// dwt_writetodevice(RF_CONF_ID,0x12,1,wr_buf); // +// +// // Reading All SAR inputs +// wr_buf[0] = 0x00; +// dwt_writetodevice(TX_CAL_ID, TC_SARL_SAR_C,1,wr_buf); +// wr_buf[0] = 0x01; // Set SAR enable +// dwt_writetodevice(TX_CAL_ID, TC_SARL_SAR_C,1,wr_buf); +// } + +/** + * @brief ISR level (need to be protected if called from APP level) + * Setup the receiver to receive the expected Final message, Final Tx + * Time + rx_timeout + * + * This function is called from the (TX) interrupt handler. + * The Node can range to one tag at a time only. + */ +static void +resp_tx_cb_setup_receiver(node_info_t *pNodeInfo) +{ + // [2] startup receiver. + uint32_t tmp; + uint16_t timeout; +#if 0 + uint64_t anchorRespTxTime; + TS2U64_MEMCPY(anchorRespTxTime, pNodeInfo->nodeRespTx_ts); + tmp = pNodeInfo->pSfConfig->tag_pollTxFinalTx_us; // time between Final + // and Poll + // transmissions on + // the tag + // need to enable receiver prior to start of Final's preamble transmission + tmp -= pNodeInfo->pSfConfig->tag_replyDly_us; // configured time when + // Node was + // transmitting + // Response (now) + tmp -= pNodeInfo->msg_time.poll.phrAndData_us; // length of data part + // of the poll + tmp -= pNodeInfo->msg_time.response.preamble_us; // length of response's + // preamble + tmp -= pNodeInfo->msg_time.final.preamble_us; // length of final's + // preamble + + tmp = (uint32_t) (util_us_to_dev_time(tmp) >> 8); + + tmp += (uint32_t)(anchorRespTxTime >> 8); + tmp &= 0xFFFFFFFE; // This is the time when to enable the receiver + + timeout = + pNodeInfo->msg_time.final.sy + + RX_RELAX_TIMEOUT_SY; // timeout for reception of Final msg + + dwt_setdelayedtrxtime(tmp); // set delayed RX : waiting the + // Final + dwt_setrxtimeout(timeout); + dwt_rxenable(DWT_START_RX_DELAYED); // start delayed RX : if late, then + // the RX will be enabled + // immediately. +#else + // turn on the receiver to receive the Final message from the tag + // use delayed receive based on the previous receive event (Poll) + + // set rx turn on delay + tmp = pNodeInfo->pSfConfig->tag_pollTxFinalTx_us; // time between Final + // and Poll + // transmissions on + // the tag + tmp -= pNodeInfo->msg_time.final.preamble_us; // length of final's + // preamble (length + // of packet until + // RMARKER) + tmp = (uint32_t) (util_us_to_dev_time(tmp) >> 8); // convert to device + // timeunits + dwt_setdelayedtrxtime(tmp); /* this time is relative to + * previous receive time (of the Poll) + * as we are using DWT_START_RX_DLY_RS below + */ + + // set timeout + timeout = + pNodeInfo->msg_time.final.sy + + RX_RELAX_TIMEOUT_SY; // timeout for reception of Final msg + dwt_setrxtimeout(timeout); + + dwt_rxenable(DWT_START_RX_DLY_RS); // delayed receive based on the + // previous receive timestamp + // (Poll's) +#endif +// start_tempvbat_sar(); //start sampling of a temperature on +// the Master chip: on reception of Final read the value. +} + +// ----------------------------------------------------------------------------- +// DW3000 callbacks section : +// if RTOS, the preemption priority of the dwt_isr() shall be such, that +// allows signal to the thread. + +/* @brief ISR level + * Real-time TWR application Tx callback + * to be called from dwt_isr() + * */ +void twr_tx_node_cb(const dwt_cb_data_t *txd) +{ + (void) txd; + node_info_t *pNodeInfo = getNodeInfoPtr(); + + if (!pNodeInfo) { + return; + } + + uint32_t tmp = rtc_counter_get(); + + // Store the Tx Time Stamp of the transmitted packet + switch (pNodeInfo->txState) + { + case Twr_Tx_Ranging_Config_Sent: // responder (node) + pNodeInfo->rangeInitRtcTimeStamp = tmp; + dwt_readtxtimestamp(pNodeInfo->rangeInitTx_ts); + break; + + case Twr_Tx_Resp_Sent: // responder (node) + pNodeInfo->respRtcTimeStamp = tmp; + dwt_readtxtimestamp(pNodeInfo->nodeRespTx_ts); + + resp_tx_cb_setup_receiver(pNodeInfo); // node additional algorithm + // for receivers + break; + + default: + break; + } +} + +/* @brief ISR level + * TWR application Rx callback + * to be called from dwt_isr() as an Rx call-back + * */ +void twr_rx_node_cb(const dwt_cb_data_t *rxd) +{ + node_info_t *pNodeInfo = getNodeInfoPtr(); + + if (!pNodeInfo) { + return; + } + + const int size = sizeof(pNodeInfo->rxPcktBuf.buf) + / sizeof(pNodeInfo->rxPcktBuf.buf[0]); + + int head = pNodeInfo->rxPcktBuf.head; + int tail = pNodeInfo->rxPcktBuf.tail; + + if (CIRC_SPACE(head, tail, size) <= 0) { + return; // no space in the fast intermediate circular buffer + } + + rx_pckt_t *p = &pNodeInfo->rxPcktBuf.buf[head]; + + p->rtcTimeStamp = rtc_counter_get(); // MCU RTC timestamp + + // TODO: DW3000 - should we use STS timestamp when using STS, and check for + // quality + + dwt_readrxtimestamp(p->timeStamp); // Raw Rx TimeStamp + p->status = rxd->status; + + if ((pNodeInfo->dw3000ChipType == AOA) + && (app.pConfig->dwt_config.pdoaMode != DWT_PDOA_M0)) { + p->PDOA.pdoa = dwt_readpdoa(); // Phase difference, signed in + // [1:-11] + } else { + p->PDOA.pdoa = 0; + memset(&p->PDOA.mDiag, 0x55, sizeof(p->PDOA.mDiag)); + } + p->rxDataLen = MIN(rxd->datalength, sizeof(p->msg)); + + dwt_readrxdata((uint8_t *)&p->msg, p->rxDataLen, 0); // Raw message + + if (app.rxTask.Handle) { // RTOS : rxTask can be not started yet + head = (head + 1) & (size - 1); + pNodeInfo->rxPcktBuf.head = head; // ISR level : do not need to + // protect + + // Sends the Signal to the application level via OS kernel. + // This will add a small delay of few us, but + // this method make sense from a program structure point of view. + if (osThreadFlagsSet(app.rxTask.Handle, app.rxTask.Signal) & osFlagsError) { + error_handler(1, _ERR_Signal_Bad); + } + } +} + +void twr_rx_timeout_cb(const dwt_cb_data_t *rxd) +{ + (void) rxd; + dwt_setrxtimeout(0); + dwt_rxenable(0); +} + +void twr_rx_error_cb(const dwt_cb_data_t *rxd) +{ + twr_rx_timeout_cb(rxd); +} + +// ----------------------------------------------------------------------------- + +// ----------------------------------------------------------------------------- +// The most real-time section of TWR algorithm + +/* return us - correction of time for a given Tag wrt uTimeStamp + * */ +static int32_t +calc_slot_correction(node_info_t *pNode, + uint16_t slot, + uint32_t uTimeStamp) + +{ + int tmp; + + tmp = uTimeStamp - pNode->gRtcSFrameZeroCnt; + + if (tmp < 0) { + tmp += RTC_WKUP_CNT_OVFLW; // RTC Timer overflow - 24 bit counter + } + + tmp = + (int)((tmp * WKUP_RESOLUTION_NS) + - (1e6f * slot * pNode->pSfConfig->slotPeriod)); + tmp /= 1e3f; + + return (tmp); // tagSleepCorrection_us +} + +/** + * @brief this function constructs the Ranging Config message, + * not including mac header + * + */ +static void +prepare_ranging_config_msg(rng_cfg_t *pRcfg, + tag_addr_slot_t *tag, + node_info_t *p, + uint32_t uTimeStamp) +{ + pRcfg->fCode = Twr_Fcode_Rng_Config; // function code (specifies if + // message is a rangeInit, + // poll, response, etc) + + pRcfg->tagAddr[0] = tag->addrShort[0]; // tag short address to be + // used in TWR + pRcfg->tagAddr[1] = tag->addrShort[1]; + + pRcfg->version = RC_VERSION_PDOA; + + { // slot period correction for the Tag wrt to + int32_t tagSleepCorrection_us = calc_slot_correction(p, + tag->slot, + uTimeStamp); + U32TOAR_MEMCPY(pRcfg->slotCorr_us, tagSleepCorrection_us); + } + + pRcfg->sframePeriod_ms[0] = p->pSfConfig->sfPeriod_ms & 0xff; + pRcfg->sframePeriod_ms[1] = p->pSfConfig->sfPeriod_ms >> 8 & 0xff; + + pRcfg->pollTxToFinalTx_us[0] = p->pSfConfig->tag_pollTxFinalTx_us + & 0xff; + pRcfg->pollTxToFinalTx_us[1] = p->pSfConfig->tag_pollTxFinalTx_us >> + 8 & 0xff; + + pRcfg->delayRx_us[0] = p->pSfConfig->tag_replyDly_us & 0xff; + pRcfg->delayRx_us[1] = p->pSfConfig->tag_replyDly_us >> 8 & 0xff; + + pRcfg->pollMultFast[0] = tag->multFast & 0xff; // tag config : + // multiplier: i.e + // poll every 1 + // periods + pRcfg->pollMultFast[1] = tag->multFast >> 8 & 0xff; // if moving + + pRcfg->pollMultSlow[0] = tag->multSlow & 0xff; // tag config : + // multiplier: i.e. + // poll every 10 + // period + pRcfg->pollMultSlow[1] = tag->multSlow >> 8 & 0xff; // if stationary + + pRcfg->mode[0] = tag->mode & 0xff; // tag config : i.e. use imu to + // identify stationary : bit 0; + pRcfg->mode[1] = tag->mode >> 8 & 0xff; // +} + +/** + * @brief This function constructs the data part of Response Msg. + * It is called after node receives a Poll from a tag. + * Mac header for the message should be created separately. + */ +static void +prepare_response_msg(resp_tag_t *pResp, + tag_addr_slot_t *tag, + node_info_t *p, + uint32_t uTimeStamp) +{ + int32_t tmp; + pResp->fCode = Twr_Fcode_Resp_Ext; + + { // slot period correction for the Tag + int32_t tagSleepCorrection_us = calc_slot_correction(p, + tag->slot, + uTimeStamp); + U32TOAR_MEMCPY(pResp->slotCorr_us, tagSleepCorrection_us); + } + + pResp->rNum = p->result[tag->slot].rangeNum; + + /* Send back to the tag the previous X, Y & clockOffset */ + tmp = (int32_t)(p->result[tag->slot].x_cm); + pResp->x_cm[0] = (uint8_t)(tmp & 0xFF); + pResp->x_cm[1] = (uint8_t)(tmp >> 8 & 0xFF); + + tmp = (int32_t)(p->result[tag->slot].y_cm); + pResp->y_cm[0] = (uint8_t)(tmp & 0xFF); + pResp->y_cm[1] = (uint8_t)(tmp >> 8 & 0xFF); + + tmp = (int32_t)(p->result[tag->slot].clockOffset_pphm); + pResp->clkOffset_pphm[0] = (uint8_t)(tmp & 0xFF); + pResp->clkOffset_pphm[1] = (uint8_t)(tmp >> 8 & 0xFF); +} + +/* @brief APP level + * part of Real-time TWR algorithm implementation (Responder) + * + * if called from ISR level, then revise/remove + * TWR_ENTER_CRITICAL() + * TWR_EXIT_CRITICAL() + * + * Note: + * Shall be called with guarantee that the DWT_IRQ will not happen + * + * @return _NO_ERR for no errors / error_e otherwise + * */ +static error_e +node_send_ranging_config(rx_pckt_t *pRxPckt, + node_info_t *p) + +{ + error_e ret; + tx_pckt_t TxPckt; /**< allocate TxPckt */ + tag_addr_slot_t *tag = pRxPckt->tag; + + /* tmp, tmp64 used to calculate the time when to send the delayed Ranging + * Config (i.e. R-Marker of RangingConfig), + * using Tag's configuration of when Tag is activating its receiver + * */ + uint32_t tmp; + uint64_t tmp64; + + /* setup the rest of Ranging Config data */ + rng_cfg_t *pRcfg; + + if (tag->reqUpdatePending == 0) { + /* When the Node receives a blink from a Tag, + * setup the Ranging Config MAC frame header with Long-Short addressing mode + * */ + rng_cfg_msg_t *pTxMsg = &TxPckt.msg.rngCfgMsg; + + /* Construct TX Ranging Config UWB message (LS) */ + TxPckt.psduLen = sizeof(rng_cfg_msg_t); + + /* See IEEE frame header description */ + pTxMsg->mac.frameCtrl[0] = Head_Msg_STD; + pTxMsg->mac.frameCtrl[1] = Frame_Ctrl_LS; /**mac.panID[0] = p->panID & 0xff; + pTxMsg->mac.panID[1] = p->panID >> 8 & 0xff; + memcpy(pTxMsg->mac.destAddr, + &tag->addr64, + sizeof(pTxMsg->mac.destAddr)); /**< tag's address */ + pTxMsg->mac.sourceAddr[0] = p->euiShort[0]; /**< node short + * address to be + * used by the tag + * in TWR */ + pTxMsg->mac.sourceAddr[1] = p->euiShort[1]; + pTxMsg->mac.seqNum = p->seqNum; + + /* rest of Ranging Config */ + pRcfg = &pTxMsg->rngCfg; + + // rcDelay_us is a SYSTEM configuration value of the delay + // after completion of a Tag's Blink Tx, when the Tag will turn on its + // receiver + // and will wait for the Ranging Config Response from the Node. + // From Node's view this is a delay between end of reception of Blink's data + // and start of transmission of preamble of Ranging Config. + tmp = app.pConfig->s.rcDelay_us; + + /* Adjust the transmission time with respect to the last System TimeStamp, + * i.e. Timestamp of Blink */ + tmp += p->msg_time.blink.phrAndData_us; // pre-calculated + // length of Blink + // packet (data) + tmp += p->msg_time.ranging_config.preamble_us; // pre-calculated + // length of preamble + // length of Ranging + // Config + tmp += p->msg_time.ranging_config.sts_us; // pre-calculated + // length of sts of + // Ranging Config + TS2U64_MEMCPY(tmp64, pRxPckt->timeStamp); // Blink's timestamp + tmp64 += util_us_to_dev_time(tmp); + tmp64 &= MASK_TXDTS; + } else { + tag->reqUpdatePending = 0; + + /* When the Node receives a poll and need to update the Tag, it sends + * Ranging Config instead of response + * setup the Ranging Config MAC frame header with Short-Short addressing + * mode + * */ + rng_cfg_upd_msg_t *pTxMsg = &TxPckt.msg.rngCfgUpdMsg; + + /* Construct TX Ranging Config Update UWB message */ + TxPckt.psduLen = sizeof(rng_cfg_upd_msg_t); + + /* See IEEE frame header description */ + pTxMsg->mac.frameCtrl[0] = Head_Msg_STD; + pTxMsg->mac.frameCtrl[1] = Frame_Ctrl_SS; + pTxMsg->mac.panID[0] = p->panID & 0xff; + pTxMsg->mac.panID[1] = (p->panID >> 8) & 0xff; + pTxMsg->mac.destAddr[0] = tag->addrShort[0]; + pTxMsg->mac.destAddr[1] = tag->addrShort[1]; + pTxMsg->mac.sourceAddr[0] = p->euiShort[0]; + pTxMsg->mac.sourceAddr[1] = p->euiShort[1]; + pTxMsg->mac.seqNum = p->seqNum; + + /* rest of Ranging Config */ + pRcfg = &pTxMsg->rngCfg; + + // tag_replyDly_us is a SYSTEM configuration value of the delay + // after completion of a Tag's Poll Tx, when the Tag will turn on its + // receiver + // and will wait for the Response from the Node. + // From Node's view this is a delay between end of reception of Poll's data + // and start of transmission of preamble of Reply (Ranging Config). + tmp = app.pConfig->s.sfConfig.tag_replyDly_us; + + /* Adjust the transmission time with respect to the last System TimeStamp, + * i.e. Timestamp of Poll */ + tmp += p->msg_time.poll.phrAndData_us; // pre-calculated length + // of Poll packet + // (data) + tmp += p->msg_time.ranging_config.preamble_us; // pre-calculated length + // of preamble length + // of Ranging Config + tmp += p->msg_time.ranging_config.sts_us; // pre-calculated length + // of sts of Ranging + // Config + TS2U64_MEMCPY(tmp64, pRxPckt->timeStamp); // Poll's timestamp + tmp64 += util_us_to_dev_time(tmp); + tmp64 &= MASK_TXDTS; + } + + /* write the rest of Ranging Config */ + prepare_ranging_config_msg(pRcfg, tag, p, pRxPckt->rtcTimeStamp); + + TxPckt.txFlag = (DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED); + TxPckt.delayedTxTimeH_sy = tmp64 >> 8; // at this time the Node will + // transmit the Ranging Config + // TimeStamp (R-Marker) + TxPckt.delayedRxTime_sy = 0; // switch on Rx after + // RangingConfig transmission + // immediately + TxPckt.delayedRxTimeout_sy = 0; + + p->txState = Twr_Tx_Ranging_Config_Sent; // indicate to TX + // ISR that the + // RangeInit has + // been sent + p->seqNum++; + + TWR_ENTER_CRITICAL(); + + ret = tx_start(&TxPckt); + + TWR_EXIT_CRITICAL(); + + if (ret != _NO_ERR) { + p->lateTxCount++; + } + + return (ret); +} + +/* @brief APP level + * part of Real-time TWR algorithm implementation (Responder) + * + * if called from ISR level, then remove + * TWR_ENTER_CRITICAL() and TWR_EXIT_CRITICAL() around tx_start() + * + * Note: + * Shall be called with guarantee that the DWT_IRQ will not happen + * + * @return _NO_ERR for no errors / error_e otherwise + * */ +static error_e +node_send_response(rx_pckt_t *pRxPckt, node_info_t *p) +{ + error_e ret; + uint32_t tmp; + uint64_t tmp64; + + /* Construct the response tx packet to the tag */ + tx_pckt_t TxPckt; + resp_pdoa_msg_t *pTxMsg = &TxPckt.msg.respMsg; + + pTxMsg->mac.frameCtrl[0] = Head_Msg_STD; + pTxMsg->mac.frameCtrl[1] = Frame_Ctrl_SS; + pTxMsg->mac.panID[0] = p->panID & 0xff; + pTxMsg->mac.panID[1] = (p->panID >> 8) & 0xff; + pTxMsg->mac.destAddr[0] = pRxPckt->tag->addrShort[0]; + pTxMsg->mac.destAddr[1] = pRxPckt->tag->addrShort[1]; + pTxMsg->mac.sourceAddr[0] = p->euiShort[0]; + pTxMsg->mac.sourceAddr[1] = p->euiShort[1]; + pTxMsg->mac.seqNum = p->seqNum; + + prepare_response_msg(&pTxMsg->resp, pRxPckt->tag, p, pRxPckt->rtcTimeStamp); + + /* [1] configure TX devtime when R-marker of Response will be transmitted */ + + // DW3000 will adjust the transmission of SFD of the response packet exactly + // to PollRx + tag_replyDly + length of PLEN of msg + + tmp = p->pSfConfig->tag_replyDly_us; // tag_replyDly_us is a SYSTEM + // configuration value of the + // delay + // after completion of a Tag PollTx, when the Tag will turn on its receiver + // and will wait for the Response from the Node. + + tmp += p->msg_time.poll.phrAndData_us; // pre-calculated length of Poll + // packet + tmp += p->msg_time.response.preamble_us; // pre-calculated length of + // Response packet + tmp += p->msg_time.response.sts_us; // pre-calculated length of + // Response packet + + /* Adjust the Response transmission time with respect to the last system + * TimeStamp */ + TS2U64_MEMCPY(tmp64, p->nodePollRx_ts); + tmp64 += util_us_to_dev_time(tmp); + tmp64 &= MASK_TXDTS; + + TxPckt.delayedTxTimeH_sy = tmp64 >> 8; // at this time the Node will + // transmit the Response's + // TimeStamp + // in the DWT_START_TX_DELAYED + // mode. + + TxPckt.psduLen = sizeof(resp_pdoa_msg_t); + TxPckt.txFlag = (DWT_START_TX_DELAYED); /* DW3000 receiver + * will be set in + * the dwt_isr() : + * twr_tx_node_cb() + */ + TxPckt.delayedRxTime_sy = 0; + TxPckt.delayedRxTimeout_sy = 0; + + p->seqNum++; + p->txState = Twr_Tx_Resp_Sent; // indicate to + // TX_IRQ that the + // Response was + // sent + + TWR_ENTER_CRITICAL(); + + ret = tx_start(&TxPckt); + + TWR_EXIT_CRITICAL(); + + // after a good tx_start(), DW_IC will be configured for reception in the + // TX_IRQ callback [2]. + // if the tx_start() failed delayed transmit, the DW_IC receiver will be + // re-enabled in the control application. + + return (ret); +} + +/* @brief APP level + * part of Real-time TWR algorithm implementation (Responder) + * + * if called from ISR level, then remove + * TWR_ENTER_CRITICAL() and TWR_EXIT_CRITICAL() + * + * Note: + * Shall be called with guarantee that the DWT_IRQ will not happen + * + * @return + * */ +static void node_read_final_diagnostics(rx_pckt_t *pRxPckt) +{ + /* read diagnostics from the chips */ + TWR_ENTER_CRITICAL(); + + { // Diagnostics data logging for the final message + if (app.pConfig->s.diagEn == 1) { + dwt_readdiagnostics(&pRxPckt->diag_dw3000); + } + + if (app.pConfig->s.accEn == 1) { + // TODO: review + // memset(pRxPckt->acc, 0, sizeof(pRxPckt->acc)); + // dwt_readaccdata(&pRxPckt->acc[0][0], sizeof(pRxPckt->acc[0]), + // ACC_OFFSET); + } + } + + TWR_EXIT_CRITICAL(); +} + +/* @brief APP level + * Real-time TWR algorithm implementation (Responder) + * + * prefer to be called from application UWB Rx thread, but + * can be called bare-metal from ISR, i.e. twr_rx_node_cb() + * directly. + * if called from ISR level, then revise/remove + * TWR_ENTER_CRITICAL() + * TWR_EXIT_CRITICAL() + * + * Note: + * Shall be called with the guarantee that the DWT_IRQ will not + * happen + * + * @return returning the result of low-level parse as error_e code: + * _NO_ERR : TX sent + * _Err_Not_Twr_Frame + * _Err_DelayedTX_Late + * _Err_Not_Twr_Frame + * _Err_Unknown_Tag + * _No_Err_New_Tag + * _No_Err_Final + * + * */ +error_e twr_responder_algorithm_rx(rx_pckt_t *pRxPckt, node_info_t *pNodeInfo) +{ + fcode_e fcode = Twr_Fcode_Not_Defined; + error_e ret = _ERR_Not_Twr_Frame; + std_msg_t *pMsg = &pRxPckt->msg.stdMsg; + + if ((pMsg->mac.frameCtrl[0] == Head_Msg_BLINK) + && (pRxPckt->rxDataLen == sizeof(blink_msg_t))) { + fcode = Twr_Fcode_Blink; + pNodeInfo->seqNum = pMsg->mac.frameCtrl[1]; // for Blink message the + // second byte is seq. + // number + } else if ((pMsg->mac.frameCtrl[0] == Head_Msg_STD) + || (pMsg->mac.frameCtrl[0] == Head_Msg_STD_AR)) { + /* Apart of Blinks only 16-bit dest/source addresses (SS) MAC headers + * supported in current Node application */ + switch (pMsg->mac.frameCtrl[1] & Frame_Ctrl_MASK) + { + case Frame_Ctrl_SS: + if ((pRxPckt->rxDataLen == sizeof(poll_msg_t)) /* Poll */ + || (pRxPckt->rxDataLen == sizeof(final_msg_accel_t))) { /* Final + * extended + */ + fcode = ((std_msg_ss_t *)pMsg)->messageData[0]; + } + break; + default: + fcode = Twr_Fcode_Not_Defined; + break; + } + } else { + fcode = Twr_Fcode_Not_Defined; + } + + /* received packet with "fcode" functional code */ + switch (fcode) + { + case Twr_Fcode_Blink: + { + /* Responder (Node) received Blink message from Tag in discovery process. + * 1. if Tag addr64 is unknown : report to upper application + * 2. if Tag addr64 is in the known Tag list, setup Timing parameters and + * send back to the Tag in the + * Ranging Config message; + * */ + uint64_t addr64; + + memcpy(&addr64, + ((blink_msg_t *)pMsg)->tagID, + sizeof(addr64)); // valid only for low endian + + pRxPckt->tag = get_tag64_from_knownTagList(addr64); + + pNodeInfo->pDestTag = NULL; /* New Blink: interrupt any + * range exchange if it + * was in progress */ + memset(pNodeInfo->nodePollRx_ts, + 0, + sizeof(pNodeInfo->nodePollRx_ts)); /* + * received + * time + * of + * poll + * message + */ + memset(pNodeInfo->nodeRespTx_ts, + 0, + sizeof(pNodeInfo->nodePollRx_ts)); /* + * anchor's + * response + * tx + * time + */ + + if (pRxPckt->tag) { + pRxPckt->tag->reqUpdatePending = 0; + ret = node_send_ranging_config(pRxPckt, pNodeInfo); + } else { + pNodeInfo->newTag_addr64 = addr64; + ret = _NO_Err_New_Tag; /* report to the upper + * application discovery + * of a new Tag + */ + } + break; + } + + case Twr_Fcode_Tag_Poll: + { + /* Responder (Node) received the Poll from a Tag. + * 1. if Tag addr16 is in the known Tag list, perform ranging sequence + * with this tag. + * 2. otherwise ignore + * */ + uint16_t addr16; + + addr16 = AR2U16(((poll_msg_t *)pMsg)->mac.sourceAddr); + + pRxPckt->tag = get_tag16_from_knownTagList(addr16); + + if (pRxPckt->tag) { + pNodeInfo->pDestTag = pRxPckt->tag; /* + * current tag we + * are ranging to + */ + TS2TS_MEMCPY(pNodeInfo->nodePollRx_ts, + pRxPckt->timeStamp); /* + * node's + * received time + * of Poll message + */ + pNodeInfo->pollRtcTimeStamp = pRxPckt->rtcTimeStamp; + + memcpy(&pNodeInfo->pollPDOA, &pRxPckt->PDOA, + sizeof(pNodeInfo->pollPDOA)); /* + * node's received + * PDOA result + */ + memset(&pNodeInfo->finalPDOA, 0, sizeof(pNodeInfo->finalPDOA)); + + if (pRxPckt->tag->reqUpdatePending) { + ret = node_send_ranging_config(pRxPckt, + pNodeInfo); /* send any updates to tag + * (e.g. change TWR rate) + */ + } else { + ret = node_send_response(pRxPckt, + pNodeInfo); /* send a response + * message - continue + * with TWR + */ + } + + if (ret == _NO_ERR) { + pNodeInfo->result[pRxPckt->tag->slot].rangeNum = + pRxPckt->msg.pollMsg.poll.rNum; + + /* Below results will be calculated in the Node on reception of Final + * from the Tag. + * If the calculations was not successful, Node will not report wrong + * values to usb/uart, + * but will send them back to the Tag (in the next Response message) + */ + pNodeInfo->result[pRxPckt->tag->slot].pdoa_raw_deg = (float)0xDEAD; + pNodeInfo->result[pRxPckt->tag->slot].clockOffset_pphm = + (float)0xDEAD; + pNodeInfo->result[pRxPckt->tag->slot].dist_cm = (float)0xDEAD; + pNodeInfo->result[pRxPckt->tag->slot].x_cm = (float)0xDEAD; + pNodeInfo->result[pRxPckt->tag->slot].y_cm = (float)0xDEAD; + } else { + pNodeInfo->pDestTag = NULL; /* no range + * exchange is + * in progress + */ + memset(pNodeInfo->nodePollRx_ts, + 0, + sizeof(pNodeInfo->nodePollRx_ts)); /* + * clear received time + * of poll message + */ + memset(pNodeInfo->nodeRespTx_ts, + 0, + sizeof(pNodeInfo->nodeRespTx_ts)); /* + * clear node's + * response tx time + */ + + pNodeInfo->lateTxCount++; + } + } else { + ret = _ERR_Unknown_Tag; + } + break; + } + + case Twr_Fcode_Tag_Accel_Final: + { + /* Responder (Node) received the Final from Tag, + * 1. if Tag is that one the node is currently ranging to : + * read registers necessary for ranging & phase difference and + * report this to upper application + * to calculate the result + * 2. otherwise ignore + */ + uint16_t addr16; + ret = _ERR_Unknown_Tag; + + addr16 = AR2U16(((final_msg_accel_t *)pMsg)->mac.sourceAddr); + + pRxPckt->tag = get_tag16_from_knownTagList(addr16); + + if (pRxPckt->tag && (pRxPckt->tag == pNodeInfo->pDestTag)) { + memcpy(&pNodeInfo->finalPDOA, &pRxPckt->PDOA, + sizeof(pNodeInfo->finalPDOA)); + + node_read_final_diagnostics(pRxPckt); + ret = _NO_Err_Final; + } + + break; + } + + default: + /* Responder (Node) received unknown data : discard previous measurements + * and restart the reception + * */ + pNodeInfo->pDestTag = NULL; + + ret = _ERR_Not_Twr_Frame; + break; + } + + return (ret); +} + +// ----------------------------------------------------------------------------- + +// ----------------------------------------------------------------------------- + +/* + * @brief setup RTC Wakeup timer + * period_ms is awaiting time in ms + * */ +void node_configure_rtc_wakeup(uint32_t period_ms) +{ + rtc_configure_wakeup_ms(period_ms); +} + +/* HAL RTC Wakeup timer callback. + * Use the LED_NODE_Pin to see synchronization of tags + * on the oscilloscope if needed + * */ +void rtcWakeUpTimerEventCallback_node(void) +{ + node_info_t *pNodeInfo = getNodeInfoPtr(); + + if (!pNodeInfo) { + return; + } + + pNodeInfo->gRtcSFrameZeroCnt = rtc_counter_get(); + rtc_reload(); + + if (app.trilatTask.Handle) { + trilat_SF_cb(); + } +} + +// ----------------------------------------------------------------------------- + +/* @brief app level + * RTOS-independent application level function. + * initializing of a TWR Node functionality. + * + * */ +error_e node_process_init() +{ + if (!psNodeInfo) { + psNodeInfo = NODE_MALLOC(sizeof(node_info_t)); + } + + node_info_t *pNodeInfo = getNodeInfoPtr(); + + if (!pNodeInfo) { + return(_ERR_Cannot_Alloc_NodeMemory); + } + + /* switch off receiver's rxTimeOut, RxAfterTxDelay, delayedRxTime, + * autoRxEnable, dblBufferMode and autoACK, + * clear all initial counters, etc. + * */ + memset(pNodeInfo, 0, sizeof(node_info_t)); + + /* Configure non-zero initial variables.1 : from app parameters */ + + /* The Node has its configuration in the app->pConfig, see DEFAULT_CONFIG. + * Tag will receive its configuration, such as + * panID, tagAddr, node0Addr and TWR delays: + * pollTx2FinalTxDelay_us and tag_replyDly_us from Range Config message. + * + * The reception timeouts calculated based on a known length of + * RangeInit and Response packets. + * + * */ + pNodeInfo->pSfConfig = &app.pConfig->s.sfConfig; /**< Super Frame + * configuration */ + pNodeInfo->panID = app.pConfig->s.panID; /**< panID */ + pNodeInfo->eui16 = app.pConfig->s.addr; /**< Node's address */ + + { // calculate two-way ranging frame timings + dwt_config_t *pCfg = &app.pConfig->dwt_config; // dwt_config : holds + // node's UWB mode + + msg_t msg; + + msg.dataRate = pCfg->dataRate; // Deca define: e.g. + // DWT_BR_6M8 + msg.txPreambLength = pCfg->txPreambLength; // Deca define: e.g. + // DWT_PLEN_128 + msg.sfdType = pCfg->sfdType; + msg.txPcode = pCfg->txCode; + msg.stsLength = + (pCfg->stsMode + & DWT_STS_CONFIG_MASK) ? ((1 << (pCfg->stsLength + 2)) * 8) : 0; + + msg.msg_len = sizeof(blink_msg_t); + calculate_msg_time(&msg, &pNodeInfo->msg_time.blink); + + msg.msg_len = sizeof(rng_cfg_msg_t); + calculate_msg_time(&msg, &pNodeInfo->msg_time.ranging_config); + + msg.msg_len = sizeof(poll_msg_t); + calculate_msg_time(&msg, &pNodeInfo->msg_time.poll); + + msg.msg_len = sizeof(resp_pdoa_msg_t); + calculate_msg_time(&msg, &pNodeInfo->msg_time.response); + + msg.msg_len = sizeof(final_msg_accel_t); + calculate_msg_time(&msg, &pNodeInfo->msg_time.final); + } + + /* dwt_xx calls in app level Must be in protected mode (DW3000 IRQ disabled) + */ + disable_dw3000_irq(); + + TWR_ENTER_CRITICAL(); + + if (dwt_initialise(DWT_DW_INIT) != DWT_SUCCESS) { /**< set callbacks to NULL + * inside + * dwt_initialise*/ + TWR_EXIT_CRITICAL(); + return (_ERR_INIT); + } + + set_dw_spi_fast_rate(); + + uint32_t dev_id = dwt_readdevid(); + + if ((dev_id == (uint32_t)DWT_DW3000_PDOA_DEV_ID) + || (dev_id == (uint32_t)DWT_QM33120_PDOA_DEV_ID)) { + pNodeInfo->dw3000ChipType = AOA; + + diag_printf("Found AOA DW3000 chip. PDoA is available.\r\n"); + } else if (dev_id == (uint32_t)DWT_DW3000_DEV_ID) { + pNodeInfo->dw3000ChipType = NON_AOA; + + app.pConfig->dwt_config.pdoaMode = DWT_PDOA_M0; + + diag_printf("Found non-AOA DW3000 chip. PDoA is not available.\r\n"); + } else { + TWR_EXIT_CRITICAL(); + diag_printf("Found unknown chip 0x%08lX. Stop.\r\n", + (unsigned long int)dev_id); + return _ERR_DEVID; + } + + /* read OTP Temperature calibration parameter */ + + /* Configure DW IC's UWB mode, sets power and antenna delays for TWR mode */ + rxtx_configure_t p; + p.pdwCfg = &app.pConfig->dwt_config; + p.frameFilter = DWT_FF_DISABLE; // DWT_FF_ENABLE_802_15_4 + p.frameFilterMode = (DWT_FF_DATA_EN | DWT_FF_ACK_EN); // FIXME + p.txAntDelay = app.pConfig->s.antTx_a; + p.rxAntDelay = app.pConfig->s.antRx_a; + p.panId = pNodeInfo->panID; // PanID : does not matter : + // DWT_FF_NOTYPE_EN : will be + // reconfigured on reception of RI + // message + p.shortadd = pNodeInfo->eui16; // ShortAddr + + rxtx_configure(&p); + if (app.pConfig->s.pdoaOffset_deg < 0) { + dwt_setpdoaoffset((int)(-1 * (app.pConfig->s.pdoaOffset_deg) * M_PI) + * (1 << 11) / 180); + } else if (app.pConfig->s.pdoaOffset_deg > 0) { + dwt_setpdoaoffset((int)((360 - (app.pConfig->s.pdoaOffset_deg)) * M_PI) + * (1 << 11) / 180); + } else { + dwt_setpdoaoffset((int)((0) * M_PI) * (1 << 11) / 180); + } + + dwt_setleds(DWT_LEDS_ENABLE + | DWT_LEDS_INIT_BLINK); /**< DEBUG I/O 2&3: + * configure the GPIOs + * which control the LEDs on HW + */ + dwt_setlnapamode(DWT_TXRX_EN); /**< DEBUG I/O 0&1 : configure TX/RX states + * to output on GPIOs + */ + + dwt_setcallbacks(twr_tx_node_cb, + twr_rx_node_cb, + twr_rx_timeout_cb, + twr_rx_error_cb, + NULL, + NULL, + NULL); + + dwt_setinterrupt(DWT_INT_TXFRS_BIT_MASK | DWT_INT_RXFCG_BIT_MASK + | (DWT_INT_ARFE_BIT_MASK | DWT_INT_RXFSL_BIT_MASK + | DWT_INT_RXSTO_BIT_MASK | DWT_INT_RXPHE_BIT_MASK + | DWT_INT_RXFCE_BIT_MASK | DWT_INT_RXFTO_BIT_MASK + + /*| DWT_INT_RXPTO_BIT_MASK*/), + 0, + DWT_ENABLE_INT_ONLY); + + dwt_configciadiag(DW_CIA_DIAG_LOG_ALL); /* DW3000 */ + + /* + * The dwt_initialize will read the default XTAL TRIM from the OTP or use the + * DEFAULT_XTAL_TRIM. + * In this case we would apply the user-configured value. + * + * Bit 0x80 can be used to overwrite the OTP settings if any. + * */ + if ((dwt_getxtaltrim() == DEFAULT_XTAL_TRIM) + || (app.pConfig->s.xtalTrim & ~XTAL_TRIM_BIT_MASK)) { + dwt_setxtaltrim(app.pConfig->s.xtalTrim & XTAL_TRIM_BIT_MASK); + } + + /* End configuration of DW IC */ + + /* Configure non-zero initial variables.2 : dwt_getlotid/dwt_getpartid are + * valid after dwt_initialise() */ + pNodeInfo->seqNum = (uint8_t)(0xff * rand() / RAND_MAX); + + { + /* configure the RTC Wakeup timer with a high priority; + * this timer is saving global Super Frame Timestamp, + * so we want this timestamp as stable as we can. + * + * */ + rtc_disable_irq(); + juniper_configure_hal_rtc_callback(rtcWakeUpTimerEventCallback_node); + node_configure_rtc_wakeup(pNodeInfo->pSfConfig->sfPeriod_ms); + } + + TWR_EXIT_CRITICAL(); + + return (_NO_ERR); +} + +/* + * @brief + * Enable DW3000 IRQ to start + * */ +void node_process_start(void) +{ + enable_dw3000_irq(); + + // start the RTC timer + rtc_enable_irq(); + + diag_printf("PDOA Node Top Application: Started\r\n"); +} + +/* @brief app level + * RTOS-independent application level function. + * deinitialize the pNodeInfo structure. + * This must be executed in protected mode. + * + * */ +void node_process_terminate(void) +{ + { // stop the RTC timer + rtc_disable_irq(); + juniper_configure_hal_rtc_callback(NULL); + } + + if (psNodeInfo) { + NODE_FREE(psNodeInfo); + psNodeInfo = NULL; + } +} + +static double pdoa2path_diff_ch5(float x) +{ + static const double xs[] = + { -169.52599388379204, -164.55029585798817, -160.03367003367003, + -155.23640661938535, -150.56637168141592, -145.09003215434083, + -138.81159420289856, -130.99603174603175, -123.18204488778055, + -113.84177215189874, -102.67299107142857, -91.66666666666667, + -78.92921686746988, -66.17794486215539, -52.489329268292686, + -38.47277936962751, -25.19344262295082, -10.773413897280967, + 0.9860681114551083, 15.321236559139784, 31.16460396039604, + 46.739010989010985, 62.48295454545455, 79.0, 93.02919708029196, + 105.17538461538462, 116.59448818897638, 125.65702479338843, + 135.40189873417722, 142.97699386503066, 151.34635416666666, + 158.2935656836461, 165.31201044386424, 172.39769820971867, 180.0, + 188.23993808049536, 198.43213296398892 }; + static const double ys[] = + { -0.02277711059704047, -0.022690436814620973, -0.022431075107181914, + -0.022000999373923726, -0.0214034827508634, -0.020643072700292697, + -0.019725556401844816, -0.018657916708562303, -0.01744827900316916, + -0.016105849359003235, -0.014640844476237574, -0.01306441392662416, + -0.011388555298520233, -0.009626022887996853, -0.007790230630944395, + -0.005895150014920516, -0.003955203747694204, -0.0019851559917306244, 0.0, + 0.001985155991730628, 0.0039552037476942034, 0.005895150014920523, + 0.007790230630944404, 0.009626022887996888, 0.011388555298520233, + 0.013064413926624103, 0.014640844476237608, 0.01610584935900323, + 0.017448279003169222, 0.018657916708562265, 0.019725556401844816, + 0.02064307270029269, 0.021403482750863533, 0.02200099937392368, + 0.02243107510718191, 0.02269043681462098, 0.022777110597040465 }; + static const int count = sizeof(xs) / sizeof(xs[0]); + + int i; + double dx, dy; + + if (x < xs[0]) { + return ys[0]; /* return minimum element */ + } + + if (x > xs[count - 1]) { + return ys[count - 1]; /* return maximum */ + } + + /* find i, such that xs[i] <= x < xs[i+1] */ + for (i = 0; i < count - 1; i++) { + if (xs[i + 1] > x) { + break; + } + } + + /* interpolate */ + dx = xs[i + 1] - xs[i]; + dy = ys[i + 1] - ys[i]; + return ys[i] + (x - xs[i]) * dy / dx; +} + +static double pdoa2path_diff_ch9(float x) +{ +// NRF M3 AND M1 - Updated 24/02/2022 + static const double xs[] = { -167.776, -162.1195, -159.04844444, -152.9295, + -150.32088889, -142.5845, -141.59333333, + -132.86577778, -131.577, + -124.13822222, -116.798, -115.41066667, + -106.68311111, -97.95555556, + -95.586, -89.228, -80.50044444, -71.77288889, + -66.012, + -63.04533333, -54.31777778, -45.59022222, + -36.86266667, -33.04, + -28.13511111, -19.40755556, -10.68, -1.95244444, + -0.0, + 6.77511111, 15.50266667, 24.23022222, + 32.95777778, 33.0005, + 41.68533333, 50.41288889, 59.14044444, 62.578, + 67.868, + 76.59555556, 85.32311111, 86.785, 94.05066667, + 102.77822222, 105.074, 111.50577778, 117.242, + 120.23333333, 126.564, 128.96088889, 133.051, + 137.68844444, 139.8965, 146.416 }; + + static const double ys[] = + { -0.0178831, -0.01761142, -0.01734181, -0.01680462, + -0.01647242, -0.01548722, -0.01532623, + -0.01390859, -0.01369925, + -0.0125898, -0.01149504, -0.01132803, + -0.01027741, -0.0092268, + -0.00894155, -0.00833418, -0.00750045, + -0.00666671, -0.00611638, + -0.00584547, -0.00504846, -0.00425146, + -0.00345446, -0.00310537, + -0.00263992, -0.00181171, -0.0009835, + -0.00015529, 0., + 0.00066095, 0.00147443, 0.00228791, 0.00310139, + 0.00310537, + 0.00398949, 0.00487796, 0.00576644, 0.00611638, + 0.00673377, + 0.00775235, 0.00877094, 0.00894155, 0.00995598, + 0.0111745, + 0.01149504, 0.01266014, 0.01369925, 0.01427299, + 0.01548722, + 0.01597399, 0.01680462, 0.01735118, 0.01761142, + 0.0178831 }; + + static const int count = sizeof(xs) / sizeof(xs[0]); + + int i; + double dx, dy; + + if (x < xs[0]) { + return ys[0]; /* return minimum element */ + } + + if (x > xs[count - 1]) { + return ys[count - 1]; /* return maximum */ + } + + /* find i, such that xs[i] <= x < xs[i+1] */ + for (i = 0; i < count - 1; i++) { + if (xs[i + 1] > x) { + break; + } + } + + /* interpolate */ + dx = xs[i + 1] - xs[i]; + dy = ys[i + 1] - ys[i]; + return ys[i] + (x - xs[i]) * dy / dx; +} + +// ----------------------------------------------------------------------------- diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/node/node/node.h b/bluetooth_uwb_dw3000_slotted_twr/src/apps/node/node/node.h new file mode 100644 index 0000000..9828588 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/node/node/node.h @@ -0,0 +1,308 @@ +/** + * @file node.h + * + * @brief Decawave + * bare implementation layer + * + * @author Decawave + * + * @attention Copyright 2017 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef __NODE__H__ +#define __NODE__H__ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#include "uwb_frames.h" +#include "msg_time.h" +#include "tag_list.h" + +#include "port.h" +#include "port_common.h" +#include "common_n.h" + +// ----------------------------------------------------------------------------- +// Definitions +#define FULL_ACC_LEN (1016) +#define ACC_OFFSET (300) + +// ----------------------------------------------------------------------------- + +// ----------------------------------------------------------------------------- + +/* WKUP timer counts Super Frame period. + * The WKUP timer resolution is (61035) counts in 1 ns. + */ +#define SPEED_OF_LIGHT (299702547.0) // in m/s in the air +#define DWT_DIAGNOSTIC_LOG_REV_5 (5) +#define INVALID_TOF (0xABCDFFFF) + +/* Bitmask for result_t flag's. + * Note, byte_0 is bitmask from a tag; + * byte_1 is other flags: + * */ +#define RES_FLAG_PDOA_OFFSET_ZERO_BIT (1 << 15) /**< This bit will be set to + * the report when the + * "pdoa_offset" is zero + */ +#define RES_FLAG_RANGE_OFFSET_ZERO_BIT (1 << 14) /**< This bit will be set to + * the report when the + * "rng_offset" is zero + */ + +// ----------------------------------------------------------------------------- + +/* + * Rx Events circular buffer : used to transfer RxPckt from ISR to APP + * 0x02, 0x04, 0x08, 0x10, etc. + * As per design, the amount of RxPckt in the buffer at any given time shall not + * be more than 1. + * */ +#define EVENT_BUF_NODE_SIZE (0x02) + +// ----------------------------------------------------------------------------- + +/* + * RX_MAIL_QUEUE_SIZE : used to transfer RxPckt from rxTask() to calkTask() + * it shall be smaller than bare-metal circular buffer EVENT_BUF_SIZE + * See note to osMailQDef(rxPcktPool_q, RX_MAIL_QUEUE_SIZE, rx_mail_t); + * */ +#define RX_MAIL_QUEUE_SIZE (0x01) + +// ----------------------------------------------------------------------------- +// Typedefs + +/* RxPckt is the structure is for the current reception */ +struct rx_pckt_s +{ + int16_t rxDataLen; + + union { + std_msg_t stdMsg; + twr_msg_t twrMsg; + blink_msg_t blinkMsg; + rng_cfg_msg_t rngCfgMsg; + poll_msg_t pollMsg; + resp_pdoa_msg_t respMsg; + final_msg_accel_t finalMsg; + } msg; + + tag_addr_slot_t *tag; /* the tag, to which the current + * range exchange is + * performing */ + + uint8_t timeStamp[TS_40B_SIZE]; /* TimeStamp of current RX: + * blinkRx, pollRx, finalRx */ + uint32_t rtcTimeStamp; /* MCU current Rx RTC timestamp: + * make sense for the Poll + * message reception */ + + uint8_t temperature; /* raw reading of temperature: + * valid at reception of Final + */ + + pdoa_t PDOA; + + /* Below is Decawave's diagnostics information */ + uint32_t status; + + dwt_rxdiag_t diag_dw3000; /*TODO: review diagnostics */ + + uint8_t acc[(FULL_ACC_LEN - ACC_OFFSET) * 4 + 1];/* TODO: + * review accumulator + * reading / + * do we need it + * in this app ? / + * Will be started + * with offset + * ACC_OFFSET : + * for ACC_OFFSET = 0 + * 2x ((1016-0)*4 + 1) + * = 2x 4065 = 8130 bytes + * for raw ACC + * + * for ACC_OFFSET = 300 + * 2x ((1016-300)*4 + 1) + * = 2x 2865 = 5730 bytes + * for raw ACC + */ +}; + +typedef struct rx_pckt_s rx_pckt_t; + +/* Mail from RxTask to CalcTask*/ +struct rx_mail_s +{ + tag_addr_slot_t *tag; /* the tag, to which the current range was + * performed */ + + result_t res; + + uint8_t tagPollTx_ts[TS_40B_SIZE]; + uint8_t tagRespRx_ts[TS_40B_SIZE]; + uint8_t tagFinalTx_ts[TS_40B_SIZE]; + + uint8_t nodePollRx_ts[TS_40B_SIZE]; + uint8_t nodeRespTx_ts[TS_40B_SIZE]; + uint8_t nodeFinalRx_ts[TS_40B_SIZE]; + + /* Below is Decawave's diagnostics information */ + dwt_rxdiag_t diag_dw3000; + uint8_t acc[(FULL_ACC_LEN - ACC_OFFSET) * 4 + 1];/* TODO: + * review accumulator + * reading / + * do we need it + * in this app ? / + * Will be started + * with offset + * ACC_OFFSET : + * for ACC_OFFSET = 0 + * 2x ((1016-0)*4 + 1) + * = 2x 4065 = 8130 bytes + * for raw ACC + * + * for ACC_OFFSET = 300 + * 2x ((1016-300)*4 + 1) + * = 2x 2865 = 5730 bytes + * for raw ACC + */ +}; + +typedef struct rx_mail_s rx_mail_t; + +/* This structure holds Node's TWR application parameters */ +struct node_info_s +{ + /* Unique short Address, uses at the ranging phase + * valid for low-endian compiler. + * */ + union { + uint8_t euiShort[2]; + uint16_t eui16; + }; + + /* circular Buffer of received Rx packets : + * uses in transferring of the data from ISR to APP level. + * */ + struct { + rx_pckt_t buf[EVENT_BUF_NODE_SIZE]; + uint16_t head; + uint16_t tail; + } rxPcktBuf; + + /* ranging run-time variables */ + struct { + /* MAC sequence number, increases on every tx_start */ + uint8_t seqNum; + + /* Node's Discovery phase : Tx time structures for DW_TX_IRQ callback */ + struct { + uint8_t rangeInitTx_ts[TS_40B_SIZE]; /**< node: rangeInitTx_ts, + * rangeInitRtcTimeStamp + */ + uint32_t rangeInitRtcTimeStamp; /**< handles the MCU RTC + * time at the DW_IRQ + */ + }; + + /* Node's Ranging phase : Tx time structures for DW_TX_IRQ callback */ + struct { + uint8_t nodeRespTx_ts[TS_40B_SIZE]; /**< node: nodeRespTx_ts, + * respRtcTimeStamp */ + uint32_t respRtcTimeStamp; /**< handles the MCU RTC + * time at the DW_IRQ + */ + }; + + /* Node's Ranging phase : Rx time structures for DW_TX_IRQ callback */ + struct { + uint8_t nodePollRx_ts[TS_40B_SIZE]; /**< temporary for current + * range exchange: + * received time of poll + * message */ + uint32_t pollRtcTimeStamp; /**< temporary for current + * range exchange: + * received time of poll + * message RTC time */ + }; + + /* Node's Ranging phase : DW3000 PDOA debug */ + pdoa_t pollPDOA; /**< DW3000 PDOA result from Poll message */ + pdoa_t finalPDOA; /**< DW3000 PDOA result from Poll message */ + + /* node is ranging to a single tag in a range exchange sequence */ + tag_addr_slot_t *pDestTag; /**< tag, with whom the current + * range is performing */ + + /* Application DW_TX_IRQ source indicator */ + tx_states_e txState; + }; + + /* pre-calculated times for different messages */ + struct { + msg_time_t blink; + msg_time_t ranging_config; + msg_time_t poll; + msg_time_t response; + msg_time_t final; + } msg_time; + + uint16_t panID; + sfConfig_t *pSfConfig; // superFrame configuration + + uint64_t newTag_addr64; // new discovered tag address + + result_t result[MAX_KNOWN_TAG_LIST_SIZE]; /* range/X/Y/Xtal_offset result + * back to the tag */ + + volatile uint32_t gRtcSFrameZeroCnt; // SuperFrame Cnt RTC timestamp + + uint32_t lateTxCount; // indicate that Delayed Tx timings + // failed + + struct { + unsigned int imuOn : 1; + unsigned int stationary : 1; + }; + + uint8_t Tmeas; // copy of Temperature calibration value for + // DW chip + + dw3000type_e dw3000ChipType; +}; + +typedef struct node_info_s node_info_t; + +// ----------------------------------------------------------------------------- +// exported functions prototypes +// +extern node_info_t * getNodeInfoPtr(void); + +// ----------------------------------------------------------------------------- +// exported functions prototypes +// + +/* responder (node) */ +error_e twr_responder_algorithm_rx(rx_pckt_t *pRxPckt, node_info_t *pNodeInfo); +void twr_configure_rtc_wakeup_ms(uint32_t period); +void synchronize_DW1000clocks(void); + +error_e node_process_init(void); +void node_process_start(void); +void node_process_terminate(void); + +error_e tof2range(param_block_t *, float *, int32_t); +void pdoa2XY(result_t *, uint8_t); + +// ----------------------------------------------------------------------------- + +#ifdef __cplusplus +} +#endif + +#endif /* __NODE__H__ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/node/task_node/task_node.c b/bluetooth_uwb_dw3000_slotted_twr/src/apps/node/task_node/task_node.c new file mode 100644 index 0000000..378ef22 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/node/task_node/task_node.c @@ -0,0 +1,466 @@ +/** + * @file task_node.c + * @brief Decawave Application level + * collection of TWR bare-metal functions for a Node + * + * @author Decawave + * + * @attention Copyright 2017 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + * ============================================================================= + * = = + * = = + * = D E C A W A V E C O N F I D E N T I A L = + * = = + * = = + * ============================================================================= + * + * This software contains Decawave confidential information and techniques, + * subject to licence and non-disclosure agreements. No part of this software + * package may be revealed to any third-party without the express permission of + * Decawave Ltd. + * + * ============================================================================= + */ +#include + +#include "task_node.h" +#include "deca_dbg.h" +#include "util.h" +#include "port_common.h" +#include "usb_uart_tx.h" +#include "node.h" + +#ifndef M_PI +#define M_PI 3.14159265358979323846f +#endif + +// ----------------------------------------------------------------------------- +// extern functions to report output data +extern void send_to_pc_diag_acc(rx_mail_t *pRxMailPckt); +extern void send_to_pc_twr(result_t *pRes); +extern void trilat_extension_node_put(result_t *pRes); + +// ----------------------------------------------------------------------------- + +/* + * @brief Node RTOS implementation + * This is normal priority task, which is awaiting for + * a mail with a new reception of raw data from a tag. + * + * On reception of a raw data mail it's performing the calculation + * of range and PDOA and reports the results. + * */ +static void CalcTask(void const *arg) +{ + node_info_t *pNodeInfo; + + while (!(pNodeInfo = getNodeInfoPtr())) + { + osDelay(5); + } + + const osMutexAttr_t thread_mutex_attr = { .attr_bits = osMutexPrioInherit }; + app.calcTask.MutexId = osMutexNew(&thread_mutex_attr); + + do { + osMutexRelease(app.calcTask.MutexId); + + rx_mail_t *pRxMailPckt = NULL; + osStatus_t sts = osMessageQueueGet(app.rxPcktQueue_q_id, + &pRxMailPckt, + NULL, + osWaitForever); + osMutexAcquire(app.calcTask.MutexId, 0); + + // Received the mail in RxPckt structure format : it includes all necessary + // raw data to + // calculate range, phase difference, (diagnostics and accumulators if any), + // etc. + if ((osOK != sts) || (NULL == pRxMailPckt)) { + continue; + } + + uint16_t slot = pRxMailPckt->tag->slot; + + result_t *pRes = &pNodeInfo->result[slot]; + + int32_t tofi = INVALID_TOF; + + { // calculate the time of flight and phase difference + memcpy(pRes, &pRxMailPckt->res, sizeof(result_t)); + + int64_t Rb, Da, Ra, Db; + + uint64_t tagPollTxTime; + uint64_t tagRespRxTime; + uint64_t tagFinalTxTime; + + uint64_t nodeRespTxTime; + uint64_t nodePollRxTime; + uint64_t nodeFinalRxTime; + + float RaRbxDaDb = 0; + float RbyDb = 0; + float RayDa = 0; + + TS2U64_MEMCPY(tagRespRxTime, pRxMailPckt->tagRespRx_ts); + TS2U64_MEMCPY(tagPollTxTime, pRxMailPckt->tagPollTx_ts); + TS2U64_MEMCPY(tagFinalTxTime, pRxMailPckt->tagFinalTx_ts); + + TS2U64_MEMCPY(nodeRespTxTime, pRxMailPckt->nodeRespTx_ts); + TS2U64_MEMCPY(nodePollRxTime, pRxMailPckt->nodePollRx_ts); + TS2U64_MEMCPY(nodeFinalRxTime, pRxMailPckt->nodeFinalRx_ts); + + pRes->pdoa_raw_deg = + ((float)pRes->finalPDOA.pdoa / (1 << 11)) * 180 / M_PI; + pRes->pdoa_raw_degP = + ((float)pRes->pollPDOA.pdoa / (1 << 11)) * 180 / M_PI; + + Ra = (int64_t)((tagRespRxTime - tagPollTxTime) & MASK_40BIT); + Db = (int64_t)((nodeRespTxTime - nodePollRxTime) & MASK_40BIT); + + Rb = (int64_t)((nodeFinalRxTime - nodeRespTxTime) & MASK_40BIT); + Da = (int64_t)((tagFinalTxTime - tagRespRxTime) & MASK_40BIT); + + RaRbxDaDb = (((float)Ra)) * (((float)Rb)) - (((float)Da)) * (((float)Db)); + + RbyDb = ((float)Rb + (float)Db); + + RayDa = ((float)Ra + (float)Da); + + tofi = (int32_t) (RaRbxDaDb / (RbyDb + RayDa)); + + // Compute clock offset, in parts per million + pRes->clockOffset_pphm = ((((float)Ra / 2) - (float)tofi) / (float)Db) + - ((((float)Rb / 2) - (float)tofi) / (float)Da); + + pRes->clockOffset_pphm *= 1e8; // hundreds of ppm + } + + /* reportTOF : correct Range bias if needed */ + float r_m; + + if (tof2range(app.pConfig, &r_m, tofi) == _NO_ERR) { + pRes->dist_cm = r_m * 100.0; + + pdoa2XY(pRes, app.pConfig->dwt_config.chan); + + if (app.trilatTask.Handle) { + trilat_extension_node_put(pRes); + } else { + /* Transmit the report to the PC std TWR reports */ + send_to_pc_twr(pRes); + send_to_pc_diag_acc(pRxMailPckt); // This maybe a slow + // blocking sending of + // large amount of + // data + // FlushTask should have + // higher priority + // than CalckTask + } + } else { + pRes->dist_cm = (float)(0xDEADBEEF); + pRes->x_cm = (float)(0xDEADBEEF); + pRes->y_cm = (float)(0xDEADBEEF); + + if (app.pConfig->s.debugEn) { + const char text[] = "Bad Range \r\n"; + port_tx_msg((uint8_t *)text, strlen(text)); + } + } + + /* remove the message from the mail queue */ + osMemoryPoolFree(app.rxPcktPool_q_id, pRxMailPckt); + }while (1); + + UNUSED(arg); +} + +/* @brief DW3000 RX : Node RTOS implementation + * this is a high-priority task, which will be executed immediately + * on reception of waiting Signal. Any task with lower priority will be + * interrupted. + * No other tasks in the system should have higher priority. + * */ +static void RxTask(void const *arg) +{ + int head, tail, size, tmp; + error_e ret; + + node_info_t *pNodeInfo; + + while (!(pNodeInfo = getNodeInfoPtr())) + { + osDelay(5); + } + + size = sizeof(pNodeInfo->rxPcktBuf.buf) / sizeof(pNodeInfo->rxPcktBuf.buf[0]); + + const osMutexAttr_t thread_mutex_attr = { .attr_bits = osMutexPrioInherit }; + app.rxTask.MutexId = osMutexNew(&thread_mutex_attr); + + taskENTER_CRITICAL(); + dwt_rxenable(DWT_START_RX_IMMEDIATE); // Start reception on the Node + taskEXIT_CRITICAL(); + + do{ + osMutexRelease(app.rxTask.MutexId); + + /* ISR is delivering RxPckt via circ_buf & Signal. + * This is the fastest method. + * */ + osThreadFlagsWait(app.rxTask.Signal, osFlagsWaitAny, osWaitForever); + + osMutexAcquire(app.rxTask.MutexId, 0); + + taskENTER_CRITICAL(); + head = pNodeInfo->rxPcktBuf.head; + tail = pNodeInfo->rxPcktBuf.tail; + taskEXIT_CRITICAL(); + + if (CIRC_CNT(head, tail, size) > 0) { + rx_pckt_t *pRxPckt = &pNodeInfo->rxPcktBuf.buf[tail]; + + ret = twr_responder_algorithm_rx(pRxPckt, pNodeInfo); /**< Run the + * bare-metal + * algorithm + */ + + if (ret == _NO_Err_Final) { + /* Mail the RxPckt to someone : another RTOS thread is awaiting for it + */ + final_accel_t *pFinalMsg = &pRxPckt->msg.finalMsg.final; + rx_mail_t *pMail = osMemoryPoolAlloc(app.rxPcktPool_q_id, + 10); // timeout 10ms + + if (pMail) { + if (app.pConfig->s.diagEn) { + memcpy(&pMail->diag_dw3000, &pRxPckt->diag_dw3000, + sizeof(pMail->diag_dw3000)); + } + + if (app.pConfig->s.accEn) { +// memcpy(&pMail->acc, &pRxPckt->acc[0], +// sizeof(pMail->acc)); + } + + /* Tag's information */ + pMail->tag = pRxPckt->tag; + pMail->res.addr16 = pRxPckt->tag->addr16; + pMail->res.rangeNum = pFinalMsg->rNum; + + /* PDoA information */ + memcpy(&pMail->res.pollPDOA, &pNodeInfo->pollPDOA, + sizeof(pMail->res.pollPDOA)); + memcpy(&pMail->res.finalPDOA, &pNodeInfo->finalPDOA, + sizeof(pMail->res.finalPDOA)); + + /* Accelerometer state of the tag: from pFinalMsg */ + pMail->res.flag = (uint16_t)(pFinalMsg->flag); + pMail->res.acc_x = (int16_t)(AR2U16(pFinalMsg->acc_x)); + pMail->res.acc_y = (int16_t)(AR2U16(pFinalMsg->acc_y)); + pMail->res.acc_z = (int16_t)(AR2U16(pFinalMsg->acc_z)); + + /* Convert raw reading of temperature using calibrated values for + * chips. + * Note, the calibrated values available only after dwt_initialize() + */ + // float temp; + // temp = (uint8_t)pRxPckt->temperature; + // temp = 23.0 + (float)(temp - pNodeInfo->Tmeas)*1.14; /**< + // UserManual 2.10: formula for conversion */ + // pMail->res.tMaster_C = (int8_t)(temp); + + /* Add flags for PC application */ + if (app.pConfig->s.pdoaOffset_deg == 0) { + pMail->res.flag |= RES_FLAG_PDOA_OFFSET_ZERO_BIT; + } + if (app.pConfig->s.rngOffset_mm == 0) { + pMail->res.flag |= RES_FLAG_RANGE_OFFSET_ZERO_BIT; + } + + /* Tag's ranging times: from pFinalMsg */ + TS2TS_MEMCPY(pMail->tagPollTx_ts, pFinalMsg->pollTx_ts); + TS2TS_MEMCPY(pMail->tagRespRx_ts, pFinalMsg->responseRx_ts); + TS2TS_MEMCPY(pMail->tagFinalTx_ts, pFinalMsg->finalTx_ts); + + /* Node's exchange timings for current Ranging */ + TS2TS_MEMCPY(pMail->nodePollRx_ts, pNodeInfo->nodePollRx_ts); + TS2TS_MEMCPY(pMail->nodeRespTx_ts, pNodeInfo->nodeRespTx_ts); + TS2TS_MEMCPY(pMail->nodeFinalRx_ts, pRxPckt->timeStamp); + + /* Final message reception time WRT to Node's Superframe (slots are + * from RTC)*/ + tmp = pNodeInfo->pollRtcTimeStamp + - pNodeInfo->gRtcSFrameZeroCnt; // NRF:RTC is CC up-counter + + if (tmp < 0) { + tmp += RTC_WKUP_CNT_OVFLW; + } + + pMail->res.resTime_us = (tmp * WKUP_RESOLUTION_NS) / 1000; + + if (osMessageQueuePut(app.rxPcktQueue_q_id, &pMail, 0, 0) != osOK) { + error_handler(1, _ERR_Cannot_Send_Mail); + } + } else { + error_handler(0, _ERR_Cannot_Alloc_Mail); // non-blocking + // error : no + // memory : + // resources, + // etc. This can + // happen when + // tags exchange + // received, but + // previous + // exchange has + // not been + // reported to + // the host yet + // For example on + // Accumulators + // readings + } + } + + if (ret == _NO_Err_New_Tag) { + if (addTagToDList(pNodeInfo->newTag_addr64)) { + signal_to_pc_new_tag_discovered(pNodeInfo->newTag_addr64); + +#if DEBUG_NO_GUI + // this will add tag with assigning of automatic adresss if GUI + // application is not in use: + // this guarantee the tag would have a unique address + // do not use this when GUI is connected + add_tag_to_knownTagList(pNodeInfo->newTag_addr64, + (uint16_t)pNodeInfo->newTag_addr64 & 0xFFFF, + 1, + 1, + 1); +#endif + } + + pNodeInfo->newTag_addr64 = 0; + } + + taskENTER_CRITICAL(); + tail = (tail + 1) & (size - 1); + pNodeInfo->rxPcktBuf.tail = tail; + taskEXIT_CRITICAL(); + + if ((ret != _NO_ERR)) { + /* If the Node is performing a Tx, then the receiver will be enabled + * after the Tx automatically and twr_responder_algorithm_rx reports + * "_NO_ERR". + * + * Otherwise always re-enable the receiver : unknown frame, not expected + * tag, + * final message, _Err_DelayedTX_Late, etc. + * */ + taskENTER_CRITICAL(); + dwt_setrxtimeout(0); + dwt_rxenable(DWT_START_RX_IMMEDIATE); // re-enable receiver + // again - no timeout + taskEXIT_CRITICAL(); + } + + /* ready to serve next raw reception */ + } + + osThreadYield(); + }while (1); + + UNUSED(arg); +} + +// ----------------------------------------------------------------------------- + +/* @brief Setup TWR tasks and timers for discovery phase. + * - blinking timer + * - blinking task + * - twr polling task + * - rx task + * Only setup, do not start. + * */ +static void node_setup_tasks(void) +{ + CREATE_NEW_TASK(CalcTask, + NULL, + "calcTask", + 512, + PRIO_CalcTask, + &app.calcTask.Handle); + app.calcTask.Signal = 1; + + /* rxTask is receive the signal from + * passing signal from RX IRQ to an actual two-way ranging algorithm. + * It awaiting of an Rx Signal from RX IRQ ISR and decides what to do next in + * TWR exchange process + * */ + CREATE_NEW_TASK(RxTask, NULL, "rxTask", 512, PRIO_RxTask, &app.rxTask.Handle); + app.rxTask.Signal = 1; + + if ((app.rxTask.Handle == NULL) \ + || (app.calcTask.Handle == NULL)) { + /*(app.imuTask.Handle == NULL))*/ + error_handler(1, _ERR_Create_Task_Bad); + } +} + +/* @brief Terminate all tasks and timers related to Node functionality, if any + * DW3000's RX and IRQ shall be switched off before task termination, + * that IRQ will not produce unexpected Signal + * */ +void node_terminate(void) +{ + TERMINATE_STD_TASK(app.rxTask); + + // TODO: need more elegant way to deallocate the mail queue + osDelay(100 / portTICK_PERIOD_MS); // wait 100ms thus the calcTask should + // receive all mail sent and delete + // them + + TERMINATE_STD_TASK(app.calcTask); + + TERMINATE_STD_TASK(app.imuTask); + + node_process_terminate(); +} + +/* @fn node_helper + * @brief this is a service function which starts the + * TWR Node functionality + * Note: the previous instance of TWR shall be killed + * with node_terminate_tasks(); + * + * Note: the node_process_init() will allocate the memory of + * sizeof(node_info_t) + * from the caller's task stack, see _malloc_r() ! + * + * */ +void node_helper(void const *argument) +{ + (void) argument; + error_e tmp; + + port_disable_dw_irq_and_reset(1); + + initDList(); /**< The List of Discovered Tags during listening of the air + */ + + /* "RTOS-independent" part : initialization of two-way ranging process */ + tmp = node_process_init(); /* allocate NodeInfo */ + + if (tmp != _NO_ERR) { + error_handler(1, tmp); + } + + node_setup_tasks(); /**< "RTOS-based" : setup (not start) all necessary + * tasks for the Node operation. */ + + node_process_start(); /**< IRQ is enabled from MASTER chip and it may + * receive UWB immediately after this point + */ +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/node/task_node/task_node.h b/bluetooth_uwb_dw3000_slotted_twr/src/apps/node/task_node/task_node.h new file mode 100644 index 0000000..ad00cff --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/node/task_node/task_node.h @@ -0,0 +1,31 @@ +/** + * @file node_task.h + * @brief + * + * @author Decawave + * + * @attention Copyright 2017-2020 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef __NODE_TASK__H__ +#define __NODE_TASK__H__ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +extern void signal_to_pc_new_tag_discovered(uint64_t addr64); + +void node_helper(void const *argument); +void node_terminate(void); +void suspend_node_tasks(void); + +#ifdef __cplusplus +} +#endif + +#endif /* __TWR_TASK__H__ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/tag/tag/tag.c b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tag/tag/tag.c new file mode 100644 index 0000000..42ba802 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tag/tag/tag.c @@ -0,0 +1,1283 @@ +/** + * @file tag.c + * @brief Decawave Application Layer + * TWR functions collection + * + * @attention + * + * Copyright 2016-2017 (c) Decawave Ltd, Dublin, Ireland. + * + * All rights reserved. + * + * @author + * + * Decawave + */ + +/* Includes */ +#include "tag.h" + +#include +#include +#include + +#include "util.h" + +#include "deca_device_api.h" + +#include "assert.h" + +#include "deca_dbg.h" +#include "common_n.h" +#include "config.h" + +// ----------------------------------------------------------------------------- +// Definitions + +#ifndef TWR_ENTER_CRITICAL +#define TWR_ENTER_CRITICAL taskENTER_CRITICAL +#endif + +#ifndef TWR_EXIT_CRITICAL +#define TWR_EXIT_CRITICAL taskEXIT_CRITICAL +#endif + +/* Two parameters will be sent to the Tag in the Ranging Config message: + * First, is the ~delay after Poll_Tx, when Tag needs to go to Rx, and the + * second is + * the ~delay between Poll_Tx's and Final_Tx's R-marks. + * Tag hardware shall be capable to meet the requested timings. + * Below defined limits, used to specify Tag's maximum HW capability. + * + * That limits depend on the MCU speed, code optimization, latency, introduced + * by + * the application architecture, especially number of reading/writing from/to + * DW1000 and RTOS latencies. + * Timings can be improved more (decreased) by placing a ranging-dependent + * functionality below RTOS, (so app will not have a delay, introduced by RTOS + * itself). + * However, current realization is not optimized for that. + * */ +#define MIN_RESPONSE_CAPABILITY_US (100) /**< time when Tag can be + * ready to receive the + * Respose after its Poll + */ +#define MIN_POLL_TX_FINAL_TX_CAPABILITY_US (1500) /**< time for tag to + * transmit Poll_TX and + * then Final_TX) + */ + +/* For best PDOA performance the clock offset between PDOA node and the tag + * should be >5ppm (but less than 30ppm). + * Because the initial default value can varry + * Set it to be in the range 5..8ppm + * In case Tag's crystal is very off, it also will be trimmed to stay in the |5 + * .. 6| ppm range, + * as defined below: + * */ +#define TARGET_XTAL_OFFSET_VALUE_PPHM_MIN (500) +#define TARGET_XTAL_OFFSET_VALUE_PPHM_MAX (800) + +/* The typical trimming range of DW3000 shield (with 2pF external caps is ~48ppm + * (-30ppm to +18ppm) over all steps */ +#define AVG_TRIM_PER_PPHM \ + ((XTAL_TRIM_BIT_MASK + 1) / 48.0f / 100) /* Trimming per 1 pphm */ + +// ----------------------------------------------------------------------------- +// TWR structure holds all TWR data +// #define TAG_STATIC_TWRINFO +#ifdef TAG_STATIC_TWRINFO +// static ("safe") implementation +static tag_info_t sTagInfo; +static tag_info_t *psTagInfo = &sTagInfo; + +#else +// dynamic allocation of TwrInfo +static tag_info_t *psTagInfo = NULL; + +#define TAG_MALLOC pvPortMalloc +#define TAG_FREE vPortFree + +#endif + +// ----------------------------------------------------------------------------- + +// ----------------------------------------------------------------------------- +// Local functions prototypes +static void rtcWakeUpTimerEventCallback_tag(void); +static void tag_configure_rtc_wakeup_ns(uint32_t); + +// ----------------------------------------------------------------------------- + +// ----------------------------------------------------------------------------- +// Implementation + +// ----------------------------------------------------------------------------- +// Support section + +/* + * @brief get pointer to the twrInfo structure + * */ +tag_info_t * +getTagInfoPtr(void) +{ +#ifdef TAG_STATIC_TWRINFO + return (&psTagInfo); +#else + return (psTagInfo); +#endif +} + +/* + * @brief ISR level (need to be protected if called from APP level) + * @param twr_info_t *pTwrInfo + * + * @note twr_info_t structure has two members xtaltrim - current trimmed + * value and + * clkOffset_pphmm, these change the DW3000 system clock and shall be + * applied + * when DW3000 is not in active Send/Receive state. + * */ +void trim_tag_proc(tag_info_t *pTwrInfo) +{ + unsigned tmp = abs(pTwrInfo->clkOffset_pphm); + + if ((tmp > TARGET_XTAL_OFFSET_VALUE_PPHM_MAX) + || (tmp < TARGET_XTAL_OFFSET_VALUE_PPHM_MIN)) { + int8_t tmp8 = pTwrInfo->xtaltrim; + tmp8 -= + (int8_t)(((TARGET_XTAL_OFFSET_VALUE_PPHM_MAX + + TARGET_XTAL_OFFSET_VALUE_PPHM_MIN) / 2 + + pTwrInfo->clkOffset_pphm) + * AVG_TRIM_PER_PPHM); /* (TARGET_XTAL_OFFSET_VALUE_PPHM_MAX + * + + * TARGET_XTAL_OFFSET_VALUE_PPHM_MIN)/2 + */ + pTwrInfo->xtaltrim = (uint8_t)(XTAL_TRIM_BIT_MASK & tmp8); + + /* Configure new Crystal Offset value */ + dwt_setxtaltrim(pTwrInfo->xtaltrim); + } +} + +#if (DIAG_READ_SUPPORT == 1) + +/* + * @brief ISR layer + * read full diagnostic data form the received frame from the two DW1000s + * offset 0 / 1 + * */ +static int +read_full_diagnostics(rx_pckt_t_t *prxPckt, + uint32_t status) +{ + uint16_t fpIndex; + diag_v5_t *p = &prxPckt->diagnostics; + + p->header = DWT_DIAGNOSTIC_LOG_REV_5; + + memcpy(p->r0F, (uint8_t *) &status, 4); // copy 4bytes + // of status + // (saved on + // entry to + // ISR) + dwt_readfromdevice(RX_FINFO_ID, 4, 5, (uint8_t *)(p + 5)); // read MSB from + // status and + // 4byte frame + // info + dwt_readfromdevice(RX_FQUAL_ID, 0, 17, (uint8_t *)(p->r12)); // read 17 bytes + // of + // diagnostic + // data from + // 0x12,13,14 + + memcpy((uint8_t *)p->r15, prxPckt->rxTimeStamp, TS_40B_SIZE); // copy TS + dwt_readfromdevice(RX_TIME_ID, RX_TIME_FP_INDEX_OFFSET, 9, + (uint8_t *)(p->r15 + 5)); // 2FP, 2Diag, 5TSraw + + // Calculate the First Path Index ((LDE0 + LDE1 << 8) / 64) + fpIndex = (*((uint8_t *)(p + 32)) >> 6) + (*((uint8_t *)(p + 33)) << 2); + + fpIndex = fpIndex * 4 + 1; // get location in the accumulator + + // printf("%d FP index %02x %02x %i %i\n", offset, *((uint8_t*)(p+32)), + // *((uint8_t*)(p+33)), fpIndex, (fpIndex-1)>>2); + // Read CIR for the First Path + 3 More samples (4*4 = 16) + dwt_readaccdata(p->r25, 17, fpIndex - 1); // read 1 extra as first will be + // dummy byte + dwt_readfromdevice(LDE_IF_ID, LDE_PPINDX_OFFSET, 2, p->r2E); + dwt_readfromdevice(DRX_CONF_ID, 0x28, 4, p->r27); + dwt_readfromdevice(LDE_IF_ID, LDE_PPAMPL_OFFSET, 2, p->r2E2); + + return (int) fpIndex; +} + +#endif + +// ----------------------------------------------------------------------------- + +// ----------------------------------------------------------------------------- +// DW3000 callbacks section : +// if RTOS, the preemption priority of the dwt_isr() shall be such, that +// allows signal to the thread. + +/* @brief ISR layer + * Real-time TWR application Tx callback + * to be called from dwt_isr() + * */ +static void +twr_tx_tag_cb(const dwt_cb_data_t *txd) +{ + (void) txd; + tag_info_t *pTwrInfo = getTagInfoPtr(); + + if (!pTwrInfo) { + return; + } + + uint32_t tmp = rtc_counter_get(); // MCU RTC time : this will be HW + // timestamped + + // Store the Tx Time Stamp of the transmitted packet + switch (pTwrInfo->txState) + { + case Twr_Tx_Blink_Sent: // tag + pTwrInfo->blinkRtcTimeStamp = tmp; + dwt_readtxtimestamp(pTwrInfo->blinkTx_ts); + break; + case Twr_Tx_Poll_Sent: // tag + pTwrInfo->pollRtcTimeStamp = tmp; + dwt_readtxtimestamp(pTwrInfo->pollTx_ts); + break; + case Twr_Tx_Final_Sent: // tag + pTwrInfo->finalRtcTimeStamp = tmp; + dwt_readtxtimestamp(pTwrInfo->finalTx_ts); + trim_tag_proc(pTwrInfo); // Trim Tag's offset + // automatically WRT to the + // PDOA-Node +#if (DW_TAG_NOT_SLEEPING == 0) + app.DwCanSleepInIRQ = DW_CAN_SLEEP; +#endif + break; + default: + break; + } +} + +/* @brief ISR layer + * TWR application Rx callback + * to be called from dwt_isr() as an Rx call-back + * */ +static void +twr_rx_cb(const dwt_cb_data_t *rxd) +{ + (void) rxd; + tag_info_t *pTwrInfo = getTagInfoPtr(); + + if (!pTwrInfo) { + return; + } + + uint16_t head = pTwrInfo->rxPcktBuf.head; + uint16_t tail = pTwrInfo->rxPcktBuf.tail; + uint16_t size = sizeof(pTwrInfo->rxPcktBuf.buf) + / sizeof(pTwrInfo->rxPcktBuf.buf[0]); + + if (CIRC_SPACE(head, tail, size) <= 0) { + return; + } + + rx_pckt_t_t *p = &pTwrInfo->rxPcktBuf.buf[head]; + + p->rtcTimeStamp = rtc_counter_get(); // MCU RTC + // timestamp + + { + dwt_readrxtimestamp(p->timeStamp); // Rx TimeStamp from Ipatov CIR when + // STS OFF or from STS when STS ON + // FIXME: should check if DW3000 STS quality good or not? + } + + p->rxDataLen = MIN(rxd->datalength, sizeof(twr_msg_t)); + + dwt_readrxdata((uint8_t *)&p->msg.stdMsg, p->rxDataLen, 0); // Raw message + +#if (DIAG_READ_SUPPORT == 1) + read_full_diagnostics(p, rxd->status); +#endif + + if (app.rxTask.Handle != NULL) { // RTOS : rxTask can be not started + head = (head + 1) & (size - 1); + pTwrInfo->rxPcktBuf.head = head; // IRQ : do not need to protect + + if (osThreadFlagsSet(app.rxTask.Handle, app.rxTask.Signal) & osFlagsError) { + error_handler(1, _ERR_Signal_Bad); + } + } +} + +/* + * @brief ISR layer + * + * */ +static void +twr_rx_timeout_cb(const dwt_cb_data_t *rxd) +{ + (void) rxd; + tag_info_t *pTwrInfo = getTagInfoPtr(); + +#if (DW_TAG_NOT_SLEEPING == 0) + app.DwCanSleepInIRQ = DW_CAN_SLEEP; +#endif + if (!pTwrInfo) { + return; + } + + if (pTwrInfo->txState == Twr_Tx_Poll_Sent) { + pTwrInfo->faultyRangesCnt++; + } +} + +static void +twr_rx_error_cb(const dwt_cb_data_t *rxd) +{ + twr_rx_timeout_cb(rxd); +} + +/* + * Called on SPI_RDY IRQ by deca_driver + */ +static void +tag_spi_rdy_cb(const dwt_cb_data_t *rxd) +{ + (void) rxd; +#if (DW_TAG_NOT_SLEEPING == 0) + tag_info_t *pTwrInfo = getTagInfoPtr(); + + if (pTwrInfo) { + app.DwSpiReady = DW_SPI_READY; + + if ((app.blinkTask.Handle) && (pTwrInfo->mode == BLINKING_MODE)) { + // signal that device is awake and poll can be sent + if (osThreadFlagsSet(app.blinkTask.Handle, + app.blinkTask.Signal) & osFlagsError) { + error_handler(1, _ERR_Signal_Bad); + } + } else { + if ((app.pollTask.Handle) && (pTwrInfo->mode == RANGING_MODE)) { + // signal that device is awake and poll can be sent + if (osThreadFlagsSet(app.pollTask.Handle, + app.pollTask.Signal) & osFlagsError) { + error_handler(1, _ERR_Signal_Bad); + } + } + } + } +#endif +} + +// ----------------------------------------------------------------------------- + +// ----------------------------------------------------------------------------- + +/* @brief app layer + * RTOS independent application layer function. + * initialising of TWR from scratch. + * This MUST be executed in protected mode. + * + * !!!! It is assumed DW IC is reset prior to calling this function !!!!! + * + * This will setup the process of: + * 1. broadcast blink / wait for Ranging Config response; + * 2. receive setup parameters from Ranging Config; + * 3. if version of Ranging Config is not compatible, keep blinking; + * 4. otherwise setup slot, new panID, framefiltering, address, TWR timings; + * 6. switch off blinking timer and switch on precise WUP timer; + * 5. range to the Node addr from MAC of Ranging Config + * */ +error_e tag_process_init(void) +{ +#ifdef TAG_STATIC_TWRINFO + psTagInfo = &TwrInfo; +#else + + psTagInfo = TAG_MALLOC(sizeof(tag_info_t)); +#endif + + tag_info_t *pTagInfo = getTagInfoPtr(); + + if (!pTagInfo) { + return(_ERR_Cannot_Alloc_Memory); + } + + /* switch off receiver's rxTimeOut, RxAfterTxDelay, delayedRxTime, + * autoRxEnable, dblBufferMode and autoACK, + * clear all initial counters, etc. + * */ + memset(pTagInfo, 0, sizeof(tag_info_t)); + + { // For Trilat demo purposes Fixed tags can supply their locations in the + // payload + pTagInfo->pos.pos_x[0] = app.pConfig->s.fixed_pos_x_mm & 0xFF; + pTagInfo->pos.pos_x[1] = app.pConfig->s.fixed_pos_x_mm >> 8; + + pTagInfo->pos.pos_y[0] = app.pConfig->s.fixed_pos_y_mm & 0xFF; + pTagInfo->pos.pos_y[1] = app.pConfig->s.fixed_pos_y_mm >> 8; + + pTagInfo->pos.pos_z[0] = app.pConfig->s.fixed_pos_z_mm & 0xFF; + pTagInfo->pos.pos_z[1] = app.pConfig->s.fixed_pos_z_mm >> 8; + } + + /* Tag will receive its configuration, such as + * panID, tagAddr, node0Addr and TWR delays: + * pollTx2FinalTxDelay_us and response rx delay from Ranging Config message. + * + * But the reception timeouts calculated based on known length of + * Ranging Config and Response packets. + * */ + + { // pre-calculate all possible two-way ranging frame timings + dwt_config_t *pCfg = &app.pConfig->dwt_config; // dwt_config : holds + // node's UWB mode + + msg_t msg; + + msg.dataRate = pCfg->dataRate; // Deca define: e.g. + // DWT_BR_6M8 + msg.txPreambLength = pCfg->txPreambLength; // Deca define: e.g. + // DWT_PLEN_128 + msg.sfdType = pCfg->sfdType; + msg.txPcode = pCfg->txCode; + msg.stsLength = + (pCfg->stsMode + & DWT_STS_CONFIG_MASK) ? ((1 << (pCfg->stsLength + 2)) * 8) : 0; + + msg.msg_len = sizeof(rng_cfg_msg_t); + calculate_msg_time(&msg, &pTagInfo->msg_time.ranging_config); + + msg.msg_len = sizeof(poll_msg_t); + calculate_msg_time(&msg, &pTagInfo->msg_time.poll); + + msg.msg_len = sizeof(resp_pdoa_msg_t); + calculate_msg_time(&msg, &pTagInfo->msg_time.response); + + msg.msg_len = sizeof(final_msg_accel_t); + calculate_msg_time(&msg, &pTagInfo->msg_time.final); + } + + /* dwt_xx calls in app level Must be in protected mode (DW3000 IRQ disabled) + */ + disable_dw3000_irq(); + + TWR_ENTER_CRITICAL(); + + if (dwt_initialise(DWT_DW_INIT | DWT_READ_OTP_PID | DWT_READ_OTP_LID) + != DWT_SUCCESS) { /**< set callbacks to NULL inside dwt_initialise */ + TWR_EXIT_CRITICAL(); + diag_printf("Tag init failed. Stop.\r\n"); + return (_ERR_INIT); // device initialise has failed + } + + set_dw_spi_fast_rate(); + + uint32_t dev_id = dwt_readdevid(); + + if ((dev_id == (uint32_t)DWT_DW3000_PDOA_DEV_ID) + || (dev_id == (uint32_t)DWT_QM33120_PDOA_DEV_ID)) { + pTagInfo->dw3000ChipType = AOA; + } else if (dev_id == (uint32_t)DWT_DW3000_DEV_ID) { + pTagInfo->dw3000ChipType = NON_AOA; + + app.pConfig->dwt_config.pdoaMode = DWT_PDOA_M0; + } else { + TWR_EXIT_CRITICAL(); + diag_printf("Found unknown chip 0x%08lX. Stop.\r\n", + (unsigned long int)dev_id); + return _ERR_DEVID; + } + + /* Configure receiver's UWB mode, set power and antenna delays for TWR mode */ + rxtx_configure_t p; + p.pdwCfg = &app.pConfig->dwt_config; + p.frameFilter = DWT_FF_DISABLE; // DWT_FF_ENABLE_802_15_4 + p.frameFilterMode = (DWT_FF_DATA_EN | DWT_FF_ACK_EN); // FIXME + p.txAntDelay = app.pConfig->s.antTx_a; + p.rxAntDelay = app.pConfig->s.antRx_a; + p.panId = 0x5555; // PanID : does not matter : DWT_FF_NOTYPE_EN : will + // be reconfigured on reception of RI message + p.shortadd = 0xAAAA; // ShortAddr : does not matter : DWT_FF_NOTYPE_EN : + // will be reconfigured on reception of RI message + + rxtx_configure(&p); + + dwt_setleds(DWT_LEDS_ENABLE + | DWT_LEDS_INIT_BLINK); /**< DEBUG I/O 2&3 : configure + * the GPIOs which control + * the LEDs on HW + */ + dwt_setlnapamode(DWT_TXRX_EN); /**< DEBUG I/O 4&5&6 : + * configure LNA/PA, + * 0&1 TX/RX states to output on GPIOs + */ + + dwt_setcallbacks(twr_tx_tag_cb, + twr_rx_cb, + twr_rx_timeout_cb, + twr_rx_error_cb, + NULL, + tag_spi_rdy_cb, + NULL); + + dwt_setinterrupt(DWT_INT_SPIRDY_BIT_MASK | DWT_INT_TXFRS_BIT_MASK + | DWT_INT_RXFCG_BIT_MASK + | (DWT_INT_ARFE_BIT_MASK | DWT_INT_RXFSL_BIT_MASK + | DWT_INT_RXSTO_BIT_MASK | DWT_INT_RXPHE_BIT_MASK + | DWT_INT_RXFCE_BIT_MASK | DWT_INT_RXFTO_BIT_MASK + + /*| DWT_INT_RXPTO_BIT_MASK*/), + 0, + DWT_ENABLE_INT_ONLY); + + dwt_configuresleep(DWT_CONFIG | DWT_PGFCAL, + DWT_PRES_SLEEP | DWT_WAKE_CSN | DWT_SLP_EN); + + init_dw3000_irq(); /**< manually init EXTI DW3000 lines IRQs */ + + /* configure non-zero initial values */ + + /* TODO: fix "4". + * to be able receive long rng_cfg_upd_msg_t relax Response RX timeout time + */ + pTagInfo->env.responseRxTo_sy = 4 * MAX(pTagInfo->msg_time.response.sy, + pTagInfo->msg_time.ranging_config.sy); + + pTagInfo->seqNum = (uint8_t)(0xff * rand() / RAND_MAX); + + /* below valid after dwt_initialise() : OTP values */ + { + uint32_t lotId = dwt_getlotid(); + uint32_t partId = dwt_getpartid(); + + if ((lotId | partId) == 0) { + if ((app.pConfig->s.partId | app.pConfig->s.lotId) == 0) { + app.pConfig->s.lotId = rand(); + app.pConfig->s.partId = rand(); + save_bssConfig(app.pConfig); + } + + lotId = app.pConfig->s.lotId; + partId = app.pConfig->s.partId; + } + pTagInfo->eui64 = ((uint64_t)lotId << 32) | partId; + } + + /* + * The dwt_initialize will read the default XTAL TRIM from the OTP or use the + * DEFAULT_XTAL_TRIM. + * In this case we would apply the user-configured value. + * + * Bit 0x80 can be used to overwrite the OTP settings if any. + * */ + if ((dwt_getxtaltrim() == DEFAULT_XTAL_TRIM) + || (app.pConfig->s.xtalTrim & ~XTAL_TRIM_BIT_MASK)) { + dwt_setxtaltrim(app.pConfig->s.xtalTrim & XTAL_TRIM_BIT_MASK); + pTagInfo->xtaltrim = dwt_getxtaltrim(); + } + + rtc_disable_irq(); + + pTagInfo->mode = BLINKING_MODE; + + juniper_configure_hal_rtc_callback(rtcWakeUpTimerEventCallback_tag); + + tag_configure_rtc_wakeup_ns(10 * 1e6); // first blink will start in 10ms + // after starting of the Tag + // application + + app.DwCanSleepInIRQ = DW_CANNOT_SLEEP; +#if (DW_TAG_NOT_SLEEPING == 1) + app.DwEnterSleep = DW_NOT_SLEEPING; // +#else + app.DwEnterSleep = DW_IS_SLEEPING_FIRST; + dwt_entersleep(DWT_DW_IDLE_RC); + app.DwSpiReady = DW_SPI_SLEEPING; +#endif + + TWR_EXIT_CRITICAL(); + + return (_NO_ERR); +} + +/* + * */ +void tag_process_start(void) +{ + enable_dw3000_irq(); /**< enable DW3000 IRQ to start */ + + // start the RTC timer + rtc_enable_irq(); + + diag_printf("Tag Top Application: Started\r\n"); +} + +/* @brief app level + * RTOS-independent application level function. + * deinitialize the pTwrInfo structure. + * This must be executed in protected mode. + * + * */ +void tag_process_terminate(void) +{ + TWR_ENTER_CRITICAL(); + + // stop the RTC timer + rtc_disable_irq(); + juniper_configure_hal_rtc_callback(NULL); + +#ifndef TAG_STATIC_TWRINFO + if (psTagInfo) { + TAG_FREE(psTagInfo); + psTagInfo = NULL; + } +#endif + + TWR_EXIT_CRITICAL(); +} + +// ----------------------------------------------------------------------------- + +// ----------------------------------------------------------------------------- + +/* @brief + * TWR : DISCOVERY PHASE + * Tag sends Blinks, waiting for a Ranging Config message + * + * application layer function + */ +error_e initiator_send_blink(tag_info_t *p) +{ + error_e ret; + tx_pckt_t txPckt; + + memset(&txPckt, 0, sizeof(txPckt)); + + blink_msg_t *pTxMsg = &txPckt.msg.blinkMsg; + + // TWR : PHASE : Initiator Sends Blink to Responder + txPckt.psduLen = sizeof(blink_msg_t); + txPckt.delayedTxTimeH_sy = 0; + txPckt.txFlag = (DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED); + + // Ranging Config: activate receiver this time SY after Blink Tx + txPckt.delayedRxTime_sy = + (uint32_t)util_us_to_sy(app.pConfig->s.rcDelay_us); + + // Ranging Config: receiver will be active for this time, SY + txPckt.delayedRxTimeout_sy = + (uint32_t)util_us_to_sy(app.pConfig->s.rcRxTo_us); + + pTxMsg->frameCtrl[0] = Head_Msg_BLINK; + pTxMsg->seqNum = p->seqNum; + memcpy(&pTxMsg->tagID, &p->eui64, sizeof(pTxMsg->tagID)); + + p->seqNum++; + p->txState = Twr_Tx_Blink_Sent; + + TWR_ENTER_CRITICAL(); + + ret = tx_start(&txPckt); + + TWR_EXIT_CRITICAL(); + + if (ret != _NO_ERR) { + p->lateTX++; + } + + return (ret); +} + +/* @brief + * TWR: RANGING PHASE + * Initiator sends Poll to the Responder + * + * application layer function + */ +error_e initiator_send_poll(tag_info_t *p) +{ + error_e ret; + tx_pckt_t txPckt; + + poll_msg_t *pTxMsg = &txPckt.msg.pollMsg; + + /* Construct TxPckt packet: Send Poll immediate and configure delayed wait for + * the Response */ + txPckt.psduLen = sizeof(poll_msg_t); + txPckt.delayedTxTimeH_sy = 0; + txPckt.txFlag = (DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED); + + txPckt.delayedRxTime_sy = p->env.delayRx_sy; // environment, + // received from + // Ranging Config + // message + txPckt.delayedRxTimeout_sy = p->env.responseRxTo_sy; // this is + // calculated + // locally based + // on UWB + // configuration + + /* Construct TX Final UWB message */ + + /* See IEEE frame header description */ + pTxMsg->mac.frameCtrl[0] = Head_Msg_STD; + pTxMsg->mac.frameCtrl[1] = Frame_Ctrl_SS; + pTxMsg->mac.panID[0] = p->env.panID & 0xff; + pTxMsg->mac.panID[1] = (p->env.panID >> 8) & 0xff; + pTxMsg->mac.destAddr[0] = p->env.nodeAddr[0]; + pTxMsg->mac.destAddr[1] = p->env.nodeAddr[1]; + pTxMsg->mac.sourceAddr[0] = p->env.tagAddr[0]; + pTxMsg->mac.sourceAddr[1] = p->env.tagAddr[1]; + pTxMsg->mac.seqNum = p->seqNum; + + /* Data */ + pTxMsg->poll.fCode = Twr_Fcode_Tag_Poll; + pTxMsg->poll.rNum = p->rangeNum; + + /* Transmit over the air */ + p->txState = Twr_Tx_Poll_Sent; + p->seqNum++; + p->rangeNum++; + + POLL_ENTER_CRITICAL(); + + ret = tx_start(&txPckt); + + POLL_EXIT_CRITICAL(); + + if (ret != _NO_ERR) { + p->lateTX++; + } + + return (ret); +} + +/* @brief Part of twr_initiator_algorithm_tx. + * this function is Interrupt level, should be executed with + * Initiator wakes DW3000 chip to send Blink or Poll message. + * + * */ +error_e tag_wakeup_dw3000_blink_poll(tag_info_t *p) +{ + error_e ret; + + p->stationary = p->stationary_imu; + + app.DwCanSleepInIRQ = DW_CANNOT_SLEEP; +#if (DW_TAG_NOT_SLEEPING == 0) + if (app.DwEnterSleep != DW_NOT_SLEEPING) { + ret = port_wakeup_dw3000_fast(); // should never return anything other + // than _NO_ERR + } else { + diag_printf("Tag NOT sleeping, skip wakeup\r\n"); + ret = _NO_ERR; + } +#else + + dwt_forcetrxoff(); // Tag may be in the RX state + + ret = _NO_ERR; +#endif + return ret; +} + +// ----------------------------------------------------------------------------- + +// ----------------------------------------------------------------------------- +// The most real-time section of TWR algorithm + +/* @brief app layer + * Real-time TWR algorithm implementation (Initiator) + * + * prefer to be called from application UWB Rx, + * but can be called from ISR, i.e. twr_rx_cb() directly. + * if called from ISR layer, then revise/remove + * TWR_ENTER_CRITICAL() + * TWR_EXIT_CRITICAL() + * + * @return error_e + * */ +error_e twr_initiator_algorithm_rx(rx_pckt_t_t *pRxPckt, tag_info_t *pTwrInfo) +{ + error_e ret = _ERR_Not_Twr_Frame; + fcode_e fcode = Twr_Fcode_Not_Defined; + + std_msg_t *pMsg = &pRxPckt->msg.stdMsg; + + if ((pMsg->mac.frameCtrl[0] == Head_Msg_STD) + || (pMsg->mac.frameCtrl[0] == Head_Msg_STD_AR)) { + /* Only SS and LS MAC headers supported in current application */ + switch (pMsg->mac.frameCtrl[1] & Frame_Ctrl_MASK) + { + case Frame_Ctrl_SS: + if ((pRxPckt->rxDataLen == sizeof(resp_pdoa_msg_t)) /* Response (with + * Coordinates + * extension) + */ + || (pRxPckt->rxDataLen == sizeof(rng_cfg_upd_msg_t))) { /* Ranging + * Config + * (update) + */ + fcode = ((std_msg_ss_t *)pMsg)->messageData[0]; + } + break; + case Frame_Ctrl_LS: + if (pRxPckt->rxDataLen == sizeof(rng_cfg_msg_t)) { /* Ranging + * Config + * (discovery) + */ + fcode = ((std_msg_ls_t *)pMsg)->messageData[0]; + } + break; + default: + fcode = Twr_Fcode_Not_Defined; + break; + } + } + + switch (fcode) + { + case Twr_Fcode_Rng_Config: + /* Initiator received Range Init message from a Responder in discovery + * process. + * 1. setup Timing parameters in accordance to Range Init message; + * 2. configure MCU RTC WKUP timer to expire on next slot time + * */ + ret = initiator_received_ranging_config(pRxPckt, pTwrInfo); + + if (ret != _NO_Err_Ranging_Update) { + ret = _NO_Err_Can_Sleep; // all good and wrong RX except Update + // would lead to dwt_entersleep() + } + break; + + case Twr_Fcode_Resp_Ext: + /* Initiator received a Response message. + * If the message from our PDOA node: + * 1. Send delayed Final_TX; + * 2. Adjust the Wakeup timer for next Poll. + * */ + if ((pTwrInfo->env.nodeAddr[0] + == ((resp_pdoa_msg_t *)pMsg)->mac.sourceAddr[0]) + || (pTwrInfo->env.nodeAddr[1] + == ((resp_pdoa_msg_t *)pMsg)->mac.sourceAddr[1])) { + // diag_printf("Twr_Fcode_Resp_Ext\r\n"); + // (1) + ret = initiator_received_response(pRxPckt, pTwrInfo); + + if (ret == _NO_ERR) { + ret = _NO_Err_Response; + } + + // (2) + TWR_ENTER_CRITICAL(); + + int32_t slotCorr_ns; + uint32_t nextWakeUpPeriod_ns, rtcNow, tmpNow; + + // casting received bytes to the int, + // this is a signed correction wrt (gRtcSFrameZeroCnt + expected_slot) + // coming from the Node + pTwrInfo->env.slotCorr_ns = 1000 + * (int32_t)AR2U32( + ((resp_pdoa_msg_t *)pMsg)->resp.slotCorr_us); + + slotCorr_ns = pTwrInfo->env.slotCorr_ns; + + // (A)start of calculation + rtcNow = rtc_counter_get(); // MCU RTC time + // : this will + // be HW + // timestamped + + tmpNow = (rtcNow > pTwrInfo->gRtcSFrameZeroCnt) + ?(rtcNow - pTwrInfo->gRtcSFrameZeroCnt) + :(rtcNow - pTwrInfo->gRtcSFrameZeroCnt + RTC_WKUP_CNT_OVFLW); + + // Next RTC_Wakeup of Tag will be aligned to exact slot expected by the + // Node's: + nextWakeUpPeriod_ns = pTwrInfo->env.sframePeriod_ns + - (WKUP_RESOLUTION_NS * tmpNow) + - (slotCorr_ns); + + if (nextWakeUpPeriod_ns < (pTwrInfo->env.sframePeriod_ns >> 1)) { + nextWakeUpPeriod_ns += (pTwrInfo->env.sframePeriod_ns); + } + tag_configure_rtc_wakeup_ns(nextWakeUpPeriod_ns); + + TWR_EXIT_CRITICAL(); + } + break; + + default: + /* Initiator received unknown data / reset initiator + * */ + ret = _ERR_Not_Twr_Frame; + break; + } + + return (ret); +} + +/* @brief check on-the air configuration that it capable + * to work on the current hardware + * */ +error_e check_air_config_correct(uint8_t ver, + uint16_t delayRx_sy, + uint16_t pollTxToFinalTx_us) +{ + error_e ret = _NO_ERR; + + if (ver != RC_VERSION_PDOA) {// This Tag knows only one version of Ranging + // Config format: RC_VERSION_PDOA + ret = _ERR_RC_Version_Unknown; + } else if ((delayRx_sy < MIN_RESPONSE_CAPABILITY_US) \ + || (pollTxToFinalTx_us < MIN_POLL_TX_FINAL_TX_CAPABILITY_US)) { + /* This Tag hardware is too slow + * and cannot range with parameters, supplied by the Node. + */ + ret = _ERR_Non_Compatible_TWR_Parameters; + } + + return ret; +} + +/* @brief Part of twr_initiator_algorithm_rx + * + * Initiator in discovery phase received RANGING CONFIG message from a + * Responder. + * 1. Check that Tag understand and can comply to requested Ranging Config + * parameters; + * 2. Setup Range Times as defined in the Ranging Config message; + * 3. Switch off the Blink Timer. + * 4. Configure & Start the Poll Timer. + * + * @parm *rx_packet, *control structure + * + * @return error codes: + * _ERR_Range_Config + * _ERR_RC_Version_Unknown + * _ERR_Non_Compatible_TWR_Parameters + * _NO_Err_Ranging_Config + * _NO_Err_Ranging_Update + * */ +error_e initiator_received_ranging_config(rx_pckt_t_t *pRxPckt, + tag_info_t *pTwrInfo) +{ + error_e ret = _ERR_Ranging_Config; + + rng_cfg_t *pRxMsg; + + uint8_t units_ms = 0; + + if (pRxPckt->rxDataLen == sizeof(rng_cfg_msg_t)) { + /* obtain Node address and PanID from the MAC of Ranging Config */ + pTwrInfo->env.panID = AR2U16(pRxPckt->msg.rngCfgMsg.mac.panID); + pTwrInfo->env.nodeAddr[0] = pRxPckt->msg.rngCfgMsg.mac.sourceAddr[0]; + pTwrInfo->env.nodeAddr[1] = pRxPckt->msg.rngCfgMsg.mac.sourceAddr[1]; + + pRxMsg = &pRxPckt->msg.rngCfgMsg.rngCfg; + } else if (pRxPckt->rxDataLen == sizeof(rng_cfg_upd_msg_t)) { + /* update */ + pRxMsg = &pRxPckt->msg.rngCfgUpdMsg.rngCfg; + } else { + return (ret); + } + + /*check units*/ + if ((pRxMsg->version == RC_VERSION_DR) + && (pRxPckt->msg.rngCfgMsg.rngCfg.ANC_RESP_DLY[1] & 0x80)) { + // units are ms + units_ms = 1; + ret = _NO_ERR; + } else { + ret = check_air_config_correct(pRxMsg->version, + AR2U16(pRxMsg->delayRx_us), + AR2U16(pRxMsg->pollTxToFinalTx_us)); + } + + if (ret == _NO_ERR) { + ret = + (pRxPckt->rxDataLen + == sizeof(rng_cfg_msg_t))?(_NO_Err_Ranging_Config):( + _NO_Err_Ranging_Update); + + TWR_ENTER_CRITICAL(); + + rtc_disable_irq(); + + /* configure environment parameters from Ranging Config message */ + pTwrInfo->env.version = pRxMsg->version; + + pTwrInfo->env.tagAddr[0] = pRxMsg->tagAddr[0]; + pTwrInfo->env.tagAddr[1] = pRxMsg->tagAddr[1]; + + pTwrInfo->env.sframePeriod_ns = + 1000000 * (uint32_t)(AR2U16(pRxMsg->sframePeriod_ms)); + + if (units_ms == 1) { + uint32_t ancResp = + (AR2U16(pRxPckt->msg.rngCfgMsg.rngCfg.ANC_RESP_DLY) & 0x7FFF); + uint32_t tagResp = + (AR2U16(pRxPckt->msg.rngCfgMsg.rngCfg.TAG_RESP_DLY) & 0x7FFF); + + pTwrInfo->env.pollTx2FinalTxDelay64 = + util_us_to_dev_time((ancResp + tagResp) * 1000); /* Time from + * RMARKER of Poll to + * RMARKER of Final. + */ + // pTwrInfo->env.delayRx_sy = (uint32_t)( + // (((uint32_t)AR2U16(pRxMsg->delayRx_us))<<16) + + // AR2U16(pRxMsg->pollTxToFinalTx_us) ); //RX turn on delay + // following Poll transmission + pTwrInfo->env.delayRx_sy = + (uint32_t)util_us_to_sy(((AR2U16(pRxPckt->msg.rngCfgMsg.rngCfg. + ANC_RESP_DLY) & 0x7FFF)) * 1000 + - pTwrInfo->msg_time.poll.us); /* RX turn on + * delay + * following + * Poll + * transmission + */ + } else { + pTwrInfo->env.pollTx2FinalTxDelay64 = + util_us_to_dev_time(AR2U16(pRxMsg->pollTxToFinalTx_us)); /* Time + * from RMARKER + * of Poll to + * RMARKER + * of Final. + */ + pTwrInfo->env.delayRx_sy = + (uint32_t)util_us_to_sy(AR2U16(pRxMsg->delayRx_us)); /* Time after + * Poll Tx + * completed + * to activate + * the RX + */ + } + + pTwrInfo->env.pollMultFast = AR2U16(pRxMsg->pollMultFast); + pTwrInfo->env.pollMultSlow = AR2U16(pRxMsg->pollMultSlow); + pTwrInfo->env.mode = AR2U16(pRxMsg->mode); + pTwrInfo->env.slotCorr_ns = + 1000 * (int32_t)AR2U32(pRxMsg->slotCorr_us); /* next wakeup correction + * to fit the slot + */ + + rxtx_configure_t p; + p.pdwCfg = &app.pConfig->dwt_config; + p.frameFilter = DWT_FF_DISABLE; ////DWT_FF_ENABLE_802_15_4 + p.frameFilterMode = (DWT_FF_DATA_EN | DWT_FF_ACK_EN); // FIXME + p.txAntDelay = app.pConfig->s.antTx_a; + p.rxAntDelay = app.pConfig->s.antRx_a; + p.panId = pTwrInfo->env.panID; + p.shortadd = AR2U16(pTwrInfo->env.tagAddr); + + /* Setup configured panID, FrameFiltering and antenna delays */ + tn_app_config(&p); + + /* Setup New High resolution RTC Wakup Timer : */ + uint32_t tmp, rtcNow, tmpNow; + + rtcNow = rtc_counter_get(); // MCU RTC time : this will be HW + // timestamped + + pTwrInfo->mode = RANGING_MODE; + + // STM32: RTC is counting down + tmpNow = (rtcNow > pTwrInfo->gRtcSFrameZeroCnt) + ?(rtcNow - pTwrInfo->gRtcSFrameZeroCnt) + :(rtcNow - pTwrInfo->gRtcSFrameZeroCnt + RTC_WKUP_CNT_OVFLW); + + // Next RTC_Wakeup of Tag will be aligned to exact slot expected by the + // Node's: + tmp = pTwrInfo->env.sframePeriod_ns + - (WKUP_RESOLUTION_NS * tmpNow) + - pTwrInfo->env.slotCorr_ns; + + if (tmp < (pTwrInfo->env.sframePeriod_ns >> 1)) { + tmp += (pTwrInfo->env.sframePeriod_ns); + } + + tag_configure_rtc_wakeup_ns(tmp); // update the RTC Wakup timer to + // start Ranging in assigned slot + + rtc_enable_irq(); + + TWR_EXIT_CRITICAL(); + } else { + } + + return (ret); +} + +/* @brief Part of twr_initiator_algorithm_rx. + * + * Initiator received the RESPONSE message. + * 1. setup and send delayed Final_TX according to control structure + * 2. if a use case : setup the delayed receive for reception of REPORT TOF + * + * @parm *rx_packet, *control structure + * + * @return delayed TxPool error code + * */ +error_e initiator_received_response(rx_pckt_t_t *prxPckt, tag_info_t *p) +{ + error_e ret; + uint64_t calcFinalTx64; + + tx_pckt_t TxPckt; /**< allocate space for Tx Packet */ + + final_msg_accel_t *pTxMsg = &TxPckt.msg.finalMsg; + + /* Construct TxPckt packet: no reception after Final transmission */ + TxPckt.psduLen = sizeof(final_msg_accel_t); + TxPckt.txFlag = (DWT_START_TX_DELAYED); + TxPckt.delayedRxTime_sy = 0; + TxPckt.delayedRxTimeout_sy = 0; + + /* Calculate timings for transmission of Final Packet */ + // Load Poll Tx time + TS2U64_MEMCPY(calcFinalTx64, p->pollTx_ts); + + // Add delay from Ranging Config message: PollTx2FinalTx + calcFinalTx64 = MASK_TXDTS + & ((uint64_t)(calcFinalTx64 + p->env.pollTx2FinalTxDelay64)); + + // DW1000 will adjust the transmission of Final TxPacket SFD exactly at + // PollTx+PollTx2FinalTx+AntTxDelay + TxPckt.delayedTxTimeH_sy = calcFinalTx64 >> 8; + + // Calculate Time Final message will be sent and embed this number to the + // Final message data. + // Sending time will be delayedReplyTime, snapped to ~125MHz or ~250MHz + // boundary by + // zeroing its low 9 bits, and then having the TX antenna delay added. + // Getting antenna delay from the config and add it to the Calculated TX Time + calcFinalTx64 = MASK_40BIT + & (uint64_t)(calcFinalTx64 + app.pConfig->s.antTx_a); + + /* Construct TX Final UWB message */ + + /* See IEEE frame header description */ + pTxMsg->mac.frameCtrl[0] = Head_Msg_STD; + pTxMsg->mac.frameCtrl[1] = Frame_Ctrl_SS; + pTxMsg->mac.panID[0] = p->env.panID & 0xff; + pTxMsg->mac.panID[1] = (p->env.panID >> 8) & 0xff; + pTxMsg->mac.destAddr[0] = p->env.nodeAddr[0]; + pTxMsg->mac.destAddr[1] = p->env.nodeAddr[1]; + pTxMsg->mac.sourceAddr[0] = p->env.tagAddr[0]; + pTxMsg->mac.sourceAddr[1] = p->env.tagAddr[1]; + pTxMsg->mac.seqNum = p->seqNum; + + /* Data */ + pTxMsg->final.fCode = (uint8_t)Twr_Fcode_Tag_Accel_Final; + pTxMsg->final.rNum = (uint8_t)p->rangeNum; + TS2TS_UWB_MEMCPY(pTxMsg->final.pollTx_ts, + p->pollTx_ts); // Embed Poll Tx time to the Final message + TS2TS_UWB_MEMCPY(pTxMsg->final.responseRx_ts, + prxPckt->timeStamp); /* Embed Response Rx time + * to the Final message + */ + U642TS_UWB_MEMCPY(pTxMsg->final.finalTx_ts, + calcFinalTx64); /* Embed Calculated Final TX time + * to the Final message + */ + pTxMsg->final.flag = p->stationary; + pTxMsg->final.acc_x[0] = p->acc.acc_x[0]; + pTxMsg->final.acc_x[1] = p->acc.acc_x[1]; + pTxMsg->final.acc_y[0] = p->acc.acc_y[0]; + pTxMsg->final.acc_y[1] = p->acc.acc_y[1]; + pTxMsg->final.acc_z[0] = p->acc.acc_z[0]; + pTxMsg->final.acc_z[1] = p->acc.acc_z[1]; + + /* Transmit over the air */ + p->txState = Twr_Tx_Final_Sent; // indicate to TX ISR that the response + // has been sent + p->seqNum++; + + TWR_ENTER_CRITICAL(); + + ret = tx_start(&TxPckt); + + TWR_EXIT_CRITICAL(); + + if (ret != _NO_ERR) { + p->lateTX++; + } + + return (ret); +} + +// ----------------------------------------------------------------------------- + +// ----------------------------------------------------------------------------- + +/* + * @brief setup RTC Wakeup timer + * period_ns is awaiting time in ns + * */ +void tag_configure_rtc_wakeup_ns(uint32_t period_ns) +{ + rtc_configure_wakeup_ns(period_ns); +} + +/* RTC Wakeup timer callback. + * Use the LED_TAG_Pin to see synchronization of tags + * on the oscilloscope if needed + * */ +void rtcWakeUpTimerEventCallback_tag(void) +{ + static int count = 0; + + tag_info_t *pTwrInfo = getTagInfoPtr(); + + if (pTwrInfo) { // RTOS : pTwrInfo can be not allocated yet + pTwrInfo->gRtcSFrameZeroCnt = rtc_counter_get(); + + if (pTwrInfo->mode == BLINKING_MODE) {// BLINKING phase + if (app.blinkTask.Handle) { // RTOS : blinkTask can be not started yet + if (osThreadFlagsSet(app.blinkTask.Handle, + app.blinkTask.Signal) & osFlagsError) { + error_handler(1, _ERR_Signal_Bad); + } + } + + uint32_t new_blink_period_ms = + (uint32_t)((float)((BLINK_PERIOD_MS / 3.0) * rand() / RAND_MAX)); + new_blink_period_ms += BLINK_PERIOD_MS; + rtc_configure_wakeup_ms(new_blink_period_ms); + } else {// RANGING phase + rtc_configure_wakeup_ns(pTwrInfo->env.sframePeriod_ns); + + if (pTwrInfo->stationary && !pTwrInfo->stationary_imu) { + pTwrInfo->stationary = false; + count = pTwrInfo->env.pollMultFast; + } + + count++; + + if ((pTwrInfo->stationary && (count >= pTwrInfo->env.pollMultSlow)) + || (!pTwrInfo->stationary && (count >= pTwrInfo->env.pollMultFast))) { + if (app.pollTask.Handle) { // RTOS : pollTask can be not started + // yet + if (osThreadFlagsSet(app.pollTask.Handle, + app.pollTask.Signal) & osFlagsError) { + error_handler(1, _ERR_Signal_Bad); + } + } + count = 0; + } + } + } +} + +// ----------------------------------------------------------------------------- diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/tag/tag/tag.h b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tag/tag/tag.h new file mode 100644 index 0000000..12756f3 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tag/tag/tag.h @@ -0,0 +1,247 @@ +/** + * @file tag.h + * + * @brief tag bare implementation + * + * @attention + * + * Copyright 2016-2017 (c) Decawave Ltd, Dublin, Ireland. + * + * All rights reserved. + * + * @author + */ + +#ifndef __TAG__H__ +#define __TAG__H__ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#include "uwb_frames.h" +#include "msg_time.h" + +#include "port.h" +#include "port_common.h" + +#define DW_TAG_NOT_SLEEPING (0) + +#define BLINK_PERIOD_MS (500) /* range init phase - Blink send + * period, ms */ + +#define DWT_DIAGNOSTIC_LOG_REV_5 (5) + +/* Rx Events circular buffer. + * 0x02, 0x04, 0x08, 0x10, etc. + * The size of the buffer at any given time should be < 2 */ +#define EVENT_BUF_TAG_SIZE (0x02) + +#define POLL_ENTER_CRITICAL() vPortEnterCritical() +#define POLL_EXIT_CRITICAL() vPortExitCritical() +// ----------------------------------------------------------------------------- +// Struct & Typedefs + +/* RxPckt */ +struct rx_pckt_t_s +{ + int16_t rxDataLen; + + union { + std_msg_t stdMsg; + std_msg_ss_t ssMsg; + std_msg_ls_t lsMsg; + twr_msg_t twrMsg; + blink_msg_t blinkMsg; + rng_cfg_msg_t rngCfgMsg; + rng_cfg_upd_msg_t rngCfgUpdMsg; + resp_pdoa_msg_t respExtMsg; + } msg; + + uint8_t timeStamp[TS_40B_SIZE]; /* Full TimeStamp */ + uint32_t rtcTimeStamp; /* MCU RTC timestamp */ + uint16_t firstPath; /* First path (raw 10.6) */ + int16_t clock_offset; + +#if (DIAG_READ_SUPPORT == 1) + diag_v5_t diagnostics; /* 66 bytes*/ +#endif +}; + +typedef struct rx_pckt_t_s rx_pckt_t_t; + +/* This structure holds application parameters: + * eui64 + * txAntennaDelay + * rxAntennaDelay + * timestamps for every phase's IRQ: + * initiator: blinkTx_ts, pollTx_ts, respRX_ts, finalTx_ts, + * (reportRx_ts) + * responder: blinkRx_ts, pollRx_ts, respTx_ts, finalRx_ts, + * (reportTx_ts) + * + * */ +struct tag_info_s +{ + /* Unique long Address, used at the discovery phase before Range Init + * reception */ + union { + uint8_t euiLong[8]; + uint64_t eui64; + }; + + /* circular Buffer of received Rx packets : + * uses in transferring of the data from ISR to APP level. + * */ + struct { + rx_pckt_t_t buf[EVENT_BUF_TAG_SIZE]; + uint16_t head; + uint16_t tail; + } rxPcktBuf; + + /* ranging variables */ + struct { + /* MAC sequence number, increases on every tx_start */ + uint8_t seqNum; + + /* Discovery phase : Tx time structures for DW_TX_IRQ callback */ + struct { + uint8_t blinkTx_ts[TS_40B_SIZE]; /**< tag: blinkTx_ts, + * blinkRtcTimeStamp */ + uint32_t blinkRtcTimeStamp; /**< handles the MCU RTC time at + * the DW_IRQ */ + }; + + /* Ranging phase : Tx time structures for DW_TX_IRQ callback */ + struct { + uint8_t pollTx_ts[TS_40B_SIZE]; /**< tag: pollTx_ts, + * pollRtcTimeStamp */ + uint32_t pollRtcTimeStamp; /**< handles the MCU RTC time at + * the DW_IRQ */ + + uint8_t finalTx_ts[TS_40B_SIZE]; /**< tag: finalTx_ts, + * finalRtcTimeStamp */ + uint32_t finalRtcTimeStamp; /**< handles the MCU RTC time at + * the DW_IRQ */ + }; + + /* Application DW_TX_IRQ source indicator */ + tx_states_e txState; + }; + + /* pre-calculated times for different messages */ + struct { + msg_time_t ranging_config; + msg_time_t poll; + msg_time_t response; + msg_time_t final; + } msg_time; + + uint32_t gRtcSFrameZeroCnt; // Local SuperFrame start, + // Timestamp + + /* Environment - configured from Range init structure. + * slotCorr_us is used to adjust slot every reception as part of Response + */ + struct env + { + uint8_t version; + + mac_header_ss_t twr_mac_header; + + uint16_t panID; + uint8_t tagAddr[ADDR_BYTE_SIZE_S]; + uint8_t nodeAddr[ADDR_BYTE_SIZE_S]; + + uint32_t sframePeriod_ns; // Superframe Period, ns + uint64_t pollTx2FinalTxDelay64; // This is delay used in TWR between + // Poll and Final sending: from + // Ranging Config message + + uint16_t responseRxTo_sy; // pre-calculated Rx timeout for Response + // Msg + uint32_t delayRx_sy; // Rx timeout from Ranging Config for + // Response Msg to the Node + + int32_t slotCorr_ns; // Slot correction from current reception, + // ns + + uint16_t pollMultFast; // multiplier for fast ranging in + // Superframe durations + uint16_t pollMultSlow; // multiplier for slow ranging in + // Superframe durations + + union { + uint16_t mode; // additional service: IMU on/off, etc.TBD + bool imuOn : 1; // tag shall use IMU to slow down its + // ranging + }; + } env; + + union { + struct acc { + uint8_t acc_x[2]; + uint8_t acc_y[2]; + uint8_t acc_z[2]; + } acc; + + struct pos { + uint8_t pos_x[2]; + uint8_t pos_y[2]; + uint8_t pos_z[2]; + } pos; + }; + + bool stationary_imu : 1; // IMU report that the Tag is stationary + bool stationary : 1; // IMU report that the Tag is stationary + + /* The number of range sequence, increases on every poll */ + uint16_t rangeNum; + + /* Tag's crystal clock offset trimming */ + int16_t clkOffset_pphm; // + uint8_t xtaltrim; // Tag crystal trim value + + volatile + uint16_t faultyRangesCnt; + + uint16_t lateTX; // used for Debug to count any lateTX + + enum + { + BLINKING_MODE, + RANGING_MODE + } mode; + + dw3000type_e dw3000ChipType; +}; + +typedef struct tag_info_s tag_info_t; + +// ----------------------------------------------------------------------------- +// exported functions prototypes + +/* initiator (tag) */ +tag_info_t * getTagInfoPtr(void); + +error_e tag_process_init(void); +void tag_process_start(void); +void tag_process_terminate(void); + +error_e twr_initiator_algorithm_rx(rx_pckt_t_t *prxPckt, tag_info_t *ptwrInfo); + +error_e tag_wakeup_dw3000_blink_poll(tag_info_t *ptwrInfo); +error_e initiator_send_blink(tag_info_t *ptwrInfo); +error_e initiator_send_poll(tag_info_t *ptwrInfo); +error_e initiator_received_ranging_config(rx_pckt_t_t *prxPckt, + tag_info_t *ptwrInfo); +error_e initiator_received_response(rx_pckt_t_t *prxPckt, tag_info_t *ptwrInfo); + +void twr_configure_rtc_wakeup_ns(uint32_t ns); +void trim_tag_proc(tag_info_t *pTwrInfo); + +#ifdef __cplusplus +} +#endif + +#endif /* __TAG__H__ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/tag/task_tag/task_tag.c b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tag/task_tag/task_tag.c new file mode 100644 index 0000000..b29c316 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tag/task_tag/task_tag.c @@ -0,0 +1,349 @@ +/* + * @file task_tag.c + * @brief Decawave Application Layer + * RTOS tag implementation + * + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#include "task_tag.h" +#include "util.h" +#include "app.h" +#include "port_common.h" +#include "tag.h" + +// ----------------------------------------------------------------------------- + +#define BLINK_PERIOD_MS (500) /* range init phase - Blink send + * period, ms */ + +// ----------------------------------------------------------------------------- + +/* + * @brief + * The thread is initiating the transmission of the blink + * on reception of app.blinkTask.Signal + * + * */ +static void +BlinkTask(void const *arg) +{ + tag_info_t *p; + + do{ + osDelay(100); + }while (!(p = getTagInfoPtr())); // wait for initialisation of psTagInfo + + do { + osMutexRelease(app.blinkTask.MutexId); + + osThreadFlagsWait(app.blinkTask.Signal, osFlagsWaitAny, osWaitForever); + + osMutexAcquire(app.blinkTask.MutexId, 0); // we do not want the task + // can be deleted in the + // middle of operation + + if (app.DwSpiReady == DW_SPI_SLEEPING) { + taskENTER_CRITICAL(); + tag_wakeup_dw3000_blink_poll(p); + taskEXIT_CRITICAL(); + } else { + taskENTER_CRITICAL(); + dwt_restoreconfig(); // restore configuration which has not been + // saved in AON + app.DwEnterSleep = DW_NOT_SLEEPING; + taskEXIT_CRITICAL(); + + initiator_send_blink(p); + } + }while (1); + + UNUSED(arg); +} + +/* + * @brief + * The thread is initiating the TWR sequence + * on reception of .signal.twrTxPoll + * + * */ +static void +TagPollTask(void const *arg) +{ + tag_info_t *p = getTagInfoPtr(); + + do{ + osDelay(100); + }while (!(p = getTagInfoPtr())); // wait for initialisation of pTwrInfo + + do { + osMutexRelease(app.pollTask.MutexId); + + osThreadFlagsWait(app.pollTask.Signal, osFlagsWaitAny, osWaitForever); + + osMutexAcquire(app.pollTask.MutexId, 0); + + if (app.DwSpiReady == DW_SPI_SLEEPING) { + taskENTER_CRITICAL(); + tag_wakeup_dw3000_blink_poll(p); + taskEXIT_CRITICAL(); + } else { + taskENTER_CRITICAL(); + dwt_restoreconfig(); // restore configuration which has not been + // saved in AON + app.DwEnterSleep = DW_NOT_SLEEPING; + taskEXIT_CRITICAL(); + + if (p->faultyRangesCnt < app.pConfig->s.faultyRanges) { + initiator_send_poll(p); + } else { + rtc_disable_irq(); + + /* This will restart Tag task completely from discovery phase */ + xEventGroupSetBits(app.xStartTaskEvent, Ev_Tag_Task); + } + } + }while (1); + + UNUSED(arg); +} + +/* @brief DW3000 RX : RTOS implementation + * + * */ +static void +TagRxTask(void const *arg) +{ + error_e ret; + uint16_t head, tail; + + tag_info_t *ptwrInfo; + + do{ + osDelay(10); + }while (!(ptwrInfo = getTagInfoPtr())); // wait for initialisation of + // pTwrInfo + + int size = sizeof(ptwrInfo->rxPcktBuf.buf) + / sizeof(ptwrInfo->rxPcktBuf.buf[0]); + + do { + osMutexRelease(app.rxTask.MutexId); + + osThreadFlagsWait(app.rxTask.Signal, osFlagsWaitAny, osWaitForever); + + osMutexAcquire(app.rxTask.MutexId, 0); + + taskENTER_CRITICAL(); + head = ptwrInfo->rxPcktBuf.head; + tail = ptwrInfo->rxPcktBuf.tail; + taskEXIT_CRITICAL(); + + /* We are using circular buffer + Signal to safely deliver packets from ISR + * to APP */ + if (CIRC_CNT(head, tail, size) > 0) { + rx_pckt_t_t *prxPckt = &ptwrInfo->rxPcktBuf.buf[tail]; + twr_res_ext_t p; + + ret = twr_initiator_algorithm_rx(prxPckt, + ptwrInfo); /**< Run bare + * twr_initiator_algorithm + */ + + switch (ret) + { + case _NO_Err_Response: + ptwrInfo->faultyRangesCnt = 0; + p.addr = AR2U16(ptwrInfo->env.tagAddr); + p.node_addr = AR2U16(ptwrInfo->env.nodeAddr); + p.rNum = prxPckt->msg.respExtMsg.resp.rNum; + p.x_cm = (int16_t)AR2U16(prxPckt->msg.respExtMsg.resp.x_cm); + p.y_cm = (int16_t)AR2U16(prxPckt->msg.respExtMsg.resp.y_cm); + p.clkOffset_pphm = (int16_t)AR2U16( + prxPckt->msg.respExtMsg.resp.clkOffset_pphm); /* Crystal Clock + * offset value + * reported back + * from the Node + */ + + { // XTAL trimming will be performed after sending of Final. + /* Instead of using a clkOffset_pphm from Response, which calculated + * by Node based on distances measurements, + * the more precise and direct method of adjusting clock offset + * using + * carrier integrator counter value will be used.*/ + float co_ppm; + co_ppm = dwt_readclockoffset(); + + /* save the offset value for future apply after Final message been + * sent */ + ptwrInfo->clkOffset_pphm = + (int)(co_ppm * (CLOCK_OFFSET_PPM_TO_RATIO * 1e6 * 100)); + } + break; + case _ERR_DelayedTX_Late: + case _ERR_Not_Twr_Frame: + case _NO_Err_Can_Sleep: +#if (DW_TAG_NOT_SLEEPING == 0) + app.DwCanSleepInIRQ = DW_CAN_SLEEP_APP; +#endif + break; + default: + break; + } + +#if (DW_TAG_NOT_SLEEPING == 0) + if ((app.DwCanSleepInIRQ == DW_CAN_SLEEP_APP) + && (app.DwEnterSleep != DW_IS_SLEEPING_IRQ)) { + taskENTER_CRITICAL(); + app.DwEnterSleep = DW_IS_SLEEPING_RX; + dwt_entersleep(DWT_DW_IDLE_RC); // manual sleeping + app.DwSpiReady = DW_SPI_SLEEPING; + taskEXIT_CRITICAL(); + } +#endif + + taskENTER_CRITICAL(); + tail = (tail + 1) & (size - 1); + ptwrInfo->rxPcktBuf.tail = tail; + taskEXIT_CRITICAL(); + + /* ready to serve next raw reception */ + + /* Report previous range/coordinates/offset back to UART/USB */ + if ((app.pConfig->s.reportLevel) && (ret == _NO_Err_Response)) { +// send_to_pc_tag_location(&p); //TODO: tag can report the +// distance to the PC from the previous location + // TODO: also tag can calculate the location from SSTWR + } + + UNUSED(p); + } + + osThreadYield(); + }while (1); + + UNUSED(arg); +} + +/* @brief Setup TWR tasks and timers for discovery phase. + * - blinking timer + * - blinking task + * - twr polling task + * - rx task + * Only setup, do not start. + * */ +static void tag_setup_tasks(void) +{ +// The RTC Timer is set as a part of tag_process_init() +// The RTC timer will produce Signals to activate either BlinkTask or +// TagPollTask +// That will help with sending of first Poll on the correct slot. +// + const osMutexAttr_t thread_mutex_attr = { .attr_bits = osMutexPrioInherit }; + + /* Blinking thread for discovery phase of the Anchor until reception of Range + * Init */ + CREATE_NEW_TASK(BlinkTask, + NULL, + "blinkTask", + 256, + PRIO_BlinkTask, + &app.blinkTask.Handle); + app.blinkTask.MutexId = osMutexNew(&thread_mutex_attr); + app.blinkTask.Signal = 1; + + /* This will be the main poll thread for the Tag after discovery completed + * Do not reduce the stack size for this thread. + * */ + CREATE_NEW_TASK(TagPollTask, + NULL, + "pollTask", + 256, + PRIO_TagPollTask, + &app.pollTask.Handle); + app.pollTask.MutexId = osMutexNew(&thread_mutex_attr); + app.pollTask.Signal = 1; + + /* rxThread is passing signal from RX IRQ to an actual two-way ranging + * algorithm. + * It awaiting of Rx Signal from RX IRQ ISR and decides what to do next in TWR + * exchange process. + * Do not reduce the stack size for this thread. + * */ + CREATE_NEW_TASK(TagRxTask, + NULL, + "rxTask", + 384, + PRIO_TagRxTask, + &app.rxTask.Handle); + app.rxTask.MutexId = osMutexNew(&thread_mutex_attr); + app.rxTask.Signal = 1; + + if ((app.blinkTask.Handle == NULL) \ + || (app.pollTask.Handle == NULL) \ + || (app.rxTask.Handle == NULL)) { + error_handler(1, _ERR_Create_Task_Bad); + } +} + +// ----------------------------------------------------------------------------- + +/* @brief + * Kill all task and timers related to tag/TWR if any + * DW3000's RX and IRQ shall be switched off before task termination, + * that IRQ will not produce unexpected Signal + * */ +void tag_terminate(void) +{ + rtc_disable_irq(); + + TERMINATE_STD_TASK(app.blinkTask); + + TERMINATE_STD_TASK(app.imuTask); + + TERMINATE_STD_TASK(app.rxTask); + + TERMINATE_STD_TASK(app.pollTask); + + tag_process_terminate(); // de-allocate Tag RAM Resources +} + +/* @fn tag_helper + * @brief this is a service function which starts the Tag + * top-level application. + * Note: If the dynamic memory allocation is used, then the + * tag_process_init() will allocate the memory of sizeof(tag_info_t) + * from the caller's task stack, see _malloc_r() ! + * */ +void tag_helper(void const *argument) +{ + (void) argument; + error_e tmp; + + port_disable_dw_irq_and_reset(1); + + taskENTER_CRITICAL(); /**< When the app will setup RTOS tasks, then if + * task has a higher priority, + * the kernel will start it immediately, thus + * we need to stop the scheduler.*/ + set_dw_spi_fast_rate(); + + tag_setup_tasks(); /**< "RTOS-based" : setup all RTOS tasks. */ + + /* "RTOS-independent" part : initialisation of two-way ranging process */ + tmp = tag_process_init(); + + if (tmp != _NO_ERR) { + error_handler(1, tmp); + } + + tag_process_start(); + + taskEXIT_CRITICAL(); /**< all RTOS tasks can be scheduled */ +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/tag/task_tag/task_tag.h b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tag/task_tag/task_tag.h new file mode 100644 index 0000000..ddf9b3f --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tag/task_tag/task_tag.h @@ -0,0 +1,42 @@ +/*!---------------------------------------------------------------------------- + * @file header for task_tag.h + * + * @brief Decawave Application Layer + * RTOS tag implementation + * + * @attention + * + * Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * + * All rights reserved. + * + * @author + * + * Decawave + */ + +#ifndef __TAG_TASK__H__ +#define __TAG_TASK__H__ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#include +typedef struct { + uint16_t addr; + uint16_t node_addr; + uint8_t rNum; + int16_t x_cm; + int16_t y_cm; + int16_t clkOffset_pphm; +}twr_res_ext_t; + +void tag_helper(void const *argument); +void tag_terminate(void); + +#ifdef __cplusplus +} +#endif + +#endif /* __TWR_TASK__H__ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcfm/task_tcfm/task_tcfm.c b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcfm/task_tcfm/task_tcfm.c new file mode 100644 index 0000000..e375475 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcfm/task_tcfm/task_tcfm.c @@ -0,0 +1,108 @@ +/* + * @file task_tcfm.c + * + * @brief Task for an extended TCFM application + * + * @author Decawave + * + * @attention Copyright 2017-2020 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#include "task_tcfm.h" + +#include "port.h" +#include "port_common.h" +#include "tcfm.h" +#include "deca_dbg.h" + +#if (DEBUG) +#define FTCFM_PRINTF diag_printf +#else +#define FTCFM_PRINTF(...) {} +#endif + +// ----------------------------------------------------------------------------- + +/* + * @brief TCFMTask + * + * */ +static void +tcfmTask(void const *argument) +{ + (void) argument; + const osMutexAttr_t thread_mutex_attr = { .attr_bits = osMutexPrioInherit }; + app.tcfmTask.MutexId = osMutexNew(&thread_mutex_attr); + + FTCFM_PRINTF("tcfmTask: irq enabled %d %d %d\n", + app.tcfm_info.nframes, + app.tcfm_info.period_ms, + app.tcfm_info.bytes); + + taskENTER_CRITICAL(); + + enable_dw3000_irq(); /**< IRQ is enabled and we can receive TX IRQ + * immediately after this point + */ + + dwt_starttx(DWT_START_TX_IMMEDIATE); /**< First frame is sent immediately + */ + + taskEXIT_CRITICAL(); + + while (1) + { + osThreadFlagsWait(app.tcfmTask.Signal, osFlagsWaitAny, osWaitForever); + osMutexAcquire(app.tcfmTask.MutexId, 0); + + tcfm_process_run(); + + osMutexRelease(app.tcfmTask.MutexId); + osThreadYield(); + } +} + +/* @brief Terminate all tcfmTask related functionality, if any. + * DW1000's RX and IRQ shall be switched off before task termination, + * that IRQ will not produce unexpected Signal + * */ +void +tcfm_terminate(void) +{ + tcfm_process_terminate(); + + TERMINATE_STD_TASK(app.tcfmTask); +} + +/* @fn tcfm_helper + * @brief this is a service function which starts the + * TX/TCFM applicaiton + * @param argument is a pointer to the tcfm_info_t structure + * + * */ +void tcfm_helper(void const *argument) +{ + (void) argument; + error_e ret; + port_disable_dw_irq_and_reset(1); + + ret = tcfm_process_init((tcfm_info_t *)&app.tcfm_info); + + if (ret != _NO_ERR) { + error_handler(1, ret); + } + + set_dw_spi_fast_rate(); + + /* "RTOS-based" : setup (not start) all necessary tasks for the power test + * operation. */ + CREATE_NEW_TASK(tcfmTask, + NULL, + "TCFMf", + configMINIMAL_STACK_SIZE * 4, + PRIO_TcfmTask, + &app.tcfmTask.Handle); + app.tcfmTask.Signal = 1; +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcfm/task_tcfm/task_tcfm.h b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcfm/task_tcfm/task_tcfm.h new file mode 100644 index 0000000..a2ffa90 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcfm/task_tcfm/task_tcfm.h @@ -0,0 +1,27 @@ +/** + * @file task_tcfm.h + * + * @brief Header file for task_tcfm.c + * + * @author Decawave + * + * @attention Copyright 2017-2020 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef TASK_TCFM_H_ +#define TASK_TCFM_H_ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +void tcfm_helper(void const *arg); +void tcfm_terminate(void); + +#ifdef __cplusplus +} +#endif + +#endif /* TASK_TCFM_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcfm/tcfm/tcfm.c b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcfm/tcfm/tcfm.c new file mode 100644 index 0000000..9765fea --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcfm/tcfm/tcfm.c @@ -0,0 +1,199 @@ +/** + * @file tcfm.c + * @brief process to run Test Continuous Frame Mode + * + * This test application will send a number of packets (e.g. 200) and + * then stop + * The payload and the inter-packet period can also be varied + * command: "TCFM N D P", where N is number of packets to TX, D is + * inter packet period (in ms), P is payload in bytes + * + * + * measure the power: + * Spectrum Analyser set: + * FREQ to be channel default e.g. 6489.6 MHz for channel 5, 7987.2 MHz for + * channel 9 + * SPAN to 1GHz + * SWEEP TIME 1s + * RBW and VBW 1MHz + * measure channel power + * measure peak power + * + * + * @author Decawave + * + * @attention + * Copyright 2017-2020 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * */ +#include +#include +#include "tcfm.h" + +#include "msg_time.h" +#include "port.h" +#include "port_common.h" +#include "uwb_frames.h" + +#include "deca_device_api.h" + +#define DW_MS_PERIOD 249600 /* 1 ms in 4ns units to program into DX_TIME_ID + */ + +struct tcfm_app_s +{ + uint8_t payload[127]; + uint16_t msg_len; + uint16_t msg_count; + uint16_t nframes; + uint8_t fixed_sts; +}; + +static struct tcfm_app_s *pTcfmMsg = NULL; + +/* IMPLEMENTATION */ + +/* @brief ISR level + * TCFM application TX callback + * to be called from dwt_isr() as an TX call-back + * */ +void tcfm_tx_cb(const dwt_cb_data_t *rxd) +{ + (void) rxd; + if (!pTcfmMsg) { + return; + } + + if (pTcfmMsg->msg_count >= pTcfmMsg->nframes) { + // we have transmitted required number of messages - stop the application + + // FreeRTOS specific implementation of how-to set the Event from the ISR + BaseType_t xHigherPriorityTaskWoken, xResult; + + xHigherPriorityTaskWoken = pdFALSE; + + xResult = xEventGroupSetBitsFromISR(app.xStartTaskEvent, Ev_Stop_All, + &xHigherPriorityTaskWoken); + + // Was the message posted successfully? + if (xResult == pdPASS) { + // If xHigherPriorityTaskWoken is now set to pdTRUE then a context + // switch should be requested. The macro used is port specific and + // will be either portYIELD_FROM_ISR() or portEND_SWITCHING_ISR() - + // refer to the documentation page for the port being used. + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + } + } else { + pTcfmMsg->msg_count++; + + pTcfmMsg->payload[1] = pTcfmMsg->msg_count; // packet counter 16-bit + pTcfmMsg->payload[2] = pTcfmMsg->msg_count >> 8; + + dwt_writetxdata(pTcfmMsg->msg_len, (uint8_t *)pTcfmMsg->payload, 0); + + dwt_writetxfctrl(pTcfmMsg->msg_len, 0, 0); + + // the configured delay in between TX packets was set in the init + if (pTcfmMsg->fixed_sts) { + // re-load the initial cp_iv value to keep STS the same for each frame + dwt_configurestsloadiv(); + } + dwt_starttx(DWT_START_TX_DLY_TS); + } +} + +/* + * @brief init function initialises all run-time environment allocated by + * the process + * it will be executed once + * + * */ +error_e tcfm_process_init(tcfm_info_t *info) +{ + error_e ret = _NO_ERR; + + pTcfmMsg = malloc(sizeof(struct tcfm_app_s)); + + if (!pTcfmMsg) { + return _ERR_Cannot_Alloc_Memory; + } + + // define some test data for the tx buffer + const uint8_t msg_data[] = "The quick brown fox jumps over the lazy dog"; + + // configure device settings based on the settings stored in app.pConfig + // it will not return if the init will fail + tcXm_configure_test_mode(); + + dwt_setcallbacks(tcfm_tx_cb, NULL, NULL, NULL, NULL, NULL, NULL); + + dwt_setinterrupt(DWT_INT_TXFRS_BIT_MASK, 0, DWT_ENABLE_INT_ONLY); + + init_dw3000_irq(); /**< manually init EXTI DW3000 lines IRQs */ + + pTcfmMsg->fixed_sts = app.pConfig->s.stsStatic; // value 0 = dynamic STS, 1 + // = fixed STS + + // configure STS KEY/IV + dwt_configurestskey(&app.pConfig->s.stsKey); + dwt_configurestsiv(&app.pConfig->s.stsIv); + // load the configured KEY/IV values + dwt_configurestsloadiv(); + + /* Setup Tx packet*/ + pTcfmMsg->msg_len = (uint16_t) info->bytes; // overall message length + pTcfmMsg->msg_count = 1; + pTcfmMsg->nframes = (uint16_t) info->nframes; + + memcpy(pTcfmMsg->payload, msg_data, info->bytes); + + /* + * The dwt_initialize will read the default XTAL TRIM from the OTP or use the + * DEFAULT_XTAL_TRIM. + * In this case we would apply the user-configured value. + * + * Bit 0x80 can be used to overwrite the OTP settings if any. + * */ + if ((dwt_getxtaltrim() == DEFAULT_XTAL_TRIM) + || (app.pConfig->s.xtalTrim & ~XTAL_TRIM_BIT_MASK)) { + dwt_setxtaltrim(app.pConfig->s.xtalTrim & XTAL_TRIM_BIT_MASK); + } + + if (info->bytes == 5) { // Special interop case + pTcfmMsg->payload[0] = 0x10; // + pTcfmMsg->payload[1] = pTcfmMsg->msg_count; // packet counter 16-bit + pTcfmMsg->payload[2] = pTcfmMsg->msg_count >> 8; + } + + dwt_writetxdata(pTcfmMsg->msg_len, (uint8_t *)pTcfmMsg->payload, 0); + + dwt_writetxfctrl(pTcfmMsg->msg_len, 0, 0); + + // If the length of the packet is > period_ms, the packets will be sent + // back-to-back + dwt_setdelayedtrxtime(DW_MS_PERIOD * info->period_ms); + + return ret; +} + +/* + * @brief run function implements continuous process functionality + * */ +void tcfm_process_run(void) +{ + /*do nothing*/ +} + +/* + * @brief stop function implements stop functionality if any + * which will be executed on reception of Stop command + * */ +void tcfm_process_terminate(void) +{ + port_stop_all_UWB(); + + if (pTcfmMsg) { + free(pTcfmMsg); + pTcfmMsg = NULL; + } +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcfm/tcfm/tcfm.h b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcfm/tcfm/tcfm.h new file mode 100644 index 0000000..9d762d9 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcfm/tcfm/tcfm.h @@ -0,0 +1,33 @@ +/** + * @file tcfm.h + * + * @brief Header file for TCFM test + * + * @author Decawave + * + * @attention Copyright 2017-2020 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef TCFM_H_ +#define TCFM_H_ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#include "error.h" +#include "app.h" + +error_e tcfm_process_init(tcfm_info_t *info); +void tcfm_process_run(void); +void tcfm_process_terminate(void); + +extern void tcXm_configure_test_mode(void); + +#ifdef __cplusplus +} +#endif + +#endif /* TCFM_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcwm/task_tcwm/task_tcwm.c b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcwm/task_tcwm/task_tcwm.c new file mode 100644 index 0000000..7a15cb6 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcwm/task_tcwm/task_tcwm.c @@ -0,0 +1,95 @@ +/* + * @file task_tcwm.c + * + * @brief task for continuous wave test + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#include "task_tcwm.h" + +#include "port.h" +#include "port_common.h" +#include "usb_uart_rx.h" +#include "tcwm.h" +#include "error.h" +#include "deca_dbg.h" + +#define TCWM_TASK_DUMMY_TMR_MS (1000) + +// ----------------------------------------------------------------------------- + +/* @fn TcwmTask + * @brief this starts the Continuous Wave test functionality. + * + * Note: Previous tasks which can call shared resources must be + * killed. + * + * */ +void TcwmTask(void const *argument) +{ + (void) argument; + const osMutexAttr_t thread_mutex_attr = { .attr_bits = osMutexPrioInherit }; + app.tcwmTask.MutexId = osMutexNew(&thread_mutex_attr); + + diag_printf("TCWMTask start\r\n"); + + while (1) + { + osMutexRelease(app.tcwmTask.MutexId); + + osDelay(TCWM_TASK_DUMMY_TMR_MS / portTICK_PERIOD_MS); + + osMutexAcquire(app.tcwmTask.MutexId, 0); + + tcwm_process_run(); + } +} + +/* @brief + * Kill all tasks and timers related to TcwmTask if any + * + * DW1000's RX and IRQ shall be switched off before task termination, + * that IRQ will not produce unexpected Signal + * + * */ +void tcwm_terminate(void) +{ + tcwm_process_terminate(); + + TERMINATE_STD_TASK(app.tcwmTask); +} + +/* @fn tcwm_helper + * @brief this is a service function which starts the + * Continuous Wave Test functionality + * Note: Previous tasks which can access shared resources must be + * killed. + * + * */ +void tcwm_helper(void const *arg) +{ + port_disable_dw_irq_and_reset(1); + + set_dw_spi_fast_rate(); + + CREATE_NEW_TASK(TcwmTask, + NULL, + "tcwmTask", + 128, + PRIO_TcwmTask, + &app.tcwmTask.Handle); + app.tcwmTask.Signal = 1; + + if (!app.tcwmTask.Handle) { + error_handler(1, _ERR_Create_Task_Bad); + } + + tcwm_process_init(); + + UNUSED(arg); +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcwm/task_tcwm/task_tcwm.h b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcwm/task_tcwm/task_tcwm.h new file mode 100644 index 0000000..c5ede31 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcwm/task_tcwm/task_tcwm.h @@ -0,0 +1,27 @@ +/** + * @file task_tcwm.h + * + * @brief Header file for task_tcwm.c + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef TASK_TCWM_H_ +#define TASK_TCWM_H_ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +void tcwm_helper(void const *arg); +void tcwm_terminate(void); + +#ifdef __cplusplus +} +#endif + +#endif /* TASK_TCWM_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcwm/tcwm/tcwm.c b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcwm/tcwm/tcwm.c new file mode 100644 index 0000000..98ebe53 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcwm/tcwm/tcwm.c @@ -0,0 +1,103 @@ +/** + * @file tcwm.c + * + * @brief Process to test continuous wave mode + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + */ + +#include "tcwm.h" + +#include "app.h" +#include "port.h" +#include "deca_device_api.h" + +/* IMPLEMETATION */ + +/* + * @brief init function initialises all run-time environment allocated by + * the process + * it will be executed once + * */ +void tcwm_process_init(void) +{ + tcXm_configure_test_mode(); + + dwt_configcwmode(); +} + +/* + * @brief run function implements continuous process functionality + * */ +void tcwm_process_run(void) +{ + /*do nothing*/ +} + +/* + * @brief stop function implements stop functionality if any + * which will be executed on reception of Stop command + * */ +void tcwm_process_terminate(void) +{ + port_stop_all_UWB(); +} + +/* + * @brief configure channel parameters and tx spectrum parameters + * */ +void tcXm_configure_test_mode(void) +{ + int result; + + result = dwt_initialise(DWT_DW_INIT); + + if (DWT_SUCCESS != result) { + error_handler(1, _ERR_INIT); + } + + dwt_setleds(DWT_LEDS_ENABLE | DWT_LEDS_INIT_BLINK); /* For debug - to see + * TX LED light up + * when in this mode + */ + dwt_setlnapamode(DWT_PA_ENABLE); /* For debug - to see + * TX state output + * and be able to + * measure the packet + * length + */ + + if (dwt_configure(&app.pConfig->dwt_config)) { /**< Configure the Physical + * Channel parameters + * (PLEN, PRF, etc) + */ + error_handler(1, _ERR_INIT); + } + + /* configure power */ + dwt_configuretxrf(&app.pConfig->s.txConfig); + + /* + * The dwt_initialize will read the default XTAL TRIM from the OTP or use the + * DEFAULT_XTAL_TRIM. + * In this case we would apply the user-configured value. + * + * The bit 0x80 can be used to overwrite the OTP settings if any. + * */ + if ((dwt_getxtaltrim() == DEFAULT_XTAL_TRIM) + || (app.pConfig->s.xtalTrim & ~XTAL_TRIM_BIT_MASK)) { + dwt_setxtaltrim(app.pConfig->s.xtalTrim & XTAL_TRIM_BIT_MASK); + } + + /* set antenna delays */ + dwt_setrxantennadelay(app.pConfig->s.antRx_a); + dwt_settxantennadelay(app.pConfig->s.antTx_a); + + dwt_setrxaftertxdelay(0); /**< no any delays set by default : part of + * config of receiver on Tx sending */ + dwt_setrxtimeout(0); /**< no any delays set by default : part of + * config of receiver on Tx sending */ +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcwm/tcwm/tcwm.h b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcwm/tcwm/tcwm.h new file mode 100644 index 0000000..5f8e328 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/tcwm/tcwm/tcwm.h @@ -0,0 +1,30 @@ +/** + * @file tcwm.h + * + * @brief Header file for prototype of continuous wave test + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef __TCWM_H_ +#define __TCWM_H_ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +void tcwm_process_init(void); +void tcwm_process_run(void); +void tcwm_process_terminate(void); + +void tcXm_configure_test_mode(void); + +#ifdef __cplusplus +} +#endif + +#endif /* SRC_INC_TCWM_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/trilat/dwm_le/dwm-math.c b/bluetooth_uwb_dw3000_slotted_twr/src/apps/trilat/dwm_le/dwm-math.c new file mode 100644 index 0000000..faec889 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/trilat/dwm_le/dwm-math.c @@ -0,0 +1,80 @@ +/** + * LEAPS - Low Energy Accurate Positioning System. + * + * Mathematic utilities. + * + * Copyright (c) 2016-2018, LEAPS. All rights reserved. + * + */ + +#include "dwm-math.h" + +/* Described in header file */ +unsigned long fact(int n) +{ + unsigned long rv = 1; + int i; + + for (i = 1; i <= n; i++) { + rv *= i; + } + + return rv; +} + +/* Described in header file */ +float flog2(float val) +{ + ieee_float_shape_t gf_u = { val }; + float tmp = gf_u.word; + + tmp *= 1.0 / (1 << 23); + + return tmp - 126.94269504f; +} + +/* Described in header file */ +float flog10(float val) +{ + return 0.30102999566f * flog2(val); +} + +static float fsqrt(float x) +{ + ieee_float_shape_t t; + + t.value = x; + + t.word = 0x5f3759df - (t.word >> 1); + t.value = t.value * (1.5f - (0.5f * x * t.value * t.value)); + + return (1 / t.value); +} + +/* Described in header file */ +double get_dist(const vec3d_t *p0, const vec3d_t *p1) +{ + return fsqrt(POW(p0->x - p1->x) + POW(p0->y - p1->y) + POW(p0->z - p1->z)); +} + +/* Described in header file */ +double get_dist_xyz(const double x0, const double y0, const double z0, + const double x1, const double y1, const double z1) +{ + return fsqrt((float)(POW(x0 - x1) + POW(y0 - y1) + POW(z0 - z1))); +} + +/* Described in header file */ +int poly_contains_point(int n, float *x, float *y, float tx, float ty) +{ + int i, j, c = 0; + + for (i = 0, j = n - 1; i < n; j = i++) { + if (((y[i] > ty) != (y[j] > ty)) + && (tx < (x[j] - x[i]) * (ty - y[i]) / (y[j] - y[i]) + x[i])) { + c = !c; + } + } + + return c; +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/trilat/dwm_le/dwm-math.h b/bluetooth_uwb_dw3000_slotted_twr/src/apps/trilat/dwm_le/dwm-math.h new file mode 100644 index 0000000..0da107c --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/trilat/dwm_le/dwm-math.h @@ -0,0 +1,134 @@ +/** + * LEAPS - Low Energy Accurate Positioning System. + * + * Mathematic utilities. + * + * Copyright (c) 2016-2018, LEAPS. All rights reserved. + * + */ + +#ifndef _DWM_MATH_H_ +#define _DWM_MATH_H_ + +#include +#include + +#define POW(x) ((x) * (x)) +#ifndef MIN +#define MIN(a, b) (((a) < (b)) ? (a) : (b)) +#endif +#ifndef MAX +#define MAX(a, b) (((a) > (b)) ? (a) : (b)) +#endif +#ifndef ABS +#define ABS(x) (((x) < 0) ? (-(x)) : (x)) +#endif + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif +#define DEG2RAD(a) ((a) * 0.01745329252) /* /180.0*M_PI */ +#define RAD2DEG(a) ((a) * 57.2957795131) /* 180.0/M_PI */ + +#define NANF 0x7ff80000 + +/* Millimeters to/from Centimeters conversion */ +#define MM_TO_M(mm) ((mm) / 1000) +#define M_TO_MM(cm) ((cm) * 1000) + +#define SPEED_OF_LIGHT (299702547.0) /* in m/s in air */ + +#define MOVING_AVERAGE(o, n, k) (((k) * (o) + (n)) / ((k) + 1)) + +typedef union +{ + float value; + uint32_t word; /* Assuming 32 bit int. */ +} ieee_float_shape_t; + +/* Get a 32 bit int from a float. */ +#define GET_FLOAT_WORD(i, d) \ + do { \ + ieee_float_shape_t gf_u; \ + gf_u.value = (d); \ + (i) = gf_u.word; \ + } while (0); + +/* Set a float from a 32 bit int. */ +#define SET_FLOAT_WORD(d, i) \ + do { \ + ieee_float_shape_t sf_u; \ + sf_u.word = (i); \ + (d) = sf_u.value; \ + } while (0); + +struct vec3d { + double x; + double y; + double z; +}; + +typedef struct vec3d vec3d_t; + +/** + * Calculate factorial of number n + * + * @param[in] n Number to calculate + * @return Returns factorial value + */ +unsigned long fact(int n); + +/** + * Calculate fast log2 + * + * @param[in] n Value + * @return Returns log2 of value + */ +float flog2(float val); + +/** + * Calculate fast log10 + * + * @param[in] n Value + * @return Returns log10 of value + */ +float flog10(float val); + +/** + * Calculate distance between two 3D points + * + * @param[in] p0 Pointer to point 1 + * @param[in] p1 Pointer to point 2 + * @return Returns distance between the points + */ +double get_dist(const vec3d_t *p0, const vec3d_t *p1); + +/** + * Calculate distance between two 3D points given by XYZ + * + * @param[in] x0 X value of point 1 + * @param[in] y0 Y value of point 1 + * @param[in] z0 Z value of point 1 + * @param[in] x1 X value of point 2 + * @param[in] y1 Y value of point 2 + * @param[in] z1 Z value of point 2 + * @return Returns distance between the points + */ +double get_dist_xyz(const double x0, const double y0, const double z0, + const double x1, const double y1, const double z1); + +/** + * Returns true if given point is inside the polygon defined by an array of + * points + * + * @param[in] n Number of points + * @param[in] x Array containing X coordinates + * @param[in] y Array containing Y coordinates + * @param[in] tx X coordinate of test point + * @param[in] ty Y coordinate of test point + * @return Returns true if given point is inside the polygon defined by an + * array of points + */ +int poly_contains_point(int n, float *x, float *y, float tx, float ty); + +#endif /* _DWM_MATH_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/trilat/dwm_le/le-trilat.c b/bluetooth_uwb_dw3000_slotted_twr/src/apps/trilat/dwm_le/le-trilat.c new file mode 100644 index 0000000..3777198 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/trilat/dwm_le/le-trilat.c @@ -0,0 +1,1473 @@ +/** + * LEAPS - Low Energy Accurate Positioning System. + * + * Location Engine - Trilateration. + * Based on codes from Decawave with improvements by LEAPS. + * + * Copyright (c) 2016-2018, Decawave Ltd, Dublin, Ireland. All rights reserved. + * Copyright (c) 2016-2018, LEAPS. All rights reserved. + * + */ + +#if defined(CFG_LE_TRILAT) && (CFG_LE_TRILAT == 1) + +#if defined(CFG_LE_TRILAT_DEBUG) && (CFG_LE_TRILAT_DEBUG == 1) +#include +#endif /* CFG_LE_TRILAT_DEBUG */ +#include +#include +#include + +#include "le-trilat.h" + +#if defined(CFG_LE_TRILAT_UTILS) && (CFG_LE_TRILAT_UTILS == 1) +static trilat_solver_t *_trilat_solver = NULL; +#endif /* CFG_LE_TRILAT_UTILS */ + +/* Largest nonnegative number still considered zero */ +#define MAXZERO 0.001 + +#define ERR_TRIL_CONCENTRIC -1 +#define ERR_TRIL_COLINEAR_2SOLUTIONS -2 +#define ERR_TRIL_SQRTNEGNUMB -3 +#define ERR_TRIL_NOINTERSECTION_SPHERE4 -4 +#define ERR_TRIL_NEEDMORESPHERE -5 + +#define CM_ERR_ADDED (10) + +#define CFG_LEGACY 1 + +#include "port.h" +#include "port_common.h" +#include "deca_dbg.h" +#define TRILAT_MALLOC pvPortMalloc +#define TRILAT_FREE vPortFree + +/* Return the difference of two vectors, (vector1 - vector2). */ +static vec3d_t vdiff(const vec3d_t vector1, const vec3d_t vector2) +{ + vec3d_t v; + + v.x = vector1.x - vector2.x; + v.y = vector1.y - vector2.y; + v.z = vector1.z - vector2.z; + + return v; +} + +/* Return the sum of two vectors. */ +static vec3d_t vsum(const vec3d_t vector1, const vec3d_t vector2) +{ + vec3d_t v; + + v.x = vector1.x + vector2.x; + v.y = vector1.y + vector2.y; + v.z = vector1.z + vector2.z; + + return v; +} + +/* Multiply vector by a number. */ +static vec3d_t vmul(const vec3d_t vector, const double n) +{ + vec3d_t v; + + v.x = vector.x * n; + v.y = vector.y * n; + v.z = vector.z * n; + + return v; +} + +/* Divide vector by a number. */ +static vec3d_t vdiv(const vec3d_t vector, const double n) +{ + vec3d_t v; + + v.x = vector.x / n; + v.y = vector.y / n; + v.z = vector.z / n; + + return v; +} + +/* Return the Euclidean norm. */ +#if 0 +static double vdist(const vec3d_t v1, const vec3d_t v2) +{ + double xd = v1.x - v2.x; + double yd = v1.y - v2.y; + double zd = v1.z - v2.z; + + return sqrt(xd * xd + yd * yd + zd * zd); +} + +#endif + +/* Return the Euclidean norm. */ +static double vnorm(const vec3d_t vector) +{ + return sqrt(vector.x * vector.x + vector.y * vector.y + vector.z * vector.z); +} + +/* Return the dot product of two vectors. */ +static double dot(const vec3d_t vector1, const vec3d_t vector2) +{ + return vector1.x * vector2.x + vector1.y * vector2.y + vector1.z * vector2.z; +} + +/* Replace vector with its cross product with another vector. */ +static vec3d_t cross(const vec3d_t vector1, const vec3d_t vector2) +{ + vec3d_t v; + + v.x = vector1.y * vector2.z - vector1.z * vector2.y; + v.y = vector1.z * vector2.x - vector1.x * vector2.z; + v.z = vector1.x * vector2.y - vector1.y * vector2.x; + + return v; +} + +/* Return the GDOP (Geometric Dilution of Precision) rate between 0-1. + * Lower GDOP rate means better precision of intersection. + */ +static double gdoprate(const vec3d_t tag, + const vec3d_t p1, + const vec3d_t p2, + const vec3d_t p3) +{ + vec3d_t ex, t1, t2, t3; + double h, gdop1, gdop2, gdop3, result; + + ex = vdiff(p1, tag); + h = vnorm(ex); + t1 = vdiv(ex, h); + + ex = vdiff(p2, tag); + h = vnorm(ex); + t2 = vdiv(ex, h); + + ex = vdiff(p3, tag); + h = vnorm(ex); + t3 = vdiv(ex, h); + + gdop1 = fabs(dot(t1, t2)); + gdop2 = fabs(dot(t2, t3)); + gdop3 = fabs(dot(t3, t1)); + + if (gdop1 < gdop2) { + result = gdop2; + } else { + result = gdop1; + } + if (result < gdop3) { + result = gdop3; + } + + return result; +} + +#if !defined(CFG_LEGACY) || (CFG_LEGACY == 0) + +/* Intersecting a sphere sc with radius of r, with a line p1-p2. + * Return zero if successful, negative error otherwise. + * mu1 & mu2 are constant to find points of intersection. + */ +static int sphereline(const vec3d_t p1, + const vec3d_t p2, + const vec3d_t sc, + double r, + double *const mu1, + double *const mu2) +{ + double a, b, c; + double bb4ac; + vec3d_t dp; + + dp.x = p2.x - p1.x; + dp.y = p2.y - p1.y; + dp.z = p2.z - p1.z; + + a = dp.x * dp.x + dp.y * dp.y + dp.z * dp.z; + + b = 2 * (dp.x * (p1.x - sc.x) + dp.y * (p1.y - sc.y) + dp.z * (p1.z - sc.z)); + + c = sc.x * sc.x + sc.y * sc.y + sc.z * sc.z; + c += p1.x * p1.x + p1.y * p1.y + p1.z * p1.z; + c -= 2 * (sc.x * p1.x + sc.y * p1.y + sc.z * p1.z); + c -= r * r; + + bb4ac = b * b - 4 * a * c; + + if ((fabs(a) == 0) || (bb4ac < 0)) { + *mu1 = 0; + *mu2 = 0; + return -1; + } + + *mu1 = (-b + sqrt(bb4ac)) / (2 * a); + *mu2 = (-b - sqrt(bb4ac)) / (2 * a); + + return 0; +} + +#endif /* !CFG_LEGACY */ + +/* Return TRIL_3SPHERES if it is performed using 3 spheres and return + * TRIL_4SPHERES if it is performed using 4 spheres + * For TRIL_3SPHERES, there are two solutions: result1 and result2 + * For TRIL_4SPHERES, there is only one solution: best_solution + * + * Return negative number for other errors + * + * To force the function to work with only 3 spheres, provide a duplicate of + * any sphere at any place among p1, p2, p3 or p4. + * + * The last parameter is the largest nonnegative number considered zero; + * it is somewhat analogous to machine epsilon (but inclusive). + */ +int trilateration(vec3d_t *const result1, + vec3d_t *const result2, + vec3d_t *const best_solution, + const vec3d_t p1, const double r1, + const vec3d_t p2, const double r2, + const vec3d_t p3, const double r3, + const vec3d_t p4, const double r4, + const double maxzero) +{ + (void) best_solution; + (void) p4; (void) r4; + vec3d_t ex, ey, ez, t1, t2; + double h, i, j, x, y, z, t; +#if !defined(CFG_LEGACY) || (CFG_LEGACY == 0) + vec3d_t t3; + double mu1, mu2, mu; + int result; +#endif /* CFG_LEGACY */ + + /*********** FINDING TWO POINTS FROM THE FIRST THREE SPHERES **********/ + + // if there are at least 2 concentric spheres within the first 3 spheres + // then the calculation may not continue, drop it with error -1 + + /* h = |p3 - p1|, ex = (p3 - p1) / |p3 - p1| */ + ex = vdiff(p3, p1); // vector p13 + h = vnorm(ex); // scalar p13 + if (h <= maxzero) { + /* p1 and p3 are concentric, not good to obtain a precise intersection point + */ + // diag_printf("concentric13 return -1\n"); + return ERR_TRIL_CONCENTRIC; + } + + /* h = |p3 - p2|, ex = (p3 - p2) / |p3 - p2| */ + ex = vdiff(p3, p2); // vector p23 + h = vnorm(ex); // scalar p23 + if (h <= maxzero) { + /* p2 and p3 are concentric, not good to obtain a precise intersection point + */ + // diag_printf("concentric23 return -1\n"); + return ERR_TRIL_CONCENTRIC; + } + + /* h = |p2 - p1|, ex = (p2 - p1) / |p2 - p1| */ + ex = vdiff(p2, p1); // vector p12 + h = vnorm(ex); // scalar p12 + if (h <= maxzero) { + /* p1 and p2 are concentric, not good to obtain a precise intersection point + */ + // diag_printf("concentric12 return -1\n"); + return ERR_TRIL_CONCENTRIC; + } + ex = vdiv(ex, h); // unit vector ex with respect to p1 (new coordinate + // system) + + /* t1 = p3 - p1, t2 = ex (ex . (p3 - p1)) */ + t1 = vdiff(p3, p1); // vector p13 + i = dot(ex, t1); // the scalar of t1 on the ex direction + t2 = vmul(ex, i); // colinear vector to p13 with the length of i + + /* ey = (t1 - t2), t = |t1 - t2| */ + ey = vdiff(t1, t2); // vector t21 perpendicular to t1 + t = vnorm(ey); // scalar t21 + if (t > maxzero) { + /* ey = (t1 - t2) / |t1 - t2| */ + ey = vdiv(ey, t); // unit vector ey with respect to p1 (new coordinate + // system) + + /* j = ey . (p3 - p1) */ + j = dot(ey, t1); // scalar t1 on the ey direction + } else { + j = 0.0; + } + + /* Note: t <= maxzero implies j = 0.0. */ + if (fabs(j) <= maxzero) { + /* Is point p1 + (r1 along the axis) the intersection? */ + t2 = vsum(p1, vmul(ex, r1)); + if ((fabs(vnorm(vdiff(p2, t2)) - r2) <= maxzero) + && (fabs(vnorm(vdiff(p3, t2)) - r3) <= maxzero)) { + /* Yes, t2 is the only intersection point. */ + if (result1) { + *result1 = t2; + } + if (result2) { + *result2 = t2; + } + return TRIL_3SPHERES; + } + + /* Is point p1 - (r1 along the axis) the intersection? */ + t2 = vsum(p1, vmul(ex, -r1)); + if ((fabs(vnorm(vdiff(p2, t2)) - r2) <= maxzero) + && (fabs(vnorm(vdiff(p3, t2)) - r3) <= maxzero)) { + /* Yes, t2 is the only intersection point. */ + if (result1) { + *result1 = t2; + } + if (result2) { + *result2 = t2; + } + return TRIL_3SPHERES; + } + + /* p1, p2 and p3 are colinear with more than one solution */ + return ERR_TRIL_COLINEAR_2SOLUTIONS; + } + + /* ez = ex x ey */ + ez = cross(ex, ey); // unit vector ez with respect to p1 (new coordinate + // system) + + x = (r1 * r1 - r2 * r2) / (2 * h) + h / 2; + y = (r1 * r1 - r3 * r3 + i * i) / (2 * j) + j / 2 - x * i / j; + z = r1 * r1 - x * x - y * y; + if (z < -maxzero) { + /* The solution is invalid, square root of negative number */ + return ERR_TRIL_SQRTNEGNUMB; + } else + if (z > 0.0) { + z = sqrt(z); + } else { + z = 0.0; + } + + /* t2 = p1 + x ex + y ey */ + t2 = vsum(p1, vmul(ex, x)); + t2 = vsum(t2, vmul(ey, y)); + + /* result1 = p1 + x ex + y ey + z ez */ + if (result1) { + *result1 = vsum(t2, vmul(ez, z)); + } + + /* result1 = p1 + x ex + y ey - z ez */ + if (result2) { + *result2 = vsum(t2, vmul(ez, -z)); + } + + /*********** END OF FINDING TWO POINTS FROM THE FIRST THREE SPHERES + * **********/ + /********* RESULT1 AND RESULT2 ARE SOLUTIONS, OTHERWISE RETURN ERROR + * *********/ + +#if defined(CFG_LEGACY) && (CFG_LEGACY == 1) + return TRIL_3SPHERES; +#else + + /************* FINDING ONE SOLUTION BY INTRODUCING ONE MORE SPHERE + * ***********/ + + // check for concentricness of sphere 4 to sphere 1, 2 and 3 + // if it is concentric to one of them, then sphere 4 cannot be used + // to determine the best solution and return -1 + + /* h = |p4 - p1|, ex = (p4 - p1) / |p4 - p1| */ + ex = vdiff(p4, p1); // vector p14 + h = vnorm(ex); // scalar p14 + if (h <= maxzero) { + /* p1 and p4 are concentric, not good to obtain a precise intersection point + */ + // diag_printf("concentric14 return 0\n"); + return TRIL_3SPHERES; + } + + /* h = |p4 - p2|, ex = (p4 - p2) / |p4 - p2| */ + ex = vdiff(p4, p2); // vector p24 + h = vnorm(ex); // scalar p24 + if (h <= maxzero) { + /* p2 and p4 are concentric, not good to obtain a precise intersection point + */ + // diag_printf("concentric24 return 0\n"); + return TRIL_3SPHERES; + } + + /* h = |p4 - p3|, ex = (p4 - p3) / |p4 - p3| */ + ex = vdiff(p4, p3); // vector p34 + h = vnorm(ex); // scalar p34 + if (h <= maxzero) { + /* p3 and p4 are concentric, not good to obtain a precise intersection point + */ + // diag_printf("concentric34 return 0\n"); + return TRIL_3SPHERES; + } + + // if sphere 4 is not concentric to any sphere, then best solution can be + // obtained + + /* find i as the distance of result1 to p4 */ + t3 = vdiff(*result1, p4); + i = vnorm(t3); + + /* find h as the distance of result2 to p4 */ + t3 = vdiff(*result2, p4); + h = vnorm(t3); + + /* pick the result1 as the nearest point to the center of sphere 4 */ + if (i > h) { + *best_solution = *result1; + *result1 = *result2; + *result2 = *best_solution; + } + + int count4 = 0; + double rr4 = r4; + result = 1; + + /* intersect result1-result2 vector with sphere 4 */ + while (result && count4 < 10) + { + result = sphereline(*result1, *result2, p4, rr4, &mu1, &mu2); + rr4 += 0.1; + count4++; + } + + if (result) { + /* No intersection between sphere 4 and the line with the gradient of + * result1-result2! */ + *best_solution = *result1; // result1 is the closer solution to sphere 4 + // return ERR_TRIL_NOINTERSECTION_SPHERE4; + } else { + if ((mu1 < 0) && (mu2 < 0)) { + /* if both mu1 and mu2 are less than 0 + * result1-result2 line segment is outside sphere 4 with no intersection + */ + if (fabs(mu1) <= fabs(mu2)) { + mu = mu1; + } else { + mu = mu2; + } + + /* h = |result2 - result1|, ex = (result2 - result1) / |result2 - result1| + */ + ex = vdiff(*result2, *result1); // vector result1-result2 + h = vnorm(ex); // scalar result1-result2 + ex = vdiv(ex, h); // unit vector ex with respect to result1 (new + // coordinate system) + + /* 50-50 error correction for mu */ + mu = 0.5 * mu; + + /* t2 points to the intersection */ + t2 = vmul(ex, mu * h); + t2 = vsum(*result1, t2); + + /* the best solution = t2 */ + *best_solution = t2; + } else if (((mu1 < 0) && (mu2 > 1)) || ((mu2 < 0) && (mu1 > 1))) { + /* if mu1 is less than zero and mu2 is greater than 1, + * or the other way around + * result1-result2 line segment is inside sphere 4 with no intersection + */ + if (mu1 > mu2) { + mu = mu1; + } else { + mu = mu2; + } + + /* h = |result2 - result1|, ex = (result2 - result1) / |result2 - result1| + */ + ex = vdiff(*result2, *result1); // vector result1-result2 + h = vnorm(ex); // scalar result1-result2 + ex = vdiv(ex, h); // unit vector ex with respect to result1 (new + // coordinate system) + + /* t2 points to the intersection */ + t2 = vmul(ex, mu * h); + t2 = vsum(*result1, t2); + + /* vector t2-result2 with 50-50 error correction on the length of t3 */ + t3 = vmul(vdiff(*result2, t2), 0.5); + + /* the best solution = t2 + t3 */ + *best_solution = vsum(t2, t3); + } else if ((((mu1 > 0) && (mu1 < 1)) && ((mu2 < 0) || (mu2 > 1))) + || (((mu2 > 0) && (mu2 < 1)) && ((mu1 < 0) || (mu1 > 1)))) { + /* if one mu is between 0 to 1 and the other is not */ + /* result1-result2 line segment intersects sphere 4 at one point */ + if ((mu1 >= 0) && (mu1 <= 1)) { + mu = mu1; + } else { + mu = mu2; + } + + /* add or subtract with 0.5*mu to distribute error equally onto every + * sphere */ + if (mu <= 0.5) { + mu -= 0.5 * mu; + } else { + mu -= 0.5 * (1 - mu); + } + + /* h = |result2 - result1|, ex = (result2 - result1) / |result2 - result1| + */ + ex = vdiff(*result2, *result1); // vector result1-result2 + h = vnorm(ex); // scalar result1-result2 + ex = vdiv(ex, h); // unit vector ex with respect to result1 (new + // coordinate system) + + /* t2 points to the intersection */ + t2 = vmul(ex, mu * h); + t2 = vsum(*result1, t2); + + /* the best solution = t2 */ + *best_solution = t2; + } else if (mu1 == mu2) { + /* if both mu1 and mu2 are between 0 and 1, and mu1 = mu2 */ + /* result1-result2 line segment is tangential to sphere 4 at one point */ + mu = mu1; + + /* add or subtract with 0.5*mu to distribute error equally onto every + * sphere */ + if (mu <= 0.25) { + mu -= 0.5 * mu; + } else if (mu <= 0.5) { + mu -= 0.5 * (0.5 - mu); + } else if (mu <= 0.75) { + mu -= 0.5 * (mu - 0.5); + } else { + mu -= 0.5 * (1 - mu); + } + + /* h = |result2 - result1|, ex = (result2 - result1) / |result2 - result1| + */ + ex = vdiff(*result2, *result1); // vector result1-result2 + h = vnorm(ex); // scalar result1-result2 + ex = vdiv(ex, h); // unit vector ex with respect to result1 (new + // coordinate system) + + /* t2 points to the intersection */ + t2 = vmul(ex, mu * h); + t2 = vsum(*result1, t2); + + /* the best solution = t2 */ + *best_solution = t2; + } else { + /* if both mu1 and mu2 are between 0 and 1 */ + /* result1-result2 line segment intersects sphere 4 at two points */ + + // return ERR_TRIL_NEEDMORESPHERE; + + mu = mu1 + mu2; + + /* h = |result2 - result1|, ex = (result2 - result1) / |result2 - result1| + */ + ex = vdiff(*result2, *result1); // vector result1-result2 + h = vnorm(ex); // scalar result1-result2 + ex = vdiv(ex, h); // unit vector ex with respect to result1 (new + // coordinate system) + + /* 50-50 error correction for mu */ + mu = 0.5 * mu; + + /* t2 points to the intersection */ + t2 = vmul(ex, mu * h); + t2 = vsum(*result1, t2); + + /* the best solution = t2 */ + *best_solution = t2; + } + } + + return TRIL_4SPHERES; + + /******* END OF FINDING ONE SOLUTION BY INTRODUCING ONE MORE SPHERE ********/ +#endif +} + +#if !defined(CFG_LEGACY) || (CFG_LEGACY == 0) + +/* This function calls trilateration to get the best solution. + * + * If any three spheres does not produce valid solution, + * then each distance is increased to ensure intersection to happens. + * + * Return the selected trilateration mode between TRIL_3SPHERES or TRIL_4SPHERES + * For TRIL_3SPHERES, there are two solutions: solution1 and solution2 + * For TRIL_4SPHERES, there is only one solution: best_solution + * + * nosolution_count = the number of failed attempt before intersection is found + * by increasing the sphere diameter. + */ +int deca_3dlocate(vec3d_t *const solution1, + vec3d_t *const solution2, + vec3d_t *const best_solution, + int *const nosolution_count, + double *const best_3derror, + double *const best_gdoprate, + vec3d_t p1, double r1, + vec3d_t p2, double r2, + vec3d_t p3, double r3, + vec3d_t p4, double r4, + int *combination) +{ + vec3d_t o1, o2, solution, ptemp; + // vec3d_t solution_compare1, solution_compare2; + double /*error_3dcompare1, error_3dcompare2,*/ rtemp; + double gdoprate_compare1, gdoprate_compare2; + double ovr_r1, ovr_r2, ovr_r3, ovr_r4; + int overlook_count, combination_counter; + int trilateration_errcounter, trilateration_mode34; + int success, concentric, result; + + trilateration_errcounter = 0; + trilateration_mode34 = 0; + + combination_counter = 4; /* four spheres combination */ + + *best_gdoprate = 1; /* put the worst gdoprate init */ + gdoprate_compare1 = 1; gdoprate_compare2 = 1; + // solution_compare1.x = 0; solution_compare1.y = 0; solution_compare1.z = 0; + // error_3dcompare1 = 0; + + do { + success = 0; + concentric = 0; + overlook_count = 0; + ovr_r1 = r1; ovr_r2 = r2; ovr_r3 = r3; ovr_r4 = r4; + + do { + result = trilateration(&o1, + &o2, + &solution, + p1, + ovr_r1, + p2, + ovr_r2, + p3, + ovr_r3, + p4, + ovr_r4, + MAXZERO); + + switch (result) + { + case TRIL_3SPHERES: // 3 spheres are used to get the result + trilateration_mode34 = TRIL_3SPHERES; + success = 1; + break; + + case TRIL_4SPHERES: // 4 spheres are used to get the result + trilateration_mode34 = TRIL_4SPHERES; + success = 1; + break; + + case ERR_TRIL_CONCENTRIC: + concentric = 1; + break; + + default: // any other return value goes here + ovr_r1 += 0.10; + ovr_r2 += 0.10; + ovr_r3 += 0.10; + ovr_r4 += 0.10; + overlook_count++; + break; + } + + // qDebug() << "while(!success)" << overlook_count << concentric << + // "result" << result; + } while (!success && (overlook_count <= CM_ERR_ADDED) && !concentric); + +// if(success) +// qDebug() << "Location" << ovr_r1 << ovr_r2 << ovr_r3 << ovr_r4 << +// "+err=" << overlook_count; +// else +// qDebug() << "No Location" << ovr_r1 << ovr_r2 << ovr_r3 << ovr_r4 +// << "+err=" << overlook_count; + + if (success) { + switch (result) + { + case TRIL_3SPHERES: + *solution1 = o1; + *solution2 = o2; + *nosolution_count = overlook_count; + + combination_counter = 0; + break; + + case TRIL_4SPHERES: + /* calculate the new gdop */ + gdoprate_compare1 = gdoprate(solution, p1, p2, p3); + + /* compare and swap with the better result */ + if (gdoprate_compare1 <= gdoprate_compare2) { + *solution1 = o1; + *solution2 = o2; + *best_solution = solution; + *nosolution_count = overlook_count; + *best_3derror = + sqrt((vnorm(vdiff(solution, + p1)) - r1) * (vnorm(vdiff(solution, p1)) - r1) + + (vnorm(vdiff(solution, + p2)) - r2) * (vnorm(vdiff(solution, p2)) - r2) + + (vnorm(vdiff(solution, + p3)) - r3) * (vnorm(vdiff(solution, p3)) - r3) + + (vnorm(vdiff(solution, + p4)) - r4) + * (vnorm(vdiff(solution, p4)) - r4)); + *best_gdoprate = gdoprate_compare1; + + /* save the previous result */ + // solution_compare2 = solution_compare1; + // error_3dcompare2 = error_3dcompare1; + gdoprate_compare2 = gdoprate_compare1; + } + *combination = 5 - combination_counter; + + ptemp = p1; p1 = p2; p2 = p3; p3 = p4; p4 = ptemp; + rtemp = r1; r1 = r2; r2 = r3; r3 = r4; r4 = rtemp; + combination_counter--; + break; + + default: + break; + } + } else { + // trilateration_errcounter++; + trilateration_errcounter = 4; + combination_counter = 0; + } + + // ptemp = p1; p1 = p2; p2 = p3; p3 = p4; p4 = ptemp; + // rtemp = r1; r1 = r2; r2 = r3; r3 = r4; r4 = rtemp; + // combination_counter--; + // qDebug() << "while(combination_counter)" << combination_counter; + } while (combination_counter); + + // if it gives error for all 4 sphere combinations then no valid result is + // given + // otherwise return the trilateration mode used + if (trilateration_errcounter >= 4) { + return -1; + } else { + return trilateration_mode34; + } +} + +int GetLocation(vec3d_t *best_solution, + int use4thAnchor, + vec3d_t *anchorArray, + int *distanceArray) +{ + vec3d_t o1, o2, p1, p2, p3, p4; + double r1 = 0, r2 = 0, r3 = 0, r4 = 0, best_3derror, best_gdoprate; + int result; + int error, combination; + +// vec3d_t t3; +// double dist1, dist2; + + /* Anchors coordinate */ + p1.x = anchorArray[0].x; + p1.y = anchorArray[0].y; + p1.z = anchorArray[0].z; + + p2.x = anchorArray[1].x; + p2.y = anchorArray[1].y; + p2.z = anchorArray[1].z; + + p3.x = anchorArray[2].x; + p3.y = anchorArray[2].y; + p3.z = anchorArray[2].z; + + p4.x = anchorArray[3].x; + p4.y = anchorArray[3].y; + p4.z = anchorArray[3].z; // 4th same as 1st - only 3 used for trilateration + + r1 = (double) distanceArray[0] / 1000.0; + r2 = (double) distanceArray[1] / 1000.0; + r3 = (double) distanceArray[2] / 1000.0; + + r4 = (double) distanceArray[3] / 1000.0; + + // qDebug() << "GetLocation" << r1 << r2 << r3 << r4; + + // r4 = r1; + + /* get the best location using 3 or 4 spheres and keep it as + * know_best_location */ + result = deca_3dlocate(&o1, + &o2, + best_solution, + &error, + &best_3derror, + &best_gdoprate, + p1, + r1, + p2, + r2, + p3, + r3, + p4, + r4, + &combination); + + // qDebug() << "GetLocation" << result << "sol1: " << o1.x << o1.y << o1.z << + // " sol2: " << o2.x << o2.y << o2.z; + + if (result >= 0) { +// if (use4thAnchor == 1) //if have 4 ranging results, then use 4th +// anchor to pick solution closest to it +// { +// double diff1, diff2; +// /* find dist1 as the distance of o1 to known_best_location */ +// t3 = vdiff(o1, anchorArray[3]); +// dist1 = vnorm(t3); +// +// t3 = vdiff(o2, anchorArray[3]); +// dist2 = vnorm(t3); +// +// /* find the distance closest to received range measurement +// from 4th anchor */ +// diff1 = fabs(r4 - dist1); +// diff2 = fabs(r4 - dist2); +// +// /* pick the closest match to the 4th anchor range */ +// if (diff1 < diff2) *best_solution = o1; else *best_solution = +// o2; +// } +// else +// { + // assume tag is below the anchors (1, 2, and 3) + if (o1.z < p1.z) { + *best_solution = o1; + } else { + *best_solution = o2; + } +// } + } + + if (result >= 0) { + return result; + } + + // return error + return -1; +} + +#endif /* !CFG_LEGACY */ + +#define CFG_FSQRT 0 + +#if defined(CFG_FSQRT) && (CFG_FSQRT == 1) +static float fsqrt(float x) +{ + ieee_float_shape_t t; + + t.value = x; + + t.word = 0x5f3759df - (t.word >> 1); + t.value = t.value * (1.5f - (0.5f * x * t.value * t.value)); + + return (1 / t.value); +} + +#define _sqrt(x) fsqrt(x) +#else +#define _sqrt(x) sqrt(x) +#endif /* CFG_FSQRT */ + +static double _get_dist(const vec3d_t *p0, const vec3d_t *p1) +{ +#if defined(CFG_FSQRT) && (CFG_FSQRT == 1) + return _sqrt(POW(p0->x - p1->x) + POW(p0->y - p1->y) + POW(p0->z - p1->z)); +#else + float x1, x2, y1, y2, z1, z2; + float fpow; + float frv; + + x1 = p0->x; + x2 = p1->x; + y1 = p0->y; + y2 = p1->y; + z1 = p0->z; + z2 = p1->z; + + fpow = POW(x1 - x2) + POW(y1 - y2) + POW(z1 - z2); + frv = _sqrt(fpow); + + return frv; +#endif /* CFG_FSQRT */ +} + +#if defined(CFG_LE_TRILAT_QSORT) && (CFG_LE_TRILAT_QSORT == 1) +static int cmpdiff(const void *a, const void *b) +{ + trilat_result_t *a_res = (trilat_result_t *)a; + trilat_result_t *b_res = (trilat_result_t *)b; + + return (fabs(a_res->e) > fabs(b_res->e)); +} + +static void trilat_solver_sort_res(trilat_result_t *res, int res_cnt) +{ + qsort(res, res_cnt, sizeof(trilat_result_t), cmpdiff); +} + +#else + +static void trilat_solver_sort_res(trilat_result_t *res, int res_cnt) +{ + trilat_result_t tres; + int i, j; + + for (i = 1; i < res_cnt; i++) { + for (j = 0; j < res_cnt - 1; j++) { + if (fabs(res[j].e) > fabs(res[i].e)) { + memcpy(&tres, &res[i], sizeof(trilat_result_t)); + memcpy(&res[i], &res[j], sizeof(trilat_result_t)); + memcpy(&res[j], &tres, sizeof(trilat_result_t)); + } + } + } +} + +#endif /* CFG_LE_TRILAT_QSORT */ + +#if defined(CFG_LE_TRILAT_DEBUG) && (CFG_LE_TRILAT_DEBUG == 1) +static void trilat_results_dump2(trilat_solver_t *trilat_solver, + trilat_result_t *res, + trilat_mask_t *mask, + int res_cnt, + vec3d_t *est, + double *min_e, + double *max_e, + double *r) +{ + static int first = 1; + + if (first) { + first = 0; + diag_printf("res:tot | variant | dmeas | " + "est[0]=x:y:z / err | est[1]=x:y:z / err | est=x:y:z / err\n"); + } + + diag_printf("%3d:%3d | %d:%d:%d | %7.3f:%7.3f:%7.3f | " + "%7.3f:%7.3f:%7.3f / <%7.3f;%7.3f> | " + "%7.3f:%7.3f:%7.3f / <%7.3f;%7.3f> | ", + res_cnt, trilat_solver->trilat_cnt, + mask->v[0], mask->v[1], mask->v[2], + r[0], r[1], r[2], + est[0].x, est[0].y, est[0].z, min_e[0], max_e[0], + est[1].x, est[1].y, est[1].z, min_e[1], max_e[1]); + + if (res) { + diag_printf("%7.3f:%7.3f:%7.3f / %7.3f\n", + res->est.x, res->est.y, res->est.z, res->e); + } else { + diag_printf("rejected\n"); + } +} + +/** + * Dump possible solutions + * + * \param trilat_res Pointer to solution structure + * \param cnt Number of solutions + */ +static void trilat_results_dump(trilat_result_t *trilat_res, int cnt) +{ + int i; + + diag_printf("res | %d\n", cnt); + for (i = 0; i < cnt; i++) { + diag_printf("%3d | %d:%d:%d | %7.3f:%7.3f:%7.3f / %7.3f\n", i, + trilat_res[i].mask.v[0], trilat_res[i].mask.v[1], + trilat_res[i].mask.v[2], + trilat_res[i].est.x, trilat_res[i].est.y, + trilat_res[i].est.z, trilat_res[i].e); + } +} + +#else +static void trilat_results_dump2(trilat_solver_t *trilat_solver, + trilat_result_t *res, + trilat_mask_t *mask, int res_cnt, + vec3d_t *est, + double *min_e, double *max_e, double *r) +{ + (void) trilat_solver; (void) res; (void) mask; + (void) res_cnt; (void) est; (void) *min_e; (void) max_e; (void) r; +} + +static void trilat_results_dump(trilat_result_t *trilat_res, int cnt) +{ + (void) trilat_res; (void) cnt; +} + +#endif /* CFG_LE_TRILAT_DEBUG */ + +/* Described in header file */ +int trilat_solver_init(trilat_solver_t *trilat_solver, + const vec3d_t *bn_pos, + unsigned int bn_cnt) +{ + vec3d_t _bn_pos[CFG_TRILAT_MEAS_PER_CALC]; + int a[CFG_TRILAT_MEAS_PER_CALC]; + trilat_t *trilat; + int trilat_cnt = 0; + int i; + + /* Reset the solver */ + memset(trilat_solver, 0, sizeof(trilat_solver_t)); + + /* Make a local copy of base node positions */ + trilat_solver->bn_pos = (vec3d_t *)TRILAT_MALLOC(sizeof(vec3d_t) * bn_cnt); + if (trilat_solver->bn_pos == NULL) { + return -10; + } + memcpy(trilat_solver->bn_pos, bn_pos, sizeof(vec3d_t) * bn_cnt); + trilat_solver->bn_cnt = bn_cnt; + + trilat_solver->trilat_cnt = fact(bn_cnt) / (fact(bn_cnt - 3) * 6); + + if (trilat_solver->trilat_cnt > CFG_TRILAT_MAX_CACHE) { + trilat_solver->trilat_cnt = CFG_TRILAT_MAX_CACHE; + } + + trilat_solver->trilat = (trilat_t *)TRILAT_MALLOC( + sizeof(trilat_t) * trilat_solver->trilat_cnt); + if (trilat_solver->trilat == NULL) { + TRILAT_FREE(trilat_solver->bn_pos); + return -11; + } + + /* Find the boundary of covered area */ + for (i = 0; i < (int)bn_cnt; i++) { + if (trilat_solver->bn_pos[i].x < trilat_solver->min.x) { + trilat_solver->min.x = trilat_solver->bn_pos[i].x; + } + if (trilat_solver->bn_pos[i].x > trilat_solver->max.x) { + trilat_solver->max.x = trilat_solver->bn_pos[i].x; + } + + if (trilat_solver->bn_pos[i].y < trilat_solver->min.y) { + trilat_solver->min.y = trilat_solver->bn_pos[i].y; + } + if (trilat_solver->bn_pos[i].y > trilat_solver->max.y) { + trilat_solver->max.y = trilat_solver->bn_pos[i].y; + } + + if (trilat_solver->bn_pos[i].z < trilat_solver->min.z) { + trilat_solver->min.z = trilat_solver->bn_pos[i].z; + } + if (trilat_solver->bn_pos[i].z > trilat_solver->max.z) { + trilat_solver->max.z = trilat_solver->bn_pos[i].z; + } + } + + trilat_solver->center.x = (trilat_solver->max.x + trilat_solver->min.x) / 2; + trilat_solver->center.y = (trilat_solver->max.y + trilat_solver->min.y) / 2; + trilat_solver->center.z = (trilat_solver->max.z + trilat_solver->min.z) / 2; + + if (trilat_solver->center.z) { + trilat_solver->min.z -= 1.0; + trilat_solver->max.z += 1.0; + } + + /* Create cache */ + for (a[0] = 0; a[0] < (int)bn_cnt; a[0]++) { + for (a[1] = a[0] + 1; a[1] < (int)bn_cnt; a[1]++) { + for (a[2] = a[1] + 1; a[2] < (int)bn_cnt; a[2]++) { + if (trilat_cnt >= trilat_solver->trilat_cnt) { + break; + } + + trilat = &trilat_solver->trilat[trilat_cnt]; + memset(trilat, 0, sizeof(trilat_t)); + + for (i = 0; i < CFG_TRILAT_MEAS_PER_CALC; i++) { + memcpy(&_bn_pos[i], &trilat_solver->bn_pos[a[i]], sizeof(vec3d_t)); + trilat->mask.v[i] = a[i]; + } + +#if defined(CFG_LE_TRILAT_DEBUG) && (CFG_LE_TRILAT_DEBUG == 1) + diag_printf("%2d:anchor:x:y:z | variant %d:%d:%d\n", + trilat_cnt, a[0], a[1], a[2]); + + for (i = 0; i < CFG_TRILAT_MEAS_PER_CALC; i++) { + diag_printf("%2d:%2d:%7.3f:%7.3f:%7.3f\n", + trilat_cnt, trilat->mask.v[i], + trilat_solver->bn_pos[trilat->mask.v[i]].x, + trilat_solver->bn_pos[trilat->mask.v[i]].y, + trilat_solver->bn_pos[trilat->mask.v[i]].z); + } + diag_printf("\n"); +#endif /* CFG_LE_TRILAT_DEBUG */ + + trilat_cnt++; + } + } + } + + return 0; +} + +/* Described in header file */ +int trilat_solver_get_pos(trilat_solver_t *trilat_solver, + vec3d_t *mn_pos, + uint8_t *qf, + const double *meas, + unsigned int meas_cnt) +{ + trilat_result_t *res; + int res_cnt = 0; + int res_tot; + double _r[4]; + double _e; + double _mae[2]; + double _mie[2]; + double _rej[2]; + vec3d_t est[2]; + int mres_cnt = 0; + int rc = 0; + int mi = 0; + int i, j, k; + int rv = 0; + + /* Total number of solutions */ + res_tot = trilat_solver->trilat_cnt * 2; + if (res_tot > CFG_TRILAT_MAX_RESULTS) { + res_tot = CFG_TRILAT_MAX_RESULTS; + } + res = (trilat_result_t *)TRILAT_MALLOC(sizeof(trilat_result_t) * res_tot); + if (res == NULL) { + return -20; + } + + /* Run through all trilat variants and calculate positions */ + for (k = 0; (k < trilat_solver->trilat_cnt) && (res_cnt < res_tot); k++) { + /* Get variants of the measurements */ + _r[0] = meas[trilat_solver->trilat[k].mask.v[0]]; + _r[1] = meas[trilat_solver->trilat[k].mask.v[1]]; + _r[2] = meas[trilat_solver->trilat[k].mask.v[2]]; + + /* Calculate position for the given variant */ +#if 0 + rv = trilat_get_pos(trilat_solver, &trilat_solver->trilat[k], + est, _r, CFG_TRILAT_MEAS_PER_CALC); +#else + vec3d_t solution; + int result; + int overlook_count = 0; + int concentric = 0; + double _ar[4]; + + memset(_ar, 0, sizeof(_ar)); + + do { + result = trilateration(&est[0], &est[1], &solution, + trilat_solver->bn_pos[trilat_solver->trilat[k].mask + .v[0]], _r[0] + _ar[0], + trilat_solver->bn_pos[trilat_solver->trilat[k].mask + .v[1]], _r[1] + _ar[1], + trilat_solver->bn_pos[trilat_solver->trilat[k].mask + .v[2]], _r[2] + _ar[2], + trilat_solver->bn_pos[trilat_solver->trilat[k].mask + .v[0]], _r[0] + _ar[0], + MAXZERO); + + switch (result) { + case TRIL_3SPHERES: + rv = 0; + break; + + case ERR_TRIL_CONCENTRIC: + rv = -101; + concentric = 1; + break; + + default: + rv = -100; + _ar[0] += 0.10; + _ar[1] += 0.10; + _ar[2] += 0.10; + overlook_count++; + break; + } + } while ((rv < 0) && (overlook_count <= CM_ERR_ADDED) && !concentric); +#endif + + if (rv < 0) { + /* No solution found */ + continue; + } + + for (i = 0; i < 2; i++) { + _mae[i] = 0; + _rej[i] = 0; + _mie[i] = -1; + + for (j = 0; j < (int)meas_cnt; j++) { + _e = meas[j] - _get_dist(&est[i], &trilat_solver->bn_pos[j]); + + if (_e < CFG_TRILAT_MIN_NEG_DIFF_M) { + _rej[i] = 1; + _mae[i] = _e; + break; + } + + if ((j == trilat_solver->trilat[k].mask.v[0]) + || (j == trilat_solver->trilat[k].mask.v[1]) + || (j == trilat_solver->trilat[k].mask.v[2])) { + if (fabs(_e) > fabs(_mae[i])) { + _mae[i] = _e; + } + } else { + if ((_mie[i] == -1) || (fabs(_e) < fabs(_mie[i]))) { + _mie[i] = _e; + + if (fabs(_mie[i]) == 0) { + break; + } + } + } + } + } + + if ((_rej[0] && _rej[1]) + || ((fabs(_mie[0]) > CFG_TRILAT_MAX_DIFF_M) + && (fabs(_mie[1]) > CFG_TRILAT_MAX_DIFF_M))) { + trilat_results_dump2(trilat_solver, + NULL, + &trilat_solver->trilat[k].mask, + res_cnt, + est, + _mie, + _mae, + _r); + continue; + } + + _mie[0] = floor(_mie[0] * 1000000) / 1000000; + _mie[1] = floor(_mie[1] * 1000000) / 1000000; + if ((est[0].z >= trilat_solver->min.z) + && (est[0].z <= trilat_solver->max.z) + && (est[1].z >= trilat_solver->min.z) + && (est[1].z <= trilat_solver->max.z)) { + if (fabs(_mie[0] - _mie[1]) < CFG_TRILAT_MIN_ERR_M) { + if (fabs(est[0].z - trilat_solver->center.z) + <= fabs(est[1].z - trilat_solver->center.z)) { + i = 0; + } else { + i = 1; + } + } else if (fabs(_mie[0]) < fabs(_mie[1])) { + i = 0; + } else { + i = 1; + } + } else if ((est[0].z >= trilat_solver->min.z) + && (est[0].z <= trilat_solver->max.z)) { + i = 0; + } else if ((est[1].z >= trilat_solver->min.z) + && (est[1].z <= trilat_solver->max.z)) { + i = 1; + } else if (fabs(_mie[0]) <= fabs(_mie[1])) { + i = 0; + } else { + i = 1; + } + + memcpy(&res[res_cnt].est, &est[i], sizeof(vec3d_t)); + res[res_cnt].e = _mie[i]; + + res[res_cnt].mask.val = trilat_solver->trilat[k].mask.val; + + trilat_results_dump2(trilat_solver, + &res[res_cnt], + &trilat_solver->trilat[k].mask, + res_cnt, + est, + _mie, + _mae, + _r); + + res_cnt++; + } + + if (res_cnt <= 0) { + rv = -21; + goto out; + } + + rv = 0; + trilat_solver_sort_res(res, res_cnt); + trilat_results_dump(res, res_cnt); + + for (i = 0; (i < res_cnt) && (res_cnt > 1); i++) { + if (fabs((fabs(res[0].e) - fabs(res[i].e))) > CFG_TRILAT_MIN_DIFF_M) { + break; + } + + rc = 1; + memcpy(&est[0], &res[i].est, sizeof(vec3d_t)); + for (j = i + 1; j < res_cnt; j++) { + if (_get_dist(&res[i].est, &res[j].est) > CFG_TRILAT_MIN_DIFF_M) { + continue; + } + + est[0].x += res[j].est.x; + est[0].y += res[j].est.y; + est[0].z += res[j].est.z; + res[j].e = CFG_TRILAT_MAX_DIFF_M * 2; + rc++; + } + + if (rc > mres_cnt) { + mres_cnt = rc; + mi = i; + } + + if (rc > 1) { + res[i].est.x = est[0].x / rc; + res[i].est.x = est[0].x / rc; + res[i].est.x = est[0].x / rc; + res[i].mask.val = rc; + } + } + + trilat_results_dump(res, (i) ? i : 1); + + mn_pos->x = res[mi].est.x; + mn_pos->y = res[mi].est.y; + mn_pos->z = res[mi].est.z; + +#if 0 + if ((res_cnt == 1) && (meas_cnt == CFG_TRILAT_MEAS_PER_CALC)) { + *qf = 50; + } else { + if (fabs(res[mi].e) > CFG_TRILAT_MAX_POS_ERR_M) { + *qf = 50; + } else { + *qf = 100 - res[mi].e / CFG_TRILAT_MAX_POS_ERR_M * 50.0f; + } + } +#else + double gdoprate_min = 1; + double gdop; + + for (k = 0; k < trilat_solver->trilat_cnt; k++) { + gdop = gdoprate(*mn_pos, + trilat_solver->bn_pos[trilat_solver->trilat[k].mask.v[0]], + trilat_solver->bn_pos[trilat_solver->trilat[k].mask.v[1]], + trilat_solver->bn_pos[trilat_solver->trilat[k].mask.v[2]]); + if (gdop < gdoprate_min) { + gdoprate_min = gdop; + } + } + + *qf = gdoprate_min * 100; +#endif + + if (*qf > 100) { + *qf = 100; + } + +#if defined(CFG_LE_TRILAT_DEBUG) && (CFG_LE_TRILAT_DEBUG == 1) + diag_printf("est(x:y:z:qf:diff) = %7.3f:%7.3f:%7.3f:%6d:%7.3f\n\n", + mn_pos->x, mn_pos->y, mn_pos->z, *qf, res[mi].e); +#endif /* CFG_LE_TRILAT_DEBUG */ + + out: + TRILAT_FREE(res); + + return rv; +} + +#if defined(CFG_LE_TRILAT_UTILS) && (CFG_LE_TRILAT_UTILS == 1) + +/* Described in header file */ +int trilat_solve(vec3d_t *bn_pos, + double *meas, + int cnt, + vec3d_t *pos_est, + uint8_t *qf) +{ +#if defined(CFG_LEGACY) && (CFG_LEGACY == 1) + int rv; + + if (_trilat_solver == NULL) { + _trilat_solver = (trilat_solver_t *)TRILAT_MALLOC(sizeof(trilat_solver_t)); + if (_trilat_solver == NULL) { + return -6; + } + + // memset(_trilat_solver, 0, sizeof(trilat_solver_t)); + } + + rv = trilat_solver_init(_trilat_solver, bn_pos, cnt); + if (rv < 0) { + return rv; + } + + rv = trilat_solver_get_pos(_trilat_solver, pos_est, qf, meas, cnt); + + trilat_reset(); + + return rv; +#else + int distances[4]; + int rv; + int i; + + if (cnt < 3) { + return -1; + } + + for (i = 0; i < cnt; i++) { + distances[i] = meas[i] * 1000; + } + + rv = GetLocation(pos_est, cnt == 3 ? 0 : 1, bn_pos, distances); + *qf = 11; + + return rv; +#endif +} + +/* Described in header file */ +void trilat_reset(void) +{ + if (_trilat_solver) { + if (_trilat_solver->bn_pos) { + TRILAT_FREE(_trilat_solver->bn_pos); + } + + if (_trilat_solver->trilat) { + TRILAT_FREE(_trilat_solver->trilat); + } + +// memset(_trilat_solver, 0, sizeof(trilat_solver_t)); + + TRILAT_FREE(_trilat_solver); + + _trilat_solver = NULL; + } +} + +#endif /* CFG_LE_TRILAT_UTILS */ + +#endif /* CFG_LE_TRILAT */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/trilat/dwm_le/le-trilat.h b/bluetooth_uwb_dw3000_slotted_twr/src/apps/trilat/dwm_le/le-trilat.h new file mode 100644 index 0000000..ce77c08 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/trilat/dwm_le/le-trilat.h @@ -0,0 +1,169 @@ +/** + * LEAPS - Low Energy Accurate Positioning System. + * + * Location Engine - Trilateration. + * Based on codes from Decawave with improvements by LEAPS. + * + * Copyright (c) 2016-2018, Decawave Ltd, Dublin, Ireland. All rights reserved. + * Copyright (c) 2016-2018, LEAPS. All rights reserved. + * + */ + +#ifndef _LE_TRILAT_H_ +#define _LE_TRILAT_H_ + +#include "dwm-math.h" + +#define CFG_TRILAT_MAX_CACHE 56 +#define CFG_TRILAT_MAX_RESULTS 112 +#define CFG_TRILAT_BN_PER_CALC 4 +#define CFG_TRILAT_MEAS_PER_CALC 3 +#define CFG_TRILAT_MAX_DIFF_M 5.0 +#define CFG_TRILAT_MIN_DIFF_M 0.15 +#define CFG_TRILAT_MIN_NEG_DIFF_M -0.50 +#define CFG_TRILAT_MIN_ERR_M 0.05 +#define CFG_TRILAT_MAX_POS_ERR_M 0.5 +#define CFG_TRILAT_MIN_RSSI 0 + +#define TRIL_3SPHERES 3 +#define TRIL_4SPHERES 4 + +union trilat_mask_union { + struct { + uint8_t v[4]; + }; + + uint32_t val; +}; + +typedef union trilat_mask_union trilat_mask_t; + +struct trilat_result_struct { + vec3d_t est; + double e; + trilat_mask_t mask; +}; + +typedef struct trilat_result_struct trilat_result_t; + +struct trilat_struct { + trilat_mask_t mask; +}; + +typedef struct trilat_struct trilat_t; + +struct trilat_solver_struct { + vec3d_t *bn_pos; + unsigned int bn_cnt; + + vec3d_t min; + vec3d_t max; + vec3d_t center; + + trilat_t *trilat; + int trilat_cnt; + + uint32_t chksum; +}; + +typedef struct trilat_solver_struct trilat_solver_t; + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Initialize matrices using base node positions + * + * \param trilat Pointer to trilateration data structure + * \param bn_pos Pointer to base node position + * \param bn_cnt Number of base nodes + * \return Returns negative on an error else return 0 + */ +int trilat_init(trilat_t *trilat, const vec3d_t *bn_pos, unsigned int bn_cnt); + +/** + * Solve quadratic equation + * + * \param trilat_solver Pointer to trilateration solver data structure + * \param trilat Pointer to trilateration data structure + * \param u Cooefficient + * \param meas Pointer to measurement values + * \return Returns zero if one solution else sign of the discriminant + */ +double trilat_solv_quad(trilat_solver_t *trilat_solver, + trilat_t *trilat, double *u, const double *meas); + +/** + * Calculate position using trilateration and ranging measurements + * + * \param trilat_solver Pointer to trilateration solver data structure + * \param trilat Pointer to trilateration data structure + * \param est Pointer to estimated positions + * \param meas Pointer to measurements + * \param meas_cnt Number of measurements + * \return Returns negative on an error else return 0 + */ +int trilat_get_pos(trilat_solver_t *trilat_solver, trilat_t *trilat, + vec3d_t *est, const double *meas, unsigned int meas_cnt); + +/** + * Initialize trilateration solver + * + * \param trilat_solver Pointer to trilateration solver data structure + * \param bn_pos Pointer to base node position + * \param bn_cnt Number of base nodes + * \return Returns negative on an error else return 0 + */ +int trilat_solver_init(trilat_solver_t *trilat_solver, + const vec3d_t *bn_pos, + unsigned int bn_cnt); + +/** + * Calculate position using trilateration and ranging measurements. + * Select the best solution and indicate position quality. + * + * \param trilat_solver Pointer to trilateration data structure + * \param mn_pos Pointer to estimated position + * \param qf Pointer to position quality factor + * \param meas Pointer to measurements + * \param meas_cnt Number of measurements + * \return Returns negative on an error else return 0 + */ +int trilat_solver_get_pos(trilat_solver_t *trilat_solver, + vec3d_t *mn_pos, + uint8_t *qf, + const double *meas, + unsigned int meas_cnt); + +#if defined(CFG_LE_TRILAT_UTILS) && (CFG_LE_TRILAT_UTILS == 1) + +/** + * Trilateration - use provided positions and measurements to calculate + * unknown position + * + * \param bn_pos Pointer to known positions + * \param meas Pointer to measurements + * \param cnt Number of measurements + * \param pos_est Estimated position + * \param qf Position quality factor for the estimated position + * \return Returns negative if error occured else return zero + */ +int trilat_solve(vec3d_t *bn_pos, + double *meas, + int cnt, + vec3d_t *pos_est, + uint8_t *qf); + +/** + * Trilateration - reset the solver + */ +void trilat_reset(void); + +#endif /* CFG_LE_TRILAT_UTILS */ + +#ifdef __cplusplus +} +#endif + +#endif /* _LE_TRILAT_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/trilat/task_trilat/task_trilat.c b/bluetooth_uwb_dw3000_slotted_twr/src/apps/trilat/task_trilat/task_trilat.c new file mode 100644 index 0000000..3a8b2c2 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/trilat/task_trilat/task_trilat.c @@ -0,0 +1,252 @@ +/* + * @file task_trilat.c + * @brief + * + * @author Decawave + * + * @attention Copyright 2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + */ +#include +#include +#include "port_common.h" +#include "node.h" +#include "task_node.h" +#include "dwm-math.h" +#include "dwm-math.h" + +/** @defgroup Trilateration + * @brief Parameters change section : allowed only in app.mode = mIdle + * @{ + */ + +// ----------------------------------------------------------------------------- +// extern functions to report output data +extern int trilat_solve(vec3d_t *bn_pos, + double *meas, + int cnt, + vec3d_t *pos_est, + uint8_t *qf); +extern error_e port_tx_msg(uint8_t *str, int len); + +#define PFIL(x) (int)(x / 1000.0) +#define PFIR(x) (int)(((float)x - 1000.0 * (int)(x / 1000))) + +// ----------------------------------------------------------------------------- + +static struct trilat_buf_s { + struct tag_res_s { + uint16_t addr16; + uint32_t resTime_us; // reception time of the end of the Final from + // the Tag wrt node's SuperFrame start, + // microseconds + double dist_mm; + vec3d_t bn_pos; + } tag_res[6]; + + int cnt; +} ttmp_buf[2] = { 0 }; + +static struct trilat_buf_s *p_ttmp = &ttmp_buf[0]; +static struct trilat_buf_s *p_ttmp_calc = &ttmp_buf[1]; + +/* Design: + * Assuming the SF (100ms) is enough time to calculate the position in + * low-priority TrilatTask(). + * + * Rx of the Final: + * - The Node is performing calculation of the tag position in the CalcTask; + * - results from all ranges during SF stored in the ttmp_buf by calling + * trilat_extension_put(result_t *); + * - max of 6 ranges can be saved; + * + * On RTC: + * - The cb called trilat_extension_SF_cb() + * - The ttmp_buf copied to the tcalc_buf, then ttmp_buf is cleared and ready + * for next SF ranges. + * - The signal is also sent + * + * Trilat_Task: + * - receiving the signal and the tcalc_buf to perform the location + * - uses statically known positions of the FixedElement; + * + * */ + +/* @brief save distances to FixedElements into the current p_ttmp buffer + * + * */ +void trilat_extension_node_put(result_t *pRes) +{ + const int size = sizeof(p_ttmp->tag_res) / sizeof(p_ttmp->tag_res[0]); + + struct tag_res_s *p = &p_ttmp->tag_res[p_ttmp->cnt]; + + if (p_ttmp->cnt < size - 1) { + p->addr16 = pRes->addr16; + p->resTime_us = pRes->resTime_us; + p->dist_mm = pRes->dist_cm * 10.0; + + p->bn_pos.x = (double)pRes->acc_x; // Fixed unit sending its X + // location instead of accel + // data, mm + p->bn_pos.y = (double)pRes->acc_y; // Fixed unit sending its Y + // location instead of accel + // data, mm + p->bn_pos.z = (double)pRes->acc_z; // Fixed unit sending its Z + // location instead of accel + // data, mm + + p_ttmp->cnt++; + } +} + +/* @brief Trilateration SuperFrame callback + * Called every superframe. + * Send stored via previous SF ranges in the p_ttmp to the low-priority + * trilatTask fn() + * clear + * */ +void trilat_SF_cb(result_t *pRes) +{ + (void) pRes; + p_ttmp_calc = p_ttmp; + p_ttmp = (p_ttmp == &ttmp_buf[0])?(&ttmp_buf[1]):(&ttmp_buf[0]); + p_ttmp->cnt = 0; + + osThreadFlagsSet(app.trilatTask.Handle, app.trilatTask.Signal); +} + +/* + * @brief Trilat Node variant implementation + * This is a low priority task, which is awaking every superframe + * to trilaterate position wrt fixed infrastructure elements + * from ranges, measured during the previous SF + * */ +static void TrilatTask(void const *arg) +{ + node_info_t *pNodeInfo; + char str[256]; + + while (!(pNodeInfo = getNodeInfoPtr())) + { + osDelay(5); + } + + const osMutexAttr_t thread_mutex_attr = { .attr_bits = osMutexPrioInherit }; + app.trilatTask.MutexId = osMutexNew(&thread_mutex_attr); + + do { + osMutexRelease(app.trilatTask.MutexId); + + osThreadFlagsWait(app.trilatTask.Signal, osFlagsWaitAny, osWaitForever); + + osMutexAcquire(app.trilatTask.MutexId, 0); + + sprintf(str, "res: "); + + for (int i = 0; i < (int)p_ttmp_calc->cnt; i++) + { + struct tag_res_s *p = &p_ttmp_calc->tag_res[i]; + + sprintf(&str[strlen(str)], + "%04x[%2d.%-3d][%2d.%-2d,%2d.%-2d,%2d.%-2d]=%2d.%-2d ", + p->addr16, + PFIL(p->resTime_us), + PFIR(p->resTime_us), + PFIL(p->bn_pos.x), + PFIR(p->bn_pos.x), + PFIL(p->bn_pos.y), + PFIR(p->bn_pos.y), + PFIL(p->bn_pos.z), + PFIR(p->bn_pos.z), + PFIL(p->dist_mm), + PFIR(p->dist_mm)); + } + + /* Assuming the time required for Trilateration is less than + * Superframe time, thus we are sure about integrity of the data in the + * p_ttmp_calc buffer. + * */ + if (p_ttmp_calc->cnt >= 3) { + vec3d_t pos_est; + uint8_t qf; + vec3d_t bn_pos[p_ttmp_calc->cnt]; + double dist[p_ttmp_calc->cnt]; + + for ( int i = 0; i < p_ttmp_calc->cnt; i++) + { + bn_pos[i].x = p_ttmp_calc->tag_res[i].bn_pos.x / 1000.0; + bn_pos[i].y = p_ttmp_calc->tag_res[i].bn_pos.y / 1000.0; + bn_pos[i].z = p_ttmp_calc->tag_res[i].bn_pos.z / 1000.0; + + dist[i] = p_ttmp_calc->tag_res[i].dist_mm / 1000.0; + } + + int ret = trilat_solve(bn_pos, + dist, + p_ttmp_calc->cnt, + &pos_est, + &qf); + + if (ret >= 0) { + sprintf(&str[strlen(str)], "qf:%3d est[%2d.%-2d,%2d.%-2d,%2d.%-2d] ", + (int) qf, + PFIL((int)(1000 * pos_est.x)), PFIR((int)(1000 * pos_est.x)), + PFIL((int)(1000 * pos_est.y)), PFIR((int)(1000 * pos_est.y)), + PFIL((int)(1000 * pos_est.z)), PFIR((int)(1000 * pos_est.z))); + } else { + sprintf(&str[strlen(str)], "no_est, errno: %d ", ret); + } + } else { + sprintf(&str[strlen(str)], "no_trilat n:%2d ", (int)p_ttmp_calc->cnt); + } + + sprintf(&str[strlen(str)], "\r\n"); + + port_tx_msg((uint8_t *)str, strlen(str)); + }while (1); + + UNUSED(arg); +} + +// ----------------------------------------------------------------------------- + +/* @brief Terminate all tasks and timers related to Trilateration functionality, + * if any + * DW3000's RX and IRQ shall be switched off before task termination, + * that IRQ will not produce unexpected Signal + * */ +void trilat_terminate(void) +{ + node_terminate(); + + TERMINATE_STD_TASK(app.trilatTask); +} + +/* @fn trilat_helper + * @brief this is a service function which starts the + * TWR Node functionality plus Trilateration service + * Note: the previous instance of TWR shall be killed + * with node_terminate_tasks(); + * + * Note: the node_process_init() will allocate the memory of + * sizeof(node_info_t) + * from the caller's task stack, see _malloc_r() ! + * + * */ +void trilat_helper(void const *argument) +{ + CREATE_NEW_TASK(TrilatTask, + NULL, + "trilatTask", + 512, + PRIO_TrilatTask, + &app.trilatTask.Handle); + app.trilatTask.Signal = 1; + + if (app.trilatTask.Handle == NULL) { + error_handler(1, _ERR_Create_Task_Bad); + } + + node_helper(argument); +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/usb2spi/task_usb2spi/task_usb2spi.c b/bluetooth_uwb_dw3000_slotted_twr/src/apps/usb2spi/task_usb2spi/task_usb2spi.c new file mode 100644 index 0000000..303a06e --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/usb2spi/task_usb2spi/task_usb2spi.c @@ -0,0 +1,93 @@ +/* + * @file task_usb2spi.c + * + * @brief usb2spi implementation + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ +#include "task_usb2spi.h" + +#include "port.h" +#include "port_common.h" +#include "usb_uart_rx.h" +#include "usb2spi.h" +#include "error.h" +#include "deca_dbg.h" + +// ----------------------------------------------------------------------------- + +/* @fn Usb2SpiTask + * @brief this starts the usb2spi functionality. + * + * Note: Previous tasks which can call shared resources must be + * killed. + * This task needs the RAM size of at least usb2spi_t + * + * */ +void Usb2SpiTask(void const *argument) +{ + (void) argument; + const osMutexAttr_t thread_mutex_attr = { .attr_bits = osMutexPrioInherit }; + app.usb2spiTask.MutexId = osMutexNew(&thread_mutex_attr); + + while (1) + { + osMutexRelease(app.usb2spiTask.MutexId); + + osThreadFlagsWait(app.usb2spiTask.Signal, osFlagsWaitAny, osWaitForever); + + osMutexAcquire(app.usb2spiTask.MutexId, 0); + + usb2spi_process_run(); // app.local_buff has a Usb2Spi protocol + // sequence + } +} + +/* @brief + * Kill all tasks and timers related to usb2spi if any + * + * */ +void usb2spi_terminate(void) +{ + TERMINATE_STD_TASK(app.usb2spiTask); + + usb2spi_process_terminate(); +} + +/* @fn usb2spi_helper + * @brief this is a service function which starts the + * Usb2Spi test functionality + * */ +void usb2spi_helper(void const *arg) +{ + error_e tmp; + + port_disable_dw_irq_and_reset(0); + + CREATE_NEW_TASK(Usb2SpiTask, + NULL, + "u2sTask", + 4096, + PRIO_Usb2SpiTask, + &app.usb2spiTask.Handle); + app.usb2spiTask.Signal = 1; + + if (!app.usb2spiTask.Handle) { + error_handler(1, _ERR_Create_Task_Bad); + } + + // set_dw_spi_fast_rate(); + + tmp = usb2spi_process_init(); + + if (tmp != _NO_ERR) { + error_handler(1, tmp); + } + + diag_printf("usb2spi: irq enabled\r\n"); + UNUSED(arg); +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/usb2spi/task_usb2spi/task_usb2spi.h b/bluetooth_uwb_dw3000_slotted_twr/src/apps/usb2spi/task_usb2spi/task_usb2spi.h new file mode 100644 index 0000000..614a69f --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/usb2spi/task_usb2spi/task_usb2spi.h @@ -0,0 +1,27 @@ +/** + * @file task_usb2spi.h + * + * @brief Header file for task_usb2spi.c + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef TASK_USB2SPI_H_ +#define TASK_USB2SPI_H_ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +void usb2spi_helper(void const *arg); +void usb2spi_terminate(void); + +#ifdef __cplusplus +} +#endif + +#endif /* TASK_USB2SPI_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/usb2spi/usb2spi/usb2spi.c b/bluetooth_uwb_dw3000_slotted_twr/src/apps/usb2spi/usb2spi/usb2spi.c new file mode 100644 index 0000000..692c948 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/usb2spi/usb2spi/usb2spi.c @@ -0,0 +1,318 @@ +/** + * @file usb2spi.c + * + * @brief Process to run USB2SPI Mode + * + * can be used in bare-metal or RTOS systems + * 1. select the CHIP to drive: setup_usb2spi_process(CHIP); + * 2. execute once init_usb2spi_process(); + * 3. continuously execute the run_usb2spi_process() on reception of + * data; + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ +#include "usb2spi.h" + +#include "port.h" +#include "port_common.h" +#include "usb_uart_rx.h" +#include "usb_uart_tx.h" +#include "deca_dbg.h" +// ----------------------------------------------------------------------------- + +/* Definitions for compatibility */ +#define USB2SPI_ENTER_CRITICAL() taskENTER_CRITICAL() +#define USB2SPI_EXIT_CRITICAL() taskEXIT_CRITICAL() + +#define USB2SPI_MALLOC pvPortMalloc +#define USB2SPI_FREE vPortFree + +#undef USB2SPI_USE_DYNAMIC_BUF +// ----------------------------------------------------------------------------- + +#define SOFTWARE_VER_STRINGUSB "EVB3000 USB2SPI 2.0" + +#define SPI_ACC_BUF_SIZE 0x3100 /**< Maximum size of buffer to read + * whole accumulator in the the + * USB2SPI */ + +/* Constants */ +const char deca[] = "deca?"; +const char dwSTOP[] = "stop"; +const char dwRESET[] = "reset"; + +extern void command_stop_received(void); + +// ----------------------------------------------------------------------------- +// local data +typedef struct +{ + uint8_t rx_buf[USB_RX_BUF_SIZE]; + uint8_t tx_buf[SPI_ACC_BUF_SIZE]; /**< need a long buffer for + * accumulator read */ + size_t tx_buf_length; +}usb2spi_t; + +#ifndef USB2SPI_USE_DYNAMIC_BUF +static usb2spi_t usb2spi_static; +#endif + +static usb2spi_t *pUsb2Spi; +// ----------------------------------------------------------------------------- + +// ----------------------------------------------------------------------------- +// Implementation + +/* + * @brief setup the speed for usb2spi: + * 0 for slow spi speed and 1 for high + **/ +// static +void configSPIspeed(int rate) +{ + static int localSPIspeed = -1; + + if (localSPIspeed != rate) { + localSPIspeed = rate; + if (localSPIspeed == 1) { + set_dw_spi_fast_rate(); + } else { + set_dw_spi_slow_rate(); + } + } +} + +/* + * @brief Decawave usb2spi standard protocol + * */ +static usb_data_e usb2spi(uint8_t *pBuf, uint16_t len) +{ + usb_data_e result = NO_DATA; + uint32_t msglength, datalength; + // first byte specifies the SPI speed and SPI read/write operation + // bit 0 = 1 for write, 0 for read + // bit 1 = 1 for high speed, 0 for low speed + + // + // + // to read from the device (e.g. 4 bytes), total length is = 1 + 1 + + // length_of_command (2) + length_of_data_to_read (2) + header length (1/2) + // + 1 + // + // to write to the device (e.g. 4 bytes), total length is = 1 + 1 + + // length_of_command (2) + length_of_data_to_write (2) + header length (1/2) + // + data length + 1 + // + // LBS comes first: 0x2, 0x2, 0x7, 0x0, 0x04, 0x00, 0x3 + + if (len > 0) { + // 0x2 = STX - start of SPI transaction data + if ((len >= 7) && (pBuf[0] == Usb_Msg_Header)) { + msglength = ((uint32_t)pBuf[3]) << 8 | (uint32_t)pBuf[2]; + if (len < msglength) { + return(result); + } + datalength = ((uint32_t)pBuf[5]) << 8 | (uint32_t)pBuf[4]; + + configSPIspeed((pBuf[1] >> 1) & 0x1); // configure SPI speed + + if ((pBuf[1] & 0x1) == 0) { // SPI read + if ((datalength + 3) > (sizeof(pUsb2Spi->tx_buf) - 1)) { + return(result); + } + + pUsb2Spi->tx_buf[0] = Usb_Msg_Header; + pUsb2Spi->tx_buf[1] = 0x0; // no error + pUsb2Spi->tx_buf[datalength + 2] = Usb_Msg_Footer; + + // max data we can read in a single SPI transaction is BUFFLEN as the + // USB/VCP tx buffer is only 4096 bytes long + if (pBuf[msglength - 1] != Usb_Msg_Footer) { + pUsb2Spi->tx_buf[1] = 0x1; // if no ETX (0x3) indicate error + pUsb2Spi->tx_buf[2] = Usb_Msg_Footer; + datalength = 0; + } else { + // do the read from the SPI + readfromspi(msglength - 7, + &pBuf[6], + datalength, + &pUsb2Spi->tx_buf[2]); // result is stored in the tx_buf + } + + pUsb2Spi->tx_buf_length = datalength + 3; + result = DATA_SEND; + } else + if ((pBuf[1] & 0x1) == 1) { // SPI write + if (datalength > msglength - 7) { + return(result); + } + int headerlength = msglength - 7 - datalength; + + pUsb2Spi->tx_buf[0] = Usb_Msg_Header; + pUsb2Spi->tx_buf[1] = 0x0; // no error + pUsb2Spi->tx_buf[2] = Usb_Msg_Footer; + + if ((pBuf[msglength - 1] != Usb_Msg_Footer) + || (headerlength < 0)) { + pUsb2Spi->tx_buf[1] = 0x1; // if no ETX (0x3) indicate error + } else { + // do the write to the SPI + writetospi(headerlength, + &pBuf[6], + datalength, + &pBuf[6 + headerlength]); // result is stored in the buffer + } + + pUsb2Spi->tx_buf_length = 3; + result = DATA_SEND; + } else { + } + } else + if ((len >= 5) && (memcmp(pBuf, deca, 5) == 0)) { + // send a reply 'y' + pUsb2Spi->tx_buf[0] = 'y'; + strcpy((char *)&pUsb2Spi->tx_buf[1], SOFTWARE_VER_STRINGUSB); + pUsb2Spi->tx_buf_length = strlen(SOFTWARE_VER_STRINGUSB) + 2; + + result = DATA_SEND; + } else + if ((len >= 5) && (memcmp(pBuf, dwRESET, 5) == 0)) {// Reset DW + reset_DW3000(); + } else + if ((len >= 4) && (memcmp(pBuf, dwSTOP, 4) == 0)) { + /* this should execute the fn_stop to exit the current process */ + command_stop_received(); // communication to the user application + result = NO_DATA; + } else { + // usb2spi_protocol_check take care of correct input + } + } + + return result; +} + +/* @fn usb2spi_frame_complete + * @brief Check if the data contains a complete USB2SPI frame + * @return DATA_READY if buffer contains a USB2SPI frame, DATA_ERROR + * otherwise + * */ +usb_data_e usb2spi_frame_complete(uint8_t *p, uint16_t len) +{ + usb_data_e ret = DATA_ERROR; + + if (len > 7) { + if ((p[0] == Usb_Msg_Header) && ((p[2] + (p[3] << 8)) == len) + && (p[len - 1] == Usb_Msg_Footer)) { + ret = DATA_READY; + } + } + + return ret; +} + +/* @fn usb2spi_protocol_check + * @brief Check if the data contains a valid USB2SPI command + * @return DATA_READY if buffer contains a USB2SPI command, DATA_ERROR + * otherwise + * */ +usb_data_e usb2spi_command_check(uint8_t *p, uint16_t len) +{ + usb_data_e ret = DATA_ERROR; + if (len < 4) { + return ret; + } + if (((len >= sizeof(deca) - 1) + && (memcmp(p, deca, sizeof(deca) - 1) == 0)) /* "deca?" */ + || ((len >= sizeof(dwSTOP) - 1) + && (memcmp(p, dwSTOP, sizeof(dwSTOP) - 1) == 0)) /* "stop" */ + || ((len >= sizeof(dwRESET) - 1) + && (memcmp(p, dwRESET, sizeof(dwRESET) - 1) == 0)) /* "reset" */ + ) { + ret = DATA_READY; + } + + return ret; +} + +// ----------------------------------------------------------------------------- +// IMPLEMETATION : interface to the process : bare-metal implementation + +/* + * @brief exporting functions to setup the bare-metal process. + * fn_init - executed once. + * fn_run - executed on reception of data. + * fn_stop - executed to terminate the process. + * */ +error_e usb2spi_process_init(void) +{ + error_e ret = _NO_ERR; + + if (pUsb2Spi) { + ret = _ERR_Usb2Spi_ptr_busy; + } else { +#if USB2SPI_USE_DYNAMIC_BUF + pUsb2Spi = USB2SPI_MALLOC(sizeof(usb2spi_t)); +#else + pUsb2Spi = &usb2spi_static; +#endif + if (!pUsb2Spi) { + ret = _ERR_Usb2Spi_ptr_alloc; + } else { + app.local_buff_length = 0; + memset(&app.local_buff, 0, sizeof(app.local_buff)); + } + } + return (ret); +} + +/* + * @brief the "run" function implements usb2spi process functionality: + * it parses the app.local_buff: app.local_buff_length + * + * */ +void usb2spi_process_run(void) +{ + usb_data_e tmp; + tmp = usb2spi(app.local_buff, app.local_buff_length); + + if (tmp != NO_DATA) { + app.local_buff_length = 0; + + if (pUsb2Spi->tx_buf_length) { + port_tx_msg(pUsb2Spi->tx_buf, pUsb2Spi->tx_buf_length); + } + } +} + +/* + * @brief stop function implements the stop functionality if any suitable + * for current process + * which will be executed on reception of Stop command + * */ +void usb2spi_process_terminate(void) +{ + if (pUsb2Spi) { + USB2SPI_ENTER_CRITICAL(); + + port_stop_all_UWB(); + + strcpy((char *)pUsb2Spi->tx_buf, "Ok\r\n"); + + pUsb2Spi->tx_buf_length = strlen((char *)pUsb2Spi->tx_buf); + + port_tx_msg(pUsb2Spi->tx_buf, pUsb2Spi->tx_buf_length); + +#if USB2SPI_USE_DYNAMIC_BUF + USB2SPI_FREE(pUsb2Spi); +#endif + pUsb2Spi = NULL; + + USB2SPI_EXIT_CRITICAL(); + } +} + +// ----------------------------------------------------------------------------- diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/apps/usb2spi/usb2spi/usb2spi.h b/bluetooth_uwb_dw3000_slotted_twr/src/apps/usb2spi/usb2spi/usb2spi.h new file mode 100644 index 0000000..5b9dcae --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/apps/usb2spi/usb2spi/usb2spi.h @@ -0,0 +1,42 @@ +/** + * @file usb2spi.h + * + * @brief Header file for usb2spi + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef __INC_USB2SPI_H_ +#define __INC_USB2SPI_H_ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#include "usb_uart_rx.h" +#include +#include + +/* Typedefs & Enumerations */ +enum { + Usb_Msg_Header = 0x02, + Usb_Msg_Footer = 0x03 +}; + +#define USB2SPI_MAX_CMD_LEN 5 // Length of deca? + +usb_data_e usb2spi_frame_complete(uint8_t *p, uint16_t len); +usb_data_e usb2spi_command_check(uint8_t *p, uint16_t len); +error_e usb2spi_process_init(void); +void usb2spi_process_run(void); +void usb2spi_process_terminate(void); + +#ifdef __cplusplus +} +#endif + +#endif /* __INC_USB2SPI_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/config/config/config.c b/bluetooth_uwb_dw3000_slotted_twr/src/config/config/config.c new file mode 100644 index 0000000..5ac78b6 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/config/config/config.c @@ -0,0 +1,112 @@ +/* + * @file config.c + * + * @brief supports NVM and bss configuration sections: + * defaultFConfig : section in RAM, where default parameters are + * saved and is not re-writabele. + * FCONFIG_ADDR : section in NVM, where current parameters are + * saved and this is re-writabele. + * bssConfig : section in RAM, which is representing config + * data exist in FCONFIG_ADDR. + * + * application on startup shall load_bssConfig() : this will copy + * data from FCONFIG_ADDR -> tmpConfig + * Accessing to variables is by pointer get_pbssConfig(); + * + * if application wants to re-write data in FCONFIG_ADDR, use + * save_bssConfig(*newRamParametersBlock); + * + * NOTE: The code is very MCU dependent and save will work with + * nRF52840 only + * + * @author Decawave Software + * + * @attention Copyright 2018 (c) DecaWave Ltd, Dublin, Ireland. + * All rights reserved. + */ + +#include +#include + +#include "em_msc.h" +#include "sl_common.h" + +#include "deca_device_api.h" +#include "version.h" + +#include "config.h" + +#define STATIC_ASSERT(condition) typedef char assert[(condition) ? 1 : -1] + +// ------------------------------------------------------------------------------ +extern const param_block_t FConfig; +extern const param_block_t defaultFConfig; + +extern uint32_t __fconfig_start[]; +extern uint32_t __fconfig_end[]; + +/* run-time parameters block. + * + * This is the RAM image of the FCONFIG_ADDR . + * + * Accessible from application by get_pbssConfig function after load_bssConfig() + * + * */ +static param_block_t tmpConfig; + +// ------------------------------------------------------------------------------ +// Size checks +STATIC_ASSERT(sizeof(tmpConfig) == FCONFIG_SIZE); +STATIC_ASSERT(sizeof(FConfig) == FCONFIG_SIZE); +STATIC_ASSERT(sizeof(defaultFConfig) == FCONFIG_SIZE); +STATIC_ASSERT(sizeof(param_block_t) == FCONFIG_SIZE); +STATIC_ASSERT(FCONFIG_SIZE <= FLASH_PAGE_SIZE); + +// ------------------------------------------------------------------------------ +// Implementation + +/* + * @brief get pointer to run-time bss param_block_t block + * + * */ +param_block_t *get_pbssConfig(void) +{ + return &tmpConfig; +} + +/* @fn load_bssConfig + * @brief copy parameters from NVM to RAM structure. + * + * assumes that memory model in the MCU of .text and .bss are the same + * */ +void load_bssConfig(void) +{ + memcpy(&tmpConfig, &FConfig, FCONFIG_SIZE); +} + +/* @fn restore_bssConfig + * @brief copy parameters from default NVM section to RAM structure. + * + * assumes that memory model in the MCU of .text and .bss are the same + * */ +void restore_bssConfig(void) +{ + save_bssConfig(&defaultFConfig); + load_bssConfig(); +} + +/* @brief save pNewRamParametersBlock to FCONFIG_ADDR + * @return _NO_ERR for success and error_e code otherwise + * */ +error_e save_bssConfig(const param_block_t *pNewRamParametersBlock) +{ + MSC_Init(); + int sts = MSC_ErasePage(__fconfig_start); + sts |= + MSC_WriteWord(__fconfig_start, + pNewRamParametersBlock, + sizeof(*pNewRamParametersBlock)); + MSC_Deinit(); + + return (sts ? _ERR_Flash_Prog : _NO_ERR); +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/config/config/config.h b/bluetooth_uwb_dw3000_slotted_twr/src/config/config/config.h new file mode 100644 index 0000000..324ed75 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/config/config/config.h @@ -0,0 +1,50 @@ +/** + * @file config.h + * + * @brief Supports NVM and bss configuration sections: + * FConfig : section in NVM, where current parameters are saved and + * this is re-writabele. + * bssConfig : section in RAM, which is representing FConfig. + * + * Application on startup shall load_bssConfig() : this will copy + * FConfig -> bssConfig + * Accessing to variables is by pointer get_pbssConfig(); + * + * If application wants to re-write FConfig, use + * save_bssConfig(*newRamParametersBlock); + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef CONFIG_H_ +#define CONFIG_H_ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#define NETBOOT_FLASH 0 +#define APPLICATION_FLASH 1 + +typedef enum { + Netboot_Erase, + Config_Erase +}eFlashErase_e; + +#include "default_config.h" +#include "error.h" + +void load_bssConfig(void); +void restore_bssConfig(void); // require defaultFConfig +param_block_t *get_pbssConfig(void); +error_e save_bssConfig(const param_block_t *); /**< save to FConfig */ + +#ifdef __cplusplus +} +#endif + +#endif /* CONFIG_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/config/default_config/default_config.c b/bluetooth_uwb_dw3000_slotted_twr/src/config/default_config/default_config.c new file mode 100644 index 0000000..e342005 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/config/default_config/default_config.c @@ -0,0 +1,37 @@ +/* + * @file default_config.c + * + * @brief supports NVM and bss configuration sections: + * FConfig : section in NVM, where current parameters are saved and + * this is re-writabele. + * + * Application on startup shall load_bssConfig() : this will copy + * FConfig -> bssConfig + * Accessing to variables is by pointer get_pbssConfig(); + * + * If application wants to re-write FConfig, use + * save_bssConfig(*newRamParametersBlock); + * + * + * NOTE: The code is very MCU dependent and save will work with STM32F4 only + * + * Linker script shall define Flash sector1 ADDR_FLASH_SECTOR_1 as sections + * + * ".fconfig" @0x08004000 + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#include +#include +#include "default_config.h" + +/* Section ".fconfig" is defined in a linker file */ +const param_block_t FConfig __attribute__((section(".fConfig"))) \ + __attribute__((aligned(FCONFIG_SIZE))) = DEFAULT_CONFIG; + +const param_block_t defaultFConfig = DEFAULT_CONFIG; diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/config/default_config/default_config.h b/bluetooth_uwb_dw3000_slotted_twr/src/config/default_config/default_config.h new file mode 100644 index 0000000..57c1ae7 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/config/default_config/default_config.h @@ -0,0 +1,367 @@ +/** + * @file default_config.h + * + * @brief Default config file for NVM initialization + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef __DEFAULT_CONFIG_H__H__ +#define __DEFAULT_CONFIG_H__H__ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#include "deca_device_api.h" +#include "tag_list.h" + +/* UWB config */ +#define DEFAULT_CHANNEL 9 +#define DEFAULT_TXPREAMBLENGTH DWT_PLEN_64 +#define DEFAULT_RXPAC DWT_PAC8 +#define DEFAULT_PCODE 9 +#define DEFAULT_NSSFD 1 //! SFD type 0 to use standard 8 symbol + //! SFD, 1 to use non-standard 8 + //! symbol, 2 for non-standard 16 + //! symbol SFD and 3 for 4z 8 symbol + //! SDF type +#define DEFAULT_DATARATE DWT_BR_6M8 +#define DEFAULT_PHRMODE DWT_PHRMODE_STD +#define DEFAULT_PHRRATE DWT_PHRRATE_STD +#define DEFAULT_SFDTO (64 + 1 + 8 - 8) +#define DEFAULT_STS_MODE (DWT_STS_MODE_1 | DWT_STS_MODE_SDC) //!< STS + //!< mode +#define DEFAULT_STS_LENGTH DWT_STS_LEN_256 //!< STS length +#define DEFAULT_PDOA_MODE DWT_PDOA_M3 //!< pdoa mode: on SP1/3 + //!< Ipatov_64 + + //!< STS_256->PDoA_M3; if + //!< Ipatov_64 + STS_64 + //!< -> PDoA Mode 1 + +#define DEFAULT_STS_STATIC 1 //! 1 to re-load STS Key & IV after each + //! Rx & Tx:: TCFM, Listener + +/* run-time config */ +#define DEFAULT_AUTOSTART_NONE (0) +#define DEFAULT_NODE_ADDR 0x0001 /**< Addr16 */ +#define DEFAULT_PANID 0xDECA /**< PanID */ +#define DEFAULT_UART 1 /**< Use UART I/O instead of USB CDC + */ +#define DEFAULT_ANTD (513.484f * 1e-9 / DWT_TIME_UNITS) /*Total + * antenna + * delay*/ +#define DEFAULT_PDOAOFF 0 /**< Phase Differences offset */ +#define DEFAULT_RNGOFF 0 /**< Ranging offset */ +#define DEFAULT_PDOA_TEMP 0 /**< Linear temperature coefficient + * for PDOA, mrad. Diff of PDOA + * when temperature changed +20C + * deg centigrade */ +#define DEFAULT_ACCUM_READING 0 /**< */ +#define DEFAULT_DIAG_READING 0 /**< */ +#define DEFAULT_PHASECORR_EN 1 /**< enable antenna related phase + * correction polynomial */ +#define DEFAULT_REPORT_LEVEL 1 /**< what to use as output of TWR: + * 1:JSON, 2:Reduced, 3:Minimal */ +#define DEFAULT_DEBUG 0 /**< if 1, then the LED_RED used to + * show an error, if any */ + +/* Slot configuration */ +#define DEFAULT_SLOT_PERIOD_MS 5 /**< Slot period (ms), the best of + * what the implementation can + * achieve */ +#define DEFAULT_NUM_SLOTS 20 /**< Number of slots: cannot be + * shorter than + * MAX_KNOWN_TAG_LIST_SIZE */ +#define DEFAULT_SF_PERIOD_MS 100 /**< SuperFrame period: cannot be + * shorter than + * NUM_SLOTS*SLOT_PERIOD */ + +#if (DEFAULT_NUM_SLOTS < MAX_KNOWN_TAG_LIST_SIZE) +#error "error config: DEFAULT_NUM_SLOTS < MAX_KNOWN_TAG_LIST_SIZE" +#endif + +#if (DEFAULT_SF_PERIOD_MS < (DEFAULT_NUM_SLOTS * DEFAULT_SLOT_PERIOD_MS)) +#error \ + "error config: DEFAULT_SF_PERIOD_MS < (DEFAULT_NUM_SLOTS * DEFAULT_SLOT_PERIOD_MS)" +#endif + +/* Below are 2 parameters to be sent to the Tag in the Ranging Config message. + * Node uses these parameters to setup its receiver during TWR exchange with a + * Tag. + * Tag hardware should be capable of performing the timings below: + * */ +#define DEFAULT_TAG_POLL_TX_RESPONSE_RX_US (700) /**< time when Tag shall be + * ready to receive the + * node's Response after + * end of transmission of + * the Poll. + * This parameter is + * Node's HW/SW dependent, + * as the Node's + * application is not + * designed to meet + * shorter timings. + * this works in + * optimizations -O2 and + * -O3 */ + +#define DEFAULT_TAG_POLL_TX_FINAL_TX_US (2000) /**< time when Tag shall + * transmit Final's + * RMARKER, calculated + * from Tag's Poll's + * RMARKER. */ + +/* This configures the delay between end of Tag's Blink_TX and Tag start Rx of + * Ranging Config message. + * From Node's view this is a delay between end of reception of Blink's data and + * start of transmission of preamble of Ranging Config. + * Should be the same for Node and Tag. + * */ +#define DEFAULT_TAG_BLINK_TX_RC_RX_US (150 * 1000) // Changed to 150 ms to + // support + // compatibility with + // DecaRanging + // Application + +/* This configures the RX timeout while waiting for Ranging Config message */ +#define DEFAULT_TAG_RC_RX_TIMEOUT_US (500) + +/* IP, NETMASK & GATEWAY addresses */ +#define IP_ADDR0 (uint8_t) 192 +#define IP_ADDR1 (uint8_t) 168 +#define IP_ADDR2 (uint8_t) 1 +#define IP_ADDR3 (uint8_t) 100 + +/*Gateway Address*/ +#define GW_ADDR0 (uint8_t) 192 +#define GW_ADDR1 (uint8_t) 168 +#define GW_ADDR2 (uint8_t) 1 +#define GW_ADDR3 (uint8_t) 1 + +/*NETMASK*/ +#define NETMASK_ADDR0 (uint8_t) 255 +#define NETMASK_ADDR1 (uint8_t) 255 +#define NETMASK_ADDR2 (uint8_t) 0 +#define NETMASK_ADDR3 (uint8_t) 0 + +#define DEFAULT_USE_STATIC_IP (0) + +/* NVM PAGE for store configuration */ +#define FCONFIG_SIZE 0x400 /**< can be up to 0x800 */ + +/* Default configuration initialization */ +#define DEFAULT_CONFIG \ + { \ + .dwt_config.chan = DEFAULT_CHANNEL, \ + .dwt_config.txPreambLength = DEFAULT_TXPREAMBLENGTH, \ + .dwt_config.rxPAC = DEFAULT_RXPAC, \ + .dwt_config.txCode = DEFAULT_PCODE, \ + .dwt_config.rxCode = DEFAULT_PCODE, \ + .dwt_config.sfdType = DEFAULT_NSSFD, \ + .dwt_config.dataRate = DEFAULT_DATARATE, \ + .dwt_config.phrMode = DEFAULT_PHRMODE, \ + .dwt_config.phrRate = DEFAULT_PHRRATE, \ + .dwt_config.sfdTO = DEFAULT_SFDTO, \ + .dwt_config.stsMode = DEFAULT_STS_MODE, \ + .dwt_config.stsLength = DEFAULT_STS_LENGTH, \ + .dwt_config.pdoaMode = DEFAULT_PDOA_MODE, \ + \ + .v.ver0 = 0xFFFFFFFF, \ + .v.ver1 = 0xFFFFFFFF, \ + .v.ver2 = 0xFFFFFFFF, \ + .v.ver3 = 0xFFFFFFFF, \ + .static_config.addr1 = IP_ADDR0, \ + .static_config.addr2 = IP_ADDR1, \ + .static_config.addr3 = IP_ADDR2, \ + .static_config.addr4 = IP_ADDR3, \ + .static_config.gw_addr1 = GW_ADDR0, \ + .static_config.gw_addr2 = GW_ADDR1, \ + .static_config.gw_addr3 = GW_ADDR2, \ + .static_config.gw_addr4 = GW_ADDR3, \ + .static_config.nm_addr1 = NETMASK_ADDR0, \ + .static_config.nm_addr2 = NETMASK_ADDR1, \ + .static_config.nm_addr3 = NETMASK_ADDR2, \ + .static_config.nm_addr4 = NETMASK_ADDR3, \ + .static_config.use_static_ip = DEFAULT_USE_STATIC_IP, \ + \ + .s.sfConfig.slotPeriod = DEFAULT_SLOT_PERIOD_MS, \ + .s.sfConfig.numSlots = DEFAULT_NUM_SLOTS, \ + .s.sfConfig.sfPeriod_ms = DEFAULT_SF_PERIOD_MS, \ + .s.sfConfig.tag_replyDly_us = DEFAULT_TAG_POLL_TX_RESPONSE_RX_US, \ + .s.sfConfig.tag_pollTxFinalTx_us = DEFAULT_TAG_POLL_TX_FINAL_TX_US, \ + \ + .s.txConfig.PGdly = 0x34, \ + .s.txConfig.power = 0xfdfdfdfdUL, \ + .s.txConfig.PGcount = 0, \ + \ + .s.addr = DEFAULT_NODE_ADDR, \ + .s.panID = DEFAULT_PANID, \ + .s.uartEn = DEFAULT_UART, \ + .s.pdoaOffset_deg = DEFAULT_PDOAOFF, \ + .s.rngOffset_mm = DEFAULT_RNGOFF, \ + .s.pdoa_temp_coeff_mrad = DEFAULT_PDOA_TEMP, \ + .s.accEn = DEFAULT_ACCUM_READING, \ + .s.diagEn = DEFAULT_DIAG_READING, \ + .s.phaseCorrEn = DEFAULT_PHASECORR_EN, \ + .s.reportLevel = DEFAULT_REPORT_LEVEL, \ + .s.faultyRanges = 5, \ + .s.debugEn = DEFAULT_DEBUG, \ + .s.rcDelay_us = DEFAULT_TAG_BLINK_TX_RC_RX_US, \ + .s.rcRxTo_us = DEFAULT_TAG_RC_RX_TIMEOUT_US, \ + \ + .s.antRx_a = (uint16_t)(0.5 * DEFAULT_ANTD), \ + .s.antTx_a = (uint16_t)(0.5 * DEFAULT_ANTD), \ + .s.antRx_b = (uint16_t)(0.5 * DEFAULT_ANTD), \ + .s.fixed_pos_x_mm = (uint16_t)(0), \ + .s.fixed_pos_y_mm = (uint16_t)(0), \ + .s.fixed_pos_z_mm = (uint16_t)(1900), \ + \ + .s.default_event = 0L, \ + .s.lotId = 0UL, \ + .s.partId = 0UL, \ + \ + .s.stsKey.key0 = 0x14EB220FUL, \ + .s.stsKey.key1 = 0xF86050A8UL, \ + .s.stsKey.key2 = 0xD1D336AAUL, \ + .s.stsKey.key3 = 0x14148674UL, \ + .s.stsIv.iv0 = 0x1F9A3DE4UL, \ + .s.stsIv.iv1 = 0xD37EC3CAUL, \ + .s.stsIv.iv2 = 0xC44FA8FBUL, \ + .s.stsIv.iv3 = 0x362EEB34UL, \ + .s.stsStatic = DEFAULT_STS_STATIC, \ + .s.xtalTrim = (DEFAULT_XTAL_TRIM) \ + } + +struct sfConfig_s +{ + uint16_t slotPeriod; /**< Slot period (time for one tag to + * range) */ + uint16_t numSlots; /**< Number of slots used in the + * SuperFrame */ + uint16_t sfPeriod_ms; /**< SuperFrame period in ms */ + uint16_t tag_replyDly_us; /**< wait4response delay after end of + * tag's Poll transmission in us */ + uint16_t tag_pollTxFinalTx_us; /**< PollTxToFinalTx period, us (i.e. + * period from Poll's RMARKER to + * Final's RMARKER)*/ +}; + +typedef struct sfConfig_s sfConfig_t; + +/* holds a run-time parameters */ +struct run_s +{ + sfConfig_t sfConfig; /**< System configuration: SuperFrame + * description */ + dwt_txconfig_t txConfig; + uint16_t addr; /**< Node's address */ + uint16_t panID; /**< System PanID */ + uint8_t uartEn; /**< Use UART to output data simultaneously to + * USB */ + int16_t pdoaOffset_deg; /**< Calibration: the Phase Differences offset + */ + int16_t rngOffset_mm; /**< Calibration: the Ranging offset */ + int16_t pdoa_temp_coeff_mrad; /**< TODO: reserved for future */ + uint8_t accEn; /**< Enable CIR reading & reporting */ + uint8_t diagEn; /**< Enable Diagnostics reading & reporting */ + uint8_t phaseCorrEn; /**< Use correction curve to use in calculation + * PDOA->X-Y */ + uint8_t reportLevel; /**< 0 - no output, 1-JSON, 2-limited */ + uint8_t faultyRanges; /**< Tag config: number of faulty ranges after + * that Tag return back to a Discovery phase + */ + uint8_t debugEn; /**< Enable Red "error" Led and error_handler() + */ + uint32_t rcDelay_us; /**< Node&Tag delay between end reception of UWB + * blink and start transmission of UWB Ranging + * Config message */ + uint16_t rcRxTo_us; /**< Tag's Receiver timeout only to save power + * on non-reception of the Ranging Config */ + + uint16_t antRx_a; /**< antenna delay values for the left port */ + uint16_t antTx_a; /**< antenna delay values for the left port */ + uint16_t antRx_b; /**< antenna delay values for the right port */ + uint16_t fixed_pos_x_mm; /**< max 65_000 mm */ + uint16_t fixed_pos_y_mm; /**< max 65_000 mm */ + uint16_t fixed_pos_z_mm; /**< max 65_000 mm */ + + int default_event; /**< this even will be generated after the + * Reset: default is set in main(), also see + * the "save" command */ + + uint32_t lotId; // workaround for ES0 chip variant:: ES0 has no + // lotId + uint32_t partId; // workaround for ES0 chip variant:: ES0 has no + // partId + + dwt_sts_cp_key_t stsKey; /**< AES Key to be used to set the STS + */ + dwt_sts_cp_iv_t stsIv; /**< AES IV to be used to set the + * initial IV */ + int stsStatic; /**< Configuration to STS/IV : value 0 = + * dynamic STS, 1 = fixed STS*/ + uint8_t xtalTrim; +} __attribute__((__packed__)); + +typedef struct run_s run_t; + +struct ver_num_s +{ + uint32_t ver0; + uint32_t ver1; + uint32_t ver2; + uint32_t ver3; +} __attribute__((__packed__)); + +typedef struct ver_num_s ver_num_t; + +struct static_ip_s +{ + uint8_t addr1; + uint8_t addr2; + uint8_t addr3; + uint8_t addr4; + + uint8_t gw_addr1; + uint8_t gw_addr2; + uint8_t gw_addr3; + uint8_t gw_addr4; + + uint8_t nm_addr1; + uint8_t nm_addr2; + uint8_t nm_addr3; + uint8_t nm_addr4; + + uint8_t use_static_ip; +} __attribute__((__packed__)); + +typedef struct static_ip_s static_ip_t; + +/* The structure, holding the changeable configuration of the application + * */ +struct param_block_s +{ + dwt_config_t dwt_config; /**< Standard Decawave driver config */ + tag_addr_slot_t knownTagList[MAX_KNOWN_TAG_LIST_SIZE]; + ver_num_t v; /**< App version */ + static_ip_t static_config; /**< Static IP config */ + run_t s; /**< Run-time parameters */ + uint8_t free[FCONFIG_SIZE + - sizeof(dwt_config_t) + - (sizeof(tag_addr_slot_t) * MAX_KNOWN_TAG_LIST_SIZE) + - sizeof(ver_num_t) - sizeof(static_ip_t) + - sizeof(run_t)]; +} __attribute__((__packed__)); + +typedef struct param_block_s param_block_t; + +#ifdef __cplusplus +} +#endif + +#endif /* __DEFAULT_CONFIG__H__ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/core/command/cmd/cmd.c b/bluetooth_uwb_dw3000_slotted_twr/src/core/command/cmd/cmd.c new file mode 100644 index 0000000..e330677 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/core/command/cmd/cmd.c @@ -0,0 +1,168 @@ +/** + * @file cmd.c + * + * @brief Command string as specified in document SWxxxx version X.x.x + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ +#include +#include +#include +#include "cmd.h" +#include "cmd_fn.h" +#include "config.h" +#include "usb_uart_tx.h" +#include "cJSON.h" + +/* + * Command interface + */ + +/* We want cJSON to use a definitive malloc/free */ +static const +cJSON_Hooks sCmdcJSON_hooks = +{ + .malloc_fn = CMD_MALLOC, + .free_fn = CMD_FREE +}; + +/* IMPLEMENTATION */ +void command_parser_init(void) +{ + cJSON_InitHooks((cJSON_Hooks *)&sCmdcJSON_hooks); +} + +/* + * @brief "error" will be sent if error during parser or command execution + * returned error + * */ +static void cmd_onERROR(const char *err) +{ + char *str = CMD_MALLOC(MAX_STR_SIZE); + + if (str) { + strcpy(str, "error \r\n"); + if (strlen(err) < (MAX_STR_SIZE - 6 - 3 - 1)) { + strcpy(&str[6], err); + strcpy(&str[6 + strlen(err)], "\r\n"); + } + port_tx_msg((uint8_t *)str, strlen(str)); + + CMD_FREE(str); + } +} + +/* @fn command_parser + * @brief checks if input "text" string in known "COMMAND" or "PARAMETER + * VALUE" format, + * checks their execution permissions, a VALUE range if restrictions + * and + * executes COMMAND or sets the PARAMETER to the VALUE + * */ +void command_parser(char *text) +{ + char *temp_str = text; + uint8_t cnt; + command_e equal; + int val = 0; + cJSON *json_root, *json_params; + char cmd[20]; + const char *ret; + + while (*temp_str) + { + *temp_str = (char)toupper(*temp_str); + temp_str++; + } + + /* Assume text may have more than one command inside. + * For example "getKLIST\nnode 0\n" : this will execute 2 commands. + * */ + text = strtok(text, "\n"); // get first token + + while (text != NULL) + { + equal = _NO_COMMAND; + cnt = 0; + json_params = NULL; + json_root = NULL; + cmd[0] = 0; // Initialize no command + + if (*text == '{') {// Probably a Json command + json_root = cJSON_Parse(text); + if (json_root != NULL) {// Got valid Json command + temp_str = + cJSON_GetObjectItem(json_root, + CMD_NAME)->valuestring; // Get command name + if (temp_str != NULL) {// Got right command name + json_params = + cJSON_GetObjectItem(json_root, + CMD_PARAMS); // Get command params + if (json_params != NULL) {// We have a Json so we need to update + // command. + sscanf(temp_str, "%9s", cmd); + } + } + } + } else {// It is not a Json command + sscanf(text, "%9s %d", cmd, &val); + } + + while (known_commands[cnt].cmnt != NULL) + { + if (known_commands[cnt].name + && (strcmp(cmd, known_commands[cnt].name) == 0)) { + equal = _COMMAND_FOUND; + + /* check command execution permissions. + * some commands can be executed only from Idle system mode: + * i.e. when no active processes are running. + * other commands can be executed at any time. + * */ + uint32_t mode = known_commands[cnt].mode & mMASK; + + if ((mode == app.mode) || (mode == mANY)) { + equal = _COMMAND_ALLOWED; + } + break; + } + cnt++; + } + + switch (equal) + { + case (_COMMAND_FOUND): + { + cmd_onERROR(" incompatible mode"); + break; + } + case (_COMMAND_ALLOWED): + { + /* execute corresponded fn() */ + param_block_t *pbss = get_pbssConfig(); + ret = known_commands[cnt].fn(text, pbss, val, json_params); + + if (ret) { + port_tx_msg((uint8_t *)ret, strlen(ret)); + } else { + cmd_onERROR(" function"); + } + break; + } + default: + break; + } + + if (json_root != NULL) { + cJSON_Delete(json_root); + } + + text = strtok(NULL, "\n"); + } +} + +/* end of cmd.c */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/core/command/cmd/cmd.h b/bluetooth_uwb_dw3000_slotted_twr/src/core/command/cmd/cmd.h new file mode 100644 index 0000000..3ec6aca --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/core/command/cmd/cmd.h @@ -0,0 +1,51 @@ +/** + * @file cmd.h + * + * @brief Header file for command.c + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef __CMD__H__ +#define __CMD__H__ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +/* command driver states */ +typedef enum +{ + _NO_COMMAND = 0, + _COMMAND_FOUND, + _COMMAND_ALLOWED +}command_e; + +/*Functionality modes names*/ +#define NODE_FUNC "NODE" +#define TAG_FUNC "TAG" +#define TRILAT_FUNC "TRILAT" +#define USPI_FUNC "USPI" +#define TCWM_FUNC "TCWM" +#define TCFM_FUNC "TCFM" +#define LISTENER_FUNC "LISTENER" + +/*For Json parsing*/ +#define CMD_NAME "CMD_NAME" +#define CMD_PARAMS "CMD_PARAMS" + +/* @fn command_driver + * @brief check if input text in known "COMMAND" or "PARAMETER=VALUE" format + * and executes COMMAND or set the PARAMETER to the VALUE + * */ +void command_parser(char *text); + +#ifdef __cplusplus +} +#endif + +#endif /* __CMD__H__ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/core/command/cmd_fn/cmd_fn.c b/bluetooth_uwb_dw3000_slotted_twr/src/core/command/cmd_fn/cmd_fn.c new file mode 100644 index 0000000..c1dbed2 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/core/command/cmd_fn/cmd_fn.c @@ -0,0 +1,1771 @@ +/** + * @file cmd_fn.c + * + * @brief Collection of executables functions from defined known_commands[] + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#include "cmd_fn.h" +#include "translate.h" +#include "version.h" +#include "config.h" +#include "task_flush.h" +#include "deca_dbg.h" +#include "port_common.h" +#include "usb_uart_tx.h" +#include "cmd.h" +#include "deca_device_api.h" +#include "em_gpio.h" + +#pragma GCC diagnostic ignored "-Wunused-parameter" + +#define CMD_COLUMN_WIDTH 10 +#define CMD_COLUMN_MAX 4 +// ----------------------------------------------------------------------------- +const char CMD_FN_RET_OK[] = "ok\r\n"; + +/***************************************************************************//* + * + * f_xx "command" FUNCTIONS + * + * REG_FN(f_node) macro will create a function + * + * const char *f_node(char *text, param_block_t *pbss, int val) + * + * */ + +// ----------------------------------------------------------------------------- +// Operation Mode change section + +/** @defgroup Application_Selection + * @brief Commands to start the particular application + * @{ + */ + +// communication to the user application +void command_stop_received(void) +{ + xEventGroupSetBits(app.xStartTaskEvent, Ev_Stop_All); +} + +/** + * @brief defaultTask will start PDoA NODE user application + * app.mode will be mNODE + * */ +REG_FN(f_node) +{ + xEventGroupSetBits(app.xStartTaskEvent, Ev_Node_Task); + return (CMD_FN_RET_OK); +} + +/** + * @brief defaultTask will start TAG user application + * app.mode will be mTAG + * */ +REG_FN(f_tag) +{ + xEventGroupSetBits(app.xStartTaskEvent, Ev_Tag_Task); + return (CMD_FN_RET_OK); +} + +/** + * @brief defaultTask will start USB2SPI user application + * app.mode will be mUspi + * */ +REG_FN(f_uspi) +{ + xEventGroupSetBits(app.xStartTaskEvent, Ev_Usb2spi_Task); + return(CMD_FN_RET_OK); +} + +/** + * @brief defaultTask will start TCWM user application + * app.mode will be mTcwm + * + * */ +REG_FN(f_tcwm) +{ + xEventGroupSetBits(app.xStartTaskEvent, Ev_Tcwm_Task); + return(CMD_FN_RET_OK); +} + +/** + * @brief defaultTask will start TCFM user application + * app.mode will be mTcfm + * */ +REG_FN(f_tcfm) +{ + char cmd[12]; + int n, nframes = 0, period = 0, nbytes = 0; + + n = sscanf(text, "%9s %d %d %d", cmd, &nframes, &period, &nbytes); + + switch (n) + { + case 1: + nframes = 1000000; + period = 1; + nbytes = 20; + break; + case 2: + period = 50; // 50ms + nbytes = 20; // 20 bytes packet + break; + case 3: + nbytes = 20; // 20 bytes packet + break; + default: + break; + } + + app.tcfm_info.period_ms = period; + app.tcfm_info.bytes = nbytes; + app.tcfm_info.nframes = nframes; + + xEventGroupSetBits(app.xStartTaskEvent, Ev_Tcfm_Task); + + return(CMD_FN_RET_OK); +} + +/** + * @brief defaultTask will start listener user application + * + * */ +REG_FN(f_listen) +{ + char cmd[12]; + int mode = 0; + + sscanf(text, "%9s %d", cmd, &mode); + + app.listener_mode = mode; + + xEventGroupSetBits(app.xStartTaskEvent, Ev_Listener_Task); + return(CMD_FN_RET_OK); +} + +/** + * @brief defaultTask will start the Trilateration task for the Node + * app.mode will be mTRILAT_N + * + * */ +REG_FN(f_trilat_n) +{ + xEventGroupSetBits(app.xStartTaskEvent, Ev_Trilat_N_Task); + return(CMD_FN_RET_OK); +} + +/** + * @brief defaultTask will stop all working threads + * app.mode will be mIDLE + * */ +REG_FN(f_stop) +{ + FlushTask_reset(); + port_tx_msg((uint8_t *)"\r\n", 2); + xEventGroupSetBits(app.xStartTaskEvent, Ev_Stop_All); + return (CMD_FN_RET_OK); +} + +/** + * @} + */ + +/** @defgroup Application_Parameters + * @brief Parameters change section : allowed only in app.mode = mIdle + * @{ + */ + +/** + * @brief set the 16bit (short) address + * */ +REG_FN(f_addr) +{ + pbss->s.addr = (uint16_t)(val); + return (CMD_FN_RET_OK); +} +REG_FN(f_panid) +{ + pbss->s.panID = (uint16_t)(val); + return (CMD_FN_RET_OK); +} +REG_FN(f_numSlots) +{ + pbss->s.sfConfig.numSlots = (uint16_t)(val); + return (CMD_FN_RET_OK); +} +REG_FN(f_slotPeriod) +{ + pbss->s.sfConfig.slotPeriod = (uint16_t)(val); + return (CMD_FN_RET_OK); +} +REG_FN(f_sfPeriod) +{ + pbss->s.sfConfig.sfPeriod_ms = (uint16_t)(val); + return (CMD_FN_RET_OK); +} +REG_FN(f_tag_replyDly_us) +{ + pbss->s.sfConfig.tag_replyDly_us = (uint16_t)(val); + return (CMD_FN_RET_OK); +} +REG_FN(f_tag_pollTxFinalTx_us) +{ + pbss->s.sfConfig.tag_pollTxFinalTx_us = (uint16_t)(val); + return (CMD_FN_RET_OK); +} +REG_FN(f_ant_tx_a) +{ + pbss->s.antTx_a = (uint16_t)(val); + return (CMD_FN_RET_OK); +} +REG_FN(f_ant_rx_a) +{ + pbss->s.antRx_a = (uint16_t)(val); + return (CMD_FN_RET_OK); +} +REG_FN(f_ant_rx_b) +{ + pbss->s.antRx_b = (uint16_t)(val); + return (CMD_FN_RET_OK); +} +REG_FN(f_pdoa_offset) +{ + pbss->s.pdoaOffset_deg = (int16_t)val; + return (CMD_FN_RET_OK); +} +REG_FN(f_rng_offset) +{ + pbss->s.rngOffset_mm = (int16_t)val; + return (CMD_FN_RET_OK); +} +REG_FN(f_phase_corr_enable) +{ + pbss->s.phaseCorrEn = (uint8_t)val; + return (CMD_FN_RET_OK); +} +REG_FN(f_pdoa_temp_coeff) +{ + pbss->s.pdoa_temp_coeff_mrad = (int16_t)val; + return (CMD_FN_RET_OK); +} +REG_FN(f_acc) +{ + pbss->s.accEn = (uint8_t)val; + return (CMD_FN_RET_OK); +} +REG_FN(f_dbg) +{ + pbss->s.debugEn = (uint8_t)(val); + return (CMD_FN_RET_OK); +} +REG_FN(f_diag) +{ + pbss->s.diagEn = (uint8_t)val; + return (CMD_FN_RET_OK); +} +REG_FN(f_uart) +{ + if (pbss->s.uartEn && !val) { + deca_uart_close(); + } else if (!pbss->s.uartEn && val) { + deca_uart_init(); + } + pbss->s.uartEn = (uint8_t)(val); + return (CMD_FN_RET_OK); +} +REG_FN(f_twr_report) +{ + pbss->s.reportLevel = (uint8_t)val; + return (CMD_FN_RET_OK); +} +REG_FN(f_rc_delay) +{ + pbss->s.rcDelay_us = (uint32_t)val; + return (CMD_FN_RET_OK); +} +REG_FN(f_x) +{ + pbss->s.fixed_pos_x_mm = (int16_t)val; + return (CMD_FN_RET_OK); +} +REG_FN(f_y) +{ + pbss->s.fixed_pos_y_mm = (int16_t)val; + return (CMD_FN_RET_OK); +} +REG_FN(f_z) +{ + pbss->s.fixed_pos_z_mm = (int16_t)val; + return (CMD_FN_RET_OK); +} +REG_FN(f_restore) +{ + CMD_ENTER_CRITICAL(); + restore_bssConfig(); + CMD_EXIT_CRITICAL(); + return (CMD_FN_RET_OK); +} + +/** + * @} + */ + +/** @defgroup Various_commands + * @brief This commands to control the PDoA Node application; + * test commands; + * @{ + */ + +/** + * @brief add all discovered tags to knownTagList + * this is automatic function for manual adding: + * it assign low u16 from addr64 as a new addr16 for every tag + * uses tag_Mode, tag_mSlow, tag_mFast as default parameters + */ +REG_FN(f_add_all_to_list) +{ + const char *ret = "All tags were added."; + + char *str = CMD_MALLOC(MAX_STR_SIZE); + + if (str) { + tag_addr_slot_t *tag; + uint64_t *pAddr64; + uint16_t size = getDList_size(); + + /* below can be removed if the d2k command, i.e. "add all discovered tags to + * known list automatically", is not used */ + uint16_t tag_mFast = 1; /**< Used to pass a "moving" + * refresh rate to all tags in + * the "d2k" command */ + uint16_t tag_mSlow = 2; /**< Used to pass a "stationary" + * refresh rate to all tags in + * the "d2k" command */ + uint16_t tag_Mode = 1; /**< Used to pass a common + * "mode" parameter to all + * tags in the "d2k" command + */ + + pAddr64 = getDList(); + + while ((*pAddr64 != 0) && (size > 0)) + { + tag = add_tag_to_knownTagList(*pAddr64, + (uint16_t)(*pAddr64), + tag_mFast, + tag_mSlow, + tag_Mode); + + if (!tag) { + sprintf(str, "Cannot add: list is full."); + sprintf(&str[strlen(str)], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + + ret = (NULL); // cannot add new tag, the knownTagList is + // full. + break; + } else { + tag->reqUpdatePending = 1; // enable update of Tag's + // configuration on its next + // Poll + } + + size--; + pAddr64++; + } + CMD_FREE(str); + } + initDList(); // clear Discovered Tag List + + return (const char *)(ret); +} + +REG_FN(f_decaid) +{ + diag_printf("Decawave device ID = 0x%8lx\r\n", dwt_readdevid()); + diag_printf("Decawave lotID = 0x%8lx, partID = 0x%8lx\r\n", + dwt_getlotid(), + dwt_getpartid()); + return (CMD_FN_RET_OK); +} + +REG_FN(f_led_glow) +{ + return(CMD_FN_RET_OK); +} + +REG_FN(f_user_button) +{ +#if !defined(HAL_IO_PORT_BUTTON) || !defined(HAL_IO_PIN_BUTTON) + diag_printf("Button is not available!\r\n"); +#else + uint32_t timer = 0; + start_timer(&timer); + diag_printf("Press the user button within 10 seconds\r\n"); + while (1) + { + if (GPIO_PinInGet(HAL_IO_PORT_BUTTON, + HAL_IO_PIN_BUTTON) == HAL_IO_BUTTON_ACTIVE_STATE) { + diag_printf("User Button pressed\r\n"); + break; + } + if (check_timer(timer, 15000)) { + diag_printf("User Button not pressed\r\n"); + break; + } + } +#endif + return(CMD_FN_RET_OK); +} + +REG_FN(f_uart_test) +{ + const char test_string[] = "Hello testing UART"; + uint16_t str_length = strlen(test_string), i = 0; // strlen(test_string); + diag_printf("UART Test, check the UART terminal\r\n"); + + // if (uart_enabled) + // { + // return(CMD_FN_RET_ERR); + // } + while (str_length > 0) + { + app_uart_put(test_string[i]); + str_length--; + i++; + } + return(CMD_FN_RET_OK); +} + +REG_FN(f_get_version) +{ + const char version[] = RTLS_APP_VERSION; + diag_printf("VERSION:%s\r\n", version); + return(CMD_FN_RET_OK); +} + +REG_FN(f_lstat) +{ + char *str = CMD_MALLOC(MAX_STR_SIZE); + + if (str) { + CMD_ENTER_CRITICAL(); + int hlen; + + /** Listener RX Event Counts object */ + hlen = sprintf(str, "JS%04X", 0x5A5A); // reserve space for length of + // JS object + sprintf(&str[strlen(str)], "{\"RX Events\":{\r\n"); + sprintf(&str[strlen(str)], "\"CRCG\":%d,\r\n", (int)app.event_counts.CRCG); + sprintf(&str[strlen(str)], "\"CRCB\":%d,\r\n", (int)app.event_counts.CRCB); + sprintf(&str[strlen(str)], "\"ARFE\":%d,\r\n", (int)app.event_counts.ARFE); + sprintf(&str[strlen(str)], "\"PHE\":%d,\r\n", (int)app.event_counts.PHE); + sprintf(&str[strlen(str)], "\"RSL\":%d,\r\n", (int)app.event_counts.RSL); + sprintf(&str[strlen(str)], "\"SFDTO\":%d,\r\n", + (int)app.event_counts.SFDTO); + sprintf(&str[strlen(str)], "\"PTO\":%d,\r\n", (int)app.event_counts.PTO); + sprintf(&str[strlen(str)], "\"FTO\":%d,\r\n", (int)app.event_counts.RTO); + sprintf(&str[strlen(str)], "\"STSE\":%d,\r\n", + (int)app.event_counts_sts_bad); + sprintf(&str[strlen(str)], + "\"STSG\":%d,\r\n", + (int)app.event_counts_sts_good); + sprintf(&str[strlen(str)], "\"SFDD\":%d}}", + (int)app.event_counts_sfd_detect); + sprintf(&str[2], "%04X", strlen(str) - hlen);// add formatted 4X of length, + // this will erase first '{' + str[hlen] = '{'; // restore the start bracket + sprintf(&str[strlen(str)], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + + CMD_FREE(str); + + memset((uint8_t *)&app.event_counts, 0, sizeof(app.event_counts)); + app.event_counts_sts_bad = 0; + app.event_counts_sts_good = 0; + app.event_counts_sfd_detect = 0; + + dwt_configeventcounters(1); // we need and can clear and enable counters + // in the chip + + CMD_EXIT_CRITICAL(); + } + return(CMD_FN_RET_OK); +} + +// ----------------------------------------------------------------------------- +// ----------------------------------------------------------------------------- +// JSON format for TagList section + +/** JSON reporting: + * + * Root elements: + * "NewTag": string (addr64) | on discovering of new Tag + * "DList" : array of strings (addr64) | reply to getDlist + * "KList" : array of tag objects | reply to getKlist + * "TagAdded" : tag object | reply to Add2List + * "TagDeleted" : string (addr64) | reply to delTag + * "TWR" : twr object | on calculation of new range + * "SN" : service object(accel) | on receiving data from IMU sensor + * + * + * */ + +/** @brief Discovered List + * + * 'JSxxxx{"DList": ["addr64_1","addr64_2","addr64_3"]}' + * + * */ +REG_FN(f_get_discovered_list) +{ + uint64_t *pAddr64; + uint16_t size; + int jlen, tmp; + + pAddr64 = getDList(); + size = getDList_size(); + + char *str = CMD_MALLOC(MAX_STR_SIZE); + + if (str) { + CMD_ENTER_CRITICAL(); + + /** 11+2 - minimum JSON length + * tmp=16+2 bytes per discovered tag: address64 plus quotes + * ',\r\n'=3 bytes per separation, if more than 1 elements + * we need to pre-calculate the length of json string for DList & KList : + * in order to keep malloc() small + */ + jlen = (11 + 2); + if (size > 0) { + tmp = 0; + tmp = strlen("\"1122334455667788\""); // 16+2 for every addr64 + jlen += (tmp + 3) * size - 3; + } + + sprintf(str, "JS%04X", jlen); // print pre-calculated + // length of JS object + sprintf(&str[strlen(str)], "{\"DList\":[ "); // +11 to json. + port_tx_msg((uint8_t *)str, strlen(str)); + + // DList cannot be with gaps + // if changed, will need to change the calculation of jlen + while (size > 0) + { + sprintf(str, "\"%08lX%08lX\"", + (uint32_t)(*pAddr64 >> 32), + (uint32_t)(*pAddr64)); // +18 to json + + if (size > 1) { + sprintf(&str[strlen(str)], ",\r\n"); // +3 to json + } + + port_tx_msg((uint8_t *)str, strlen(str)); + + pAddr64++; + size--; + } + + port_tx_msg((uint8_t *)"]}\r\n", 4); // +2 to json + + CMD_EXIT_CRITICAL(); + + initDList(); // clear the Discovered + // list + + CMD_FREE(str); + } + + return (CMD_FN_RET_OK); +} + +/** @brief This function shall return fixed length tag object staring + * 66 characters : do not change : see f_get_known_list + * */ +static void fill_json_tag(char *str, tag_addr_slot_t *tag) +{ + sprintf(str, + "{\"slot\":\"%04X\",\"a64\":\"%08lX%08lX\",\"a16\":\"%04X\"," + "\"F\":\"%04X\",\"S\":\"%04X\",\"M\":\"%04X\"}", + tag->slot, + (uint32_t)(tag->addr64 >> 32), + (uint32_t)(tag->addr64), + tag->addr16, + tag->multFast, + tag->multSlow, + tag->mode); +} + +/** @brief Known List + * + * 'JSxxxx{"KList": + * [ + * { + * "slot":, //hex + * "a64":, //address64, string + * "a16":, //address16, string + * "F":, //multFast, hex + * "S":, //multSlow, hex + * "M": //mode, hex + * }, + * { + * "slot":, //hex + * "a64":, //address64, string + * "a16":, //address16, string + * "F":, //multFast, hex + * "S":, //multSlow, hex + * "M": //mode, hex + * } + * ]}' + * + */ +REG_FN(f_get_known_list) +{ + tag_addr_slot_t *tag; + int jlen, size, tmp; + + char *str = CMD_MALLOC(MAX_STR_SIZE); + + if (str) { + CMD_ENTER_CRITICAL(); + + /** 16 bytes overhead for JSON + * 66 bytes per known tag + * 3 bytes per separation + * we need to pre-calculate the length of json string for DList & KList : + * in order to keep malloc() small + */ + tag = get_knownTagList(); + size = get_knownTagList_size(); + + jlen = (10 + 2); + + if (size > 0) { + tmp = 0; + fill_json_tag(str, tag); + tmp = strlen(str); // +NN to json for + // every known tag + jlen += (tmp + 3) * size - 3; + } + + sprintf(str, "JS%04X", jlen); // 6 print + // pre-calculated + // length of JS + // object + sprintf(&str[strlen(str)], "{\"KList\":["); // 10 + port_tx_msg((uint8_t *)str, strlen(str)); + + // KList can be with gaps, so need to scan it whole + for (int i = 0; i < MAX_KNOWN_TAG_LIST_SIZE; i++) + { + if (tag->slot != (uint16_t)(0)) { + fill_json_tag(str, tag); // NN + + if (size > 1) { // last element should not have ',\r\n' + sprintf(&str[strlen(str)], ",\r\n"); // 3 + } + + size--; + + port_tx_msg((uint8_t *)str, strlen(str)); + } + + tag++; + } + + port_tx_msg((uint8_t *)"]}\r\n", 4); + + CMD_EXIT_CRITICAL(); + + CMD_FREE(str); + } + + return (CMD_FN_RET_OK); +} + +/** + * add tag from incoming command to knownTagList + * + * report: + * 'JSxxxx{"TagAdded": + * { + * "slot":1, + * "a64":,//address64, string + * "a16":,//address16, string + * "F":, //multFast, int + * "S":, //multSlow, int + * "M": //mode, int + * } + * }' + * + * Note: sscanf needs at least 212 bytes from stack + * and it uses malloc but not RTOS' malloc + * + * */ +REG_FN(f_add_tag_to_list) +{ + const char *ret = NULL; + + char *str = CMD_MALLOC(MAX_STR_SIZE); + + if (str) { + tag_addr_slot_t *tag; + uint64_t addr64 = 0; + unsigned int addr1 = 0, addr2 = 0; + unsigned int addr16 = 0, multFast = 0, multSlow = 0, mode = 0, n = 1, + hlen; + char tmp[10]; + + /** "addtag 11AABB4455FF7788 10AA 1 2 1" */ + n = sscanf(text, "%8s %08x%08x %x %x %x %x", + tmp, &addr1, &addr2, &addr16, &multFast, &multSlow, &mode); + + if (!(multFast == 0) && !(multSlow == 0) && (n == 7)) { + addr64 = (uint64_t)((((uint64_t)addr1) << 32) | addr2); + + tag = add_tag_to_knownTagList(addr64, (uint16_t)addr16, 1, 1, 1); + + if (tag) { + tag->multFast = (uint16_t)multFast; + tag->multSlow = (uint16_t)multSlow; + tag->mode = (uint16_t)mode; + tag->reqUpdatePending = 1; // update Tag's + // configuration on + // its next Poll + + hlen = sprintf(str, "JS%04X", 0x5A5A); // reserve space for + // length of JS + // object + sprintf(&str[strlen(str)], "{\"TagAdded\": "); + + fill_json_tag(&str[strlen(str)], tag); + + sprintf(&str[strlen(str)], "}"); // \r\n + + sprintf(&str[2], "%04X", strlen(str) - hlen); // add formatted 4X of + // length, this will + // kill first '{' + str[hlen] = '{'; // restore the start + // bracket + + sprintf(&str[strlen(str)], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + + ret = CMD_FN_RET_OK; + } + } + + CMD_FREE(str); + } + + return (ret); +} + +/** + * @brief delete the tag addr64 from knownTagList + * the function will always report the tag was deleted. + * report: + * 'JSxxxx{"TagDeleted": + * //address64, string + * }' + * + * */ +REG_FN(f_del_tag_from_list) +{ + const char *ret = NULL; + + char *str = CMD_MALLOC(MAX_STR_SIZE); + + if (str) { + uint64_t addr64 = 0; + unsigned int addr1 = 0, addr2 = 0, hlen; + char tmp[10]; + + /** "delTag 11AABB4455FF7788" */ + sscanf(text, "%8s %08x%08x", tmp, &addr1, &addr2); + + addr64 = (uint64_t)((((uint64_t)addr1) << 32) | (uint64_t)addr2); + if (addr64 > 0xFFFF) { + del_tag64_from_knownTagList(addr64); + } else { + del_tag16_from_knownTagList((uint16_t)addr64); + } + + hlen = sprintf(str, "JS%04X", 0x5A5A); // reserve space for length of + // JS object + sprintf(&str[strlen(str)], "{\"TagDeleted\": \"%08x%08x\"}", addr1, addr2); + + sprintf(&str[2], "%04X", strlen(str) - hlen);// add formatted 4X of length, + // this will erase first '{' + str[hlen] = '{'; // restore the start bracket + sprintf(&str[strlen(str)], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + + ret = CMD_FN_RET_OK; + + CMD_FREE(str); + } + + return (const char *)(ret); +} + +/** @brief + * */ +REG_FN(f_decaJuniper) +{ + const char *ret = NULL; + const char ver[] = FULL_VERSION; + + char *str = CMD_MALLOC(MAX_STR_SIZE); + + if (str) { + int hlen; + + hlen = sprintf(str, "JS%04X", 0x5A5A); // reserve space for length of + // JS object + + sprintf(&str[strlen(str)], "{\"Info\":{\r\n"); + sprintf(&str[strlen(str)], "\"Device\":\"Juniper "); + sprintf(&str[strlen(str)], "%s\",\r\n", + (app.mode == mIDLE)?("STOP") + :(app.mode == mPNODE)?("PDoA NODE") + :(app.mode == mPTAG)?("PDoA TAG") + :(app.mode == mTCWM)?("TCWM") + :(app.mode == mTCFM)?("TCFM") + :(app.mode == mUSB2SPI)?("USB2SPI") + :(app.mode == mLISTENER)?(LISTENER_FUNC) + :("unknown")); + sprintf(&str[strlen(str)], "\"Version\":\"%s\",\r\n", ver); + sprintf(&str[strlen(str)], "\"Build\":\"%s %s\",\r\n", __DATE__, __TIME__); + + sprintf(&str[strlen(str)], + "\"Apps\":[\"%s\",\"%s\",\"%s\",\"%s\",\"%s\",\"%s\",\"%s\"],\r\n", + NODE_FUNC, + TAG_FUNC, + TRILAT_FUNC, + USPI_FUNC, + TCWM_FUNC, + TCFM_FUNC, + LISTENER_FUNC); + + sprintf(&str[strlen(str)], "\"Driver\":\"%s\"}}", dwt_version_string()); + + sprintf(&str[2], "%04X", strlen(str) - hlen); // add formatted 4X of + // length, this will erase + // first '{' + str[hlen] = '{'; // restore the start bracket + sprintf(&str[strlen(str)], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + + CMD_FREE(str); + ret = CMD_FN_RET_OK; + } + + return (ret); +} + +// ----------------------------------------------------------------------------- + +/** + * @brief show current mode of operation, + * version, and the configuration in JSON format + * Should be executed from STOP as + * + * */ +REG_FN(f_jstat) +{ + const char *ret = NULL; + + char *str = CMD_MALLOC(MAX_STR_SIZE); + + if (str) { + CMD_ENTER_CRITICAL(); + + int hlen; + + /** System Config object */ + hlen = sprintf(str, "JS%04X", 0x5A5A); // reserve space for length of + // JS object + sprintf(&str[strlen(str)], "{\"System Config\":{\r\n"); + sprintf(&str[strlen(str)], + "\"ADDR\":\"%04X\",\r\n", + (unsigned int)pbss->s.addr); + sprintf(&str[strlen(str)], + "\"PANID\":\"%04X\",\r\n", + (unsigned int)pbss->s.panID); + sprintf(&str[strlen(str)], + "\"NUMSLOT\":%d,\r\n", + (unsigned int)pbss->s.sfConfig.numSlots); + sprintf(&str[strlen(str)], + "\"SLOTPER\":%d,\r\n", + (unsigned int)pbss->s.sfConfig.slotPeriod); + sprintf(&str[strlen(str)], + "\"SFPER\":%d,\r\n", + (unsigned int)pbss->s.sfConfig.sfPeriod_ms); + sprintf(&str[strlen(str)], + "\"REPDEL\":%d,\r\n", + (unsigned int)pbss->s.sfConfig.tag_replyDly_us); + sprintf(&str[strlen(str)], + "\"P2FDEL\":%d,\r\n", + (unsigned int)pbss->s.sfConfig.tag_pollTxFinalTx_us); + sprintf(&str[strlen(str)], + "\"RCDEL\":%d}}", + (unsigned int)pbss->s.rcDelay_us); + + sprintf(&str[2], "%04X", strlen(str) - hlen);// add formatted 4X of length, + // this will erase first '{' + str[hlen] = '{'; // restore the start bracket + sprintf(&str[strlen(str)], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + + /** Run Time object */ + hlen = sprintf(str, "JS%04X", 0x5A5A); // reserve space for length of + // JS object + sprintf(&str[strlen(str)], "{\"Run Time\":{\r\n"); + sprintf(&str[strlen(str)], "\"UART\":%d,\r\n", pbss->s.uartEn); + sprintf(&str[strlen(str)], "\"PCREP\":%d}}", pbss->s.reportLevel); + sprintf(&str[2], "%04X", strlen(str) - hlen);// add formatted 4X of length, + // this will erase first '{' + str[hlen] = '{'; // restore the start bracket + sprintf(&str[strlen(str)], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + + /** Calibration object */ + hlen = sprintf(str, "JS%04X", 0x5A5A); // reserve space for length of + // JS object + sprintf(&str[strlen(str)], "{\"Calibration\":{\r\n"); + sprintf(&str[strlen(str)], "\"X_MM\":%d,\r\n", (int)pbss->s.fixed_pos_x_mm); + sprintf(&str[strlen(str)], "\"Y_MM\":%d,\r\n", (int)pbss->s.fixed_pos_y_mm); + sprintf(&str[strlen(str)], "\"Z_MM\":%d,\r\n", (int)pbss->s.fixed_pos_z_mm); + sprintf(&str[strlen(str)], "\"ANTTXA\":%d,\r\n", (int)pbss->s.antTx_a); + sprintf(&str[strlen(str)], "\"ANTRXA\":%d,\r\n", (int)pbss->s.antRx_a); + sprintf(&str[strlen(str)], "\"ANTRXB\":%d,\r\n", (int)pbss->s.antRx_b); + sprintf(&str[strlen(str)], + "\"PDOAOFF\":%d,\r\n", + (int)pbss->s.pdoaOffset_deg); + sprintf(&str[strlen(str)], "\"RNGOFF\":%d}}", (int)pbss->s.rngOffset_mm); + sprintf(&str[2], "%04X", strlen(str) - hlen);// add formatted 4X of length, + // this will erase first '{' + str[hlen] = '{'; // restore the start bracket + sprintf(&str[strlen(str)], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + + CMD_FREE(str); + + CMD_EXIT_CRITICAL(); + + ret = CMD_FN_RET_OK; + } + return (ret); +} + +/** + * @brief set or show current Key & IV parameters in JSON format + * @param no param - show current Key & IV + * correct scanned string - set the params and then show them + * incorrect scanned string - error + * + * */ +REG_FN(f_power) +{ + const char *ret = CMD_FN_RET_OK; + + char *str = CMD_MALLOC(MAX_STR_SIZE); + + if (str) { + /* Display the Key Config */ + int n, hlen; + + unsigned int pwr, pgDly; + int pgCnt; + + n = sscanf(text, "%9s 0X%08x 0X%08x 0X%08x", str, &pwr, &pgDly, &pgCnt); + + if (n == 4) { + pbss->s.txConfig.power = pwr; + pbss->s.txConfig.PGdly = pgDly; + pbss->s.txConfig.PGcount = pgCnt; + } else if (n != 1) { + ret = NULL; + } + + hlen = sprintf(str, "JS%04X", 0x5A5A); // reserve space for length of + // JS object + sprintf(&str[strlen(str)], "{\"TX POWER\":{\r\n"); + + sprintf(&str[strlen(str)], + "\"PWR\":\"0x%08X\",\r\n", + (unsigned int)pbss->s.txConfig.power); + sprintf(&str[strlen(str)], + "\"PGDLY\":\"0x%08X\",\r\n", + (unsigned int)pbss->s.txConfig.PGdly); + sprintf(&str[strlen(str)], + "\"PGCOUNT\":\"0x%08X\"}}", + (unsigned int)pbss->s.txConfig.PGcount); + + sprintf(&str[2], "%04X", strlen(str) - hlen); // add formatted 4X of + // length, this will erase + // first '{' + str[hlen] = '{'; // restore the start bracket + sprintf(&str[strlen(str)], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + + CMD_FREE(str); + } + return (ret); +} + +/** + * @brief set or show current Key & IV parameters in JSON format + * @param no param - show current Key & IV + * correct scanned string - set the params and then show them + * incorrect scanned string - error + * + * */ +REG_FN(f_stskeyiv) +{ + const char *ret = CMD_FN_RET_OK; + + char *str = CMD_MALLOC(MAX_STR_SIZE); + + if (str) { + /* Display the Key Config */ + int hlen, n; + + unsigned int key0, key1, key2, key3; + unsigned int iv0, iv1, iv2, iv3; + unsigned int sMode; + + n = sscanf(text, + "%9s 0X%08x%08x%08x%08x 0X%08x%08x%08x%08x %d", + str, + &key3, + &key2, + &key1, + &key0, + &iv3, + &iv2, + &iv1, + &iv0, + &sMode); + + if ((n == 9) || (n == 10)) { + pbss->s.stsStatic = sMode; + + pbss->s.stsIv.iv0 = iv0; + pbss->s.stsIv.iv1 = iv1; + pbss->s.stsIv.iv2 = iv2; + pbss->s.stsIv.iv3 = iv3; + + pbss->s.stsKey.key0 = key0; + pbss->s.stsKey.key1 = key1; + pbss->s.stsKey.key2 = key2; + pbss->s.stsKey.key3 = key3; + } else if (n != 1) { + ret = NULL; + } + + hlen = sprintf(str, "JS%04X", 0x5A5A); // reserve space for length of + // JS object + sprintf(&str[strlen(str)], "{\"STS KEY_IV\":{\r\n"); + + sprintf(&str[strlen(str)], "\"STS KEY\":\"0x%08X%08X%08X%08X\",\r\n", + (unsigned int)pbss->s.stsKey.key3, + (unsigned int)pbss->s.stsKey.key2, + (unsigned int)pbss->s.stsKey.key1, + (unsigned int)pbss->s.stsKey.key0); + sprintf(&str[strlen(str)], "\"STS IV\":\"0x%08X%08X%08X%08X\",\r\n", + (unsigned int)pbss->s.stsIv.iv3, + (unsigned int)pbss->s.stsIv.iv2, + (unsigned int)pbss->s.stsIv.iv1, + (unsigned int)pbss->s.stsIv.iv0); + sprintf(&str[strlen(str)], "\"STS_STATIC\":\"%d\"}}", + (int)pbss->s.stsStatic); + + sprintf(&str[2], "%04X", strlen(str) - hlen); // add formatted 4X of + // length, this will erase + // first '{' + str[hlen] = '{'; // restore the start bracket + sprintf(&str[strlen(str)], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + + CMD_FREE(str); + } + return (ret); +} + +/** + * @brief set or show current XTAL TRIM in JSON format + * @param no param - show current Xtal Trim code + * correct scanned string - set the Trim code + * incorrect scanned string - do not set XTAL TRIM + * + * */ +REG_FN(f_xtal_trim) +{ + const char *ret = CMD_FN_RET_OK; + + char *str = CMD_MALLOC(MAX_STR_SIZE); + + int n, xtalTrim; + + if (str) { + n = sscanf(text, "%9s 0X%02x", str, &xtalTrim); + + CMD_ENTER_CRITICAL(); + + if (n == 2) { + dwt_setxtaltrim((uint8_t)xtalTrim & 0x7F); + } else { + dwt_setxtaltrim(app.pConfig->s.xtalTrim & XTAL_TRIM_BIT_MASK); + xtalTrim = dwt_getxtaltrim() + | (app.pConfig->s.xtalTrim & ~XTAL_TRIM_BIT_MASK); + } + + app.pConfig->s.xtalTrim = xtalTrim; // it can have the 0x80 bit set to + // be able overwrite OTP values + // during APP starts. + + CMD_EXIT_CRITICAL(); + + /* Display the XTAL object */ + int hlen; + + hlen = sprintf(str, "JS%04X", 0x5A5A); // reserve space for length of + // JS object + sprintf(&str[strlen(str)], "{\"XTAL\":{\r\n"); + sprintf(&str[strlen(str)], "\"TEMP TRIM\":\"0x%02x\"}}", xtalTrim); + + sprintf(&str[2], "%04X", strlen(str) - hlen);// add formatted 4X of length, + // this will erase first '{' + str[hlen] = '{'; // restore the start bracket + sprintf(&str[strlen(str)], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + + CMD_FREE(str); + } + + return (ret); +} + +/** + * @brief set or show current UWB parameters in JSON format + * @param if cmd "UWBCFG" has no params, then show the current UWB config + * if it has correctly scanned string - set the UWB config and then show + * them + * if it has incorrect scanned string - indicate an error (however no + * error check would + * be performed, so all incorrect parameters would be set to UWB + * settings) + * + * */ +REG_FN(f_uwbcfg) +{ + const char *ret = CMD_FN_RET_OK; + + char *str = CMD_MALLOC(MAX_STR_SIZE); + + int n; + int chan; //!< Channel number (5 or 9) + int txPreambLength; //!< DWT_PLEN_64..DWT_PLEN_4096 + int rxPAC; //!< Acquisition Chunk Size (Relates to RX preamble length) + int txCode; //!< TX preamble code (the code configures the PRF, e.g. 9 + //!< -> PRF of 64 MHz) + int rxCode; //!< RX preamble code (the code configures the PRF, e.g. 9 + //!< -> PRF of 64 MHz) + int sfdType; //!< SFD type (0 for short IEEE 8-bit standard, 1 for DW + //!< 8-bit, 2 for DW 16-bit, 3 for 4z BPRF) + int dataRate; //!< Data rate {DWT_BR_850K or DWT_BR_6M8} + int phrMode; //!< PHR mode {0x0 - standard DWT_PHRMODE_STD, 0x3 - + //!< extended frames DWT_PHRMODE_EXT} + int phrRate; //!< PHR rate {0x0 - standard DWT_PHRRATE_STD, 0x1 - at + //!< datarate DWT_PHRRATE_DTA} + int sfdTO; //!< SFD timeout value (in symbols) + int stsMode; //!< STS mode (no STS, STS before PHR or STS after data) + int stsLength; //!< STS length (the allowed values are listed in + //!< dwt_sts_lengths_e + int pdoaMode; //!< PDOA mode + + if (str) { + n = sscanf(text, + "%9s %d %d %d %d %d %d %d %d %d %d %d %d %d", + str, + &chan, + &txPreambLength, + &rxPAC, + &txCode, + &rxCode, + &sfdType, + &dataRate, + &phrMode, + &phrRate, + &sfdTO, + &stsMode, + &stsLength, + &pdoaMode); + if (n == 14) {// set parameters :: this is unsafe, TODO :: add a range check + pbss->dwt_config.chan = chan_to_deca(chan); + pbss->dwt_config.txPreambLength = plen_to_deca(txPreambLength); + pbss->dwt_config.rxPAC = pac_to_deca(rxPAC); + pbss->dwt_config.dataRate = bitrate_to_deca(dataRate); + pbss->dwt_config.stsLength = sts_length_to_deca(stsLength); + + pbss->dwt_config.txCode = txCode; + pbss->dwt_config.rxCode = rxCode; + pbss->dwt_config.sfdType = sfdType; + pbss->dwt_config.phrMode = phrMode; + pbss->dwt_config.phrRate = phrRate; + pbss->dwt_config.sfdTO = sfdTO; + pbss->dwt_config.stsMode = stsMode; + pbss->dwt_config.pdoaMode = pdoaMode; + } else if (n != 1) { + ret = NULL; // produce an error + } + + /* Display the UWB Config object */ + int hlen; + + hlen = sprintf(str, "JS%04X", 0x5A5A); // reserve space for length of + // JS object + sprintf(&str[strlen(str)], "{\"UWB PARAM\":{\r\n"); + + sprintf(&str[strlen(str)], "\"CHAN\":%d,\r\n", + deca_to_chan(pbss->dwt_config.chan)); + sprintf(&str[strlen(str)], "\"PLEN\":%d,\r\n", + deca_to_plen(pbss->dwt_config.txPreambLength)); + sprintf(&str[strlen(str)], "\"PAC\":%d,\r\n", + deca_to_pac(pbss->dwt_config.rxPAC)); + sprintf(&str[strlen(str)], "\"TXCODE\":%d,\r\n", pbss->dwt_config.txCode); + sprintf(&str[strlen(str)], "\"RXCODE\":%d,\r\n", pbss->dwt_config.rxCode); + sprintf(&str[strlen(str)], "\"SFDTYPE\":%d,\r\n", pbss->dwt_config.sfdType); + sprintf(&str[strlen(str)], "\"DATARATE\":%d,\r\n", + deca_to_bitrate(pbss->dwt_config.dataRate)); + sprintf(&str[strlen(str)], "\"PHRMODE\":%d,\r\n", pbss->dwt_config.phrMode); + sprintf(&str[strlen(str)], "\"PHRRATE\":%d,\r\n", pbss->dwt_config.phrRate); + sprintf(&str[strlen(str)], "\"SFDTO\":%d,\r\n", pbss->dwt_config.sfdTO); + sprintf(&str[strlen(str)], "\"STSMODE\":%d,\r\n", pbss->dwt_config.stsMode); + sprintf(&str[strlen(str)], "\"STSLEN\":%d,\r\n", + deca_to_sts_length(pbss->dwt_config.stsLength)); + sprintf(&str[strlen(str)], "\"PDOAMODE\":%d}}", pbss->dwt_config.pdoaMode); + + sprintf(&str[2], "%04X", strlen(str) - hlen);// add formatted 4X of length, + // this will erase first '{' + str[hlen] = '{'; // restore the start bracket + sprintf(&str[strlen(str)], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + + CMD_FREE(str); + } + return (ret); +} + +/** + * @brief show current mode of operation, + * version, and the configuration + * + * */ +REG_FN(f_stat) +{ + const char *ret = CMD_FN_RET_OK; + + char *str = CMD_MALLOC(MAX_STR_SIZE); + + if (str) { + sprintf(str, "MODE: %s\r\n" + "LAST ERR CODE: %d\r\n" + "MAX MSG LEN: %d\r\n", + (app.mode == mIDLE)?("STOP") + :(app.mode == mPNODE)?("PDoA NODE") + :(app.mode == mPTAG)?("PDoA TAG") + :(app.mode == mTCWM)?("TCWM") + :(app.mode == mTCFM)?("TCFM") + :(app.mode == mUSB2SPI)?("USB2SPI") + :(app.mode == mLISTENER)?("LISTENER") + :(app.mode == mTRILAT_N)?("TRILAT") + :("UNKNOWN"), + app.lastErrorCode, + app.maxMsgLen); + + port_tx_msg((uint8_t *)str, strlen(str)); + + CMD_FREE(str); + app.lastErrorCode = 0; + app.maxMsgLen = 0; + + f_decaJuniper(NULL, pbss, 0, NULL); + f_jstat(NULL, pbss, 0, NULL); + f_get_discovered_list(NULL, NULL, 0, NULL); + f_get_known_list(NULL, NULL, 0, NULL); + } + + ret = CMD_FN_RET_OK; + return (ret); +} + +/** + * @brief Show all available commands + * + * */ +REG_FN(f_help_std) +{ + int indx = 0, cnt = 0; + const char *ret = NULL; + char *str = CMD_MALLOC(MAX_STR_SIZE); + + if (str) { + CMD_ENTER_CRITICAL(); + + while (known_commands[indx].cmnt != NULL) + { + uint32_t mode = known_commands[indx].mode; + + if (((mode & mMASK) == app.mode) || ((mode & mMASK) == mANY) + || (mIDLE == app.mode)) { + switch (mode & mCmdGrpMASK) + { + case mCmdGrp0: + if (cnt > 0) { + sprintf(&str[cnt], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + cnt = 0; + } + + /*print the Group name */ + sprintf(str, "---- %s---\r\n", known_commands[indx].cmnt); + port_tx_msg((uint8_t *)str, strlen(str)); + break; + + case mCmdGrp1: + /* print appropriate list of parameters for the current application + */ + if (known_commands[indx].name) { + sprintf(&str[cnt], "%-10s", known_commands[indx].name); + cnt += CMD_COLUMN_WIDTH; + if (cnt >= CMD_COLUMN_WIDTH * CMD_COLUMN_MAX) { + sprintf(&str[cnt], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + cnt = 0; + } + } + break; + + case mCmdGrp2: + case mCmdGrp3: + case mCmdGrp4: + case mCmdGrp5: + case mCmdGrp6: + case mCmdGrp7: + case mCmdGrp8: + case mCmdGrp9: + case mCmdGrp10: + case mCmdGrp11: + case mCmdGrp12: + case mCmdGrp13: + case mCmdGrp14: + /*reserved for the future*/ + default: + break; + } + } + + indx++; + } + + sprintf(&str[cnt], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + + CMD_EXIT_CRITICAL(); + + CMD_FREE(str); + ret = CMD_FN_RET_OK; + } + + return (ret); +} + +/* + * @brief This fn() displays the help information to the function, + * i.e. comment field. + * usage: "help help", "help tag" etc + * + * */ +REG_FN(f_help_help) +{ + int indx = 0, n = 0; + const char *ret = NULL; + char *str = CMD_MALLOC(MAX_STR_SIZE); + + if (str) { + CMD_ENTER_CRITICAL(); + + while (known_commands[indx].cmnt != NULL) + { + uint32_t mode = known_commands[indx].mode; + + if (((mode & mMASK) == app.mode) || ((mode & mMASK) == mANY) + || (mIDLE == app.mode)) { + if (strcmp(known_commands[indx].name, text) == 0) { + sprintf(str, "\r\n%s:\r\n", text); + port_tx_msg((uint8_t *)str, strlen(str)); + + const char *ptr = known_commands[indx].cmnt; + + while (ptr) + { + // CMD_COLUMN_WIDTH*CMD_COLUMN_MAX + n = snprintf(str, 78, "%s", ptr); + sprintf(&str[strlen(str)], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + if (n < 78) { + ptr = NULL; + } else { + ptr += 77; + } + } + + break; + } + } + + indx++; + } + + sprintf(str, "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + + CMD_EXIT_CRITICAL(); + + CMD_FREE(str); + ret = CMD_FN_RET_OK; + } + + return ret; +} + +/** + * @brief Show all available commands + * + * */ +REG_FN(f_help_app) +{ + char help[12]; + char cmd[12]; + int n; + const char *ret = NULL; + + n = sscanf(text, "%9s %10s", cmd, help); + + switch (n) + { + case 1: + ret = f_help_std(cmd, pbss, val, params); + break; + case 2: + if (help[0] == 0) { + ret = f_help_help(&help[1], pbss, val, params); + } else { + ret = f_help_help(&help[0], pbss, val, params); + } + break; + default: + break; + } + + return (ret); +} + +// ----------------------------------------------------------------------------- +// Communication change section + +/** + * @brief save configuration + * + * */ +REG_FN(f_save) +{ + error_e err_code; + + CMD_ENTER_CRITICAL(); + + switch (app.mode) + { + case mPNODE: + app.pConfig->s.default_event = Ev_Node_Task; + break; + case mPTAG: + app.pConfig->s.default_event = Ev_Tag_Task; + break; + case mTRILAT_N: + app.pConfig->s.default_event = Ev_Trilat_N_Task; + break; + default: + app.pConfig->s.default_event = 0; // default app is defined in the + // main(); + break; + } + + err_code = save_bssConfig(pbss); + + CMD_EXIT_CRITICAL(); + + if (err_code != _NO_ERR) { + error_handler(0, err_code); // not a fatal error + return (NULL); + } + + return (CMD_FN_RET_OK); +} + +/** + * @} + */ + +// ----------------------------------------------------------------------------- + +/** end f_xx command functions */ + +const char COMMENT_PDOA_NODE_OPT[] = { "PDoA Node Options ----" }; +const char COMMENT_PDOA_NODE_CMD[] = { "PDoA Node commands ---" }; +const char COMMENT_PDOA_TAG_OPT[] = { "PDoA Tag Options -----" }; +const char COMMENT_LISTENER_OPT[] = { "LISTENER Options -----" }; +const char COMMENT_ANYTIME_OPTIONS[] = { "Anytime commands -----" }; +const char COMMENT_APPSELECTION[] = { "Application selection " }; +const char COMMENT_SERVICE[] = { "Service commands -----" }; + +const char STD_CMD_COMMENT[] = +{ "This command described in the documentation" }; + +const char COMMENT_RCDEL[] = +{ + "Time, \'us\', between end of Tag's Blink_TX and Tag start Rx of Ranging Config message. From Node's view this is a delay between end of reception of Blink's data and start of transmission of preamble of the Ranging Config message. This time should be set to the same value on the Tag and on the Node." +}; +const char COMMENT_REPDEL[] = +{ + "Time, \'us\', when Tag shall be ready to receive the Node's Response after end of transmission of the Poll (wait4response). This parameter is defined by the Node and sent to the Tag in the Ranging Config message." +}; +const char COMMENT_P2FDEL[] = +{ + "Time, \'us\', when Tag shall transmit Final's RMARKER, calculated from Tag's Poll's RMARKER. This parameter is defined by the Node and sent to the Tag in the Ranging Config message." +}; + +const char COMMENT_STOP[] = { "Stops running any top-level applications" }; +const char COMMENT_STAT[] = { "Displays the Status information" }; +const char COMMENT_SAVE[] = { "Saves the configuration to the NVM" }; +const char COMMENT_DECAJUNIPER[] = +{ "This command reports the running application and the version information" }; +const char COMMENT_HELP[] = +{ + "This command displays the help information. Usage: \"help\" or \"help \", is the command from the list, i.e. \"help tag\"." +}; + +const char COMMENT_NODE[] = { "Slotted DS-TWR PDoA Node application." }; +const char COMMENT_TAG[] = { "Slotted DS-TWR Tag application." }; +const char COMMENT_TRILAT[] = +{ + "Trilateration application runs on top of the Node application. It requires at least 3 Tags with configured and placed to X-Y-Z corners of the room to demonstarte the Trilateration Location Solver." +}; +const char COMMENT_USPI[] = +{ + "Starts the USB2SPI mode. This is a special binary protocol to read/write registers of DW chip using external application. DecaRanging application prior to 3.24 require this mode to be started manually." +}; +const char COMMENT_TCWM[] = +{ + "Test Continuous Wave mode generates the base frequency of the corresponded channel on the RF port." +}; +const char COMMENT_TCFM[] = +{ + "Test Continuous Frame mode is to transmit packets for test purposes. Usage: \"tcfm \" >: number of packets to transmit. : pause in between packets in ms. : length of the transmit payload, bytes\r\n" +}; +const char COMMENT_LISTENER[] = +{ + "Listen for the UWB packjets using the UWB configuration.\r\nUsage: \"Listener \" : if present or 0, this is priority of speed. In this mode Listener will output maximum six first bytes from the input string. With set to 1, the priority is on data and listener will output maximum 127 bytes of a payload." +}; + +const char COMMENT_LSTAT[] = +{ "Displays the statistics inside the Listener application." }; + +const char COMMENT_X[] = +{ + "X-coordinate of this Tag in the room, mm. Used only in the tag, which is a part of a Trilateration example running on the fift unit." +}; +const char COMMENT_Y[] = +{ + "Y-coordinate of this Tag in the room, mm. Used only in the tag, which is a part of a Trilateration example running on the fift unit." +}; +const char COMMENT_Z[] = +{ + "Z-coordinate of this Tag in the room, mm. Used only in the tag, which is a part of a Trilateration example running on the fift unit." +}; + +const char COMMENT_ADDR[] = +{ "Sets the 16-bit address to the Node.\r\nUsage: \" \"" }; +const char COMMENT_PANID[] = +{ + "Sets the 16-bit PanID for the Node.Node will use this PANID to setup the Tag. \r\nUsage: \" \"" +}; +const char COMMENT_NUMSLOT[] = +{ + "Sets the number of Slots used for Ranging. \r\nUsage: \"numslot 20\". Cannot be smaller than MAX_KNOWN_TAG_LIST_SIZE" +}; +const char COMMENT_SLOTPER[] = +{ "Sets the slot period in ms.\r\nUsage: \"slotper 5\"" }; +const char COMMENT_SFPER[] = +{ + "Sets the superframe period in ms.\r\nUsage: \"sfper 100\". Cannot be smaller than NUM_SLOTS*SLOT_PERIOD" +}; + +const char COMMENT_JSTAT[] = { "Status in JSON format" }; +const char COMMENT_GETDLIST[] = +{ "Discovered Tags's list in JSON format." }; +const char COMMENT_GETKLIST[] = { "Known Tags list in JSON format." }; +const char COMMENT_ADDTAG[] = +{ + "Add the tag to Known list using specified parameters.\r\nUsage: \"addtag \"" +}; +const char COMMENT_DELTAG[] = +{ + "Delete tag from Known Tags list\r\nUsage: \"deltag \"" +}; +const char COMMENT_PDOAOFF[] = +{ "Phase Difference offset for this Node\r\nUsage: \" \"" }; +const char COMMENT_RNGOFF[] = { "Range offset for this node \r\nUsage: \" \"" }; +const char COMMENT_PDOATEMP[] = { "TBD" }; +const char COMMENT_PHCORREN[] = { "TBD" }; + +const char COMMENT_UART[] = +{ + "Switch the interface between UART and USB CDC.\r\rUsage: \"uart \"\r\n ENABLE_UART: 1 to use UART, 0 to use USB CDC" +}; +const char COMMENT_ANTTXA[] = { "Antenna TX delay \r\nUsage: \"anttx \"" }; +const char COMMENT_ANTRXA[] = +{ "Antenna RX delay \r\nUsage: \"antrxa \"" }; +const char COMMENT_ANTRXB[] = { "TBD" }; + +const char COMMENT_RESTORE[] = +{ "Restores the default configuration, both UWB and System." }; +const char COMMENT_PCREP[] = { "Report level to the COM port" }; +const char COMMENT_D2K[] = +{ "Service command to add all Tag from the DList to the KList" }; +const char COMMENT_DIAG[] = { "TBD" }; +const char COMMENT_ACCUM[] = { "TBD" }; + +const char COMMENT_UWBCFG[] = +{ + "UWB configuration\r\nUsage: To see UWB parameters \"uwbcfg\". To set the UWB config, list the parameters as a string argument \"uwbcfg \"" +}; +const char COMMENT_STSKEYIV[] = +{ + "Sets STS Key, IV and their behavior mode.\r\nUsage: To see STS KEY, IV and Mode \"stskeyiv\". To set\"stskeyiv 0x 0x \".\r\n: 1 use fixed STS (Default), 0 use dynamic STS" +}; +const char COMMENT_XTALTRIM[] = +{ + "Xtal trimming value.\r\nUsage: To see Crystal Trim value \"xtaltrim\". To set the Crystal trim value [0..7F] \"uwbcfg 0x\"" +}; +const char COMMENT_TXPOWER[] = +{ + "Tx Power settings.\r\nUsage: To see Tx power \"txpower\". To set the Tx power \"txpower 0x 0x 0x\"" +}; + +const char COMMENT_DECAID[] = { "TBD" }; +const char COMMENT_VERSION[] = { "Shows version of the SW" }; + +const char COMMENT_DEBUGLED[] = { "TBD" }; + +// ----------------------------------------------------------------------------- + +/** list of known commands: + * NAME, allowed_MODE, REG_FN(fn_name) + * */ +const struct command_s known_commands[] = { + /** CMDNAME MODE fn comment */ + /** Anytime commands */ + { NULL, mCmdGrp0 | mANY, NULL, COMMENT_ANYTIME_OPTIONS }, + { "STOP", mCmdGrp1 | mANY, f_stop, COMMENT_STOP }, + { "STAT", mCmdGrp1 | mANY, f_stat, COMMENT_STAT }, + { "SAVE", mCmdGrp1 | mANY, f_save, COMMENT_SAVE }, + { "DECA$", mCmdGrp1 | mANY, f_decaJuniper, COMMENT_DECAJUNIPER }, + { "HELP", mCmdGrp1 | mANY, f_help_app, COMMENT_HELP }, + { "?", mCmdGrp1 | mANY, f_help_app, COMMENT_HELP }, + + /** 3. app start commands */ + { NULL, mCmdGrp0 | mIDLE, NULL, COMMENT_APPSELECTION }, + { NODE_FUNC, mCmdGrp1 | mIDLE, f_node, COMMENT_NODE }, + { TAG_FUNC, mCmdGrp1 | mIDLE, f_tag, COMMENT_TAG }, + { TRILAT_FUNC, mCmdGrp1 | mIDLE, f_trilat_n, COMMENT_TRILAT }, + { USPI_FUNC, mCmdGrp1 | mIDLE, f_uspi, COMMENT_USPI }, + { TCWM_FUNC, mCmdGrp1 | mIDLE, f_tcwm, COMMENT_TCWM }, + { TCFM_FUNC, mCmdGrp1 | mIDLE, f_tcfm, COMMENT_TCFM }, + { LISTENER_FUNC, mCmdGrp1 | mIDLE, f_listen, COMMENT_LISTENER }, + +#if 1 // (LISTENER == 1) + { NULL, mCmdGrp0 | mLISTENER, NULL, COMMENT_LISTENER_OPT }, + { "LSTAT", mCmdGrp1 | mLISTENER, f_lstat, COMMENT_LSTAT }, +#endif + +#if 1 // (PDOA_TAG == 1) + { NULL, mCmdGrp0 | mPTAG, NULL, COMMENT_PDOA_TAG_OPT }, + { "RCDEL", mCmdGrp1 | mPTAG, f_rc_delay, COMMENT_RCDEL }, + { "X_MM", mCmdGrp1 | mPTAG, f_x, COMMENT_X }, + { "Y_MM", mCmdGrp1 | mPTAG, f_y, COMMENT_Y }, + { "Z_MM", mCmdGrp1 | mPTAG, f_z, COMMENT_Z }, +#endif + +#if 1 // (PDOA_NODE == 1) + + /** 2. commands to set system config, run-time and calibration variables */ + { NULL, mCmdGrp0 | mPNODE, NULL, COMMENT_PDOA_NODE_OPT }, + { "ADDR", mCmdGrp1 | mPNODE, f_addr, COMMENT_ADDR }, + { "PANID", mCmdGrp1 | mPNODE, f_panid, COMMENT_PANID }, + { "NUMSLOT", mCmdGrp1 | mPNODE, f_numSlots, COMMENT_NUMSLOT }, + { "SLOTPER", mCmdGrp1 | mPNODE, f_slotPeriod, COMMENT_SLOTPER }, + { "SFPER", mCmdGrp1 | mPNODE, f_sfPeriod, COMMENT_SFPER }, + { "REPDEL", mCmdGrp1 | mPNODE, f_tag_replyDly_us, COMMENT_REPDEL }, + { "P2FDEL", mCmdGrp1 | mPNODE, f_tag_pollTxFinalTx_us, COMMENT_P2FDEL }, + { "RCDEL", mCmdGrp1 | mPNODE, f_rc_delay, COMMENT_RCDEL }, + + /** 4. node application commands */ + { NULL, mCmdGrp0 | mPNODE, NULL, COMMENT_PDOA_NODE_CMD }, + { "JSTAT", mCmdGrp1 | mPNODE, f_jstat, COMMENT_JSTAT }, + { "GETDLIST", mCmdGrp1 | mPNODE, f_get_discovered_list, COMMENT_GETDLIST }, + { "GETKLIST", mCmdGrp1 | mPNODE, f_get_known_list, COMMENT_GETKLIST }, + { "ADDTAG", mCmdGrp1 | mPNODE, f_add_tag_to_list, COMMENT_ADDTAG }, + { "DELTAG", mCmdGrp1 | mPNODE, f_del_tag_from_list, COMMENT_DELTAG }, + { "PDOAOFF", mCmdGrp1 | mPNODE, f_pdoa_offset, COMMENT_PDOAOFF }, + { "RNGOFF", mCmdGrp1 | mPNODE, f_rng_offset, COMMENT_RNGOFF }, + { "PDOATEMP", mCmdGrp1 | mPNODE, f_pdoa_temp_coeff, COMMENT_PDOATEMP }, + { "PHCORREN", mCmdGrp1 | mPNODE, f_phase_corr_enable, COMMENT_PHCORREN }, +#endif + + { "UART", mCmdGrp1 | mIDLE, f_uart, COMMENT_UART }, + { "ANTTXA", mCmdGrp1 | mIDLE, f_ant_tx_a, COMMENT_ANTTXA }, + { "ANTRXA", mCmdGrp1 | mIDLE, f_ant_rx_a, COMMENT_ANTRXA }, + { "ANTRXB", mCmdGrp1 | mIDLE, f_ant_rx_b, COMMENT_ANTRXB }, + + /** 5. service commands */ + { NULL, mCmdGrp0 | mIDLE, NULL, COMMENT_SERVICE }, + { "RESTORE", mCmdGrp1 | mIDLE, f_restore, COMMENT_RESTORE }, + { "PCREP", mCmdGrp1 | mIDLE, f_twr_report, COMMENT_PCREP }, + { "D2K", mCmdGrp1 | mPNODE, f_add_all_to_list, COMMENT_D2K }, + { "DIAG", mCmdGrp1 | mIDLE, f_diag, COMMENT_DIAG }, + { "ACCUM", mCmdGrp1 | mIDLE, f_acc, COMMENT_ACCUM }, + + { "UWBCFG", mCmdGrp1 | mIDLE, f_uwbcfg, COMMENT_UWBCFG }, + { "STSKEYIV", mCmdGrp1 | mIDLE, f_stskeyiv, COMMENT_STSKEYIV }, + { "XTALTRIM", mCmdGrp1 | mIDLE, f_xtal_trim, COMMENT_XTALTRIM }, + { "TXPOWER", mCmdGrp1 | mIDLE, f_power, COMMENT_TXPOWER }, + { "DECAID", mCmdGrp1 | mIDLE, f_decaid, COMMENT_DECAID }, + { "VERSION", mCmdGrp1 | mIDLE, f_get_version, COMMENT_VERSION }, + + { "LEDGLOW", mCmdGrp1 | mIDLE, f_led_glow, STD_CMD_COMMENT }, + { "BUTTON", mCmdGrp1 | mIDLE, f_user_button, STD_CMD_COMMENT }, + { "UARTTEST", mCmdGrp1 | mIDLE, f_uart_test, STD_CMD_COMMENT }, + { "DEBUGLED", mCmdGrp1 | mIDLE, f_dbg, COMMENT_DEBUGLED }, + + { NULL, mANY, NULL, NULL } +}; diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/core/command/cmd_fn/cmd_fn.h b/bluetooth_uwb_dw3000_slotted_twr/src/core/command/cmd_fn/cmd_fn.h new file mode 100644 index 0000000..cd51050 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/core/command/cmd_fn/cmd_fn.h @@ -0,0 +1,81 @@ +/** + * @file cmd_fn.h + * + * @brief Header file for macros, structures and protypes cmd_fn.c + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef INC_CMD_FN_H_ +#define INC_CMD_FN_H_ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#include "app.h" +#include "cJSON.h" + +// ----------------------------------------------------------------------------- + +/* module DEFINITIONS */ +#define MAX_STR_SIZE 255 + +#define CMD_MALLOC pvPortMalloc +#define CMD_FREE vPortFree +#define CMD_ENTER_CRITICAL() taskENTER_CRITICAL() +#define CMD_EXIT_CRITICAL() taskEXIT_CRITICAL() + +typedef enum { + mCmdGrp0 = (0x1 << 16), /* This group of commands is a delimiter */ + mCmdGrp1 = (0x2 << 16), /* This group of commands has format #1 */ + mCmdGrp2 = (0x4 << 16), /* below reserved for the future */ + mCmdGrp3 = (0x8 << 16), + mCmdGrp4 = (0x10 << 16), + mCmdGrp5 = (0x20 << 16), + mCmdGrp6 = (0x40 << 16), + mCmdGrp7 = (0x80 << 16), + mCmdGrp8 = (0x100 << 16), + mCmdGrp9 = (0x200 << 16), + mCmdGrp10= (0x400 << 16), + mCmdGrp11= (0x800 << 16), + mCmdGrp12= (0x1000 << 16), + mCmdGrp13= (0x2000 << 16), + mCmdGrp14= (0x4000 << 16), + mCmdGrp15= (0x8000 << 16), + mCmdGrpMASK = 0xFFFF0000L +}cmdGroup_e; + +// ----------------------------------------------------------------------------- + +/* All cmd_fn functions have unified input: (char *text, param_block_t *pbss, + * int val) + */ + +/* use REG_FN(x) macro */ +#define REG_FN(x) const char *x(char *text, \ + param_block_t * pbss, \ + int val, \ + cJSON * params) + +/* command table structure definition */ +struct command_s +{ + const char *name; /**< Command name string */ + const uint32_t mode; /**< allowed execution operation mode */ + REG_FN((*fn)); /**< function() */ + const char *cmnt; +}; + +typedef struct command_s command_t; +extern const command_t known_commands[]; + +#ifdef __cplusplus +} +#endif + +#endif /* INC_CMD_FN_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/core/task_ctrl/task_ctrl.c b/bluetooth_uwb_dw3000_slotted_twr/src/core/task_ctrl/task_ctrl.c new file mode 100644 index 0000000..b1627ef --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/core/task_ctrl/task_ctrl.c @@ -0,0 +1,54 @@ +/** + * @file task_ctrl.c + * + * @brief Control task for USB/UART + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + */ +#include "cmd.h" +#include "usb_uart_rx.h" +#include "usb_uart_tx.h" +#include "app.h" + +/** + * @brief this is a Command Control and Data task. + * this task is activated on the startup + * there 2 sources of control data: Uart and Usb. + * + * */ +void CtrlTask(void const *arg) +{ + (void) arg; + usb_data_e res; + + while (1) + { + osThreadFlagsWait(app.ctrlTask.Signal, + osFlagsWaitAny, + osWaitForever); /* signal from USB/UART + * that some data has been + * received + */ + + taskENTER_CRITICAL(); + + /* mutex if usb2spiTask using the app.local_buf*/ + res = usb_uart_rx(); /*< processes usb/uart input : + * copy the input to the app.local_buff[ + * local_buff_length ] + * for future processing */ + taskEXIT_CRITICAL(); + + if (res == COMMAND_READY) { + command_parser((char *)app.local_buff); // parse and execute the + // command + } else if (res == DATA_READY) { + if (app.usb2spiTask.Handle) { + osThreadFlagsSet(app.usb2spiTask.Handle, app.usb2spiTask.Signal); + } + } + } +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/core/task_ctrl/task_ctrl.h b/bluetooth_uwb_dw3000_slotted_twr/src/core/task_ctrl/task_ctrl.h new file mode 100644 index 0000000..e7b4946 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/core/task_ctrl/task_ctrl.h @@ -0,0 +1,26 @@ +/** + * @file ctrl_task.h + * + * @brief header file for ctrl_task.c + * + * @author Decawave + * + * @attention Copyright 2017 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef __INC_CTRL_TASK_H_ +#define __INC_CTRL_TASK_H_ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +void CtrlTask(void const *arg); + +#ifdef __cplusplus +} +#endif + +#endif /* __INC_CTRL_TASK_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/core/task_flush/task_flush.c b/bluetooth_uwb_dw3000_slotted_twr/src/core/task_flush/task_flush.c new file mode 100644 index 0000000..3fe693a --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/core/task_flush/task_flush.c @@ -0,0 +1,43 @@ +/* + * @file task_flush.c + * + * @brief Flush task + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + */ + +#include "task_flush.h" + +#include "usb_uart_tx.h" +#include "port_common.h" + +#define USB_FLUSH_MS 5 + +/* + * @brief this thread is + * flushing report buffer on demand or every USB_FLUSH_MS ms + * */ +void FlushTask(void const *argument) +{ + (void) argument; + while (1) + { + osThreadFlagsWait(app.flushTask.Signal, + osFlagsWaitAny, + USB_FLUSH_MS / portTICK_PERIOD_MS); + + flush_report_buf(); + } +} + +void FlushTask_reset(void) +{ + if (app.flushTask.Handle) { + taskENTER_CRITICAL(); + reset_report_buf(); + taskEXIT_CRITICAL(); + } +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/core/task_flush/task_flush.h b/bluetooth_uwb_dw3000_slotted_twr/src/core/task_flush/task_flush.h new file mode 100644 index 0000000..397b82e --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/core/task_flush/task_flush.h @@ -0,0 +1,27 @@ +/** + * @file task_flush.h + * + * @brief header file for flush task + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef __FLUSH_TASK__H__ +#define __FLUSH_TASK__H__ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +void FlushTask(void const *argument); +void FlushTask_reset(void); + +#ifdef __cplusplus +} +#endif + +#endif /* __FLUSH_TASK__H__ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/core/usb_uart_rx/usb_uart_rx.c b/bluetooth_uwb_dw3000_slotted_twr/src/core/usb_uart_rx/usb_uart_rx.c new file mode 100644 index 0000000..86cad9d --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/core/usb_uart_rx/usb_uart_rx.c @@ -0,0 +1,273 @@ +/** + * @file usb_uart_rx.c + * + * @brief This file supports Decawave USB-TO-SPI and Control modes. + * Functions can be used in both bare-metal and RTOS implementations. + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +/* Includes */ +#include "usb_uart_rx.h" + +#include + +#include "port_common.h" +#include "error.h" + +#include "app.h" +#include "usb2spi.h" +#include "cmd.h" +#include "usb_uart_tx.h" + +/* For bare-metal implementation this critical defines may be required. */ +#define USB_UART_ENTER_CRITICAL() +#define USB_UART_EXIT_CRITICAL() + +usb_data_e waitForCommand(uint8_t *pBuf, + uint16_t len, + uint16_t *read_offset, + uint16_t cyclic_size); +usb_data_e waitForData(uint8_t *pBuf, + uint16_t len, + uint16_t *read_offset, + uint16_t cyclic_size); + +/* Receiving command type status */ +typedef enum +{ + cmdREGULAR=0,/* Regular command */ + cmdJSON,/* JSON command */ + cmdUNKNOWN_TYPE/* Unknown command */ +}command_type_e; + +// ----------------------------------------------------------------------------- +// IMPLEMENTATION + +/* + * @brief Waits only commands from incoming stream. + * The binary interface (deca_usb2spi stream) is not allowed. + * + * @return COMMAND_READY : the data for future processing can be found in + * app.local_buff : app.local_buff_len + * NO_DATA : no command yet + */ +usb_data_e waitForCommand(uint8_t *pBuf, + uint16_t len, + uint16_t *read_offset, + uint16_t cyclic_size) +{ + usb_data_e ret = NO_DATA; + static uint16_t cmdLen = 0; + static uint8_t cmdBuf[MAX_CMD_LENGTH]; /* Commands buffer */ + uint16_t cnt; + static command_type_e command_type = cmdUNKNOWN_TYPE; + static uint8_t brackets_cnt; + + app.local_buff_length = 0; + + for (cnt = 0; cnt < len; cnt++)// Loop over the buffer rx data + { + if (pBuf[*read_offset] == '\b') { // erase of a char in the terminal + port_tx_msg((uint8_t *)"\b\x20\b", 3); + if (cmdLen) { + cmdLen--; + } + } else { + port_tx_msg(&pBuf[*read_offset], 1); + if ((pBuf[*read_offset] == '\n') || (pBuf[*read_offset] == '\r')) { + if ((cmdLen != 0) && (command_type == cmdREGULAR)) {// Checks if need to + // handle regular + // command + // Need to update the app commands buffer + memcpy(&app.local_buff[app.local_buff_length], cmdBuf, cmdLen); + app.local_buff[app.local_buff_length + cmdLen] = '\n'; + app.local_buff_length += (cmdLen + 1); + cmdLen = 0; + command_type = cmdUNKNOWN_TYPE; + ret = COMMAND_READY; + } + } else if (command_type == cmdUNKNOWN_TYPE) {// Need to find out if + // getting regular command + // or JSON + cmdBuf[cmdLen] = pBuf[*read_offset]; + if (pBuf[*read_offset] == '{') {// Start Json command + command_type = cmdJSON; + brackets_cnt = 1; + } else {// Start regular command + command_type = cmdREGULAR; + } + cmdLen++; + } else if (command_type == cmdREGULAR) {// Regular command + cmdBuf[cmdLen] = pBuf[*read_offset]; + cmdLen++; + } else {// Json command + cmdBuf[cmdLen] = pBuf[*read_offset]; + cmdLen++; + if (pBuf[*read_offset] == '{') { + brackets_cnt++; + } else if (pBuf[*read_offset] == '}') { + brackets_cnt--; + if (brackets_cnt == 0) {// Got a full Json command + // Need to update the app commands buffer + memcpy(&app.local_buff[app.local_buff_length], cmdBuf, cmdLen); + app.local_buff[app.local_buff_length + cmdLen] = '\n'; + app.local_buff_length += (cmdLen + 1); + cmdLen = 0; + command_type = cmdUNKNOWN_TYPE; + ret = COMMAND_READY; + } + } + } + } + *read_offset = (*read_offset + 1) & cyclic_size; + if (cmdLen >= sizeof(cmdBuf)) {/* Checks if command too long and we need to + * reset it */ + cmdLen = 0; + command_type = cmdUNKNOWN_TYPE; + } + } + + if (ret == COMMAND_READY) {// If there is at least 1 command, add 0 at the end + app.local_buff[app.local_buff_length] = 0; + app.local_buff_length++; + } + return (ret); +} + +/* + * @brief Waits for binary interface (deca_usb2spi stream) from incoming + * stream. + * + * @return DATA_READY : the data is ready for future processing in usb2spi + * application + * data can be found in app.local_buff : + * app.local_buff_len + * NO_DATA : no valid data yet + */ +usb_data_e waitForData(uint8_t *pBuf, + uint16_t len, + uint16_t *read_offset, + uint16_t cyclic_size) +{ + static uint16_t dataLen = 0; + usb_data_e ret; + uint16_t cnt; + static bool frame_started = false; + + ret = NO_DATA; + + /* wait for valid usb2spi message from pBuf */ + if ((len + dataLen) < sizeof(app.local_buff) - 1) { + for (cnt = 0; cnt < len && ret != DATA_READY; cnt++) + { + if (!frame_started) { + if ((pBuf[*read_offset] == '\n') || (pBuf[*read_offset] == '\r')) { + // Do not store CR or LF when not receiving USB2SPI frames. E.g + // Decaranging + // adds CRLF to the end of some USB2SPI commands. + // Reset the dataLen to allow CR or LF to reset any command. + dataLen = 0; + *read_offset = (*read_offset + 1) & cyclic_size; + continue; + } else if (pBuf[*read_offset] == Usb_Msg_Header) { + // Remove any old data. If it contained a USB2SPI command, + // it was handled before. + dataLen = 0; + frame_started = true; + } + } + + app.local_buff[dataLen] = pBuf[*read_offset]; + *read_offset = (*read_offset + 1) & cyclic_size; + dataLen++; + + if (frame_started) { + if (usb2spi_frame_complete(app.local_buff, dataLen) == DATA_READY) { + frame_started = false; + ret = DATA_READY; + } + } else { + ret = usb2spi_command_check(app.local_buff, dataLen); + } + } + if (ret == DATA_READY) { + app.local_buff_length = dataLen; + app.local_buff[dataLen] = 0; + dataLen = 0; + } + } else { /* overflow in usb2spi protocol : flush the buffer */ + dataLen = 0; + } + + return (ret); +} + +/* @fn usb_uart_rx + * @brief this should be calling on a reception of a data from UART or USB. + * uses platform-dependent + * + * */ +usb_data_e usb_uart_rx(void) +{ + usb_data_e ret = NO_DATA; + uint16_t uartLen, usbLen; + uint16_t headUart, tailUart; + uint16_t headUsb, tailUsb; + + /* USART control prevails over USB control if both at the same time */ + + USB_UART_ENTER_CRITICAL(); + headUart = app.uartRx.head; + + headUsb = app.usbRx.head; + USB_UART_EXIT_CRITICAL(); + + tailUart = app.uartRx.tail; + tailUsb = app.usbRx.tail; + + uartLen = CIRC_CNT(headUart, tailUart, sizeof(app.uartRx.buf)); + usbLen = CIRC_CNT(headUsb, tailUsb, sizeof(app.usbRx.buf)); + + if (uartLen > 0) { + if (app.pConfig->s.uartEn) { + ret = (app.mode == mUSB2SPI) + ?waitForData(app.uartRx.buf, uartLen, &tailUart, + sizeof(app.uartRx.buf) - 1) + :waitForCommand(app.uartRx.buf, uartLen, &tailUart, + sizeof(app.uartRx.buf) - 1); + + USB_UART_ENTER_CRITICAL(); + app.uartRx.tail = tailUart; + USB_UART_EXIT_CRITICAL(); + } else { + // Ignore data in UART buffer. Should not happen since UART is disabled. + app.uartRx.tail = (app.uartRx.tail + uartLen) + & (sizeof(app.uartRx.buf) - 1); + } + } + if (usbLen > 0) { + if (!app.pConfig->s.uartEn && (app.usbState == USB_CONFIGURED)) { + ret = (app.mode == mUSB2SPI) + ?waitForData(app.usbRx.buf, + usbLen, + &tailUsb, + sizeof(app.usbRx.buf) - 1) + :waitForCommand(app.usbRx.buf, usbLen, &tailUsb, + sizeof(app.usbRx.buf) - 1); + + USB_UART_ENTER_CRITICAL(); + app.usbRx.tail = tailUsb; + USB_UART_EXIT_CRITICAL(); + } else { + // Ignore data from USB + app.usbRx.tail = (app.usbRx.tail + usbLen) & (sizeof(app.usbRx.buf) - 1); + } + } + + return ret; +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/core/usb_uart_rx/usb_uart_rx.h b/bluetooth_uwb_dw3000_slotted_twr/src/core/usb_uart_rx/usb_uart_rx.h new file mode 100644 index 0000000..7000d8c --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/core/usb_uart_rx/usb_uart_rx.h @@ -0,0 +1,35 @@ +/** + * @file usb_uart_rx.h + * + * @brief Header file for usb_uart_rx.c contains enum for usb data + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef __INC_USB_UART_RX_H_ +#define __INC_USB_UART_RX_H_ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum { + NO_DATA = 0, + DATA_READY, + COMMAND_READY, + DATA_SEND, + DATA_FLUSH, + DATA_ERROR +}usb_data_e; + +usb_data_e usb_uart_rx(void); + +#ifdef __cplusplus +} +#endif + +#endif /* __INC_USB_UART_TX_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/core/usb_uart_tx/usb_uart_tx.c b/bluetooth_uwb_dw3000_slotted_twr/src/core/usb_uart_tx/usb_uart_tx.c new file mode 100644 index 0000000..6330107 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/core/usb_uart_tx/usb_uart_tx.c @@ -0,0 +1,214 @@ +/** + * @file usb_uart_tx.c + * + * @brief Puts message to circular buffer which will be transmitted by + * flushing thread + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#include "usb_uart_tx.h" + +#include "port_common.h" + +// ----------------------------------------------------------------------------- +// USB/UART report section + +// the size is such long because of possible ACCUMULATORS sending +#define USB_REPORT_BUFSIZE \ + 0x8000 /**< the size of USB report buffer, must + * be 1< len) { + while (len > 0) + { + txHandle.Report.buf[head] = *(str++); + head = (head + 1) & (size - 1); + len--; + } + + txHandle.Report.head = head; + } else { + /* if packet can not fit, setup TX Buffer overflow ERROR and exit */ + error_handler(0, _ERR_TxBuf_Overflow); + ret = _ERR_TxBuf_Overflow; + } + + __HAL_UNLOCK(&txHandle); + return ret; +} + +/* @fn port_tx_msg() + * @brief wrap for copy_tx_msg + * Puts message to circular report buffer + * + * @return see copy_tx_msg() + * */ +error_e port_tx_msg(uint8_t *str, int len) +{ + error_e ret; + + if (app.maxMsgLen < len) { + app.maxMsgLen = len; + } + + ret = copy_tx_msg(str, len); + + if (app.mode != mLISTENER) { + if (app.flushTask.Handle) { // RTOS : usbFlushTask can be not started yet + osThreadFlagsSet(app.flushTask.Handle, app.flushTask.Signal); + } + } + + return (ret); +} + +// ----------------------------------------------------------------------------- +// USB/UART report : platform - dependent section +// can be in platform port file + +/* @fn flush_report_buff() + * @brief FLUSH should have higher priority than port_tx_msg() + * This shall be called periodically from process, which can not be + * locked, + * i.e. from independent high priority thread / timer etc. + * */ +error_e flush_report_buf(void) +{ + int size = sizeof(txHandle.Report.buf) + / sizeof(txHandle.Report.buf[0]); + int chunk; + error_e ret = _NO_ERR; + uint32_t tmr; + + if ((app.usbState != USB_CONFIGURED) && (app.pConfig->s.uartEn == 0)) { + return _ERR_Busy; + } + + __HAL_LOCK_FAIL_RETURN(&txHandle, _ERR_Busy); // "return HAL_BUSY;" if + // locked + + int head = txHandle.Report.head; + int tail = txHandle.Report.tail; + + int len = CIRC_CNT(head, tail, size); + + int old_tail = txHandle.Report.tail; + + start_timer(&tmr); + + if (len > 0) { + do{ + if (check_timer(tmr, USB_UART_TX_TIMEOUT_MS)) { + break; // max timeout for any output on the 115200bps rate + // (currently ~1400ms if over the UART) + } + + /* copy MAX allowed length from circular buffer to linear buffer */ + chunk = MIN((int)sizeof(ubuf), len); + + for (int i = 0; i < chunk; i++) + { + ubuf[i] = txHandle.Report.buf[tail]; + tail = (tail + 1) & (size - 1); + } + + len -= chunk; + + txHandle.Report.tail = tail; + + if (app.pConfig->s.uartEn == 1) { + /* setup UART DMA transfer */ + if (deca_uart_transmit((char *)ubuf, chunk) != 0) { + error_handler(0, _ERR_UART_TX); /**< indicate UART transmit + * error */ + ret = _ERR_UART_TX; + break; + } + } else { + /* setup USB IT transfer */ + if (deca_usb_transmit((char *)ubuf, chunk) != 0) { + error_handler(0, _ERR_Usb_Tx); /**< indicate USB transmit + * error */ + txHandle.Report.tail = old_tail; + ret = _ERR_Usb_Tx; + break; + } else { + old_tail = tail; + } + } + }while (len > 0 && app.mode == mUSB2SPI); + } + + __HAL_UNLOCK(&txHandle); + return ret; +} + +// END OF Report section diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/core/usb_uart_tx/usb_uart_tx.h b/bluetooth_uwb_dw3000_slotted_twr/src/core/usb_uart_tx/usb_uart_tx.h new file mode 100644 index 0000000..d0900af --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/core/usb_uart_tx/usb_uart_tx.h @@ -0,0 +1,31 @@ +/** + * @file usb_uart_tx.h + * + * @brief Header file for usb_uart_tx.c + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef __INC_USB_UART_TX_H_ +#define __INC_USB_UART_TX_H_ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "error.h" + +error_e port_tx_msg(uint8_t *str, int len); +error_e flush_report_buf(void); +int reset_report_buf(void); + +#ifdef __cplusplus +} +#endif + +#endif /* __INC_USB_UART_TX_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/inc/app.h b/bluetooth_uwb_dw3000_slotted_twr/src/inc/app.h new file mode 100644 index 0000000..a9ca871 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/inc/app.h @@ -0,0 +1,275 @@ +/** + * @file app.h + * + * @brief Decawave Application Layer header contains all macros and + * structures related apllicaitions + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef APP_H_ +#define APP_H_ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "default_config.h" +#include "cmsis_os2.h" +#include "FreeRTOS.h" +#include "event_groups.h" +#include "error.h" +#include "common.h" +#include "uwb_frames.h" + +/* SPI/USART/USB buffers */ +#define UART_RX_BUF_SIZE 0x800 // 0x100 Read buffer for UART + // reception, shall be 1<rx[tail].msg.blinkMsg. \ + frameCtrl[0] == HEAD_MSG_BLINK) +#define APP_EV_CLOCK_SYNC_RX ((phyevent->rx[tail].msg.ccpMsg. \ + messageData[0] \ + == RTLS_MSG_ANCH_CLK_SYNC) && \ + (phyevent->rx[tail].rxDataLen \ + == sizeof(ccp_msg_t))) +#define APP_EV_DATA_RX (phyevent->rx[tail].msg.bcastMsg. \ + messageData[0] == APP_MSG_DATA) +#define APP_EV_UWB_BH_DWNSTREAM_RX ((phyevent->rx[tail].msg.bcastMsg. \ + messageData[0] \ + == APP_MSG_UWB_BH_DWNSTREAM) && \ + (phyevent->rx[tail].rxDataLen \ + == sizeof(uwb_bh_dwnstream_msg_t))) +#define APP_EV_UWB_BH_UPSTREAM_RX ((phyevent->rx[tail].msg.bcastMsg. \ + messageData[0] \ + == APP_MSG_UWB_BH_UPSTREAM) && \ + (phyevent->rx[tail].rxDataLen \ + == sizeof(uwb_bh_upstream_msg_t))) + +// ----------------------------------------------------------------------------- +// common macros + +#ifndef SWAP +#define SWAP(a, b) { a ^= b; b ^= a; a ^= b; } +#endif /* SWAP */ + +#ifndef MIN +#define MIN(a, b) (((a) < (b)) ? (a) : (b)) +#endif /* MIN */ + +#ifndef MAX +#define MAX(a, b) (((a) > (b)) ? (a) : (b)) +#endif /* MAX */ + +#ifndef TRUE +#define TRUE 1 +#endif /* TRUE */ + +#ifndef FALSE +#define FALSE 0 +#endif /* FALSE */ + +#define MASK_40BIT (0x00FFFFFFFFFFULL) // DW1000 counter is 40 bits +#define MASK_TXDTS (0x00FFFFFFFE00ULL) // The TX timestamp will snap + // to 8 ns resolution - + // mask lower 9 bits. + +#ifdef __cplusplus +} +#endif + +#endif /* COMMON_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/inc/deca_dbg.h b/bluetooth_uwb_dw3000_slotted_twr/src/inc/deca_dbg.h new file mode 100644 index 0000000..02a2b60 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/inc/deca_dbg.h @@ -0,0 +1,127 @@ +/** + * @file deca_dbg.h + * + * @brief Debug macros for debug prints + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ +#ifndef DECA_DBG_H_ +#define DECA_DBG_H_ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#include "usb_uart_tx.h" +#include +#include + +#if 0 +#define DBG_PRINTF diag_printf + +#define DBG_TRACE_EVENTS if (!TX_EVENT_RECEIVED) printf( \ + "MsgPtrs [ %d - %d %d %d - %3d - 0x%X] \n", \ + rxData[ \ + phyEvent[app.eventIdx].rxDataPtr].rxDataLen, \ + phyCfg.eventIdx, \ + app.eventIdx, \ + phyEvent[app.eventIdx].type, \ + rxData[ \ + phyEvent[app.eventIdx].rxDataPtr].msg.stdMsg.seqNum, \ + rxData[ \ + phyEvent[app.eventIdx].rxDataPtr].msg.stdMsg.messageData[0]); + +#define DBG_TRACE_CLOCK_SYNC_TX printf( \ + "CS Sent at .. @ .. 0x%02x%02x%02x%02x%02x \n", \ + phyEvent[app.eventIdx].timeStamp[4], \ + phyEvent[app.eventIdx].timeStamp[3], \ + phyEvent[app.eventIdx].timeStamp[2], \ + phyEvent[app.eventIdx].timeStamp[1], \ + phyEvent[app.eventIdx].timeStamp[0]); \ + printf("C:%d\n", app.rtls_info.ccpSeqNum); + +#define DBG_TRACE_CLOCK_SYNC_RX printf( \ + "CS Received .. @ .. 0x%02x%02x%02x%02x%02x \n", \ + phyEvent[app.eventIdx].timeStamp[4], \ + phyEvent[app.eventIdx].timeStamp[3], \ + phyEvent[app.eventIdx].timeStamp[2], \ + phyEvent[app.eventIdx].timeStamp[1], \ + phyEvent[app.eventIdx].timeStamp[0]); \ + printf("C:%d\n", rxMsg.msg.bcastMsg.seqNum); + +#define DBG_TRACE_TAG_BLINK printf( \ + "Tag Blink : Seq : %d : Time : %lld \n", \ + rxMsg.msg.blinkMsg.seqNum, \ + phyEvent[app.eventIdx].timeStamp); \ + // printf("S:%d\n", rxMsg.msg.blinkMsg.seqNum); + +#define DBG_TRACE_NETWORK_MSG printf( \ + "Received Network Configuration : %s %d \n", \ + (app.rtls_info.master?"MASTER":"SLAVE"), \ + app.rtls_info.id); + +#define DBG_TRACE_CONFIGURATION printf( \ + "Configuration Message %x %x : Time : %lld \n", \ + rxData[phyEvent[app.eventIdx]. \ + rxDataPtr].msg.stdMsg.messageData[0], \ + rxData[phyEvent[app.eventIdx]. \ + rxDataPtr].msg.stdMsg.messageData[1], \ + phyEvent[app.eventIdx].timeStamp); + +#else + +#define DIAG_BUF_LEN 0x8 +#define DIAG_STR_LEN 64 +// note, (DIAG_BUF_LEN*(DIAG_STR_LEN+4)) shall be < HTTP_PAGE_MALLOC_SIZE-16 +//
=4 +typedef struct +{ + uint8_t buf[DIAG_BUF_LEN][DIAG_STR_LEN]; + int head; +}gDiagPrintFStr_t; + +extern gDiagPrintFStr_t gDiagPrintFStr; + +#define diag_printf(...) do{ \ + snprintf((char *)(&gDiagPrintFStr.buf[gDiagPrintFStr.head][0]), \ + DIAG_STR_LEN, \ + __VA_ARGS__); \ + port_tx_msg(&gDiagPrintFStr.buf[gDiagPrintFStr.head][0], \ + strlen((char *)(&gDiagPrintFStr.buf[gDiagPrintFStr.head][0 \ + ]))); \ + gDiagPrintFStr.head = (gDiagPrintFStr.head + 1) & (DIAG_BUF_LEN - 1); \ +}while (0) + +#define DBG_PRINTF diag_printf +#define DBG_TRACE_EVENTS +#define DBG_TRACE_CLOCK_SYNC_TX +#define DBG_TRACE_CLOCK_SYNC_RX +#define DBG_TRACE_TAG_BLINK +#define DBG_TRACE_NETWORK_MSG +#define DBG_TRACE_CONFIGURATION +#endif + +#define DBG_TRACE(x) { uint8_t buf; buf = x; dwt_writetodevice(0xb, \ + 0, \ + 1, \ + &buf); } + +/* TRACE_DEBUG and TRACE_INFO used in drivers when -DDEBUG in Makefile defined + */ +#ifdef DEBUG +#define TRACE_DEBUG diag_printf +#define TRACE_INFO diag_printf +#else +#define TRACE_DEBUG +#define TRACE_INFO diag_printf +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* DECA_DBG_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/inc/error.h b/bluetooth_uwb_dw3000_slotted_twr/src/inc/error.h new file mode 100644 index 0000000..3e5a32f --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/inc/error.h @@ -0,0 +1,106 @@ +/** + * @file error.h + * + * @brief Header file for errors + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef INC_ERROR_H_ +#define INC_ERROR_H_ + +// #include + +typedef enum { + _NO_ERR = 0, + _ERR, + _ERR_Busy, + _ERR_Timeout, + _ERR_DEVID, + _ERR_IWDG, + _ERR_INSTANCE, + _ERR_INIT, + _ERR_IMU_INIT, + _ERR_TxBuf_Overflow, + _ERR_RxBuf_Overflow, + _ERR_Usb_Tx, + _ERR_Flash_Ob, + _ERR_Flash_Prog, + _ERR_Flash_Erase, + _ERR_Flash_Error, + _ERR_Flash_Verify, + _ERR_Flash_Protected, + _ERR_LSM_R, + _ERR_LSM_W, + _ERR_SPI, + _ERR_SPI_RRX, + _ERR_SPI_WTX, + _ERR_SPI_DMA, + _ERR_UART_DMA, + _ERR_UART_INIT, + _ERR_UART_RX, + _ERR_UART_TX, + _ERR_UART_RxCplt, + _ERR_UART_RxCplt_Overflow, + _ERR_USB_UART_RX, + _ERR_TCFM, + _ERR_TWR_CANNOT_START, + _ERR_MEM_CORRUPTED, + _ERR_Configure_WKUP_Timer, + _ERR_PLL, + +/*TWR*/ + _ERR_Twr_Bad_State, + _ERR_Not_Twr_Frame, + _ERR_Unknown_Tag, + _ERR_DelayedTX_Late, + _ERR_Range_Calculation, + _ERR_Ranging_Config, + _ERR_RC_Version_Unknown, + _ERR_Non_Compatible_TWR_Parameters, + _NO_Err_New_Tag, + _NO_Err_Tx_Sent, + _NO_Err_Start_Rx, + _NO_Err_Final, + _NO_Err_Ranging_Config, + _NO_Err_Ranging_Update, + _NO_Err_Response, + _NO_Err_Idata, + _NO_Err_Rdata, + _NO_Err_Can_Sleep, + +/*USB2SPI*/ + _ERR_Usb2Spi_ptr_busy, + _ERR_Usb2Spi_ptr_alloc, + +/*RTOS*/ + _ERR_General_Error, + _ERR_Create_Task_Bad, + _ERR_Timer_Create_Bad, + _ERR_Timer_Start_Bad, + _ERR_Signal_Bad, + _ERR_Cannot_Delete_Timer, + _ERR_Cannot_Delete_Task, + _ERR_Cannot_Delete_usb2spiTask, + _ERR_Cannot_Delete_tcfmTask, + _ERR_Cannot_Delete_tcwmTask, + _ERR_Cannot_Delete_imuTask, + _ERR_Cannot_Delete_rtlsTask, + _ERR_Cannot_Delete_rxTask, + _ERR_Cannot_Delete_calcTask, + _ERR_Cannot_Delete_twrTask, + _ERR_Cannot_Delete_commTask, + _ERR_Cannot_Send_Mail, + _ERR_Cannot_Alloc_Mail, + _ERR_Cannot_Alloc_Memory, + _ERR_Cannot_Alloc_NodeMemory, + _ERR_Malloc_Failed, // 71 + _ERR_Stack_Overflow, + _ERR_No_pTwrInfo +}error_e; + +#endif /* INC_ERROR_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/inc/rtls_interface.h b/bluetooth_uwb_dw3000_slotted_twr/src/inc/rtls_interface.h new file mode 100644 index 0000000..1009f21 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/inc/rtls_interface.h @@ -0,0 +1,430 @@ +/** + * @file rtls_interface.h + * + * @brief Decawave RTLS CLE / RTLS anchor common definitions / APIs + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef RTLS_INTERFACE_H_ +#define RTLS_INTERFACE_H_ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#include "uwb_frames.h" + +// Protocol structs and enums : +enum RTLS_CMD { + RTLS_CMD_PARM_STOP = 0, RTLS_CMD_PARM_START = 1, + + RTLS_CMD_REQ_CFG = 0x42, // Anchor: request configuration + RTLS_CMD_SET_CFG_CCP = 0x44, // CLE: response on RTLS_CMD_REQ_CFG request + + // Anchor -> LE + RTLS_CMD_DEBUG = 0x37, // Anchor: Debug Message + + RTLS_CMD_REPORT_TOA = 0x3A, // v.3 TDOA + RTLS_CMD_REPORT_TOA_EX = 0x3B, // v.3 TDOA with extra data (e.g. IMU data) + RTLS_CMD_REPORT_TX_CS = 0x3E, // v.3 CCP Tx report + RTLS_CMD_REPORT_RX_CS = 0x3F, // v.3 CCP Rx report + + RTLS_CMD_REPORT_TX_CS_V4 = 0x30, // v.4 CCP Tx report + RTLS_CMD_REPORT_RX_CS_V4 = 0x31, // v.4 CCP Rx report + RTLS_CMD_REPORT_TOA_V4 = 0x32, // v.4 TDOA with FP + RTLS_CMD_REPORT_TOA_IMU_V4 = 0x33, // v.4 TDOA with FP and IMU data + + // LE -> Anchor + RTLS_COMM_TEST_START_REQ = 0x53, + RTLS_COMM_TEST_RESULT_REQ = 0x54, + RTLS_RANGE_MEAS_REQ = 0x55, + RTLS_INIT_REQ = 0x56, + RTLS_START_REQ = 0x57, + RTLS_POWER_TEST_START_REQ = 0x58, + RTLS_RESET_REQ = 0x59, + RTLS_SINGLE_TWR_MODE_REQ = 0x5A, + RTLS_ASYMM_TWR_MODE_REQ = 0x5B, + + RTLS_CFG_IND = 0x80, + RTLS_COMM_TEST_DONE_IND = 0x81, + RTLS_COMM_TEST_RESULT_IND = 0x82, + RTLS_RANGE_MEAS_IND = 0x83, + RTLS_RANGE_MEAS_IND_FINAL = 0x84, + RTLS_POWER_TEST_DONE_IND = 0x85, + + RTLS_TEMP_VBAT_IND = 0x87, // Temperature and VBAT + RTLS_LOG_ACCUMULATOR_REQ = 0x88, // Request Accumulator for CCP or Blink + RTLS_LOG_ACCUMULATOR_IND = 0x91 // Accumulator Report for CCP or Blink +}; + +#define RTLS_CMD_SET_CFG_CCP_LEN sizeof(cmd_config_t) +#define RTLS_POWER_TEST_START_LEN sizeof(cmd_power_test_t) +#define RTLS_COMM_TEST_START_LEN sizeof(cmd_comm_test_t) +#define RTLS_COMM_TEST_RESULT_REQ_LEN (1) +#define RTLS_RANGE_MEAS_REQ_LEN (21) +#define RTLS_SINGLE_TWR_MODE_REQ_LEN (17) +#define RTLS_ASYMM_TWR_MODE_REQ_LEN (27) +#define RTLS_START_REQ_LEN (2) +#define RTLS_LOG_ACC_REQ_LEN (4) +#define RTLS_RESET_REQ_LEN (18) + +#define RTLS_CMD_REPORT_TOA_LEN (16) +#define RTLS_CMD_REPORT_TOA_EX_LEN (17) // minimum length + // of longer TDoA + // report, extra + // byte, + // specifying + // extra data + // length +#define RTLS_CMD_REPORT_TX_CS_LEN (8) +#define RTLS_CMD_REPORT_RX_CS_LEN (16) + +#define RTLS_CMD_REPORT_TOA_V4_LEN (18) +#define RTLS_CMD_REPORT_TX_CS_V4_LEN (8) +#define RTLS_CMD_REPORT_RX_CS_V4_LEN (18) + +#define RTLS_CMD_DEBUG_LEN (3) + +#define RTLS_POWER_TEST_DONE_IND_LEN (1) +#define RTLS_COMM_TEST_DONE_IND_LEN (1) +#define RTLS_COMM_TEST_RESULT_IND_LEN (5) +#define RTLS_RANGE_MEAS_IND_LEN (14) +#define RTLS_TEMP_VBAT_IND_LEN (12) + +#define RTLS_ACC_LOG_HEADER_LEN (15) // 1 byte (MSG ID) + // + 1 byte frame + // type (CCP or + // Blink) + 1 + // byte seq Num + + // 4 bytes log + // Num + 8 bytes + // of source + // address (CCP + // Master or + // Blink Tag) +#define RTLS_ACC_LOG_REG_LEN (16) // fpIndex, + // maxNoise, + // firstPathAmp1, + // stdNoise, + // firstPathAmp2, + // firstPathAmp3, + // maxGrowthCIR, + // rxPreamCount + +#define RTLS_DW_ACCUMULATOR_LEN_16 \ + (992 * 4 + 1) // 16M PRF is 992*4+1 +#define RTLS_DW_ACCUMULATOR_LEN_64 \ + (1016 * 4 + 1) // 64M PRF is 1016*4+1 + +#define RTLS_LOG_ACCUMULATOR_IND_LEN_16 \ + (RTLS_ACC_LOG_HEADER_LEN \ + + RTLS_ACC_LOG_REG_LEN \ + + RTLS_DW_ACCUMULATOR_LEN_16) +#define RTLS_LOG_ACCUMULATOR_IND_LEN_64 \ + (RTLS_ACC_LOG_HEADER_LEN \ + + RTLS_ACC_LOG_REG_LEN \ + + RTLS_DW_ACCUMULATOR_LEN_64) +#define RTLS_LOG_ACCUMULATOR_IND_ \ + ((RTLS_LOG_ACCUMULATOR_IND_LEN_16 \ + < RTLS_LOG_ACCUMULATOR_IND_LEN_64) ? \ + RTLS_LOG_ACCUMULATOR_IND_LEN_16 \ + : \ + RTLS_LOG_ACCUMULATOR_IND_LEN_64) // minimumn of 2 + +#define ACCUM_TYPE_BLINK 0xA1 // accumulator + // reading is + // only used to + // read blinks + // and CCPs +#define ACCUM_TYPE_CCP 0xA2 // accumulator + // reading is + // only used to + // read blinks + // and CCPs + +#define DWT_DIAGNOSTIC_LOG_REV_5 (5) +#define DWT_DIAGNOSTIC_LOG_V_5 (5) +#define DWT_SIZEOFDIAGNOSTICDATA_5 (66) +#define DWT_SIZE_OF_IMUDATA (30) + +#define DWT_LOG_NUM_SIZE (4) + +/* The data is framed as follows : + *.....> + * STX = 0x2 + * LEN is the length of data message(16 bits) + * CRC is the 16 - bit CRC of the data bytes + * ETX = 0x3 + * FC = is the function code(API code) + * + */ +#define FRAME_HEADER_LEN (6) +#define FRAME_START_IDX (0) +#define FRAME_LENGTH_IDX (1) +#define FRAME_DATA_IDX (3) + +enum RTLS_DATA { + RTLS_DATA_ANCHOR_REQ = 0x41, + + RTLS_DATA_ANCHOR = 0x61, + RTLS_DATA_BLINK = 0x62, + RTLS_DATA_STATS = 0x63, + RTLS_DATA_IMU = 0x64, + RTLS_DATA_BLINK_EXT = 0x65 +}; + +/* Network commands + * use byte access if in doubt + */ + +#define UN16(x) union { uint16_t x ## 16; uint8_t x[2];} + +#define UN32(x) union { uint32_t x ## 32; uint8_t x[4];} + +/* Wire format of messages from Anchor to CLE */ +#pragma pack (push, 1) + +/* Standard Diagnostics v5 */ +struct diag_v5_s { + // NOTE: diagnostics data format rev 5 (DWT_DIAGNOSTIC_LOG_REV_5) + uint8_t header; // 00 this could be a header (format version number) + uint8_t r0F[5]; // 01 register 0xF - length 5 bytes + uint8_t r10[4]; // 06 register 0x10 - length 4 bytes + uint8_t r12[8]; // 10 register 0x12 - length 8 bytes + uint8_t r13[4]; // 18 register 0x13 - length 4 bytes + uint8_t r14[5]; // 22 register 0x14 - length 5 bytes + uint8_t r15[14]; // 27 register 0x15 - length 14 bytes (5 TS, 2 FP, 2 + // Diag, 5 TSraw) + uint8_t r25[16]; // 41 register 0x25 @FP (first path) -> 16 bytes + // starting at FP + 1 dummy + uint8_t r2E[2]; // 58 register 0x2E (0x1000) - 2 bytes + uint8_t r27[4]; // 60 register 0x27 (0x28) - 4 bytes + uint8_t r2E2[2]; // 64 register 0x2E (0x1002) - 2 bytes + uint8_t dummy; + // 66 total +}; + +typedef struct diag_v5_s diag_v5_t; + +struct ccp_rx_v4_s { + uint8_t type; // 0 : type = CCP RX timestamp + uint8_t seqNum; // 1 : CCP Seq Num + uint8_t masterID[ADDR_BYTE_SIZE_L]; // 2-9 : master anchor ID (that sent + // the CCP frame) + uint8_t csRxTime[TS_40B_SIZE]; // 10-14 : CCP Rx time + UN16(firstPath); // 15-16 : raw Firstpath + uint8_t extLen; // 17 : length of ext below +}; + +typedef struct ccp_rx_v4_s ccp_rx_v4_t; + +struct toa_v4_s { + uint8_t type; // 0 : type = TOA report + uint8_t seqNum; // 1 : Blink message sequence number + uint8_t tagID[ADDR_BYTE_SIZE_L]; // 2-9 : tag ID + uint8_t rawTOA[TS_40B_SIZE]; // 10-14 : raw timestamp + UN16(firstPath); // 15-16 : raw Firstpath + uint8_t extLen; // 17 : length of ext below +}; + +typedef struct toa_v4_s toa_v4_t; + +/* RTLS_CMD_REPORT_TOA_IMU_V4 */ +struct report_toa_imu_v4_s { + toa_v4_t toa; + union { + uint8_t ext[DWT_SIZE_OF_IMUDATA + sizeof(uint32_t) + sizeof(diag_v5_t)]; + struct { + uint8_t dataIMU[DWT_SIZE_OF_IMUDATA]; // : IMU data sent from the + // tag(2 (ench + exth) + 1 + // (dwh)+27 (dwp)) + UN32(logNum); + diag_v5_t diag; + }; + }; +}; + +typedef struct report_toa_imu_v4_s report_toa_imu_v4_t; + +/* RTLS_CMD_REPORT_TOA_V4 */ +struct report_toa_v4_s { + toa_v4_t toa; + union { + uint8_t ext[sizeof(uint32_t) + sizeof(diag_v5_t)]; + struct { + UN32(logNum); + diag_v5_t diag; + }; + }; +}; + +typedef struct report_toa_v4_s report_toa_v4_t; + +/* RTLS_CMD_REPORT_RX_CS_V4 */ +struct report_ccp_rx_v4_s { + ccp_rx_v4_t ccp; + + union { + uint8_t ext[sizeof(uint32_t) + sizeof(diag_v5_t)]; + struct { + UN32(logNum); + diag_v5_t diag; + }; + }; +}; + +typedef struct report_ccp_rx_v4_s report_ccp_rx_v4_t; + +/* RTLS_CMD_REPORT_TX_CS_V4 */ +struct report_ccp_tx_v4_s { + uint8_t type; // 0 : type = CCP TX timestamp + uint8_t seqNum; // 1 : CCP Seq Num + uint8_t csTxTime[TS_40B_SIZE]; // 2-6 : CCP Tx time + uint8_t extLen; // 7 : length of ext below + union { + uint8_t ext[sizeof(uint32_t)]; + UN32(logNum); + }; +}; + +typedef struct report_ccp_tx_v4_s report_ccp_tx_v4_t; + +/* RTLS_TEMP_VBAT_IND */ +struct report_temp_vbat_s { + uint8_t type; // 0: type = Battery abnd Voltage Level + // report + uint8_t bat; // 1 : battery voltage value + uint8_t temp; // 2 : temperature value + UN16(atemp); // 3-4 : ambient Sensor + // temperature value + uint8_t apress[3]; // 5-7 : ambient Sensor pressure + // value + UN32(logNum); // 8-11 : log number +}; + +typedef struct report_temp_vbat_s report_temp_vbat_t; + +/* to be structured: + * RTLS_COMM_TEST_RESULT_REQ + * RTLS_START_REQ + * RTLS_LOG_ACCUMULATOR_REQ + * RTLS_RESET_REQ + */ + +/* RTLS_CMD_SET_CFG_CCP */ +struct cmd_config_s { + uint8_t command; // 0 + uint8_t id; // 1 + uint8_t master; // 2 + uint8_t prf_ch; // 3 + uint8_t datarate; // 4 + uint8_t code; // 5 + uint8_t txPreambLength; // 6 + uint8_t nsSFD_rxPAC; // 7 + UN16(delay_rx); // 8-9 + UN16(delay_tx); // 10-11 + uint8_t free_12; // 12 + uint8_t debug_logs; // 13 + uint8_t eui64_to_follow[8]; // 14-21 + UN32(lag_delay); // 22-25 +}; + +typedef struct cmd_config_s cmd_config_t; + +/* RTLS_SINGLE_TWR_MODE_REQ */ +struct cmd_twr_single_s { + uint8_t command; // 0 : RTLS_SINGLE_TWR_MODE_REQ + uint8_t role; // 1 : initiator==1 responder==0 + uint8_t use_ant_delay; // 2 + UN16(delay_tx); // 3-4 + UN16(delay_rx); // 5-6 + UN16(response_delay); // 7-8 + UN16(addr); // 9-10 + uint8_t lna_on; // 11 + UN32(power); // 12-15 + uint8_t log_all; // 16 +}; + +typedef struct cmd_twr_single_s cmd_twr_single_t; + +/* RTLS_RANGE_MEAS_REQ */ +struct cmd_twr_s { + uint8_t command; // 0 : RTLS_RANGE_MEAS_REQ + uint8_t log_all; // 1 + uint8_t role; // 2 + UN16(num_ranges); // 3-4 + uint8_t use_ant_delay; // 5 + UN16(delay_tx); // 6-7 + UN16(delay_rx); // 8-9 + UN16(response_delay); // 10-11 + UN16(initiator_addr); // 12-13 + UN16(responder_addr); // 14-15 + uint8_t lna_on; // 16 + UN32(power); // 17-20 +}; + +typedef struct cmd_twr_s cmd_twr_t; + +/* RTLS_RANGE_MEAS_REQ */ +struct cmd_twr_asymm_s { + uint8_t command; // 0 : RTLS_RANGE_MEAS_REQ + uint8_t log_all; // 1 + uint8_t role; // 2 + UN16(num_ranges); // 3-4 + uint8_t use_ant_delay; // 5 + UN16(delay_tx); // 6-7 + UN16(delay_rx); // 8-9 + UN16(response_delay); // 10-11 + UN16(final_delay); // 12-13 + UN16(report_delay); // 14-15 + UN16(poll_period); // 16-17 + UN16(initiator_addr); // 18-19 + UN16(responder_addr); // 20-21 + uint8_t lna_on; // 22 + UN32(power); // 23-26 +}; + +typedef struct cmd_twr_asymm_s cmd_twr_asymm_t; + +/* RTLS_POWER_TEST_START_REQ */ +struct cmd_power_test_s { + uint8_t command; // 0 + uint8_t start; // 1 + UN16(duration); // 2-3 +}; + +typedef struct cmd_power_test_s cmd_power_test_t; + +/* RTLS_COMM_TEST_START_REQ */ +struct cmd_comm_test_s { + uint8_t command; // 0 + uint8_t transmit[1]; // 1 + UN16(data); // 2-3 +}; + +typedef struct cmd_comm_test_s cmd_comm_test_t; + +/* */ +struct request_configCle_s +{ + uint8_t head; + uint8_t uid[ADDR_BYTE_SIZE_L]; + char ver[64 - (ADDR_BYTE_SIZE_L + 1)]; // hardcoded to be 64 bytes long +}; + +typedef struct request_configCle_s request_configCle_t; + +#pragma pack(pop) + +#undef UN16 +#undef UN32 + +#ifdef __cplusplus +} +#endif +#endif //RTLS_INTERFACE_H diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/inc/uwb_frames.h b/bluetooth_uwb_dw3000_slotted_twr/src/inc/uwb_frames.h new file mode 100644 index 0000000..ad5c594 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/inc/uwb_frames.h @@ -0,0 +1,450 @@ +/** + * @file uwb_frames.h + * + * @brief UWB message frames definitions and typedefs + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef UWB_FRAMES_H_ +#define UWB_FRAMES_H_ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#define STANDARD_FRAME_SIZE 127 + +#define ADDR_BYTE_SIZE_L (8) +#define ADDR_BYTE_SIZE_S (2) + +#define TS_40B_SIZE (5) +#define TS_UWB_SIZE (5) + +#define FRAME_CONTROL_BYTES 2 +#define FRAME_SEQ_NUM_BYTES 1 +#define FRAME_PANID 2 +#define FRAME_CRC 2 +#define FRAME_SOURCE_ADDRESS_S (ADDR_BYTE_SIZE_S) +#define FRAME_DEST_ADDRESS_S (ADDR_BYTE_SIZE_S) +#define FRAME_SOURCE_ADDRESS_L (ADDR_BYTE_SIZE_L) +#define FRAME_DEST_ADDRESS_L (ADDR_BYTE_SIZE_L) +#define FRAME_CTRLP (FRAME_CONTROL_BYTES + FRAME_SEQ_NUM_BYTES \ + + FRAME_PANID) /* 5 */ +#define FRAME_CTRL_AND_ADDRESS_L \ + (FRAME_DEST_ADDRESS_L \ + + FRAME_SOURCE_ADDRESS_L + FRAME_CTRLP) /* 21 bytes + * for 64-bit addresses) + */ +#define FRAME_CTRL_AND_ADDRESS_S \ + (FRAME_DEST_ADDRESS_S \ + + FRAME_SOURCE_ADDRESS_S + FRAME_CTRLP) /* + * 9 bytes + * for 16-bit addresses) + */ +#define FRAME_CTRL_AND_ADDRESS_LS \ + (FRAME_DEST_ADDRESS_L \ + + FRAME_SOURCE_ADDRESS_S + FRAME_CTRLP) /* 15 bytes for 1 + * 16-bit address and + * 1 64-bit address) + */ +#define MAX_USER_PAYLOAD_STRING_LL \ + (STANDARD_FRAME_SIZE \ + - FRAME_CTRL_AND_ADDRESS_L - FRAME_CRC) /* 127 - 21 - 2 = 104 */ +#define MAX_USER_PAYLOAD_STRING_SS \ + (STANDARD_FRAME_SIZE \ + - FRAME_CTRL_AND_ADDRESS_S - FRAME_CRC) /* 127 - 9 - 2 = 116 */ +#define MAX_USER_PAYLOAD_STRING_LS \ + (STANDARD_FRAME_SIZE \ + - FRAME_CTRL_AND_ADDRESS_LS - FRAME_CRC)/* 127 - 15 - 2 = 110 */ + +#define FRAME_DEST_ADDRESS_S_IDX (FRAME_CTRLP) +#define FRAME_SRC_ADDRESS_S_IDX (FRAME_CTRLP + ADDR_BYTE_SIZE_S) +// NOTE: the user payload assumes that there are only 88 "free" bytes to be used +// for the user message (it does not scale according to the addressing modes) +#define MAX_USER_PAYLOAD_STRING MAX_USER_PAYLOAD_STRING_LL + +#define RC_VERSION_PDOA (3) +#define RC_VERSION_DR (4) + +enum { + Head_Msg_BLINK = 0xC5, + Head_Msg_STD = (0x40 | 0x01), + Head_Msg_STD_AR = (0x40 | 0x20 | 0x01), + Frame_Ctrl_SS = (0x80 | 0x08), // Message addressing: destination + // short (16-bit), source short + // (16-bit) + Frame_Ctrl_LS = (0x80 | 0x0C), // Message addressing: destination + // long (64-bit), source short + // (16-bit) + Frame_Ctrl_MASK = 0xCC +}; + +/* enumeration of function codes used in PDoA TWR protocol */ +typedef enum { + Twr_Fcode_Not_Defined = 0xFF, // Special : nothing + Twr_Fcode_Blink = 0xEE, // Special : Blink + Twr_Fcode_Rng_Config = 0x20, // Responder (Node) Ranging Config + // message : reply to blink + Twr_Fcode_Tag_Poll = 0x84, // Initiator (Tag) Poll message + // : twr start + // message + Twr_Fcode_Resp_Ext = 0x72, // Responder (Node) Response Extended + // : reply to Poll with + // X/Y previous results + Twr_Fcode_Tag_Final = 0x88, // Initiator (Tag) Final message back + // to Responder : reply to Response + Twr_Fcode_Tag_Accel_Final = 0x89, // Initiator (Tag) Final message back + // to Responder : reply to Response + + // Accelerometer data +}fcode_e; + +// TDOA TWR +enum { + RTLS_TWR_MSG_RNG_INIT = Twr_Fcode_Rng_Config, // Ranging + // initiation + // message + RTLS_TWR_MSG_TAG_POLL = 0x81, // Initiator (Tag) poll message + RTLS_TWR_MSG_ANCH_RESP = 0x70, // Responder (Anchor) response to + // poll + RTLS_TWR_MSG_TAG_FINAL = 0x82, // Initiator (Tag) final message + // back to Responder + RTLS_TWR_MSG_ANCH_TOFR = 0x71, // Responder (Anchor) TOF Report + // message to Initiator + + RTLS_MSG_ANCH_CLK_SYNC = 0x2C, // Anchor CLK SYNC message + // (broadcast) + + HEAD_MSG_BLINK = Head_Msg_BLINK, // Tag standard Blink + // message + HEAD_MSG_STD = Head_Msg_STD, // Std message header + + APP_MSG_DATA = 0x7A, // Data Transfer Message (this is + // Communications Test function + // code) + + APP_MSG_UWB_BH_DWNSTREAM = 0x7B, // UWB Command from CLE via Master + // to Slave + APP_MSG_UWB_BH_UPSTREAM = 0x7C, // UWB Data Backhaul back from Slave + // to CLE via Master +}; + +/* UWB packet types : MAC headers */ +typedef struct +{ + uint8_t frameCtrl[2]; // frame control bytes 00-01 + uint8_t seqNum; // sequence_number 02 + uint8_t panID[2]; // PAN ID 03-04 + uint8_t destAddr[ADDR_BYTE_SIZE_S]; // 05-06 + uint8_t sourceAddr[ADDR_BYTE_SIZE_S]; // 07-08 +}__attribute__((packed)) +mac_header_ss_t; + +typedef struct +{ + uint8_t frameCtrl[2]; // frame control bytes 00-01 + uint8_t seqNum; // sequence_number 02 + uint8_t panID[2]; // PAN ID 03-04 + uint8_t destAddr[ADDR_BYTE_SIZE_L]; // 05-12 or using 64 bit + // addresses (05-12) + uint8_t sourceAddr[ADDR_BYTE_SIZE_L]; // 13-20 or using 64 bit + // addresses (13-20) +}__attribute__((packed)) +mac_header_ll_t; + +typedef struct +{ + uint8_t frameCtrl[2]; // frame control bytes 00-01 + uint8_t seqNum; // sequence_number 02 + uint8_t panID[2]; // PAN ID 03-04 + uint8_t destAddr[ADDR_BYTE_SIZE_L]; // 05-12 using 64 bit addresses + uint8_t sourceAddr[ADDR_BYTE_SIZE_S]; // 13-14 +}__attribute__((packed)) +mac_header_ls_t; + +typedef struct +{ + uint8_t frameCtrl[2]; // frame control bytes 00-01 + uint8_t seqNum; // sequence_number 02 + uint8_t panID[2]; // PAN ID 03-04 + uint8_t destAddr[ADDR_BYTE_SIZE_S]; // 05-06 + uint8_t sourceAddr[ADDR_BYTE_SIZE_L]; // 7-14 using 64 bit addresses +}__attribute__((packed)) +mac_header_sl_t; + +/* General UWB packet types : Messages */ +typedef struct +{ + mac_header_ll_t mac; + uint8_t messageData[MAX_USER_PAYLOAD_STRING_LL]; // 21-124 + // (application + // data and any + // user payload) + uint8_t fcs[2]; // 125-126 we allow space for the CRC as it is + // logically part of the message. + // DW1000 calculates and adds these bytes. +}__attribute__((packed)) +std_msg_ll_t; + +typedef struct +{ + mac_header_ss_t mac; + uint8_t messageData[MAX_USER_PAYLOAD_STRING_SS]; // 09-124 + // (application + // data and any + // user payload) + uint8_t fcs[2]; +}__attribute__((packed)) +std_msg_ss_t; + +typedef struct +{ + mac_header_ls_t mac; + uint8_t messageData[MAX_USER_PAYLOAD_STRING_LS]; // 15-124 + // (application + // data and any + // user payload) + uint8_t fcs[2]; +}__attribute__((packed)) +std_msg_ls_t; + +// 12 octets for Minimum IEEE ID blink +typedef struct +{ + uint8_t frameCtrl[1]; // frame control bytes 00 + uint8_t seqNum; // sequence_number 01 + uint8_t tagID[ADDR_BYTE_SIZE_L]; // 02-09 64 bit addresses + uint8_t fcs[2]; +}__attribute__((packed)) +blink_msg_t; + +/* Ranging Config rest of configuration. + * The Node's address and PanId, are in the MAC header */ +typedef struct +{ + /* Compatibility to the EVK's Ranging Init */ + uint8_t fCode; + uint8_t tagAddr[ADDR_BYTE_SIZE_S]; // tag's short address + uint8_t ANC_RESP_DLY[2]; // backward compatibility to EVK1000 RI + // message : delayRx_us + poll_us, + // coded in us with bit 15 == 0, coded + // in ms when bit 15 == 1 + uint8_t TAG_RESP_DLY[2]; // backward compatibility to EVK1000 RI + // message : , coded in us with bit 15 + // == 0, coded in ms when bit 15 == 1 + + /* PDOA TWR unique message */ + uint8_t version; // version + + uint8_t sframePeriod_ms[2]; // Super Frame period, ms + uint8_t pollTxToFinalTx_us[2]; // time from the RMARKER of Poll to the + // RMARKER of Final that the Tag shall + // set. + uint8_t delayRx_us[2]; // time from end of transmission of the Poll + // to start of reception of Response, that + // the Tag shall set. + uint8_t slotCorr_us[4]; // Slot correction for current reception + // (i.e. for blink), us + uint8_t pollMultFast[2]; // multiplier for fast ranging in Super + // Frame counts + uint8_t pollMultSlow[2]; // multiplier for slow ranging in Super + // Frame counts + + union + { + uint8_t mode[2]; // bitfields for mode of operation: IMU + // on/off, etc. + struct + { + uint8_t imuOn : 1; // currently only IMU ON switch is + // defined/used + }; + }; +}__attribute__((packed)) +rng_cfg_t; + +typedef struct +{ + uint8_t fCode; // msgdata+0 + uint8_t rNum; +}__attribute__((packed)) +poll_t; + +typedef struct +{ + uint8_t fCode; // msgdata+0 + uint8_t slotCorr_us[4]; + uint8_t rNum; + uint8_t x_cm[2]; // X coordinate of Tag wrt Node in + // centimeters [0..65535cm] + uint8_t y_cm[2]; // Y coordinate of Tag wrt Node in + // centimeters [0..65535cm] + uint8_t clkOffset_pphm[2]; // part per (hundreds of millions) = + // (100*ppm) +}__attribute__((packed)) +resp_tag_t; + +typedef struct +{ + uint8_t fCode; // msgdata+0 + uint8_t rNum; + uint8_t pollTx_ts[TS_UWB_SIZE]; + uint8_t responseRx_ts[TS_UWB_SIZE]; + uint8_t finalTx_ts[TS_UWB_SIZE]; + uint8_t flag; // 1 data bytes bitfields. IMU:0 + uint8_t acc_x[2]; // Normalized accel data X from the + // Tag, mg + uint8_t acc_y[2]; // Normalized accel data Y from the + // Tag, mg + uint8_t acc_z[2]; // Normalized accel data Z from the + // Tag, mg +}__attribute__((packed)) +final_accel_t; + +/* UWB packet types : Application-specific messages */ + +/* Ranging Config message during Discovery + * the Node's address and PanId are in the MAC header */ +typedef struct +{ + mac_header_ls_t mac; + rng_cfg_t rngCfg; + uint8_t fcs[2]; +}__attribute__((packed)) +rng_cfg_msg_t; + +typedef struct +{ + mac_header_ss_t mac; + rng_cfg_t rngCfg; + uint8_t fcs[2]; +}__attribute__((packed)) +rng_cfg_upd_msg_t; + +typedef struct +{ + mac_header_ss_t mac; + poll_t poll; + uint8_t fcs[2]; +}__attribute__((packed)) +poll_msg_t; + +typedef struct +{ + mac_header_ss_t mac; + resp_tag_t resp; + uint8_t fcs[2]; +}__attribute__((packed)) +resp_pdoa_msg_t; + +typedef struct +{ + mac_header_ss_t mac; + final_accel_t final; + uint8_t fcs[2]; +}__attribute__((packed)) +final_msg_accel_t; + +typedef struct +{ + mac_header_sl_t mac; + uint8_t messageData[1]; // 09 or 15 + // header(1) + uint8_t fcs[2]; // 15-16 or + // 21-22 we + // allow space + // for the CRC + // as it is + // logically + // part of the + // message. + // However + // ScenSor TX + // calculates + // and adds + // these bytes. +}__attribute__((__packed__)) +ccp_msg_t; + +typedef struct +{ + mac_header_sl_t mac; + uint8_t messageData[MAX_USER_PAYLOAD_STRING_LS]; // 15-124 (application + // data and any user + // payload) + uint8_t fcs[2]; // 125-126 we allow + // space for the CRC as + // it is logically part + // of the message. + // However ScenSor TX + // calculates and adds + // these bytes. +}__attribute__((packed)) +bcast_msg_t; + +// Up to 42 octets for extended blink message with IMU and other data as defined +// below +typedef struct +{ + uint8_t frameCtrl[1]; // frame control bytes 00 + uint8_t seqNum; // sequence_number 01 + uint8_t tagID[ADDR_BYTE_SIZE_L]; // 02-09 64 bit addresses + uint8_t ench; // 10 - encoding header + // (0x43: No Extended ID, no + // Temperature Data, No + // Battery Status) + uint8_t exth; // 11 - extension header + // (0x00: No blink + // Rate/Listening or other + // Data) + uint8_t dwh; // 12 - Decawave Blink + // Header: 0x01: SENSOR DATA + // TYPE 1 + // [According to ISO + // 24730-62, this octet and + // any subsequent data can + // only be interpreted if we + // identify from the Tag ID + // field that this is a + // Decawave tag] + uint8_t dwp[27]; // 13 - 39 Decawave Blink + // Payload – Defined below + // for Sensor Data Type 1 + // [0] - mask, [1] DWtemp, + // [2] DWvbat, [3] DWGPIO, + // [4-9] Magneto, [10-15] + // Accel, [16-21] Gyro, + // [22-24] Atmosph Pressure, + // [25-26] Atmosph Temp + uint8_t fcs[2]; // 40-41 we allow space for + // the CRC as it is + // logically part of the + // message. However ScenSor + // TX calculates and adds + // these bytes. +}__attribute__((packed)) +blinkIMU_msg_t; + +typedef struct +{ + uint8_t frameCtrl[2]; // frame control bytes + // 00-01 + uint8_t seqNum; // sequence_number 02 + uint8_t fcs[2]; // 03-04 CRC +}__attribute__((packed)) +ack_msg_t; + +typedef std_msg_ll_t std_msg_t; +typedef std_msg_ss_t twr_msg_t; + +#ifdef __cplusplus +} +#endif + +#endif /* UWB_FRAMES_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/inc/version.h b/bluetooth_uwb_dw3000_slotted_twr/src/inc/version.h new file mode 100644 index 0000000..6903505 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/inc/version.h @@ -0,0 +1,111 @@ +/** + * @file version.h + * + * @brief version number + * Construct the version name as "MAJOR.MINOR.YYMMDD" + * VER_MAJOR 0..999 + * VER_MINOR 0..999 + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef VERSION_H_ +#define VERSION_H_ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#define VER_MAJOR 6 +#define VER_MINOR 0 + +/** + * construction of the version name as "MAJOR.MINOR.YYMMDD" + * e.g. "Oct 7 2014" => "141007" + **/ + +#define YEAR_CH2 (__DATE__[9]) +#define YEAR_CH3 (__DATE__[10]) + +#define MONTH_IS_JAN (__DATE__[0] == 'J' && __DATE__[1] == 'a' \ + && __DATE__[2] == 'n') +#define MONTH_IS_FEB (__DATE__[0] == 'F') +#define MONTH_IS_MAR (__DATE__[0] == 'M' && __DATE__[1] == 'a' \ + && __DATE__[2] == 'r') +#define MONTH_IS_APR (__DATE__[0] == 'A' && __DATE__[1] == 'p') +#define MONTH_IS_MAY (__DATE__[0] == 'M' && __DATE__[1] == 'a' \ + && __DATE__[2] == 'y') +#define MONTH_IS_JUN (__DATE__[0] == 'J' && __DATE__[1] == 'u' \ + && __DATE__[2] == 'n') +#define MONTH_IS_JUL (__DATE__[0] == 'J' && __DATE__[2] == 'l') +#define MONTH_IS_AUG (__DATE__[0] == 'A' && __DATE__[1] == 'u') +#define MONTH_IS_SEP (__DATE__[0] == 'S') +#define MONTH_IS_OCT (__DATE__[0] == 'O') +#define MONTH_IS_NOV (__DATE__[0] == 'N') +#define MONTH_IS_DEC (__DATE__[0] == 'D') + +#define MONTH_CH0 ((MONTH_IS_OCT || MONTH_IS_NOV \ + || MONTH_IS_DEC) ? '1' : '0') + +#define MONTH_CH1 \ + ((MONTH_IS_JAN) ? '1' : \ + (MONTH_IS_FEB) ? '2' : \ + (MONTH_IS_MAR) ? '3' : \ + (MONTH_IS_APR) ? '4' : \ + (MONTH_IS_MAY) ? '5' : \ + (MONTH_IS_JUN) ? '6' : \ + (MONTH_IS_JUL) ? '7' : \ + (MONTH_IS_AUG) ? '8' : \ + (MONTH_IS_SEP) ? '9' : \ + (MONTH_IS_OCT) ? '0' : \ + (MONTH_IS_NOV) ? '1' : \ + (MONTH_IS_DEC) ? '2' : \ + /* default */ '?' \ + ) + +#define DAY_CH0 ((__DATE__[4] >= '0') ? (__DATE__[4]) : '0') +#define DAY_CH1 (__DATE__[5]) + +#if VER_MAJOR >= 100 +#define VMAJOR ((VER_MAJOR / 100) + '0'), \ + (((VER_MAJOR % 100) / 10) + '0'), \ + ((VER_MAJOR % 10) + '0') +#elif VER_MAJOR >= 10 +#define VMAJOR ((VER_MAJOR / 10) + '0'), \ + ((VER_MAJOR % 10) + '0') +#else +#define VMAJOR (VER_MAJOR + '0') +#endif + +#if VER_MINOR >= 100 +#define VMINOR ((VER_MINOR / 100) + '0'), \ + (((VER_MINOR % 100) / 10) + '0'), \ + ((VER_MINOR % 10) + '0') +#elif VER_MINOR >= 10 +#define VMINOR ((VER_MINOR / 10) + '0'), \ + ((VER_MINOR % 10) + '0') +#else +#define VMINOR (VER_MINOR + '0') +#endif + +/* VERSION */ +#define FULL_VERSION { VMAJOR, '.', VMINOR, '.', \ + YEAR_CH2, YEAR_CH3, \ + MONTH_CH0, MONTH_CH1, \ + DAY_CH0, DAY_CH1, \ + '\0' } + +#define DATE_VERSION { YEAR_CH2, YEAR_CH3, MONTH_CH0, MONTH_CH1, DAY_CH0, \ + DAY_CH1, '\0' } + +#define RTLS_APP_VERSION FULL_VERSION + +#ifdef __cplusplus +} +#endif + +#endif // VERSION_H_ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/main.c b/bluetooth_uwb_dw3000_slotted_twr/src/main.c new file mode 100644 index 0000000..d0e3ead --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/main.c @@ -0,0 +1,339 @@ +/*! ---------------------------------------------------------------------------- + * @file main.c + * @brief This is the a variant of implementation of the Slotted TWR PDoA Node + * on Nordic nRF52840 platform with FreeRTOS + * + * @author Decawave Applications + * + * @attention Copyright 2017-2019 (c) DecaWave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +/* Includes ------------------------------------------------------------------*/ + +#include "em_gpio.h" +#include "sl_system_init.h" +#include "sl_event_handler.h" +#include "port.h" +#include "app.h" +#include "crc16.h" +#include "config.h" +#include "task_usb2spi.h" +#include "node.h" +#include "task_node.h" +#include "task_tag.h" +#include "task_tcfm.h" +#include "task_tcwm.h" +#include "task_ctrl.h" +#include "task_flush.h" +#include "deca_dbg.h" +#include "deca_probe_interface.h" +#include "task_listener.h" + +#define BUTTON_TIME_TAG_MS (3000) +#define USB_DRV_UPDATE_MS 5000 +#define DEFAULT_EVENT (Ev_Stop_All) + +/* Private variables ---------------------------------------------------------*/ +osThreadId_t defaultTaskHandle; +gDiagPrintFStr_t gDiagPrintFStr; +app_t app; /**< All global variables are in the "app" structure + */ + +// ----------------------------------------------------------------------------- +void trilat_helper(void const *argument); +void trilat_terminate(void); + +static void StartDefaultTask(void const *argument); + +/** + * @brief Function for application main entry. + */ +int main(void) +{ + // Initialize Silicon Labs device, system, service(s) and protocol stack(s). + // Note that if the kernel is present, processing task(s) will be created by + // this call. + sl_system_init(); + + wdt_init(30000); + rtc_init(); + init_timer(); + port_init_dw_chip(); + dw_irq_init(); + + init_crc16(); + + load_bssConfig(); /**< load the RAM Configuration parameters + * from NVM block */ + app.pConfig = get_pbssConfig(); + app.xStartTaskEvent = xEventGroupCreate(); /**< xStartTaskEvent indicates + * which tasks to be started */ + +#ifdef ENABLE_USB_PRINT + if (!app.pConfig->s.uartEn) { + deca_usb_init(); + } else +#endif + { + deca_uart_init(); + } + + reset_DW3000(); // this will reset DW device + + if (dwt_probe((struct dwt_probe_s *)&dw3000_probe_interf)) { + error_handler(1, _ERR_INIT); + } + + /* This initialization is added to avoid crashing the board when calling APIs + * that writes inside local data + * like setxtaltrim */ + if (dwt_initialise(DWT_DW_INIT) != DWT_SUCCESS) { + error_handler(1, _ERR_INIT); + } + + /* initialize inter-task communication mail queue for Node : + * + * The RxTask need to send the rxPckt to the CalcTask. + * + * TODO: rxPcktPool_q_id should be a part of NodeInfo, but + * FreeRTOS cannot free resources efficiently on task deletion. + * + * Current code has an implementation where NodeInfo is statically defined + * and rxPcktPool_q is a part of FreeRtos Heap. + * + * Note, the debug accumulator & diagnostics readings are a part of + * mail queue. Every rx_mail_t has a size of ~6kB. + * + * */ + app.rxPcktPool_q_id = osMemoryPoolNew(RX_MAIL_QUEUE_SIZE, + sizeof(rx_mail_t), + NULL); + app.rxPcktQueue_q_id = + osMessageQueueNew(RX_MAIL_QUEUE_SIZE, sizeof(rx_mail_t *), NULL); + if (!app.rxPcktPool_q_id) { + error_handler(1, _ERR_Cannot_Alloc_Mail); + } + + /* Create the thread(s) + * definition and creation of defaultTask + * Note. The DefaultTask is responsible for starting & stopping of TOP Level + * applications. + */ + CREATE_NEW_TASK(StartDefaultTask, + NULL, + "defaultTask", + configMINIMAL_STACK_SIZE * 2, + PRIO_StartDefaultTask, + &defaultTaskHandle); + + /* FlushTask is always working and flushing the output buffer to uart/usb */ + CREATE_NEW_TASK(FlushTask, + NULL, + "flushTask", + configMINIMAL_STACK_SIZE, + PRIO_FlushTask, + &app.flushTask.Handle); + app.flushTask.Signal = 1; + + /* ctrlTask is always working serving rx from uart/usb */ + // 2K for CTRL task: it needs a lot of memory: it uses mallocs(512), + // sscanf(212bytes) + CREATE_NEW_TASK(CtrlTask, + NULL, + "ctrlTask", + 512, + PRIO_CtrlTask, + &app.ctrlTask.Handle); + app.ctrlTask.Signal = 1; + + if (!defaultTaskHandle | !app.flushTask.Handle | !app.ctrlTask.Handle) { + error_handler(1, _ERR_Create_Task_Bad); + } + + /* Start scheduler */ + sl_kernel_start(); + + /* We should never get here as control is now taken by the scheduler */ + while (1) + { + } +} + +static EventBits_t check_the_user_button(void) +{ + EventBits_t ret = DEFAULT_EVENT; + +#if defined(HAL_IO_PORT_BUTTON) && defined(HAL_IO_PIN_BUTTON) + GPIO_PinModeSet(HAL_IO_PORT_BUTTON, HAL_IO_PIN_BUTTON, gpioModeInput, 0); + if (GPIO_PinInGet(HAL_IO_PORT_BUTTON, + HAL_IO_PIN_BUTTON) != HAL_IO_BUTTON_ACTIVE_STATE) { + if (app.pConfig->s.default_event != 0) { + ret = app.pConfig->s.default_event; + } + } else { + uint32_t tmr; + start_timer(&tmr); + bool tmp = false; + + while ((GPIO_PinInGet(HAL_IO_PORT_BUTTON, + HAL_IO_PIN_BUTTON) == HAL_IO_BUTTON_ACTIVE_STATE) + && !(tmp = check_timer(tmr, BUTTON_TIME_TAG_MS))) + { + wdt_refresh(); + } + ret = (tmp) ? (Ev_Node_Task) : (Ev_Tag_Task); + } +#endif + return ret; +} + +/* StartDefaultTask function */ +static void StartDefaultTask(void const *argument) +{ + (void)argument; + + const EventBits_t bitsWaitForAny = + (Ev_Node_Task | Ev_Tag_Task | Ev_Trilat_N_Task + | Ev_Usb2spi_Task | Ev_Tcwm_Task + | Ev_Tcfm_Task | Ev_Listener_Task | Ev_Stop_All); + + EventBits_t uxBits; + + uxBits = check_the_user_button(); + + xEventGroupSetBits(app.xStartTaskEvent, uxBits); + + /* Infinite loop */ + while (1) + { + uxBits = xEventGroupWaitBits(app.xStartTaskEvent, bitsWaitForAny, + pdTRUE, pdFALSE, + USB_DRV_UPDATE_MS / portTICK_PERIOD_MS); + + wdt_refresh(); // WDG_Refresh + + uxBits &= bitsWaitForAny; + + if (uxBits) { + app.lastErrorCode = _NO_ERR; + + /* need to switch off DW chip's RX and IRQ before killing tasks */ + if (app.mode != mIDLE) { + disable_dw3000_irq(); + reset_DW3000(); + dwt_setcallbacks(NULL, + NULL, + NULL, + NULL, + NULL, + NULL, + NULL); /* DW_IRQ is disabled: safe to cancel + * all user call-backs + */ + } + + /* Event to start/stop task received */ + /* 1. free the resources: kill all user threads and timers */ + tag_terminate(); + node_terminate(); + usb2spi_terminate(); + tcfm_terminate(); + tcwm_terminate(); + trilat_terminate(); + listener_terminate(); + + FlushTask_reset(); + + app.lastErrorCode = _NO_ERR; + + // incoming Events are slow, and usually User-events, i.e. from a slow + // I/O, + // however the Ev_Stop_All can be generated internally and may OR with + // other event, + // because they can be received asynchronously. + // Ev_Stop_All event should be tracked separately. + app.mode = mIDLE; + uxBits &= ~Ev_Stop_All; + } + + wdt_refresh(); // WDG_Refresh + osThreadYield(); // force switch of context + + taskENTER_CRITICAL(); + + /* 2. Start appropriate RTOS top-level application or run a + * usb_vbus_driver() if a dummy loop */ + switch (uxBits) + { + case Ev_Listener_Task: + app.mode = mLISTENER; + listener_helper(NULL); /* call Listener helper function which will + * setup sub-tasks for Listener process + */ + break; + +#if (PDOA_TAG == 1) + + /* PDoA */ + case Ev_Tag_Task: + app.mode = mPTAG; + tag_helper(NULL); /* call Tag helper function which will setup + * sub-tasks for Tag process */ + break; +#endif + +#if (PDOA_NODE == 1) + case Ev_Node_Task: + app.mode = mPNODE; + node_helper(NULL); /* call Node helper function which will setup + * sub-tasks for Node process */ + break; +#endif + +#if (CFG_LE_TRILAT == 1) + case Ev_Trilat_N_Task: + app.mode = mTRILAT_N; + trilat_helper(NULL); /* call Trilat helper function which will + * setup sub-tasks for Node process & + * Trilat */ + break; +#endif + + /* Service apps */ + case Ev_Usb2spi_Task: + /* Setup a Usb2Spi task : 8K of stack is required to this task */ + app.mode = mUSB2SPI; + usb2spi_helper(NULL); + break; + + case Ev_Tcfm_Task: + /* Setup a TCFM task */ + app.mode = mTCFM; + tcfm_helper(NULL); /* call tcfm helper function which will + * setup sub-tasks for Power test */ + break; + + case Ev_Tcwm_Task: + /* Setup a TCWM task */ + app.mode = mTCWM; + tcwm_helper(NULL); /* call tcwm helper function which will + * setup sub-tasks for Power test */ + break; + + case Ev_Stop_All: + app.mode = mIDLE; + break; + + default: + break; + } + taskEXIT_CRITICAL(); // ready to switch to a created task + + wdt_refresh(); // WDG_Refresh + osThreadYield(); + } +} + +/** @} */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/deca_uart.c b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/deca_uart.c new file mode 100644 index 0000000..09db1bf --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/deca_uart.c @@ -0,0 +1,298 @@ +/*! ---------------------------------------------------------------------------- + * @file deca_uart.c + * @brief HW specific definitions and functions for UART Interface + * + * @attention + * + * Copyright 2016 (c) DecaWave Ltd, Dublin, Ireland. + * + * All rights reserved. + * + * @author Deepa Gopinath + */ + +#include "port_common.h" +#ifdef HAL_UART_PERIPHERAL +#include +#include "app.h" +#include "uartdrv.h" +#include "sl_common.h" +#ifdef SL_CATALOG_POWER_MANAGER_PRESENT +#include "sl_power_manager.h" +#endif +#include "sl_sleeptimer.h" +#include "gpiointerrupt.h" + +#define UART_RX_IRQ_NUMBER HAL_CAT3(HAL_UART_TYPE, \ + HAL_UART_NUMBER, \ + _RX_IRQn) +#define UART_RX_IRQ_HANDLER HAL_CAT3(HAL_UART_TYPE, \ + HAL_UART_NUMBER, \ + _RX_IRQHandler) + +#if HAL_UART_IS_EUART +#define UART_INIT_STRUCTURE_T UARTDRV_InitEuart_t +#define uart_init(handle, init_data) UARTDRV_InitEuart(handle, \ + init_data) +#define uart_is_rx_data_available(eusart) (EUSART_STATUS_RXFL & \ + EUSART_StatusGet(eusart)) +#define uart_rx_data_get(eusart) EUSART_Rx(eusart) +#define uart_is_rx_timeout(eusart) (EUSART_IF_RXTO & EUSART_IntGet( \ + eusart)) +#define uart_clear_all_interrupt(eusart) EUSART_IntClear(eusart, \ + _EUSART_IF_MASK) +#define uart_rx_timeout_start(eusart) \ + do { \ + EUSART_Enable(eusart, eusartDisable); \ + eusart->CFG1 |= EUSART_CFG1_RXTIMEOUT_SEVENFRAMES; \ + EUSART_Enable(eusart, eusartEnable); \ + EUSART_IntEnable(eusart, EUSART_IF_RXTO); \ + }while (0) +#define uart_rx_timeout_restart(eusart) +#define uart_blocking_tx(eusart, data) EUSART_Tx(eusart, data) +#else +#define UART_RX_TIMEOUT_CFG (USART_TIMECMP1_TSTOP_RXACT \ + | USART_TIMECMP1_TSTART_RXEOF \ + | (0xff << \ + _USART_TIMECMP1_TCMPVAL_SHIFT)) +#define UART_INIT_STRUCTURE_T UARTDRV_InitUart_t +#define uart_init(handle, init_data) UARTDRV_InitUart(handle, init_data) +#define uart_is_rx_data_available(usart) (USART_STATUS_RXDATAV & \ + USART_StatusGet(usart)) +#define uart_rx_data_get(usart) USART_RxDataGet(usart) +#define uart_is_rx_timeout(usart) (USART_IF_TCMP1 & USART_IntGet( \ + usart)) +#define uart_clear_all_interrupt(usart) USART_IntClear(usart, \ + _USART_IF_MASK) +#define uart_rx_timeout_start(usart) \ + do { \ + usart->TIMECMP1 = UART_RX_TIMEOUT_CFG; \ + USART_IntEnable(usart, USART_IF_TCMP1); \ + }while (0) +#define uart_rx_timeout_restart(usart) \ + do { \ + usart->TIMECMP1 &= ~_USART_TIMECMP1_TSTART_MASK; \ + usart->TIMECMP1 = UART_RX_TIMEOUT_CFG; \ + }while (0) +#define uart_blocking_tx(usart, data) USART_Tx(usart, data) +#endif +// +#ifndef HAL_UART_SEND_BLOCKING +#define HAL_UART_SEND_BLOCKING 1 +#endif +#if !HAL_UART_SEND_BLOCKING && !defined(HAL_UART_SIMPLE_ASYNCH_SEND) +// Simple asynchronous send expects a static buffer until the send is finished, +// currently it is not always true. +#define HAL_UART_SIMPLE_ASYNCH_SEND 0 +#endif + +static void deca_uart_receive_callback(UARTDRV_Handle_t handle, + Ecode_t transferStatus, + uint8_t *data, + UARTDRV_Count_t transferCount); +static inline void deca_uart_update_fifo(uint32_t received_data_count); + +static UARTDRV_HandleData_t uart_driver_handle; +static uint32_t uart_driver_receive_items_remaining; +// Define RX and TX buffer data queues +DEFINE_BUF_QUEUE(1, uart_driver_rx_buffer); // continuous receive into a global + // buffer --> no need for multiple + // receive queues +DEFINE_BUF_QUEUE(16, uart_driver_tx_buffer); + +/* @fn deca_uart_init + * + * @brief Function for initializing the UART module. + * + * @param[in] void + * */ +void deca_uart_init(void) +{ + const UART_INIT_STRUCTURE_T init_data = { + .port = HAL_UART_PERIPHERAL, + .baudRate = HAL_UART_BAUDRATE, + .txPort = HAL_UART_TX_PORT, + .rxPort = HAL_UART_RX_PORT, + .txPin = HAL_UART_TX_PIN, + .rxPin = HAL_UART_RX_PIN, + .uartNum = HAL_UART_NUMBER, + .stopBits = HAL_UART_STOP_BITS, + .parity = HAL_UART_PARITY, + .oversampling = HAL_UART_OVERSAMPLING, + .fcType = uartdrvFlowControlNone, + .rxQueue = (void *)&uart_driver_rx_buffer, + .txQueue = (void *)&uart_driver_tx_buffer, + }; + uart_init(&uart_driver_handle, &init_data); + +#if defined(HAL_UART_ENABLE_PORT) && defined(HAL_UART_ENABLE_PIN) + GPIO_PinModeSet(HAL_UART_ENABLE_PORT, HAL_UART_ENABLE_PIN, gpioModePushPull, + 1); +#endif +#ifdef SL_CATALOG_POWER_MANAGER_PRESENT + sl_power_manager_add_em_requirement(SL_POWER_MANAGER_EM1); +#endif + + // Clear pending RX interrupt flag in NVIC + NVIC_ClearPendingIRQ(UART_RX_IRQ_NUMBER); + NVIC_EnableIRQ(UART_RX_IRQ_NUMBER); + uart_clear_all_interrupt(HAL_UART_PERIPHERAL); + + uart_rx_timeout_start(HAL_UART_PERIPHERAL); + deca_uart_receive(); +} + +void deca_uart_close(void) +{ + UARTDRV_Abort(&uart_driver_handle, uartdrvAbortAll); + UARTDRV_DeInit(&uart_driver_handle); +#if defined(HAL_UART_ENABLE_PORT) && defined(HAL_UART_ENABLE_PIN) + GPIO_PinModeSet(HAL_UART_ENABLE_PORT, HAL_UART_ENABLE_PIN, gpioModeDisabled, + 0); +#endif + + // clean internal data + app.uartRx.head = 0; + app.uartRx.tail = 0; + uart_driver_receive_items_remaining = 0; + memset(&uart_driver_handle, 0, sizeof(uart_driver_handle)); + +#ifdef SL_CATALOG_POWER_MANAGER_PRESENT + // Allow deeper sleep state + sl_power_manager_remove_em_requirement(SL_POWER_MANAGER_EM1); +#endif +} + +/* @fn deca_uart_transmit + * + * @brief Function for transmitting data on UART + * + * @param[in] ptr char Pointer that contains the base address of the data + * buffer. + * @param[in] size int The size of the data buffet to be sent. + * @return Returns a related error code. + * + * */ +int deca_uart_transmit(const char *ptr, int size) +{ + if ((NULL == ptr) || (size <= 0)) { + return -1; + } + +#if HAL_UART_SEND_BLOCKING + // Blocking transmit + for (int i = 0; i < size; i++) + { + uart_blocking_tx(HAL_UART_PERIPHERAL, ptr[i]); + } + return 0; +#elif HAL_UART_SIMPLE_ASYNCH_SEND + return UARTDRV_Transmit(&uart_driver_handle, ptr, size, NULL); +#else + // Will be used as a WR only buffer. + // ignore overwrite because it should not happen if printing speed and buffer + // size is chosen correctly. + static data_circ_buf_t tx_fifo; + + // We must provide continuous buffer to UARTDRV_Transmit() + int sc = 0; + const size_t remaining_space = sizeof(tx_fifo.buf) - tx_fifo.tail; + if (size > remaining_space) { + // copy data to static buffer + memcpy(&tx_fifo.buf[tx_fifo.tail], ptr, remaining_space); + sc |= UARTDRV_Transmit(&uart_driver_handle, + &tx_fifo.buf[tx_fifo.tail], + remaining_space, + NULL); + // Update pointers + size -= remaining_space; + ptr += remaining_space; + tx_fifo.tail = 0; + } + if (size) { + memcpy(&tx_fifo.buf[tx_fifo.tail], ptr, size); + sc |= UARTDRV_Transmit(&uart_driver_handle, + &tx_fifo.buf[tx_fifo.tail], + size, + NULL); + tx_fifo.tail = (tx_fifo.tail + size) & (sizeof(uartRx->buf) - 1); + } + return sc; +#endif +} + +/* @fn deca_uart_receive + * + * @brief Function for receive data from UART and store into rx_buf + * (global array). + * + * @param[in] void + * */ +void deca_uart_receive(void) +{ + if (uart_driver_handle.peripheral.uart // initialized + && (0 == uart_driver_receive_items_remaining) // receiving is completed + && (CIRC_SPACE(app.uartRx.head, app.uartRx.tail, + sizeof(app.uartRx.buf)) > 0)) { // have free space + // Start reception with continuous memory blocks + int start_idx = app.uartRx.head; + int tail = app.uartRx.tail; + uart_driver_receive_items_remaining = + (start_idx + < tail) ? (tail - start_idx) : (int)sizeof(app.uartRx.buf) - start_idx; + UARTDRV_Receive(&uart_driver_handle, + &app.uartRx.buf[start_idx], + uart_driver_receive_items_remaining, + deca_uart_receive_callback); + } +} + +void UART_RX_IRQ_HANDLER(void) +{ + // RX timeout, stop transfer and handle what we got in buffer + if (uart_is_rx_timeout(HAL_UART_PERIPHERAL)) { + uart_rx_timeout_restart(HAL_UART_PERIPHERAL); + + // notify upper layer (receive restart not needed) + int items_remaining = 0; + if (ECODE_OK + == DMADRV_TransferRemainingCount(uart_driver_handle.rxDmaCh, + &items_remaining)) { + deca_uart_update_fifo( + uart_driver_receive_items_remaining - items_remaining); + } + } + uart_clear_all_interrupt(HAL_UART_PERIPHERAL); +} + +static void deca_uart_receive_callback(UARTDRV_Handle_t handle, + Ecode_t transferStatus, + uint8_t *data, + UARTDRV_Count_t transferCount) +{ + (void)handle; + (void)data; + (void)transferCount; + + if (ECODE_OK == transferStatus) { // DMA transfer completed + deca_uart_update_fifo(uart_driver_receive_items_remaining); + deca_uart_receive(); + } +} + +static inline void deca_uart_update_fifo(uint32_t received_data_count) +{ + uart_driver_receive_items_remaining -= received_data_count; + + // data is received directly to app.uartRx.buf just update the head + app.uartRx.head = (app.uartRx.head + received_data_count) + & (sizeof(app.uartRx.buf) - 1); + + // Notify upper layer + if (app.ctrlTask.Handle) { // RTOS : ctrlTask could be not started yet + // signal to the ctrl thread : USB data ready + osThreadFlagsSet(app.ctrlTask.Handle, app.ctrlTask.Signal); + } +} + +#endif //HAL_UART_PERIPHERAL diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/deca_usb.c b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/deca_usb.c new file mode 100644 index 0000000..39ac5bd --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/deca_usb.c @@ -0,0 +1,30 @@ +/*! ---------------------------------------------------------------------------- + * @file deca_usb.c + * @brief HW specific definitions and functions for USB Interface + * + * @author Decawave + * + * @attention + * + * Copyright 2018 (c) Decawave Ltd, Dublin, Ireland. + * + * All rights reserved. + * + */ + +// +// USB IS NOT SUPPORTED BY EFR32xG FAMILY! +// + +int deca_usb_transmit(char *tx_buffer, int size) +{ + (void)tx_buffer; + (void)size; + return -1; +} + +void deca_usb_init(void) +{ +} + +/** @} */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_common.h b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_common.h new file mode 100644 index 0000000..e968c96 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_common.h @@ -0,0 +1,231 @@ +/** + * @file port.h + * + * @brief port headers file to EFR32BG22 + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef PORT_COMMON__H_ +#define PORT_COMMON__H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "app.h" +#include "error.h" +#include "port_uart.h" +#include "deca_spi.h" +#include "deca_device_api.h" +#include "cmsis_os2.h" // only for the OS priority definitions +#include "FreeRTOS.h" +#include "FreeRTOSConfig.h" +#include "task.h" +#include "semphr.h" + +// ----------------------------------------------------------------------------- + +/* Definitions */ + +enum Priorities { + /* Interrupts, which cannot use FreeRTOS API functions */ + PRIO_SPI_IRQn = configMAX_SYSCALL_INTERRUPT_PRIORITY - 2, + + /* Interrupt safe FreeRTOS API functions below is CMSIS IRQ's */ + PRIO_RTC_WKUP_IRQn = configMAX_SYSCALL_INTERRUPT_PRIORITY, // equivalent + // to the + // highest + // in the + // FreeRTOS + PRIO_USART_IRQn = configMAX_SYSCALL_INTERRUPT_PRIORITY + 1, + + /* Application-specific priorities : CMSIS-OS priorities +3..0..-3 + * osPriorityRealtime is not used by Application level + * osPriorityHigh => configMAX_SYSCALL_INTERRUPT_PRIORITY + ? + * osPriorityAboveNormal + * + * */ + PRIO_FlushTask = osPriorityAboveNormal, /* FlushTask should have + * higher priority than + * CalckTask */ + PRIO_CtrlTask = osPriorityNormal, + PRIO_StartDefaultTask = osPriorityLow, + + PRIO_RxTask = osPriorityHigh, + PRIO_CalcTask = osPriorityNormal, + PRIO_TrilatTask = osPriorityBelowNormal, + + PRIO_TagPollTask = osPriorityHigh, + PRIO_TagRxTask = osPriorityHigh, + PRIO_BlinkTask = osPriorityNormal, + + PRIO_TcfmTask = osPriorityNormal, + PRIO_TcwmTask = osPriorityNormal, + PRIO_Usb2SpiTask = osPriorityNormal +}; + +typedef enum { + Twr_Tx_Done, + Twr_Tx_Blink_Sent, // tag sends blink + Twr_Tx_Ranging_Config_Sent, // node sends range init + Twr_Tx_Poll_Sent, // tag sends poll + Twr_Tx_Resp_Sent, // node sends response + Twr_Tx_Final_Sent, // tag sends final +} tx_states_e; + +typedef enum +{ + DW_HAL_NODE_UNLOCKED, + DW_HAL_NODE_LOCKED +} dw_hal_lockTypeDef; + +#define __HAL_LOCK_FAIL_RETURN(__HANDLE__, RET_VAL) \ + do{ \ + if ((__HANDLE__)->lock == DW_HAL_NODE_LOCKED) { \ + return RET_VAL; \ + } else { \ + (__HANDLE__)->lock = DW_HAL_NODE_LOCKED; \ + } \ + }while (0U) + +#define __HAL_LOCK(__HANDLE__) \ + do{ \ + if ((__HANDLE__)->lock == DW_HAL_NODE_LOCKED) { \ + return DW_HAL_NODE_LOCKED; \ + } else { \ + (__HANDLE__)->lock = DW_HAL_NODE_LOCKED; \ + } \ + }while (0U) + +#define __HAL_UNLOCK(__HANDLE__) \ + do{ \ + (__HANDLE__)->lock = DW_HAL_NODE_UNLOCKED; \ + }while (0U) + +// ----------------------------------------------------------------------------- +// common macros + +#ifndef UNUSED +#define UNUSED(x) (void)(x) +#endif + +#ifndef CIRC_CNT +#define CIRC_CNT(head, tail, size) (((head) - (tail)) & ((size) - 1)) +#endif /* Return count in buffer. */ + +#ifndef CIRC_SPACE +#define CIRC_SPACE(head, tail, size) CIRC_CNT((tail), ((head) + 1), (size)) +#endif /* Return space available, 0..size-1 */ + +#ifndef SWAP +#define SWAP(a, b) { a ^= b; b ^= a; a ^= b; } +#endif /* SWAP */ + +#ifndef MIN +#define MIN(a, b) (((a) < (b)) ? (a) : (b)) +#endif /* MIN */ + +#ifndef MAX +#define MAX(a, b) (((a) > (b)) ? (a) : (b)) +#endif /* MAX */ + +#ifndef TRUE +#define TRUE 1 +#endif /* TRUE */ + +#ifndef FALSE +#define FALSE 0 +#endif /* FALSE */ + +#define MASK_40BIT (0x00FFFFFFFFFFULL) // DW1000 counter is 40 bits +#define MASK_TXDTS (0x00FFFFFFFE00ULL) // The TX timestamp will snap + // to 8 ns resolution - + // mask lower 9 bits. + +// ----------------------------------------------------------------------------- +// experimental FREERtos macros: TODO: + +#define TERMINATE_STD_TASK(x) \ + do{ \ + if ((x).Handle) { \ + if ((x).MutexId) { \ + xSemaphoreTake((x).MutexId, osWaitForever); \ + taskENTER_CRITICAL(); \ + xSemaphoreGive((x).MutexId); \ + } else { \ + taskENTER_CRITICAL(); \ + } \ + vTaskDelete((x).Handle); \ + if ((x).MutexId) { \ + vSemaphoreDelete((x).MutexId); \ + } \ + (x).Handle = NULL; \ + (x).MutexId = NULL; \ + taskEXIT_CRITICAL(); \ + } \ + } while (0) + +#define CREATE_NEW_TASK(func, \ + func_arg, \ + task_name, \ + task_stack_size, \ + task_prio, \ + thread_id_ptr) \ + xTaskCreate((TaskFunction_t)(func), (task_name), \ + (uint16_t)(task_stack_size), (func_arg), (task_prio), \ + (TaskHandle_t *)(thread_id_ptr)) + +/* port functions prototypes + * + * */ +void error_handler(int block, error_e err); + +/* mutex's */ +decaIrqStatus_t decamutexon(void); +void decamutexoff(decaIrqStatus_t s); + +/* RTC */ +void rtc_configure_wakeup_ns(uint32_t period_ns); +void rtc_configure_wakeup_ms(uint32_t period_ms); +void rtc_reload(void); +void rtc_disable_irq(void); +void rtc_enable_irq(void); +uint32_t rtc_counter_get(void); +uint32_t rtc_init(void); +void rtc_deinit(void); +void juniper_configure_hal_rtc_callback(void (*cb)(void)); + +/* Time section */ +uint32_t init_timer(void); +void start_timer(volatile uint32_t *p_timestamp); +bool check_timer(uint32_t timestamp, uint32_t time); +void usleep(uint32_t usec); +void Sleep(uint32_t); + +/* Watchdog timer */ +void wdt_init(int ms); +void wdt_refresh(void); + +/* UART */ +#define app_uart_put(char) deca_uart_transmit(&(char), sizeof(char)); +void deca_uart_init(void); +void deca_uart_close(void); +int deca_uart_transmit(const char *tx_buffer, int size); +void deca_uart_receive(void); + +/* USB (NOT supported)*/ +void deca_usb_init(void); +int deca_usb_transmit(char *tx_buffer, int size); + +#ifdef __cplusplus +} +#endif + +#endif /* PORT__H__ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_error.c b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_error.c new file mode 100644 index 0000000..abe89e9 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_error.c @@ -0,0 +1,101 @@ +/*! ---------------------------------------------------------------------------- + * @file port_error.c + * @brief Project specific definitions and functions for error handling + * + * @author Decawave + * + * @attention + * + * Copyright 2018 (c) Decawave Ltd, Dublin, Ireland. + * + * All rights reserved. + * + */ + +#include "app.h" +#include "port_common.h" +#include "FreeRTOS.h" +#include "task.h" +#include "em_gpio.h" + +#define HAL_ERROR_LOG(...) hal_trace_debug_print(__VA_ARGS__) + +void error_handler(int block, error_e err) +{ + app.lastErrorCode = err; + + /* Flash Error Led*/ + HAL_ERROR_LOG("\r\n Error: %d\r\n", err); + while (block) + { + for (int i = err; i > 0; i--) + { + for (int j = 3; j > 0; j--) + { +#if DEBUG + wdt_refresh(); +#endif + +#if defined(HAL_IO_PORT_LED_ERROR) && defined(HAL_IO_PIN_LED_ERROR) + GPIO_PinModeSet(HAL_IO_PORT_LED_ERROR, + HAL_IO_PIN_LED_ERROR, + gpioModePushPull, + 0); + usleep(100000); + GPIO_PinModeSet(HAL_IO_PORT_LED_ERROR, + HAL_IO_PIN_LED_ERROR, + gpioModePushPull, + 1); +#endif + usleep(100000); + } + usleep(1000000); + } + } +} + +static void port_error_assert(const char *file, + int line, + const char *function_name, + const char *expression, + error_e err) +{ + HAL_ERROR_LOG("ASSERT!"); + HAL_ERROR_LOG("\r\n File: %s", file); + HAL_ERROR_LOG("\r\n Line: %d", line); + HAL_ERROR_LOG("\r\n Function: %s", function_name); + HAL_ERROR_LOG("\r\n Expression: %s", expression); + + usleep(1000000); // printing might be asynch + taskDISABLE_INTERRUPTS(); + error_handler(1, err); +} + +void __assert_func(const char *file, + int line, + const char *function_name, + const char *expression) +{ + port_error_assert(file, line, function_name, expression, _ERR_General_Error); + while (1) { // Calm the compiler that we won't return + } +} + +void assertEFM(const char *file, int line) +{ + port_error_assert(file, line, __FUNCTION__, "", _ERR_General_Error); +} + +void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) +{ + port_error_assert(pcTaskName, + (int)xTask, + __FUNCTION__, + "", + _ERR_Stack_Overflow); +} + +void vApplicationMallocFailedHook(void) +{ + port_error_assert(__FILE__, __LINE__, __FUNCTION__, "", _ERR_Malloc_Failed); +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_memory.c b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_memory.c new file mode 100644 index 0000000..b3365bd --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_memory.c @@ -0,0 +1,48 @@ +/*! ---------------------------------------------------------------------------- + * @file port_memory.c + * @brief Project specific definitions and functions for dynamic memory + * handling + * + * @author Decawave + * + * @attention + * + * Copyright 2018 (c) Decawave Ltd, Dublin, Ireland. + * + * All rights reserved. + * + */ +#include +#include +#include "FreeRTOS.h" +#include "task.h" + +/* + * The standard syscall malloc/free used in sscanf/sprintf. + * We want them to be replaced with FreeRTOS's implementation. + * + * This leads that the memory allocation will be managed by FreeRTOS heap4 + * memory. + * */ +void * _calloc_r(struct _reent *re, size_t num, size_t size) +{ + (void)re; + + void *ptr = pvPortMalloc(num * size); + if (ptr) { + memset(ptr, 0, num * size); + } + return ptr; +} + +void * _malloc_r(struct _reent *re, size_t size) +{ + (void)re; + return pvPortMalloc(size); +} + +void _free_r(struct _reent *re, void *ptr) +{ + (void)re; + vPortFree(ptr); +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_rtc.c b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_rtc.c new file mode 100644 index 0000000..2e56a87 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_rtc.c @@ -0,0 +1,120 @@ +/*! ---------------------------------------------------------------------------- + * @file port_rtc.c + * @brief HW specific definitions and functions for RTC Interface + * + * @attention + * + * Copyright 2016 (c) DecaWave Ltd, Dublin, Ireland. + * + * All rights reserved. + * + * @author + */ + +#include "app.h" +#include "port_common.h" +#include "em_cmu.h" +#include "em_burtc.h" // Sleeptimer could be also used but callaback overhead is + // bigger --> delay --> "global Super Frame" timestamp + // would be less accurate +#include "sl_common.h" + +#define RTC_FREQUENCY 32768ULL + +static uint32_t rtc_wakeup_time_in_ticks; + +void juniper_configure_hal_rtc_callback(void (*cb)(void)) +{ + app.HAL_RTCEx_WakeUpTimerEventCb = cb; +} + +void BURTC_IRQHandler(void) +{ + BURTC_IntClear(_BURTC_IF_MASK); + if (app.HAL_RTCEx_WakeUpTimerEventCb) { + app.HAL_RTCEx_WakeUpTimerEventCb(); + } +} + +/* + * @brief setup RTC Wakeup timer + * period_ms is awaiting time in ms + * */ +void rtc_configure_wakeup_ns(uint32_t period_ns) +{ +#if ((UINT32_MAX * RTC_FREQUENCY) / 1000000000ULL) > UINT32_MAX +#error period_ns saturation is needed! +#endif + // This API is bit of an overkill, most of the RTC counter increments with + // ~30.5 us + rtc_wakeup_time_in_ticks = + (uint32_t)(((uint64_t)period_ns * RTC_FREQUENCY) / 1000000000ULL); + rtc_reload(); +} + +/* + * @brief setup RTC Wakeup timer + * period_ms is awaiting time in ms + * */ +void rtc_configure_wakeup_ms(uint32_t period_ms) +{ + period_ms = SL_MIN(((UINT32_MAX * 1000ULL) / RTC_FREQUENCY), period_ms); + rtc_wakeup_time_in_ticks = + (uint32_t)(((uint64_t)period_ms * RTC_FREQUENCY) / 1000); + rtc_reload(); +} + +void rtc_reload(void) +{ + uint32_t new_cc = BURTC_CounterGet() + rtc_wakeup_time_in_ticks; + BURTC_CompareSet(0, new_cc); + rtc_enable_irq(); +} + +void rtc_disable_irq(void) +{ + BURTC_IntDisable(BURTC_IF_COMP); +} + +void rtc_enable_irq(void) +{ + BURTC_IntClear(BURTC_IF_COMP); + BURTC_IntEnable(BURTC_IF_COMP); +} + +uint32_t rtc_counter_get(void) +{ + return BURTC_CounterGet(); +} + +/** @brief Initialization of the RTC driver instance + */ +uint32_t rtc_init(void) +{ + const BURTC_Init_TypeDef burtc_init_structure = { + .start = true, + .debugRun = false, + .clkDiv = burtcClkDiv_1, + .compare0Top = false, // wrap to 0 on compare match event + .em4comp = false, + .em4overflow = false + }; + CMU_ClockEnable(cmuClock_BURTC, true); + BURTC_Init(&burtc_init_structure); + + // configure the RTC Wakeup timer with a high priority; + // this timer is saving global Super Frame Timestamp, + // so we want this timestamp as stable as we can. + NVIC_SetPriority(BURTC_IRQn, PRIO_RTC_WKUP_IRQn); + NVIC_ClearPendingIRQ(BURTC_IRQn); + NVIC_EnableIRQ(BURTC_IRQn); + + return _NO_ERR; +} + +void rtc_deinit(void) +{ + // stop the RTC timer + BURTC_Reset(); + NVIC_DisableIRQ(BURTC_IRQn); +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_timer.c b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_timer.c new file mode 100644 index 0000000..385441c --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_timer.c @@ -0,0 +1,113 @@ +/*! ---------------------------------------------------------------------------- + * @file port_timer.c + * @brief HW specific definitions and functions for Timing Interface(s) + * + * @attention + * + * Copyright 2016 (c) DecaWave Ltd, Dublin, Ireland. + * + * All rights reserved. + * + * @author + */ + +#include "app.h" +#include "port_common.h" +#include "sl_common.h" +#include "sl_udelay.h" +#include "sl_component_catalog.h" +#include "sl_sleeptimer.h" +#include "em_cmu.h" +#ifdef SL_CATALOG_KERNEL_PRESENT +#include "FreeRTOS.h" +#include "task.h" +#endif + +// sl_udelay_wait() is a SW loop thus its' precision is very bad let's use +// DWT->CYCNT instead if this macro is enabled. +#define SLEEP_USE_DWT_AS_US_TIMER 1 + +// OS delay is less accurate but allows the MCU to sleep +#define SLEEP_USE_OS_DELAY 1 + +/* @fn init_timer(void) + * @brief initiate timestamp (in CLOCKS_PER_SEC) + * @parm p_timestamp pointer on current system timestamp + */ +uint32_t init_timer(void) +{ + // Standard timer function(s), sleeptimer can be used (already initialized by + // GSDK) + return 0; +} + +/* @fn start_timer(uint32 *p_timestamp) + * @brief save system timestamp (in CLOCKS_PER_SEC) + * @parm p_timestamp pointer on current system timestamp + */ +void start_timer(volatile uint32_t *p_timestamp) +{ + if (p_timestamp) { + *p_timestamp = sl_sleeptimer_get_tick_count(); + } +} + +/* @fn check_timer(uint32 timestamp , uint32 time) + * @brief check if time from current timestamp over expectation + * @param [in] timestamp - current timestamp + * @param [in] time - time expectation (in CLOCKS_PER_SEC) + * @return true - time is over + * false - time is not over yet + */ +bool check_timer(uint32_t timestamp, uint32_t time) +{ + // Will wrap correctly no additional effort needed + uint32_t ticks_elapsed = sl_sleeptimer_get_tick_count() - timestamp; + return (sl_sleeptimer_tick_to_ms(ticks_elapsed) >= time); +} + +/* @brief Sleep + * -DDEBUG defined in Makefile prevents __WFI + */ +void Sleep(uint32_t dwMs) +{ +#if defined(SL_CATALOG_KERNEL_PRESENT) && SLEEP_USE_OS_DELAY + // if kernel is present and running use its' delay function because it allow + // the system to sleep + if (osKernelRunning == osKernelGetState()) { + osDelay(dwMs / osKernelGetTickFreq()); + } else +#endif + { + sl_sleeptimer_delay_millisecond(dwMs); + } +} + +/* @fn usleep + * @brief precise usleep() delay + * */ +void usleep(unsigned long time_us) +{ +#if SLEEP_USE_DWT_AS_US_TIMER + // init DWT + ITM->LAR = 0xC5ACCE55; + const uint32_t orig_DEMCR = CoreDebug->DEMCR; // CoreDebug_DEMCR_TRCENA_Msk + // might be set during + // debugging + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; + + // WAIT + const uint32_t start_tick = DWT->CYCCNT; + const uint32_t counter_freq_mhz = CMU_ClockFreqGet(cmuClock_CORE) / 1000000UL; + time_us = SL_MIN((UINT32_MAX / counter_freq_mhz) - 1, time_us); + while ((DWT->CYCCNT - start_tick) < (time_us * counter_freq_mhz)) {} + + // deinit DWT + CoreDebug->DEMCR = orig_DEMCR; + DWT->CTRL &= (~DWT_CTRL_CYCCNTENA_Msk); + ITM->LAR = 0; +#else + sl_udelay_wait(time_us); +#endif +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_uart.h b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_uart.h new file mode 100644 index 0000000..d9b87b3 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_uart.h @@ -0,0 +1,81 @@ +/** + * @file port.h + * + * @brief port Hardware Abstraction Layer headers file to EFR32BG22 + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#define _HAL_CAT2EXP(a, b) a ## b +#define HAL_CAT2(a, b) _HAL_CAT2EXP(a, b) +#define _HAL_CAT3EXP(a, b, c) a ## b ## c +#define HAL_CAT3(a, b, c) _HAL_CAT3EXP(a, b, c) + +// UART +#define HAL_UART_IS_EUART 1 +#if HAL_UART_IS_EUART +#define HAL_UART_TYPE EUSART +#define HAL_UART_STOP_BITS eusartStopbits1 +#define HAL_UART_PARITY eusartNoParity +#define HAL_UART_OVERSAMPLING eusartOVS4 +#else +#define HAL_UART_TYPE USART +#define HAL_UART_STOP_BITS usartStopbits1 +#define HAL_UART_PARITY usartNoParity +#define HAL_UART_OVERSAMPLING usartOVS4 +#endif +#define HAL_UART_NUMBER 0 +#define HAL_UART_PERIPHERAL HAL_CAT2(HAL_UART_TYPE, \ + HAL_UART_NUMBER) +#define HAL_UART_BAUDRATE 115200 +// #define HAL_UART_ENABLE_PORT gpioPortC +// #define HAL_UART_ENABLE_PIN 9 +#define HAL_UART_TX_PORT gpioPortA +#define HAL_UART_TX_PIN 5 +#define HAL_UART_RX_PORT gpioPortA +#define HAL_UART_RX_PIN 6 +#define HAL_UART_SLEEP_RX_IRQ_NBR HAL_UART_RX_PIN + +// BUTTON +#define HAL_IO_BUTTON_ACTIVE_STATE 0 +#define HAL_IO_PORT_BUTTON gpioPortB +#define HAL_IO_PIN_BUTTON 2 + +// ERROR +#define HAL_IO_PORT_LED_ERROR gpioPortA +#define HAL_IO_PIN_LED_ERROR 4 + +#ifndef HAL_TRACE_DEBUG_LOG_ENABLED +#if DEBUG +#define HAL_TRACE_DEBUG_LOG_ENABLED 1 +#else +#define HAL_TRACE_DEBUG_LOG_ENABLED 0 +#endif +#endif +#if HAL_TRACE_DEBUG_LOG_ENABLED +#include +#include +#define HAL_TRACE_DEBUG_LOG_LINE_BUFFER_SIZE 64 +int deca_uart_transmit(char *tx_buffer, int size); + +#endif + +static inline void hal_trace_debug_print(const char *format, ...) +{ +#if !HAL_TRACE_DEBUG_LOG_ENABLED + (void)format; +#else + char print_buffer[HAL_TRACE_DEBUG_LOG_LINE_BUFFER_SIZE]; + + va_list args; + va_start(args, format); + + int len = vsnprintf(print_buffer, sizeof(print_buffer), format, args); + deca_uart_transmit(print_buffer, len); + va_end(args); +#endif +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_wdt.c b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_wdt.c new file mode 100644 index 0000000..f8de525 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/platform/port/port_wdt.c @@ -0,0 +1,56 @@ +/*! ---------------------------------------------------------------------------- + * @file port_wdt.c + * @brief HW specific definitions and functions for Watchdog Interface + * + * @attention + * + * Copyright 2016 (c) DecaWave Ltd, Dublin, Ireland. + * + * All rights reserved. + * + * @author + */ + +#include "port_common.h" +#include "em_cmu.h" +#include "em_wdog.h" + +#if defined(WDOG0) +#define WDT_INSTANCE WDOG0 +#define WDT_CMU_CLOCK cmuClock_WDOG0 +#elif defined(WDOG) +#define WDT_INSTANCE WDOG +#define WDT_CMU_CLOCK cmuClock_WDOG +#endif +#define WDT_FREQUENCY_HZ 1000 /*ULFRCO*/ + +void wdt_init(int ms) +{ + WDOG_Init_TypeDef wdog_init = WDOG_INIT_DEFAULT; +#if defined(_WDOG_CTRL_CLKSEL_MASK) + wdog_init.clkSel = wdogClkSelULFRCO; +#else + CMU_ClockSelectSet(WDT_CMU_CLOCK, cmuSelect_ULFRCO); +#endif + + // Select the closest period time + bool found = false; + WDOG_PeriodSel_TypeDef period_to_use = wdogPeriod_9; + for (int32_t factor = 8; (period_to_use < wdogPeriod_256k) && !(found); + factor *= 2) { + found = (ms < ((1000 * factor) / WDT_FREQUENCY_HZ)); + if (!found) { + period_to_use++; + } + } + wdog_init.perSel = period_to_use; + + // Initialize and start the wdog + CMU_ClockEnable(WDT_CMU_CLOCK, true); + WDOGn_Init(WDT_INSTANCE, &wdog_init); +} + +void wdt_refresh(void) +{ + WDOGn_Feed(WDT_INSTANCE); +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/srv/common_n/common_n.c b/bluetooth_uwb_dw3000_slotted_twr/src/srv/common_n/common_n.c new file mode 100644 index 0000000..6584b34 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/srv/common_n/common_n.c @@ -0,0 +1,172 @@ +/* + * @file common_n.c + * @brief Decawave Application Layer + * Common functions for twr tag and node + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#include "app.h" +#include "common_n.h" +#include "port_common.h" + +#pragma GCC diagnostic ignored "-Wunused-function" + +/* + * @brief This fn() sets the STS_ND (SP3) and loads the STS key and IV + * + * dwt_configure(pdwCfg) should be executed thereafter + * */ +static void +set_cfg_sp3(dwt_config_t *pdwCfg, + dwt_sts_cp_key_t *key, + dwt_sts_cp_iv_t *iv) +{ + (void) pdwCfg; + pdwCfg->stsMode = DWT_STS_MODE_ND; + pdwCfg->sfdType = 3; + + /* + * Set STS encryption key and IV (nonce) + */ + dwt_configurestskey(key); // this can be done only 1 time + // in the application + dwt_configurestsiv(iv); // the IV should be synchronised + dwt_configurestsloadiv(); + // END STS +} + +/* + * @brief This fn() sets the STS1 (SP1) and loads the STS key and IV + * + * dwt_configure(pdwCfg) should be executed thereafter + * */ +static void +set_cfg_sp1(dwt_config_t *pdwCfg, + dwt_sts_cp_key_t *key, + dwt_sts_cp_iv_t *iv) +{ + (void) pdwCfg; + pdwCfg->stsMode = DWT_STS_MODE_1; + pdwCfg->sfdType = 3; + + /* + * Set STS encryption key and IV (nonce) + */ + dwt_configurestskey(key); // this can be done only 1 time + // in the application + dwt_configurestsiv(iv); // the IV should be synchronised + dwt_configurestsloadiv(); + // END STS +} + +/* + * @brief This fn() revert back SP0 frame mode + * dwt_configure(pdwCfg) should be executed thereafter + * */ +static void +set_cfg_sp0A(dwt_config_t *pdwCfg) +{ + (void) pdwCfg; + pdwCfg->stsMode = DWT_STS_MODE_OFF; + pdwCfg->sfdType = 0; +} + +static void +set_cfg_sp0Z(dwt_config_t *pdwCfg) +{ + (void) pdwCfg; + pdwCfg->stsMode = DWT_STS_MODE_OFF; + pdwCfg->sfdType = 3; +} + +/* + * @brief configure tan/node application: frame filtering, PANID, address, + * antenna delays + * + * + * @note + * */ +void +tn_app_config(rxtx_configure_t *p) +{ + /* set antenna delays */ + dwt_setrxantennadelay(p->rxAntDelay); + dwt_settxantennadelay(p->txAntDelay); + + dwt_setrxaftertxdelay(0); /**< no any delays set by default : part of + * config of receiver on Tx sending + */ + dwt_setrxtimeout(0); /**< no any delays set by default : part of + * config of receiver on Tx sending + */ + + dwt_configureframefilter(p->frameFilter, p->frameFilterMode); + + dwt_setpanid(p->panId); + + dwt_setaddress16(p->shortadd); +} + +/* + * @brief ISR level (need to be protected if called from APP level) + * low-level configuration for DW1000 + * + * if called from app, shall be performed with DW IRQ off & + * TAG_ENTER_CRITICAL(); / TAG_EXIT_CRITICAL(); + * + * + * @note + * */ +void +rxtx_configure(rxtx_configure_t *p) +{ + if (dwt_configure(p->pdwCfg)) { /**< Configure the Physical Channel + * parameters (PLEN, PRF, etc) */ + error_handler(1, _ERR_INIT); + } + + /* configure power */ + dwt_configuretxrf(&app.pConfig->s.txConfig); + + tn_app_config(p); +} + +/** + * @brief ISR level (need to be protected if called from APP level) + * Transmit packet + * */ +error_e +tx_start(tx_pckt_t *pTxPckt) +{ + error_e ret = _NO_ERR; + + if (pTxPckt->psduLen) { + dwt_writetxdata(pTxPckt->psduLen, (uint8_t *) &pTxPckt->msg.stdMsg, 0); + dwt_writetxfctrl(pTxPckt->psduLen, 0, 1); + } + + // Setup for delayed Transmit time (units are 4ns) + if (pTxPckt->txFlag + & (DWT_START_TX_DELAYED | DWT_START_TX_DLY_REF | DWT_START_TX_DLY_RS + | DWT_START_TX_DLY_TS)) { + dwt_setdelayedtrxtime(pTxPckt->delayedTxTimeH_sy); + } + + // Setup for delayed Receive after Tx (units are sy = 1.0256 us) + if (pTxPckt->txFlag & DWT_RESPONSE_EXPECTED) { + dwt_setrxaftertxdelay(pTxPckt->delayedRxTime_sy); + dwt_setrxtimeout(pTxPckt->delayedRxTimeout_sy); + } + + // Begin delayed TX of frame + if (dwt_starttx(pTxPckt->txFlag) != DWT_SUCCESS) { + ret = _ERR_DelayedTX_Late; + } + + return (ret); +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/srv/common_n/common_n.h b/bluetooth_uwb_dw3000_slotted_twr/src/srv/common_n/common_n.h new file mode 100644 index 0000000..b3ee9db --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/srv/common_n/common_n.h @@ -0,0 +1,160 @@ +/** + * @file common_n.h + * + * @brief common TWR defines, types and fn() + * + * @attention + * + * Copyright 2016-2020 (c) Decawave Ltd, Dublin, Ireland. + * + * All rights reserved. + * + * @author + */ + +#ifndef CORE_SRC_SRV_COMMON_N_H_ +#define CORE_SRC_SRV_COMMON_N_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "deca_device_api.h" +#include "deca_interface.h" +#include "uwb_frames.h" + +#define BPRF_SET_1 (1) // SP0 IEEE SFD +#define BPRF_SET_2 (2) // SP0 4z SFD +#define BPRF_SET_3 (3) // SP1 4z SFD +#define BPRF_SET_4 (4) // SP3 4z SFD + +// Comment below to see the debug information in a real-time, diag_printf is a +// buffered I/O +#define DEBUG_SP3_MSG(...) + +#ifndef DEBUG_SP3_MSG +#include "deca_dbg.h" +#define DEBUG_SP3_MSG diag_printf +#endif + +#ifndef TWR_ENTER_CRITICAL +#define TWR_ENTER_CRITICAL taskENTER_CRITICAL +#endif + +#ifndef TWR_EXIT_CRITICAL +#define TWR_EXIT_CRITICAL taskEXIT_CRITICAL +#endif + +#ifndef M_PI +#define M_PI (3.141592654f) +#endif + +#ifndef M_PI_2 +#define M_PI_2 (1.570796327f) +#endif + +#ifndef TWO_PI +#define TWO_PI (2 * M_PI) +#endif + +// ---------------------------------------------------------------------------- + +struct mini_diag_s +{// temporary diagnostics for PDoA debugging + uint8_t DTUNE5; + uint32_t CIA_TDOA_0; + uint32_t CIA_TDOA_1_PDOA; + uint16_t IP_DIAG_10; + uint16_t CY0_DIAG_10; + uint16_t CY0_TOA_HI; + uint16_t CY1_TOA_HI; +}; + +typedef struct mini_diag_s mini_diag_t; + +struct pdoa_s{ + int16_t pdoa; /* DW3000 PDOA */ + mini_diag_t mDiag; +}; + +typedef struct pdoa_s pdoa_t; + +struct result_s +{ + uint16_t addr16; + uint16_t rangeNum; // number from Tag Poll and Final messages, + // which indicates the current range number + uint32_t resTime_us; // reception time of the end of the Final from + // the Tag wrt node's SuperFrame start, + // microseconds + float pdoa_raw_deg; // pdoa_raw: phase differences in degrees + // without any correction [-180 .. 180] + float pdoa_raw_degP; // pdoa_raw: phase differences in degrees from + // Poll message + float dist_cm; // distance to the tag in cm, corrected to a + // rngOffset_mm + float x_cm; // X of the tag wrt to the node, cm + float y_cm; // Y of the tag wrt to the node, cm + float clockOffset_pphm; // clock offset in hundredths of ppm (i.e. 1ppm + // is 100) + uint16_t flag; // service message data from the tag (low byte) + // and node (high byte), bitmask (defined as + // "RES_FLAG_") + int16_t acc_x; // Normalized accel data X from the Tag, mg: + // acc_x + int16_t acc_y; // Normalized accel data Y from the Tag, mg: + // acc_y + int16_t acc_z; // Normalized accel data Z from the Tag, mg: + // acc_z + + pdoa_t pollPDOA; + pdoa_t finalPDOA; + + int8_t tMaster_C; // temperature of Master in degree centigrade + + float path_diff; +}; + +typedef struct result_s result_t; + +/* TxPckt */ +struct tx_pckt_s +{ + int16_t psduLen; + + union { + std_msg_t stdMsg; + twr_msg_t twrMsg; + + blink_msg_t blinkMsg; + poll_msg_t pollMsg; + final_msg_accel_t finalMsg; + + rng_cfg_msg_t rngCfgMsg; + rng_cfg_upd_msg_t rngCfgUpdMsg; + resp_pdoa_msg_t respMsg; + } msg; + + uint8_t txFlag; // Holds Tx sending parameters: extended set + // for DW3000 + + uint32_t delayedTxTimeH_sy; // Delayed transmit time (in 4ns) + uint32_t delayedRxTime_sy; // Delay after Tx when to switch on receiver + // (in SY=1.0256us) + uint16_t delayedRxTimeout_sy; // How long the receiver will be switched on + // after Tx (in SY=1.0256us) +}; + +typedef struct tx_pckt_s tx_pckt_t; + +// ---------------------------------------------------------------------------- +// +void tn_app_config(rxtx_configure_t *p); +void rxtx_configure(rxtx_configure_t *p); +error_e tx_start(tx_pckt_t *pTxPckt); + +#ifdef __cplusplus +} +#endif + +#endif /* CORE_SRC_SRV_COMMON_N_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/srv/crc16/crc16.c b/bluetooth_uwb_dw3000_slotted_twr/src/srv/crc16/crc16.c new file mode 100644 index 0000000..4648e41 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/srv/crc16/crc16.c @@ -0,0 +1,117 @@ +/** + * @file crc16.c + * + * @brief Checks crc16 of frame + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + */ + +#include "crc16.h" + +#define GP 0x11021 /* x^16 + x^12 + x^5 + 1 */ +#define DI 0x1021 + +static uint16_t crc16Table[256]; /* 8-bit table */ +static uint8_t reverseByte[256]; + +/*local functions*/ +static void init_table_crc16(void); +static void init_reverse_byte(void); + +void init_crc16(void) +{ + init_table_crc16(); + init_reverse_byte(); +} + +/* @brief Should be called to init crc tables. + * + * */ +void init_reverse_byte(void) +{ + int i; + for (i = 0; i < 256; i++) + { + reverseByte[i] = ((i & 0x01) << 7) + | ((i & 0x02) << 5) + | ((i & 0x04) << 3) + | ((i & 0x08) << 1) + | ((i & 0x10) >> 1) + | ((i & 0x20) >> 3) + | ((i & 0x40) >> 5) + | ((i & 0x80) >> 7); + } +} + +/* @brief Should be called to init crc tables. + * + * */ +void init_table_crc16(void) +{ + int i, j; + uint32_t crc; + + for (i = 0; i < 256; i++) { + crc = (i << 8); + for (j = 0; j < 8; j++) { + crc = (crc << 1) ^ ((crc & 0x8000) ? DI : 0); + } + crc16Table[i] = crc & 0xFFFF; + } +} + +/* @brief check CRC16 of frame which includes a CRC + * @param flen - frame length + 2 bytes of CRC + * + * */ +crc_err_e check_crc16(uint8_t *frame, uint16_t flen) +{ + uint8_t crcHiRev, crcLoRev; + uint16_t crcvalue; + uint16_t i; + + crc_err_e ret = CRC_ERROR; + crcvalue = 0; + + for (i = 0; i < flen - 2; i++) + { + crcvalue = + crc16Table[(((crcvalue) >> + 8) ^ reverseByte[frame[i]]) & 0xFF] ^ ((crcvalue) << 8); + } + crcHiRev = reverseByte[(crcvalue >> 8) & 0xFF]; + crcLoRev = reverseByte[crcvalue & 0xFF]; + + if ((frame[flen - 2] == crcHiRev) && (frame[flen - 1] == crcLoRev)) { + ret = CRC_OKAY; + } + + return (ret); +} + +/* @brief calculates crc16 of frame + * @param flen - frame length to calculate crc + * @return crc16 value of frame + * */ +uint16_t calc_crc16(uint8_t *frame, uint16_t flen) +{ + uint8_t crcHiRev, crcLoRev; + int16_t crcvalue; + int16_t i; + + crcvalue = 0; + + for (i = 0; i < flen; i++) + { + crcvalue = + crc16Table[(((crcvalue) >> + 8) ^ reverseByte[frame[i]]) & 0xFF] ^ ((crcvalue) << 8); + } + crcHiRev = reverseByte[(crcvalue >> 8) & 0xFF]; + crcLoRev = reverseByte[crcvalue & 0xFF]; + + return ((crcHiRev << 8) + crcLoRev); +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/srv/crc16/crc16.h b/bluetooth_uwb_dw3000_slotted_twr/src/srv/crc16/crc16.h new file mode 100644 index 0000000..67d1504 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/srv/crc16/crc16.h @@ -0,0 +1,28 @@ +/** + * @file crc16.h + * + * @brief Header file for crc16 checksum + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + */ + +#ifndef CRC16_H_ +#define CRC16_H_ + +#include + +typedef enum { + CRC_OKAY = 0, + CRC_ERROR = -1 +}crc_err_e; + +uint16_t calc_crc16(uint8_t *frame, uint16_t flen); + +crc_err_e check_crc16(uint8_t *frame, uint16_t flen); + +void init_crc16(void); + +#endif /* CRC16_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/srv/json/cJSON.c b/bluetooth_uwb_dw3000_slotted_twr/src/srv/json/cJSON.c new file mode 100644 index 0000000..94b7f2c --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/srv/json/cJSON.c @@ -0,0 +1,2888 @@ +/* + * Copyright (c) 2009-2017 Dave Gamble and cJSON contributors + * + * Permission is hereby granted, free of charge, to any person obtaining a + * copy + * of this software and associated documentation files (the "Software"), to + * deal + * in the Software without restriction, including without limitation the + * rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + * THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +/* cJSON */ +/* JSON parser in C. */ + +/* disable warnings about old C89 functions in MSVC */ +#if !defined(_CRT_SECURE_NO_DEPRECATE) && defined(_MSC_VER) +#define _CRT_SECURE_NO_DEPRECATE +#endif + +#ifdef __GNUC__ +#pragma GCC visibility push(default) +#endif +#if defined(_MSC_VER) +#pragma warning (push) + +/* disable warning about single line comments in system headers */ +#pragma warning (disable : 4001) +#endif + +#include +#include +#include +#include +#include +#include + +#ifdef ENABLE_LOCALES +#include +#endif + +#if defined(_MSC_VER) +#pragma warning (pop) +#endif +#ifdef __GNUC__ +#pragma GCC visibility pop +#endif + +#include "cJSON.h" + +/* define our own boolean type */ +#ifdef true +#undef true +#endif +#define true ((cJSON_bool)1) + +#ifdef false +#undef false +#endif +#define false ((cJSON_bool)0) + +typedef struct { + const unsigned char *json; + size_t position; +} error; +static error global_error = { NULL, 0 }; + +CJSON_PUBLIC(const char *) cJSON_GetErrorPtr(void) +{ + return (const char *) (global_error.json + global_error.position); +} + +CJSON_PUBLIC(char *) cJSON_GetStringValue(cJSON * item) { + if (!cJSON_IsString(item)) { + return NULL; + } + + return item->valuestring; +} + +/* This is a safeguard to prevent copy-pasters from using incompatible C and + * header files */ +#if (CJSON_VERSION_MAJOR != 1) || (CJSON_VERSION_MINOR != 7) \ + || (CJSON_VERSION_PATCH != 12) +#error \ + cJSON.h and cJSON.c have different versions. Make sure that both have the same. +#endif + +CJSON_PUBLIC(const char *) cJSON_Version(void) +{ + static char version[15]; + sprintf(version, + "%i.%i.%i", + CJSON_VERSION_MAJOR, + CJSON_VERSION_MINOR, + CJSON_VERSION_PATCH); + + return version; +} + +/* Case insensitive string comparison, doesn't consider two NULL pointers equal + * though */ +static int case_insensitive_strcmp(const unsigned char *string1, + const unsigned char *string2) +{ + if ((string1 == NULL) || (string2 == NULL)) { + return 1; + } + + if (string1 == string2) { + return 0; + } + + for (; tolower(*string1) == tolower(*string2); (void)string1++, string2++) + { + if (*string1 == '\0') { + return 0; + } + } + + return tolower(*string1) - tolower(*string2); +} + +typedef struct internal_hooks +{ + void *(CJSON_CDECL * allocate)(size_t size); + void(CJSON_CDECL * deallocate)(void *pointer); + void *(CJSON_CDECL * reallocate)(void *pointer, size_t size); +} internal_hooks; + +#if defined(_MSC_VER) + +/* work around MSVC error C2322: '...' address of dllimport '...' is not static + */ +static void * CJSON_CDECL internal_malloc(size_t size) +{ + return malloc(size); +} + +static void CJSON_CDECL internal_free(void *pointer) +{ + free(pointer); +} + +static void * CJSON_CDECL internal_realloc(void *pointer, size_t size) +{ + return realloc(pointer, size); +} + +#else +#define internal_malloc malloc +#define internal_free free +#define internal_realloc realloc +#endif + +/* strlen of character literals resolved at compile time */ +#define static_strlen(string_literal) (sizeof(string_literal) - sizeof("")) + +static internal_hooks global_hooks = +{ internal_malloc, internal_free, internal_realloc }; + +static unsigned char * cJSON_strdup(const unsigned char *string, + const internal_hooks * const hooks) +{ + size_t length = 0; + unsigned char *copy = NULL; + + if (string == NULL) { + return NULL; + } + + length = strlen((const char *)string) + sizeof(""); + copy = (unsigned char *)hooks->allocate(length); + if (copy == NULL) { + return NULL; + } + memcpy(copy, string, length); + + return copy; +} + +CJSON_PUBLIC(void) cJSON_InitHooks(cJSON_Hooks * hooks) +{ + if (hooks == NULL) { + /* Reset hooks */ + global_hooks.allocate = malloc; + global_hooks.deallocate = free; + global_hooks.reallocate = realloc; + return; + } + + global_hooks.allocate = malloc; + if (hooks->malloc_fn != NULL) { + global_hooks.allocate = hooks->malloc_fn; + } + + global_hooks.deallocate = free; + if (hooks->free_fn != NULL) { + global_hooks.deallocate = hooks->free_fn; + } + + /* use realloc only if both free and malloc are used */ + global_hooks.reallocate = NULL; + if ((global_hooks.allocate == malloc) && (global_hooks.deallocate == free)) { + global_hooks.reallocate = realloc; + } +} + +/* Internal constructor. */ +static cJSON *cJSON_New_Item(const internal_hooks * const hooks) +{ + cJSON *node = (cJSON *)hooks->allocate(sizeof(cJSON)); + if (node) { + memset(node, '\0', sizeof(cJSON)); + } + + return node; +} + +/* Delete a cJSON structure. */ +CJSON_PUBLIC(void) cJSON_Delete(cJSON * item) +{ + cJSON *next = NULL; + while (item != NULL) + { + next = item->next; + if (!(item->type & cJSON_IsReference) && (item->child != NULL)) { + cJSON_Delete(item->child); + } + if (!(item->type & cJSON_IsReference) && (item->valuestring != NULL)) { + global_hooks.deallocate(item->valuestring); + } + if (!(item->type & cJSON_StringIsConst) && (item->string != NULL)) { + global_hooks.deallocate(item->string); + } + global_hooks.deallocate(item); + item = next; + } +} + +/* get the decimal point character of the current locale */ +static unsigned char get_decimal_point(void) +{ +#ifdef ENABLE_LOCALES + struct lconv *lconv = localeconv(); + return (unsigned char) lconv->decimal_point[0]; +#else + return '.'; +#endif +} + +typedef struct +{ + const unsigned char *content; + size_t length; + size_t offset; + size_t depth; /* How deeply nested (in arrays/objects) is the input at the + * current offset. */ + internal_hooks hooks; +} parse_buffer; + +/* check if the given size is left to read + * in a given parse buffer (starting with 1) + */ +#define can_read(buffer, size) \ + ((buffer != NULL) && (((buffer)->offset + size) <= (buffer)->length)) + +/* check if the buffer can be accessed at the given index (starting with 0) */ +#define can_access_at_index(buffer, index) \ + ((buffer != NULL) && (((buffer)->offset + index) < (buffer)->length)) + +#define cannot_access_at_index(buffer, index) \ + (!can_access_at_index(buffer, index)) + +/* get a pointer to the buffer at the position */ +#define buffer_at_offset(buffer) \ + ((buffer)->content + (buffer)->offset) + +/* Parse the input text to generate a number, and populate the result into item. + */ +static cJSON_bool parse_number(cJSON * const item, + parse_buffer * const input_buffer) +{ + double number = 0; + unsigned char *after_end = NULL; + unsigned char number_c_string[64]; + unsigned char decimal_point = get_decimal_point(); + size_t i = 0; + + if ((input_buffer == NULL) || (input_buffer->content == NULL)) { + return false; + } + + /* copy the number into a temporary buffer and replace '.' with the decimal + * point + * of the current locale (for strtod) + * This also takes care of '\0' not necessarily being available for marking + * the end of the input + */ + for (i = 0; + (i < (sizeof(number_c_string) - 1)) + && can_access_at_index(input_buffer, i); + i++) + { + switch (buffer_at_offset(input_buffer)[i]) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + case '+': + case '-': + case 'e': + case 'E': + number_c_string[i] = buffer_at_offset(input_buffer)[i]; + break; + + case '.': + number_c_string[i] = decimal_point; + break; + + default: + goto loop_end; + } + } + loop_end: + number_c_string[i] = '\0'; + + number = strtod((const char *)number_c_string, (char **)&after_end); + if (number_c_string == after_end) { + return false; /* parse_error */ + } + + item->valuedouble = number; + + /* use saturation in case of overflow */ + if (number >= INT_MAX) { + item->valueint = INT_MAX; + } else if (number <= (double)INT_MIN) { + item->valueint = INT_MIN; + } else { + item->valueint = (int)number; + } + + item->type = cJSON_Number; + + input_buffer->offset += (size_t)(after_end - number_c_string); + return true; +} + +/* don't ask me, but the original cJSON_SetNumberValue returns an integer or + * double */ +CJSON_PUBLIC(double) cJSON_SetNumberHelper(cJSON * object, double number) +{ + if (number >= INT_MAX) { + object->valueint = INT_MAX; + } else if (number <= (double)INT_MIN) { + object->valueint = INT_MIN; + } else { + object->valueint = (int)number; + } + + return object->valuedouble = number; +} + +typedef struct +{ + unsigned char *buffer; + size_t length; + size_t offset; + size_t depth; /* current nesting depth (for formatted printing) */ + cJSON_bool noalloc; + cJSON_bool format; /* is this print a formatted print */ + internal_hooks hooks; +} printbuffer; + +/* realloc printbuffer if necessary to have at least "needed" bytes more */ +static unsigned char * ensure(printbuffer * const p, size_t needed) +{ + unsigned char *newbuffer = NULL; + size_t newsize = 0; + + if ((p == NULL) || (p->buffer == NULL)) { + return NULL; + } + + if ((p->length > 0) && (p->offset >= p->length)) { + /* make sure that offset is valid */ + return NULL; + } + + if (needed > INT_MAX) { + /* sizes bigger than INT_MAX are currently not supported */ + return NULL; + } + + needed += p->offset + 1; + if (needed <= p->length) { + return p->buffer + p->offset; + } + + if (p->noalloc) { + return NULL; + } + + /* calculate new buffer size */ + if (needed > (INT_MAX / 2)) { + /* overflow of int, use INT_MAX if possible */ + if (needed <= INT_MAX) { + newsize = INT_MAX; + } else { + return NULL; + } + } else { + newsize = needed * 2; + } + + if (p->hooks.reallocate != NULL) { + /* reallocate with realloc if available */ + newbuffer = (unsigned char *)p->hooks.reallocate(p->buffer, newsize); + if (newbuffer == NULL) { + p->hooks.deallocate(p->buffer); + p->length = 0; + p->buffer = NULL; + + return NULL; + } + } else { + /* otherwise reallocate manually */ + newbuffer = (unsigned char *)p->hooks.allocate(newsize); + if (!newbuffer) { + p->hooks.deallocate(p->buffer); + p->length = 0; + p->buffer = NULL; + + return NULL; + } + if (newbuffer) { + memcpy(newbuffer, p->buffer, p->offset + 1); + } + p->hooks.deallocate(p->buffer); + } + p->length = newsize; + p->buffer = newbuffer; + + return newbuffer + p->offset; +} + +/* calculate the new length of the string in a printbuffer and update the offset + */ +static void update_offset(printbuffer * const buffer) +{ + const unsigned char *buffer_pointer = NULL; + if ((buffer == NULL) || (buffer->buffer == NULL)) { + return; + } + buffer_pointer = buffer->buffer + buffer->offset; + + buffer->offset += strlen((const char *)buffer_pointer); +} + +/* Render the number nicely from the given item into a string. */ +static cJSON_bool print_number(const cJSON * const item, + printbuffer * const output_buffer) +{ + unsigned char *output_pointer = NULL; + double d = item->valuedouble; + int length = 0; + size_t i = 0; + unsigned char number_buffer[26]; /* temporary buffer to print the number + * into + */ + unsigned char decimal_point = get_decimal_point(); + double test; + + if (output_buffer == NULL) { + return false; + } + + /* This checks for NaN and Infinity */ + if ((d * 0) != 0) { + length = sprintf((char *)number_buffer, "null"); + } else { + /* Try 15 decimal places of precision to avoid nonsignificant nonzero digits + */ + length = sprintf((char *)number_buffer, "%1.15g", d); + + /* Check whether the original double can be recovered */ + if ((sscanf((char *)number_buffer, "%lg", + &test) != 1) || ((double)test != d)) { + /* If not, print with 17 decimal places of precision */ + length = sprintf((char *)number_buffer, "%1.17g", d); + } + } + + /* sprintf failed or buffer overrun occurred */ + if ((length < 0) || (length > (int)(sizeof(number_buffer) - 1))) { + return false; + } + + /* reserve appropriate space in the output */ + output_pointer = ensure(output_buffer, (size_t)length + sizeof("")); + if (output_pointer == NULL) { + return false; + } + + /* copy the printed number to the output and replace locale + * dependent decimal point with '.' + */ + for (i = 0; i < ((size_t)length); i++) + { + if (number_buffer[i] == decimal_point) { + output_pointer[i] = '.'; + continue; + } + + output_pointer[i] = number_buffer[i]; + } + output_pointer[i] = '\0'; + + output_buffer->offset += (size_t)length; + + return true; +} + +/* parse 4 digit hexadecimal number */ +static unsigned parse_hex4(const unsigned char * const input) +{ + unsigned int h = 0; + size_t i = 0; + + for (i = 0; i < 4; i++) + { + /* parse digit */ + if ((input[i] >= '0') && (input[i] <= '9')) { + h += (unsigned int) input[i] - '0'; + } else if ((input[i] >= 'A') && (input[i] <= 'F')) { + h += (unsigned int) 10 + input[i] - 'A'; + } else if ((input[i] >= 'a') && (input[i] <= 'f')) { + h += (unsigned int) 10 + input[i] - 'a'; + } else { /* invalid */ + return 0; + } + + if (i < 3) { + /* shift left to make place for the next nibble */ + h = h << 4; + } + } + + return h; +} + +/* converts a UTF-16 literal to UTF-8 + * A literal can be one or two sequences of the form \uXXXX + */ +static unsigned char utf16_literal_to_utf8( + const unsigned char * const input_pointer, + const unsigned char * const input_end, + unsigned char **output_pointer) +{ + long unsigned int codepoint = 0; + unsigned int first_code = 0; + const unsigned char *first_sequence = input_pointer; + unsigned char utf8_length = 0; + unsigned char utf8_position = 0; + unsigned char sequence_length = 0; + unsigned char first_byte_mark = 0; + + if ((input_end - first_sequence) < 6) { + /* input ends unexpectedly */ + goto fail; + } + + /* get the first utf16 sequence */ + first_code = parse_hex4(first_sequence + 2); + + /* check that the code is valid */ + if (((first_code >= 0xDC00) && (first_code <= 0xDFFF))) { + goto fail; + } + + /* UTF16 surrogate pair */ + if ((first_code >= 0xD800) && (first_code <= 0xDBFF)) { + const unsigned char *second_sequence = first_sequence + 6; + unsigned int second_code = 0; + sequence_length = 12; /* \uXXXX\uXXXX */ + + if ((input_end - second_sequence) < 6) { + /* input ends unexpectedly */ + goto fail; + } + + if ((second_sequence[0] != '\\') || (second_sequence[1] != 'u')) { + /* missing second half of the surrogate pair */ + goto fail; + } + + /* get the second utf16 sequence */ + second_code = parse_hex4(second_sequence + 2); + + /* check that the code is valid */ + if ((second_code < 0xDC00) || (second_code > 0xDFFF)) { + /* invalid second half of the surrogate pair */ + goto fail; + } + + /* calculate the unicode codepoint from the surrogate pair */ + codepoint = 0x10000 + + (((first_code & 0x3FF) << 10) | (second_code & 0x3FF)); + } else { + sequence_length = 6; /* \uXXXX */ + codepoint = first_code; + } + + /* encode as UTF-8 + * takes at maximum 4 bytes to encode: + * 11110xxx 10xxxxxx 10xxxxxx 10xxxxxx */ + if (codepoint < 0x80) { + /* normal ascii, encoding 0xxxxxxx */ + utf8_length = 1; + } else if (codepoint < 0x800) { + /* two bytes, encoding 110xxxxx 10xxxxxx */ + utf8_length = 2; + first_byte_mark = 0xC0; /* 11000000 */ + } else if (codepoint < 0x10000) { + /* three bytes, encoding 1110xxxx 10xxxxxx 10xxxxxx */ + utf8_length = 3; + first_byte_mark = 0xE0; /* 11100000 */ + } else if (codepoint <= 0x10FFFF) { + /* four bytes, encoding 1110xxxx 10xxxxxx 10xxxxxx 10xxxxxx */ + utf8_length = 4; + first_byte_mark = 0xF0; /* 11110000 */ + } else { + /* invalid unicode codepoint */ + goto fail; + } + + /* encode as utf8 */ + for (utf8_position = (unsigned char)(utf8_length - 1); utf8_position > 0; + utf8_position--) + { + /* 10xxxxxx */ + (*output_pointer)[utf8_position] = + (unsigned char)((codepoint | 0x80) & 0xBF); + codepoint >>= 6; + } + + /* encode first byte */ + if (utf8_length > 1) { + (*output_pointer)[0] = + (unsigned char)((codepoint | first_byte_mark) & 0xFF); + } else { + (*output_pointer)[0] = (unsigned char)(codepoint & 0x7F); + } + + *output_pointer += utf8_length; + + return sequence_length; + + fail: + return 0; +} + +/* Parse the input text into an unescaped cinput, and populate item. */ +static cJSON_bool parse_string(cJSON * const item, + parse_buffer * const input_buffer) +{ + const unsigned char *input_pointer = buffer_at_offset(input_buffer) + 1; + const unsigned char *input_end = buffer_at_offset(input_buffer) + 1; + unsigned char *output_pointer = NULL; + unsigned char *output = NULL; + + /* not a string */ + if (buffer_at_offset(input_buffer)[0] != '\"') { + goto fail; + } + + { + /* calculate approximate size of the output (overestimate) */ + size_t allocation_length = 0; + size_t skipped_bytes = 0; + while (((size_t)(input_end - input_buffer->content) + < input_buffer->length) && (*input_end != '\"')) + { + /* is escape sequence */ + if (input_end[0] == '\\') { + if ((size_t)(input_end + 1 - input_buffer->content) + >= input_buffer->length) { + /* prevent buffer overflow when last input character is a backslash */ + goto fail; + } + skipped_bytes++; + input_end++; + } + input_end++; + } + if (((size_t)(input_end - input_buffer->content) >= input_buffer->length) + || (*input_end != '\"')) { + goto fail; /* string ended unexpectedly */ + } + + /* This is at most how much we need for the output */ + allocation_length = (size_t) (input_end - buffer_at_offset(input_buffer)) + - skipped_bytes; + output = (unsigned char *)input_buffer->hooks.allocate( + allocation_length + sizeof("")); + if (output == NULL) { + goto fail; /* allocation failure */ + } + } + + output_pointer = output; + + /* loop through the string literal */ + while (input_pointer < input_end) + { + if (*input_pointer != '\\') { + *output_pointer++ = *input_pointer++; + } else { /* escape sequence */ + unsigned char sequence_length = 2; + if ((input_end - input_pointer) < 1) { + goto fail; + } + + switch (input_pointer[1]) + { + case 'b': + *output_pointer++ = '\b'; + break; + case 'f': + *output_pointer++ = '\f'; + break; + case 'n': + *output_pointer++ = '\n'; + break; + case 'r': + *output_pointer++ = '\r'; + break; + case 't': + *output_pointer++ = '\t'; + break; + case '\"': + case '\\': + case '/': + *output_pointer++ = input_pointer[1]; + break; + + /* UTF-16 literal */ + case 'u': + sequence_length = utf16_literal_to_utf8(input_pointer, + input_end, + &output_pointer); + if (sequence_length == 0) { + /* failed to convert UTF16-literal to UTF-8 */ + goto fail; + } + break; + + default: + goto fail; + } + input_pointer += sequence_length; + } + } + + /* zero terminate the output */ + *output_pointer = '\0'; + + item->type = cJSON_String; + item->valuestring = (char *)output; + + input_buffer->offset = (size_t) (input_end - input_buffer->content); + input_buffer->offset++; + + return true; + + fail: + if (output != NULL) { + input_buffer->hooks.deallocate(output); + } + + if (input_pointer != NULL) { + input_buffer->offset = (size_t)(input_pointer - input_buffer->content); + } + + return false; +} + +/* Render the cstring provided to an escaped version that can be printed. */ +static cJSON_bool print_string_ptr(const unsigned char * const input, + printbuffer * const output_buffer) +{ + const unsigned char *input_pointer = NULL; + unsigned char *output = NULL; + unsigned char *output_pointer = NULL; + size_t output_length = 0; + + /* numbers of additional characters needed for escaping */ + size_t escape_characters = 0; + + if (output_buffer == NULL) { + return false; + } + + /* empty string */ + if (input == NULL) { + output = ensure(output_buffer, sizeof("\"\"")); + if (output == NULL) { + return false; + } + strcpy((char *)output, "\"\""); + + return true; + } + + /* set "flag" to 1 if something needs to be escaped */ + for (input_pointer = input; *input_pointer; input_pointer++) + { + switch (*input_pointer) + { + case '\"': + case '\\': + case '\b': + case '\f': + case '\n': + case '\r': + case '\t': + /* one character escape sequence */ + escape_characters++; + break; + default: + if (*input_pointer < 32) { + /* UTF-16 escape sequence uXXXX */ + escape_characters += 5; + } + break; + } + } + output_length = (size_t)(input_pointer - input) + escape_characters; + + output = ensure(output_buffer, output_length + sizeof("\"\"")); + if (output == NULL) { + return false; + } + + /* no characters have to be escaped */ + if (escape_characters == 0) { + output[0] = '\"'; + memcpy(output + 1, input, output_length); + output[output_length + 1] = '\"'; + output[output_length + 2] = '\0'; + + return true; + } + + output[0] = '\"'; + output_pointer = output + 1; + + /* copy the string */ + for (input_pointer = input; *input_pointer != '\0'; + (void)input_pointer++, output_pointer++) + { + if ((*input_pointer > 31) && (*input_pointer != '\"') + && (*input_pointer != '\\')) { + /* normal character, copy */ + *output_pointer = *input_pointer; + } else { + /* character needs to be escaped */ + *output_pointer++ = '\\'; + switch (*input_pointer) + { + case '\\': + *output_pointer = '\\'; + break; + case '\"': + *output_pointer = '\"'; + break; + case '\b': + *output_pointer = 'b'; + break; + case '\f': + *output_pointer = 'f'; + break; + case '\n': + *output_pointer = 'n'; + break; + case '\r': + *output_pointer = 'r'; + break; + case '\t': + *output_pointer = 't'; + break; + default: + /* escape and print as unicode codepoint */ + sprintf((char *)output_pointer, "u%04x", *input_pointer); + output_pointer += 4; + break; + } + } + } + output[output_length + 1] = '\"'; + output[output_length + 2] = '\0'; + + return true; +} + +/* Invoke print_string_ptr (which is useful) on an item. */ +static cJSON_bool print_string(const cJSON * const item, printbuffer * const p) +{ + return print_string_ptr((unsigned char *)item->valuestring, p); +} + +/* Predeclare these prototypes. */ +static cJSON_bool parse_value(cJSON * const item, + parse_buffer * const input_buffer); +static cJSON_bool print_value(const cJSON * const item, + printbuffer * const output_buffer); +static cJSON_bool parse_array(cJSON * const item, + parse_buffer * const input_buffer); +static cJSON_bool print_array(const cJSON * const item, + printbuffer * const output_buffer); +static cJSON_bool parse_object(cJSON * const item, + parse_buffer * const input_buffer); +static cJSON_bool print_object(const cJSON * const item, + printbuffer * const output_buffer); + +/* Utility to jump whitespace and cr/lf */ +static parse_buffer *buffer_skip_whitespace(parse_buffer * const buffer) +{ + if ((buffer == NULL) || (buffer->content == NULL)) { + return NULL; + } + + while (can_access_at_index(buffer, 0) && (buffer_at_offset(buffer)[0] <= 32)) + { + buffer->offset++; + } + + if (buffer->offset == buffer->length) { + buffer->offset--; + } + + return buffer; +} + +/* skip the UTF-8 BOM (byte order mark) if it is at the beginning of a buffer */ +static parse_buffer *skip_utf8_bom(parse_buffer * const buffer) +{ + if ((buffer == NULL) || (buffer->content == NULL) || (buffer->offset != 0)) { + return NULL; + } + + if (can_access_at_index(buffer, + 4) + && (strncmp((const char *)buffer_at_offset(buffer), "\xEF\xBB\xBF", + 3) == 0)) { + buffer->offset += 3; + } + + return buffer; +} + +/* Parse an object - create a new root, and populate. */ +CJSON_PUBLIC(cJSON *) cJSON_ParseWithOpts(const char *value, + const char **return_parse_end, + cJSON_bool require_null_terminated) +{ + parse_buffer buffer = { 0, 0, 0, 0, { 0, 0, 0 } }; + cJSON *item = NULL; + + /* reset error position */ + global_error.json = NULL; + global_error.position = 0; + + if (value == NULL) { + goto fail; + } + + buffer.content = (const unsigned char *)value; + buffer.length = strlen((const char *)value) + sizeof(""); + buffer.offset = 0; + buffer.hooks = global_hooks; + + item = cJSON_New_Item(&global_hooks); + if (item == NULL) { /* memory fail */ + goto fail; + } + + if (!parse_value(item, buffer_skip_whitespace(skip_utf8_bom(&buffer)))) { + /* parse failure. ep is set. */ + goto fail; + } + + /* if we require null-terminated JSON without appended garbage, skip and then + * check for a null terminator */ + if (require_null_terminated) { + buffer_skip_whitespace(&buffer); + if ((buffer.offset >= buffer.length) + || (buffer_at_offset(&buffer)[0] != '\0')) { + goto fail; + } + } + if (return_parse_end) { + *return_parse_end = (const char *)buffer_at_offset(&buffer); + } + + return item; + + fail: + if (item != NULL) { + cJSON_Delete(item); + } + + if (value != NULL) { + error local_error; + local_error.json = (const unsigned char *)value; + local_error.position = 0; + + if (buffer.offset < buffer.length) { + local_error.position = buffer.offset; + } else if (buffer.length > 0) { + local_error.position = buffer.length - 1; + } + + if (return_parse_end != NULL) { + *return_parse_end = (const char *)local_error.json + local_error.position; + } + + global_error = local_error; + } + + return NULL; +} + +/* Default options for cJSON_Parse */ +CJSON_PUBLIC(cJSON *) cJSON_Parse(const char *value) +{ + return cJSON_ParseWithOpts(value, 0, 0); +} + +#define cjson_min(a, b) ((a < b) ? a : b) + +static unsigned char *print(const cJSON * const item, + cJSON_bool format, + const internal_hooks * const hooks) +{ + static const size_t default_buffer_size = 256; + printbuffer buffer[1]; + unsigned char *printed = NULL; + + memset(buffer, 0, sizeof(buffer)); + + /* create buffer */ + buffer->buffer = (unsigned char *) hooks->allocate(default_buffer_size); + buffer->length = default_buffer_size; + buffer->format = format; + buffer->hooks = *hooks; + if (buffer->buffer == NULL) { + goto fail; + } + + /* print the value */ + if (!print_value(item, buffer)) { + goto fail; + } + update_offset(buffer); + + /* check if reallocate is available */ + if (hooks->reallocate != NULL) { + printed = (unsigned char *) hooks->reallocate(buffer->buffer, + buffer->offset + 1); + if (printed == NULL) { + goto fail; + } + buffer->buffer = NULL; + } else { /* otherwise copy the JSON over to a new buffer */ + printed = (unsigned char *) hooks->allocate(buffer->offset + 1); + if (printed == NULL) { + goto fail; + } + memcpy(printed, + buffer->buffer, + cjson_min(buffer->length, buffer->offset + 1)); + printed[buffer->offset] = '\0'; /* just to be sure */ + + /* free the buffer */ + hooks->deallocate(buffer->buffer); + } + + return printed; + + fail: + if (buffer->buffer != NULL) { + hooks->deallocate(buffer->buffer); + } + + if (printed != NULL) { + hooks->deallocate(printed); + } + + return NULL; +} + +/* Render a cJSON item/entity/structure to text. */ +CJSON_PUBLIC(char *) cJSON_Print(const cJSON * item) +{ + return (char *)print(item, true, &global_hooks); +} + +CJSON_PUBLIC(char *) cJSON_PrintUnformatted(const cJSON * item) +{ + return (char *)print(item, false, &global_hooks); +} + +CJSON_PUBLIC(char *) cJSON_PrintBuffered(const cJSON * item, + int prebuffer, + cJSON_bool fmt) +{ + printbuffer p = { 0, 0, 0, 0, 0, 0, { 0, 0, 0 } }; + + if (prebuffer < 0) { + return NULL; + } + + p.buffer = (unsigned char *)global_hooks.allocate((size_t)prebuffer); + if (!p.buffer) { + return NULL; + } + + p.length = (size_t)prebuffer; + p.offset = 0; + p.noalloc = false; + p.format = fmt; + p.hooks = global_hooks; + + if (!print_value(item, &p)) { + global_hooks.deallocate(p.buffer); + return NULL; + } + + return (char *)p.buffer; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_PrintPreallocated(cJSON * item, + char *buf, + const int len, + const cJSON_bool fmt) +{ + printbuffer p = { 0, 0, 0, 0, 0, 0, { 0, 0, 0 } }; + + if ((len < 0) || (buf == NULL)) { + return false; + } + + p.buffer = (unsigned char *)buf; + p.length = (size_t)len; + p.offset = 0; + p.noalloc = true; + p.format = fmt; + p.hooks = global_hooks; + + return print_value(item, &p); +} + +/* Parser core - when encountering text, process appropriately. */ +static cJSON_bool parse_value(cJSON * const item, + parse_buffer * const input_buffer) +{ + if ((input_buffer == NULL) || (input_buffer->content == NULL)) { + return false; /* no input */ + } + + /* parse the different types of values */ + /* null */ + if (can_read(input_buffer, + 4) + && (strncmp((const char *)buffer_at_offset(input_buffer), "null", + 4) == 0)) { + item->type = cJSON_NULL; + input_buffer->offset += 4; + return true; + } + + /* false */ + if (can_read(input_buffer, + 5) + && (strncmp((const char *)buffer_at_offset(input_buffer), "false", + 5) == 0)) { + item->type = cJSON_False; + input_buffer->offset += 5; + return true; + } + + /* true */ + if (can_read(input_buffer, + 4) + && (strncmp((const char *)buffer_at_offset(input_buffer), "true", + 4) == 0)) { + item->type = cJSON_True; + item->valueint = 1; + input_buffer->offset += 4; + return true; + } + + /* string */ + if (can_access_at_index(input_buffer, + 0) && (buffer_at_offset(input_buffer)[0] == '\"')) { + return parse_string(item, input_buffer); + } + + /* number */ + if (can_access_at_index(input_buffer, + 0) + && ((buffer_at_offset(input_buffer)[0] == '-') + || ((buffer_at_offset(input_buffer)[0] >= '0') + && (buffer_at_offset(input_buffer)[0] <= '9')))) { + return parse_number(item, input_buffer); + } + + /* array */ + if (can_access_at_index(input_buffer, + 0) && (buffer_at_offset(input_buffer)[0] == '[')) { + return parse_array(item, input_buffer); + } + + /* object */ + if (can_access_at_index(input_buffer, + 0) && (buffer_at_offset(input_buffer)[0] == '{')) { + return parse_object(item, input_buffer); + } + + return false; +} + +/* Render a value to text. */ +static cJSON_bool print_value(const cJSON * const item, + printbuffer * const output_buffer) +{ + unsigned char *output = NULL; + + if ((item == NULL) || (output_buffer == NULL)) { + return false; + } + + switch ((item->type) & 0xFF) + { + case cJSON_NULL: + output = ensure(output_buffer, 5); + if (output == NULL) { + return false; + } + strcpy((char *)output, "null"); + return true; + + case cJSON_False: + output = ensure(output_buffer, 6); + if (output == NULL) { + return false; + } + strcpy((char *)output, "false"); + return true; + + case cJSON_True: + output = ensure(output_buffer, 5); + if (output == NULL) { + return false; + } + strcpy((char *)output, "true"); + return true; + + case cJSON_Number: + return print_number(item, output_buffer); + + case cJSON_Raw: + { + size_t raw_length = 0; + if (item->valuestring == NULL) { + return false; + } + + raw_length = strlen(item->valuestring) + sizeof(""); + output = ensure(output_buffer, raw_length); + if (output == NULL) { + return false; + } + memcpy(output, item->valuestring, raw_length); + return true; + } + + case cJSON_String: + return print_string(item, output_buffer); + + case cJSON_Array: + return print_array(item, output_buffer); + + case cJSON_Object: + return print_object(item, output_buffer); + + default: + return false; + } +} + +/* Build an array from input text. */ +static cJSON_bool parse_array(cJSON * const item, + parse_buffer * const input_buffer) +{ + cJSON *head = NULL; /* head of the linked list */ + cJSON *current_item = NULL; + + if (input_buffer->depth >= CJSON_NESTING_LIMIT) { + return false; /* to deeply nested */ + } + input_buffer->depth++; + + if (buffer_at_offset(input_buffer)[0] != '[') { + /* not an array */ + goto fail; + } + + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (can_access_at_index(input_buffer, + 0) && (buffer_at_offset(input_buffer)[0] == ']')) { + /* empty array */ + goto success; + } + + /* check if we skipped to the end of the buffer */ + if (cannot_access_at_index(input_buffer, 0)) { + input_buffer->offset--; + goto fail; + } + + /* step back to character in front of the first element */ + input_buffer->offset--; + + /* loop through the comma separated array elements */ + do + { + /* allocate next item */ + cJSON *new_item = cJSON_New_Item(&(input_buffer->hooks)); + if (new_item == NULL) { + goto fail; /* allocation failure */ + } + + /* attach next item to list */ + if (head == NULL) { + /* start the linked list */ + current_item = head = new_item; + } else { + /* add to the end and advance */ + current_item->next = new_item; + new_item->prev = current_item; + current_item = new_item; + } + + /* parse next value */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_value(current_item, input_buffer)) { + goto fail; /* failed to parse value */ + } + buffer_skip_whitespace(input_buffer); + } + while (can_access_at_index(input_buffer, + 0) && (buffer_at_offset(input_buffer)[0] == ',')); + + if (cannot_access_at_index(input_buffer, + 0) || (buffer_at_offset(input_buffer)[0] != ']')) { + goto fail; /* expected end of array */ + } + + success: + input_buffer->depth--; + + item->type = cJSON_Array; + item->child = head; + + input_buffer->offset++; + + return true; + + fail: + if (head != NULL) { + cJSON_Delete(head); + } + + return false; +} + +/* Render an array to text */ +static cJSON_bool print_array(const cJSON * const item, + printbuffer * const output_buffer) +{ + unsigned char *output_pointer = NULL; + size_t length = 0; + cJSON *current_element = item->child; + + if (output_buffer == NULL) { + return false; + } + + /* Compose the output array. */ + /* opening square bracket */ + output_pointer = ensure(output_buffer, 1); + if (output_pointer == NULL) { + return false; + } + + *output_pointer = '['; + output_buffer->offset++; + output_buffer->depth++; + + while (current_element != NULL) + { + if (!print_value(current_element, output_buffer)) { + return false; + } + update_offset(output_buffer); + if (current_element->next) { + length = (size_t) (output_buffer->format ? 2 : 1); + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) { + return false; + } + *output_pointer++ = ','; + if (output_buffer->format) { + *output_pointer++ = ' '; + } + *output_pointer = '\0'; + output_buffer->offset += length; + } + current_element = current_element->next; + } + + output_pointer = ensure(output_buffer, 2); + if (output_pointer == NULL) { + return false; + } + *output_pointer++ = ']'; + *output_pointer = '\0'; + output_buffer->depth--; + + return true; +} + +/* Build an object from the text. */ +static cJSON_bool parse_object(cJSON * const item, + parse_buffer * const input_buffer) +{ + cJSON *head = NULL; /* linked list head */ + cJSON *current_item = NULL; + + if (input_buffer->depth >= CJSON_NESTING_LIMIT) { + return false; /* to deeply nested */ + } + input_buffer->depth++; + + if (cannot_access_at_index(input_buffer, + 0) || (buffer_at_offset(input_buffer)[0] != '{')) { + goto fail; /* not an object */ + } + + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (can_access_at_index(input_buffer, + 0) && (buffer_at_offset(input_buffer)[0] == '}')) { + goto success; /* empty object */ + } + + /* check if we skipped to the end of the buffer */ + if (cannot_access_at_index(input_buffer, 0)) { + input_buffer->offset--; + goto fail; + } + + /* step back to character in front of the first element */ + input_buffer->offset--; + + /* loop through the comma separated array elements */ + do + { + /* allocate next item */ + cJSON *new_item = cJSON_New_Item(&(input_buffer->hooks)); + if (new_item == NULL) { + goto fail; /* allocation failure */ + } + + /* attach next item to list */ + if (head == NULL) { + /* start the linked list */ + current_item = head = new_item; + } else { + /* add to the end and advance */ + current_item->next = new_item; + new_item->prev = current_item; + current_item = new_item; + } + + /* parse the name of the child */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_string(current_item, input_buffer)) { + goto fail; /* failed to parse name */ + } + buffer_skip_whitespace(input_buffer); + + /* swap valuestring and string, because we parsed the name */ + current_item->string = current_item->valuestring; + current_item->valuestring = NULL; + + if (cannot_access_at_index(input_buffer, + 0) + || (buffer_at_offset(input_buffer)[0] != ':')) { + goto fail; /* invalid object */ + } + + /* parse the value */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_value(current_item, input_buffer)) { + goto fail; /* failed to parse value */ + } + buffer_skip_whitespace(input_buffer); + } + while (can_access_at_index(input_buffer, + 0) && (buffer_at_offset(input_buffer)[0] == ',')); + + if (cannot_access_at_index(input_buffer, + 0) || (buffer_at_offset(input_buffer)[0] != '}')) { + goto fail; /* expected end of object */ + } + + success: + input_buffer->depth--; + + item->type = cJSON_Object; + item->child = head; + + input_buffer->offset++; + return true; + + fail: + if (head != NULL) { + cJSON_Delete(head); + } + + return false; +} + +/* Render an object to text. */ +static cJSON_bool print_object(const cJSON * const item, + printbuffer * const output_buffer) +{ + unsigned char *output_pointer = NULL; + size_t length = 0; + cJSON *current_item = item->child; + + if (output_buffer == NULL) { + return false; + } + + /* Compose the output: */ + length = (size_t) (output_buffer->format ? 2 : 1); /* fmt: {\n */ + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) { + return false; + } + + *output_pointer++ = '{'; + output_buffer->depth++; + if (output_buffer->format) { + *output_pointer++ = '\n'; + } + output_buffer->offset += length; + + while (current_item) + { + if (output_buffer->format) { + size_t i; + output_pointer = ensure(output_buffer, output_buffer->depth); + if (output_pointer == NULL) { + return false; + } + for (i = 0; i < output_buffer->depth; i++) + { + *output_pointer++ = '\t'; + } + output_buffer->offset += output_buffer->depth; + } + + /* print key */ + if (!print_string_ptr((unsigned char *)current_item->string, + output_buffer)) { + return false; + } + update_offset(output_buffer); + + length = (size_t) (output_buffer->format ? 2 : 1); + output_pointer = ensure(output_buffer, length); + if (output_pointer == NULL) { + return false; + } + *output_pointer++ = ':'; + if (output_buffer->format) { + *output_pointer++ = '\t'; + } + output_buffer->offset += length; + + /* print value */ + if (!print_value(current_item, output_buffer)) { + return false; + } + update_offset(output_buffer); + + /* print comma if not last */ + length = + ((size_t)(output_buffer->format ? 1 : 0) + + (size_t)(current_item->next ? 1 : 0)); + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) { + return false; + } + if (current_item->next) { + *output_pointer++ = ','; + } + + if (output_buffer->format) { + *output_pointer++ = '\n'; + } + *output_pointer = '\0'; + output_buffer->offset += length; + + current_item = current_item->next; + } + + output_pointer = + ensure(output_buffer, + output_buffer->format ? (output_buffer->depth + 1) : 2); + if (output_pointer == NULL) { + return false; + } + if (output_buffer->format) { + size_t i; + for (i = 0; i < (output_buffer->depth - 1); i++) + { + *output_pointer++ = '\t'; + } + } + *output_pointer++ = '}'; + *output_pointer = '\0'; + output_buffer->depth--; + + return true; +} + +/* Get Array size/item / object item. */ +CJSON_PUBLIC(int) cJSON_GetArraySize(const cJSON * array) +{ + cJSON *child = NULL; + size_t size = 0; + + if (array == NULL) { + return 0; + } + + child = array->child; + + while (child != NULL) + { + size++; + child = child->next; + } + + /* FIXME: Can overflow here. Cannot be fixed without breaking the API */ + + return (int)size; +} + +static cJSON * get_array_item(const cJSON *array, size_t index) +{ + cJSON *current_child = NULL; + + if (array == NULL) { + return NULL; + } + + current_child = array->child; + while ((current_child != NULL) && (index > 0)) + { + index--; + current_child = current_child->next; + } + + return current_child; +} + +CJSON_PUBLIC(cJSON *) cJSON_GetArrayItem(const cJSON * array, int index) +{ + if (index < 0) { + return NULL; + } + + return get_array_item(array, (size_t)index); +} + +static cJSON *get_object_item(const cJSON * const object, + const char * const name, + const cJSON_bool case_sensitive) +{ + cJSON *current_element = NULL; + + if ((object == NULL) || (name == NULL)) { + return NULL; + } + + current_element = object->child; + if (case_sensitive) { + while ((current_element != NULL) && (current_element->string != NULL) + && (strcmp(name, current_element->string) != 0)) + { + current_element = current_element->next; + } + } else { + while ((current_element != NULL) + && (case_insensitive_strcmp((const unsigned char *)name, + (const unsigned char *)(current_element-> + string)) != 0)) + { + current_element = current_element->next; + } + } + + if ((current_element == NULL) || (current_element->string == NULL)) { + return NULL; + } + + return current_element; +} + +CJSON_PUBLIC(cJSON *) cJSON_GetObjectItem(const cJSON * const object, + const char * const string) +{ + return get_object_item(object, string, false); +} + +CJSON_PUBLIC(cJSON *) cJSON_GetObjectItemCaseSensitive( + const cJSON * const object, + const char * const string) +{ + return get_object_item(object, string, true); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_HasObjectItem(const cJSON * object, + const char *string) +{ + return cJSON_GetObjectItem(object, string) ? 1 : 0; +} + +/* Utility for array list handling. */ +static void suffix_object(cJSON *prev, cJSON *item) +{ + prev->next = item; + item->prev = prev; +} + +/* Utility for handling references. */ +static cJSON *create_reference(const cJSON *item, + const internal_hooks * const hooks) +{ + cJSON *reference = NULL; + if (item == NULL) { + return NULL; + } + + reference = cJSON_New_Item(hooks); + if (reference == NULL) { + return NULL; + } + + memcpy(reference, item, sizeof(cJSON)); + reference->string = NULL; + reference->type |= cJSON_IsReference; + reference->next = reference->prev = NULL; + return reference; +} + +static cJSON_bool add_item_to_array(cJSON *array, cJSON *item) +{ + cJSON *child = NULL; + + if ((item == NULL) || (array == NULL)) { + return false; + } + + child = array->child; + + if (child == NULL) { + /* list is empty, start new one */ + array->child = item; + } else { + /* append to the end */ + while (child->next) + { + child = child->next; + } + suffix_object(child, item); + } + + return true; +} + +/* Add item to array/object. */ +CJSON_PUBLIC(void) cJSON_AddItemToArray(cJSON * array, cJSON * item) +{ + add_item_to_array(array, item); +} + +#if defined(__clang__) || (defined(__GNUC__) && ((__GNUC__ > 4) \ + || ((__GNUC__ == 4) && (__GNUC_MINOR__ > 5)))) +#pragma GCC diagnostic push +#endif +#ifdef __GNUC__ +#pragma GCC diagnostic ignored "-Wcast-qual" +#endif + +/* helper function to cast away const */ +static void * cast_away_const(const void *string) +{ + return (void *)string; +} + +#if defined(__clang__) || (defined(__GNUC__) && ((__GNUC__ > 4) \ + || ((__GNUC__ == 4) && (__GNUC_MINOR__ > 5)))) +#pragma GCC diagnostic pop +#endif + +static cJSON_bool add_item_to_object(cJSON * const object, + const char * const string, + cJSON * const item, + const internal_hooks * const hooks, + const cJSON_bool constant_key) +{ + char *new_key = NULL; + int new_type = cJSON_Invalid; + + if ((object == NULL) || (string == NULL) || (item == NULL)) { + return false; + } + + if (constant_key) { + new_key = (char *)cast_away_const(string); + new_type = item->type | cJSON_StringIsConst; + } else { + new_key = (char *)cJSON_strdup((const unsigned char *)string, hooks); + if (new_key == NULL) { + return false; + } + + new_type = item->type & ~cJSON_StringIsConst; + } + + if (!(item->type & cJSON_StringIsConst) && (item->string != NULL)) { + hooks->deallocate(item->string); + } + + item->string = new_key; + item->type = new_type; + + return add_item_to_array(object, item); +} + +CJSON_PUBLIC(void) cJSON_AddItemToObject(cJSON * object, + const char *string, + cJSON * item) +{ + add_item_to_object(object, string, item, &global_hooks, false); +} + +/* Add an item to an object with constant string as key */ +CJSON_PUBLIC(void) cJSON_AddItemToObjectCS(cJSON * object, + const char *string, + cJSON * item) +{ + add_item_to_object(object, string, item, &global_hooks, true); +} + +CJSON_PUBLIC(void) cJSON_AddItemReferenceToArray(cJSON * array, cJSON * item) +{ + if (array == NULL) { + return; + } + + add_item_to_array(array, create_reference(item, &global_hooks)); +} + +CJSON_PUBLIC(void) cJSON_AddItemReferenceToObject(cJSON * object, + const char *string, + cJSON * item) +{ + if ((object == NULL) || (string == NULL)) { + return; + } + + add_item_to_object(object, + string, + create_reference(item, &global_hooks), + &global_hooks, + false); +} + +CJSON_PUBLIC(cJSON *) cJSON_AddNullToObject(cJSON * const object, + const char * const name) +{ + cJSON *null = cJSON_CreateNull(); + if (add_item_to_object(object, name, null, &global_hooks, false)) { + return null; + } + + cJSON_Delete(null); + return NULL; +} + +CJSON_PUBLIC(cJSON *) cJSON_AddTrueToObject(cJSON * const object, + const char * const name) +{ + cJSON *true_item = cJSON_CreateTrue(); + if (add_item_to_object(object, name, true_item, &global_hooks, false)) { + return true_item; + } + + cJSON_Delete(true_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) cJSON_AddFalseToObject(cJSON * const object, + const char * const name) +{ + cJSON *false_item = cJSON_CreateFalse(); + if (add_item_to_object(object, name, false_item, &global_hooks, false)) { + return false_item; + } + + cJSON_Delete(false_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) cJSON_AddBoolToObject(cJSON * const object, + const char * const name, + const cJSON_bool boolean) +{ + cJSON *bool_item = cJSON_CreateBool(boolean); + if (add_item_to_object(object, name, bool_item, &global_hooks, false)) { + return bool_item; + } + + cJSON_Delete(bool_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) cJSON_AddNumberToObject(cJSON * const object, + const char * const name, + const double number) +{ + cJSON *number_item = cJSON_CreateNumber(number); + if (add_item_to_object(object, name, number_item, &global_hooks, false)) { + return number_item; + } + + cJSON_Delete(number_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) cJSON_AddStringToObject(cJSON * const object, + const char * const name, + const char * const string) +{ + cJSON *string_item = cJSON_CreateString(string); + if (add_item_to_object(object, name, string_item, &global_hooks, false)) { + return string_item; + } + + cJSON_Delete(string_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) cJSON_AddRawToObject(cJSON * const object, + const char * const name, + const char * const raw) +{ + cJSON *raw_item = cJSON_CreateRaw(raw); + if (add_item_to_object(object, name, raw_item, &global_hooks, false)) { + return raw_item; + } + + cJSON_Delete(raw_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) cJSON_AddObjectToObject(cJSON * const object, + const char * const name) +{ + cJSON *object_item = cJSON_CreateObject(); + if (add_item_to_object(object, name, object_item, &global_hooks, false)) { + return object_item; + } + + cJSON_Delete(object_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) cJSON_AddArrayToObject(cJSON * const object, + const char * const name) +{ + cJSON *array = cJSON_CreateArray(); + if (add_item_to_object(object, name, array, &global_hooks, false)) { + return array; + } + + cJSON_Delete(array); + return NULL; +} + +CJSON_PUBLIC(cJSON *) cJSON_DetachItemViaPointer(cJSON * parent, + cJSON * const item) +{ + if ((parent == NULL) || (item == NULL)) { + return NULL; + } + + if (item->prev != NULL) { + /* not the first element */ + item->prev->next = item->next; + } + if (item->next != NULL) { + /* not the last element */ + item->next->prev = item->prev; + } + + if (item == parent->child) { + /* first element */ + parent->child = item->next; + } + + /* make sure the detached item doesn't point anywhere anymore */ + item->prev = NULL; + item->next = NULL; + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromArray(cJSON * array, int which) +{ + if (which < 0) { + return NULL; + } + + return cJSON_DetachItemViaPointer(array, + get_array_item(array, (size_t)which)); +} + +CJSON_PUBLIC(void) cJSON_DeleteItemFromArray(cJSON * array, int which) +{ + cJSON_Delete(cJSON_DetachItemFromArray(array, which)); +} + +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObject(cJSON * object, + const char *string) +{ + cJSON *to_detach = cJSON_GetObjectItem(object, string); + + return cJSON_DetachItemViaPointer(object, to_detach); +} + +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObjectCaseSensitive(cJSON * object, + const char *string) +{ + cJSON *to_detach = cJSON_GetObjectItemCaseSensitive(object, string); + + return cJSON_DetachItemViaPointer(object, to_detach); +} + +CJSON_PUBLIC(void) cJSON_DeleteItemFromObject(cJSON * object, + const char *string) +{ + cJSON_Delete(cJSON_DetachItemFromObject(object, string)); +} + +CJSON_PUBLIC(void) cJSON_DeleteItemFromObjectCaseSensitive(cJSON * object, + const char *string) +{ + cJSON_Delete(cJSON_DetachItemFromObjectCaseSensitive(object, string)); +} + +/* Replace array/object items with new ones. */ +CJSON_PUBLIC(void) cJSON_InsertItemInArray(cJSON * array, + int which, + cJSON * newitem) +{ + cJSON *after_inserted = NULL; + + if (which < 0) { + return; + } + + after_inserted = get_array_item(array, (size_t)which); + if (after_inserted == NULL) { + add_item_to_array(array, newitem); + return; + } + + newitem->next = after_inserted; + newitem->prev = after_inserted->prev; + after_inserted->prev = newitem; + if (after_inserted == array->child) { + array->child = newitem; + } else { + newitem->prev->next = newitem; + } +} + +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemViaPointer(cJSON * const parent, + cJSON * const item, + cJSON * replacement) +{ + if ((parent == NULL) || (replacement == NULL) || (item == NULL)) { + return false; + } + + if (replacement == item) { + return true; + } + + replacement->next = item->next; + replacement->prev = item->prev; + + if (replacement->next != NULL) { + replacement->next->prev = replacement; + } + if (replacement->prev != NULL) { + replacement->prev->next = replacement; + } + if (parent->child == item) { + parent->child = replacement; + } + + item->next = NULL; + item->prev = NULL; + cJSON_Delete(item); + + return true; +} + +CJSON_PUBLIC(void) cJSON_ReplaceItemInArray(cJSON * array, + int which, + cJSON * newitem) +{ + if (which < 0) { + return; + } + + cJSON_ReplaceItemViaPointer(array, + get_array_item(array, (size_t)which), + newitem); +} + +static cJSON_bool replace_item_in_object(cJSON *object, + const char *string, + cJSON *replacement, + cJSON_bool case_sensitive) +{ + if ((replacement == NULL) || (string == NULL)) { + return false; + } + + /* replace the name in the replacement */ + if (!(replacement->type & cJSON_StringIsConst) + && (replacement->string != NULL)) { + cJSON_free(replacement->string); + } + replacement->string = (char *)cJSON_strdup((const unsigned char *)string, + &global_hooks); + replacement->type &= ~cJSON_StringIsConst; + + cJSON_ReplaceItemViaPointer(object, + get_object_item(object, string, case_sensitive), + replacement); + + return true; +} + +CJSON_PUBLIC(void) cJSON_ReplaceItemInObject(cJSON * object, + const char *string, + cJSON * newitem) +{ + replace_item_in_object(object, string, newitem, false); +} + +CJSON_PUBLIC(void) cJSON_ReplaceItemInObjectCaseSensitive(cJSON * object, + const char *string, + cJSON * newitem) +{ + replace_item_in_object(object, string, newitem, true); +} + +/* Create basic types: */ +CJSON_PUBLIC(cJSON *) cJSON_CreateNull(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) { + item->type = cJSON_NULL; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateTrue(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) { + item->type = cJSON_True; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateFalse(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) { + item->type = cJSON_False; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateBool(cJSON_bool b) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) { + item->type = b ? cJSON_True : cJSON_False; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateNumber(double num) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) { + item->type = cJSON_Number; + item->valuedouble = num; + + /* use saturation in case of overflow */ + if (num >= INT_MAX) { + item->valueint = INT_MAX; + } else if (num <= (double)INT_MIN) { + item->valueint = INT_MIN; + } else { + item->valueint = (int)num; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateString(const char *string) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) { + item->type = cJSON_String; + item->valuestring = (char *)cJSON_strdup((const unsigned char *)string, + &global_hooks); + if (!item->valuestring) { + cJSON_Delete(item); + return NULL; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateStringReference(const char *string) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) { + item->type = cJSON_String | cJSON_IsReference; + item->valuestring = (char *)cast_away_const(string); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateObjectReference(const cJSON * child) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) { + item->type = cJSON_Object | cJSON_IsReference; + item->child = (cJSON *)cast_away_const(child); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateArrayReference(const cJSON * child) { + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) { + item->type = cJSON_Array | cJSON_IsReference; + item->child = (cJSON *)cast_away_const(child); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateRaw(const char *raw) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) { + item->type = cJSON_Raw; + item->valuestring = (char *)cJSON_strdup((const unsigned char *)raw, + &global_hooks); + if (!item->valuestring) { + cJSON_Delete(item); + return NULL; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateArray(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) { + item->type = cJSON_Array; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateObject(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) { + item->type = cJSON_Object; + } + + return item; +} + +/* Create Arrays: */ +CJSON_PUBLIC(cJSON *) cJSON_CreateIntArray(const int *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) { + return NULL; + } + + a = cJSON_CreateArray(); + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber(numbers[i]); + if (!n) { + cJSON_Delete(a); + return NULL; + } + if (!i) { + a->child = n; + } else { + suffix_object(p, n); + } + p = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateFloatArray(const float *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber((double)numbers[i]); + if (!n) { + cJSON_Delete(a); + return NULL; + } + if (!i) { + a->child = n; + } else { + suffix_object(p, n); + } + p = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateDoubleArray(const double *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber(numbers[i]); + if (!n) { + cJSON_Delete(a); + return NULL; + } + if (!i) { + a->child = n; + } else { + suffix_object(p, n); + } + p = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateStringArray(const char **strings, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (strings == NULL)) { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateString(strings[i]); + if (!n) { + cJSON_Delete(a); + return NULL; + } + if (!i) { + a->child = n; + } else { + suffix_object(p, n); + } + p = n; + } + + return a; +} + +/* Duplication */ +CJSON_PUBLIC(cJSON *) cJSON_Duplicate(const cJSON * item, cJSON_bool recurse) +{ + cJSON *newitem = NULL; + cJSON *child = NULL; + cJSON *next = NULL; + cJSON *newchild = NULL; + + /* Bail on bad ptr */ + if (!item) { + goto fail; + } + + /* Create new item */ + newitem = cJSON_New_Item(&global_hooks); + if (!newitem) { + goto fail; + } + + /* Copy over all vars */ + newitem->type = item->type & (~cJSON_IsReference); + newitem->valueint = item->valueint; + newitem->valuedouble = item->valuedouble; + if (item->valuestring) { + newitem->valuestring = (char *)cJSON_strdup( + (unsigned char *)item->valuestring, + &global_hooks); + if (!newitem->valuestring) { + goto fail; + } + } + if (item->string) { + newitem->string = + (item->type + & cJSON_StringIsConst) ? item->string : (char *)cJSON_strdup((unsigned + char + *)item->string, + &global_hooks); + if (!newitem->string) { + goto fail; + } + } + + /* If non-recursive, then we're done! */ + if (!recurse) { + return newitem; + } + + /* Walk the ->next chain for the child. */ + child = item->child; + while (child != NULL) + { + newchild = cJSON_Duplicate(child, true); /* Duplicate (with recurse) + * each item in the ->next + * chain */ + if (!newchild) { + goto fail; + } + if (next != NULL) { + /* If newitem->child already set, then crosswire ->prev and ->next and + * move on */ + next->next = newchild; + newchild->prev = next; + next = newchild; + } else { + /* Set newitem->child and move to it */ + newitem->child = newchild; + next = newchild; + } + child = child->next; + } + + return newitem; + + fail: + if (newitem != NULL) { + cJSON_Delete(newitem); + } + + return NULL; +} + +static void skip_oneline_comment(char **input) +{ + *input += static_strlen("//"); + + for (; (*input)[0] != '\0'; ++(*input)) + { + if ((*input)[0] == '\n') { + *input += static_strlen("\n"); + return; + } + } +} + +static void skip_multiline_comment(char **input) +{ + *input += static_strlen("/*"); + + for (; (*input)[0] != '\0'; ++(*input)) + { + if (((*input)[0] == '*') && ((*input)[1] == '/')) { + *input += static_strlen("*/"); + return; + } + } +} + +static void minify_string(char **input, char **output) +{ + (*output)[0] = (*input)[0]; + *input += static_strlen("\""); + *output += static_strlen("\""); + + for (; (*input)[0] != '\0'; (void)++(*input), ++(*output)) { + (*output)[0] = (*input)[0]; + + if ((*input)[0] == '\"') { + (*output)[0] = '\"'; + *input += static_strlen("\""); + *output += static_strlen("\""); + return; + } else if (((*input)[0] == '\\') && ((*input)[1] == '\"')) { + (*output)[1] = (*input)[1]; + *input += static_strlen("\""); + *output += static_strlen("\""); + } + } +} + +CJSON_PUBLIC(void) cJSON_Minify(char *json) +{ + char *into = json; + + if (json == NULL) { + return; + } + + while (json[0] != '\0') + { + switch (json[0]) + { + case ' ': + case '\t': + case '\r': + case '\n': + json++; + break; + + case '/': + if (json[1] == '/') { + skip_oneline_comment(&json); + } else if (json[1] == '*') { + skip_multiline_comment(&json); + } else { + json++; + } + break; + + case '\"': + minify_string(&json, (char **)&into); + break; + + default: + into[0] = json[0]; + json++; + into++; + } + } + + /* and null-terminate. */ + *into = '\0'; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsInvalid(const cJSON * const item) +{ + if (item == NULL) { + return false; + } + + return (item->type & 0xFF) == cJSON_Invalid; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsFalse(const cJSON * const item) +{ + if (item == NULL) { + return false; + } + + return (item->type & 0xFF) == cJSON_False; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsTrue(const cJSON * const item) +{ + if (item == NULL) { + return false; + } + + return (item->type & 0xff) == cJSON_True; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsBool(const cJSON * const item) +{ + if (item == NULL) { + return false; + } + + return (item->type & (cJSON_True | cJSON_False)) != 0; +} +CJSON_PUBLIC(cJSON_bool) cJSON_IsNull(const cJSON * const item) +{ + if (item == NULL) { + return false; + } + + return (item->type & 0xFF) == cJSON_NULL; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsNumber(const cJSON * const item) +{ + if (item == NULL) { + return false; + } + + return (item->type & 0xFF) == cJSON_Number; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsString(const cJSON * const item) +{ + if (item == NULL) { + return false; + } + + return (item->type & 0xFF) == cJSON_String; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsArray(const cJSON * const item) +{ + if (item == NULL) { + return false; + } + + return (item->type & 0xFF) == cJSON_Array; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsObject(const cJSON * const item) +{ + if (item == NULL) { + return false; + } + + return (item->type & 0xFF) == cJSON_Object; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsRaw(const cJSON * const item) +{ + if (item == NULL) { + return false; + } + + return (item->type & 0xFF) == cJSON_Raw; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_Compare(const cJSON * const a, + const cJSON * const b, + const cJSON_bool case_sensitive) +{ + if ((a == NULL) || (b == NULL) || ((a->type & 0xFF) != (b->type & 0xFF)) + || cJSON_IsInvalid(a)) { + return false; + } + + /* check if type is valid */ + switch (a->type & 0xFF) + { + case cJSON_False: + case cJSON_True: + case cJSON_NULL: + case cJSON_Number: + case cJSON_String: + case cJSON_Raw: + case cJSON_Array: + case cJSON_Object: + break; + + default: + return false; + } + + /* identical objects are equal */ + if (a == b) { + return true; + } + + switch (a->type & 0xFF) + { + /* in these cases and equal type is enough */ + case cJSON_False: + case cJSON_True: + case cJSON_NULL: + return true; + + case cJSON_Number: + if (a->valuedouble == b->valuedouble) { + return true; + } + return false; + + case cJSON_String: + case cJSON_Raw: + if ((a->valuestring == NULL) || (b->valuestring == NULL)) { + return false; + } + if (strcmp(a->valuestring, b->valuestring) == 0) { + return true; + } + + return false; + + case cJSON_Array: + { + cJSON *a_element = a->child; + cJSON *b_element = b->child; + + for (; (a_element != NULL) && (b_element != NULL);) + { + if (!cJSON_Compare(a_element, b_element, case_sensitive)) { + return false; + } + + a_element = a_element->next; + b_element = b_element->next; + } + + /* one of the arrays is longer than the other */ + if (a_element != b_element) { + return false; + } + + return true; + } + + case cJSON_Object: + { + cJSON *a_element = NULL; + cJSON *b_element = NULL; + cJSON_ArrayForEach(a_element, a) + { + /* TODO This has O(n^2) runtime, which is horrible! */ + b_element = get_object_item(b, a_element->string, case_sensitive); + if (b_element == NULL) { + return false; + } + + if (!cJSON_Compare(a_element, b_element, case_sensitive)) { + return false; + } + } + + /* doing this twice, once on a and b to prevent true comparison if a + * subset of b + * TODO: Do this the proper way, this is just a fix for now */ + cJSON_ArrayForEach(b_element, b) + { + a_element = get_object_item(a, b_element->string, case_sensitive); + if (a_element == NULL) { + return false; + } + + if (!cJSON_Compare(b_element, a_element, case_sensitive)) { + return false; + } + } + + return true; + } + + default: + return false; + } +} + +CJSON_PUBLIC(void *) cJSON_malloc(size_t size) +{ + return global_hooks.allocate(size); +} + +CJSON_PUBLIC(void) cJSON_free(void *object) +{ + global_hooks.deallocate(object); +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/srv/json/cJSON.h b/bluetooth_uwb_dw3000_slotted_twr/src/srv/json/cJSON.h new file mode 100644 index 0000000..eb70a48 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/srv/json/cJSON.h @@ -0,0 +1,411 @@ +/* + * Copyright (c) 2009-2017 Dave Gamble and cJSON contributors + * + * Permission is hereby granted, free of charge, to any person obtaining a + * copy + * of this software and associated documentation files (the "Software"), to + * deal + * in the Software without restriction, including without limitation the + * rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + * THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifndef cJSON__h +#define cJSON__h + +#ifdef __cplusplus +extern "C" +{ +#endif + +#if !defined(__WINDOWS__) && (defined(WIN32) || defined(WIN64) \ + || defined(_MSC_VER) || defined(_WIN32)) +#define __WINDOWS__ +#endif + +#ifdef __WINDOWS__ + +/* When compiling for windows, we specify a specific calling convention to avoid + * issues where we are being called from a project with a different default + * calling convention. For windows you have 3 define options: + * + * CJSON_HIDE_SYMBOLS - Define this in the case where you don't want to ever + * dllexport symbols + * CJSON_EXPORT_SYMBOLS - Define this on library build when you want to + * dllexport symbols (default) + * CJSON_IMPORT_SYMBOLS - Define this if you want to dllimport symbol + * + * For *nix builds that support visibility attribute, you can define similar + * behavior by + * + * setting default visibility to hidden by adding + * -fvisibility=hidden (for gcc) + * or + * -xldscope=hidden (for sun cc) + * to CFLAGS + * + * then using the CJSON_API_VISIBILITY flag to "export" the same symbols the + * way CJSON_EXPORT_SYMBOLS does + * + */ + +#define CJSON_CDECL __cdecl +#define CJSON_STDCALL __stdcall + +/* export symbols by default, this is necessary for copy pasting the C and + * header file */ +#if !defined(CJSON_HIDE_SYMBOLS) && !defined(CJSON_IMPORT_SYMBOLS) \ + && !defined(CJSON_EXPORT_SYMBOLS) +#define CJSON_EXPORT_SYMBOLS +#endif + +#if defined(CJSON_HIDE_SYMBOLS) +#define CJSON_PUBLIC(type) type CJSON_STDCALL +#elif defined(CJSON_EXPORT_SYMBOLS) +#define CJSON_PUBLIC(type) __declspec(dllexport) type CJSON_STDCALL +#elif defined(CJSON_IMPORT_SYMBOLS) +#define CJSON_PUBLIC(type) __declspec(dllimport) type CJSON_STDCALL +#endif +#else /* !__WINDOWS__ */ +#define CJSON_CDECL +#define CJSON_STDCALL + +#if (defined(__GNUC__) || defined(__SUNPRO_CC) || defined (__SUNPRO_C)) \ + && defined(CJSON_API_VISIBILITY) +#define CJSON_PUBLIC(type) __attribute__((visibility("default"))) type +#else +#define CJSON_PUBLIC(type) type +#endif +#endif + +/* project version */ +#define CJSON_VERSION_MAJOR 1 +#define CJSON_VERSION_MINOR 7 +#define CJSON_VERSION_PATCH 12 + +#include + +/* cJSON Types: */ +#define cJSON_Invalid (0) +#define cJSON_False (1 << 0) +#define cJSON_True (1 << 1) +#define cJSON_NULL (1 << 2) +#define cJSON_Number (1 << 3) +#define cJSON_String (1 << 4) +#define cJSON_Array (1 << 5) +#define cJSON_Object (1 << 6) +#define cJSON_Raw (1 << 7) /* raw json */ + +#define cJSON_IsReference 256 +#define cJSON_StringIsConst 512 + +/* The cJSON structure: */ +typedef struct cJSON +{ + /* next/prev allow you to walk array/object chains. Alternatively, use + * GetArraySize/GetArrayItem/GetObjectItem */ + struct cJSON *next; + struct cJSON *prev; + + /* An array or object item will have a child pointer pointing to a chain of + * the items in the array/object. */ + struct cJSON *child; + + /* The type of the item, as above. */ + int type; + + /* The item's string, if type==cJSON_String and type == cJSON_Raw */ + char *valuestring; + + /* writing to valueint is DEPRECATED, use cJSON_SetNumberValue instead */ + int valueint; + + /* The item's number, if type==cJSON_Number */ + double valuedouble; + + /* The item's name string, if this item is the child of, or is in the list of + * subitems of an object. */ + char *string; +} cJSON; + +typedef struct cJSON_Hooks +{ + /* malloc/free are CDECL on Windows regardless of the default calling + * convention of the compiler, so ensure the hooks allow passing those + * functions directly. */ + void *(CJSON_CDECL * malloc_fn)(size_t sz); + void(CJSON_CDECL * free_fn)(void *ptr); +} cJSON_Hooks; + +typedef int cJSON_bool; + +/* Limits how deeply nested arrays/objects can be before cJSON rejects to parse + * them. + * This is to prevent stack overflows. */ +#ifndef CJSON_NESTING_LIMIT +#define CJSON_NESTING_LIMIT 1000 +#endif + +/* returns the version of cJSON as a string */ +CJSON_PUBLIC(const char *) cJSON_Version(void); + +/* Supply malloc, realloc and free functions to cJSON */ +CJSON_PUBLIC(void) cJSON_InitHooks(cJSON_Hooks * hooks); + +/* Memory Management: the caller is always responsible to free the results from + * all variants of cJSON_Parse (with cJSON_Delete) and cJSON_Print (with + * stdlib free, cJSON_Hooks.free_fn, or cJSON_free as appropriate). The + * exception is cJSON_PrintPreallocated, where the caller has full + * responsibility of the buffer. */ +/* Supply a block of JSON, and this returns a cJSON object you can interrogate. + */ +CJSON_PUBLIC(cJSON *) cJSON_Parse(const char *value); + +/* ParseWithOpts allows you to require (and check) that the JSON is null + * terminated, and to retrieve the pointer to the final byte parsed. + */ +/* If you supply a ptr in return_parse_end and parsing fails, then + * return_parse_end will contain a pointer to the error so will match + * cJSON_GetErrorPtr(). */ +CJSON_PUBLIC(cJSON *) cJSON_ParseWithOpts(const char *value, + const char **return_parse_end, + cJSON_bool require_null_terminated); + +/* Render a cJSON entity to text for transfer/storage. */ +CJSON_PUBLIC(char *) cJSON_Print(const cJSON * item); + +/* Render a cJSON entity to text for transfer/storage without any formatting. */ +CJSON_PUBLIC(char *) cJSON_PrintUnformatted(const cJSON * item); + +/* Render a cJSON entity to text using a buffered strategy. prebuffer is a guess + * at the final size. guessing well reduces reallocation. fmt=0 gives + * unformatted, =1 gives formatted */ +CJSON_PUBLIC(char *) cJSON_PrintBuffered(const cJSON * item, + int prebuffer, + cJSON_bool fmt); + +/* Render a cJSON entity to text using a buffer already allocated in memory with + * given length. Returns 1 on success and 0 on failure. */ +/* NOTE: cJSON is not always 100% accurate in estimating how much memory it will + * use, so to be safe allocate 5 bytes more than you actually need */ +CJSON_PUBLIC(cJSON_bool) cJSON_PrintPreallocated(cJSON * item, + char *buffer, + const int length, + const cJSON_bool format); + +/* Delete a cJSON entity and all subentities. */ +CJSON_PUBLIC(void) cJSON_Delete(cJSON * item); + +/* Returns the number of items in an array (or object). */ +CJSON_PUBLIC(int) cJSON_GetArraySize(const cJSON * array); + +/* Retrieve item number "index" from array "array". Returns NULL if + * unsuccessful. */ +CJSON_PUBLIC(cJSON *) cJSON_GetArrayItem(const cJSON * array, int index); + +/* Get item "string" from object. Case insensitive. */ +CJSON_PUBLIC(cJSON *) cJSON_GetObjectItem(const cJSON * const object, + const char * const string); +CJSON_PUBLIC(cJSON *) cJSON_GetObjectItemCaseSensitive( + const cJSON * const object, + const char * const string); +CJSON_PUBLIC(cJSON_bool) cJSON_HasObjectItem(const cJSON * object, + const char *string); + +/* For analysing failed parses. This returns a pointer to the parse error. + * You'll probably need to look a few chars back to make sense of it. Defined + * when cJSON_Parse() returns 0. 0 when cJSON_Parse() succeeds. */ +CJSON_PUBLIC(const char *) cJSON_GetErrorPtr(void); + +/* Check if the item is a string and return its valuestring */ +CJSON_PUBLIC(char *) cJSON_GetStringValue(cJSON * item); + +/* These functions check the type of an item */ +CJSON_PUBLIC(cJSON_bool) cJSON_IsInvalid(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsFalse(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsTrue(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsBool(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsNull(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsNumber(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsString(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsArray(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsObject(const cJSON * const item); +CJSON_PUBLIC(cJSON_bool) cJSON_IsRaw(const cJSON * const item); + +/* These calls create a cJSON item of the appropriate type. */ +CJSON_PUBLIC(cJSON *) cJSON_CreateNull(void); +CJSON_PUBLIC(cJSON *) cJSON_CreateTrue(void); +CJSON_PUBLIC(cJSON *) cJSON_CreateFalse(void); +CJSON_PUBLIC(cJSON *) cJSON_CreateBool(cJSON_bool boolean); +CJSON_PUBLIC(cJSON *) cJSON_CreateNumber(double num); +CJSON_PUBLIC(cJSON *) cJSON_CreateString(const char *string); + +/* raw json */ +CJSON_PUBLIC(cJSON *) cJSON_CreateRaw(const char *raw); +CJSON_PUBLIC(cJSON *) cJSON_CreateArray(void); +CJSON_PUBLIC(cJSON *) cJSON_CreateObject(void); + +/* Create a string where valuestring references a string so + * it will not be freed by cJSON_Delete */ +CJSON_PUBLIC(cJSON *) cJSON_CreateStringReference(const char *string); + +/* Create an object/array that only references it's elements so + * they will not be freed by cJSON_Delete */ +CJSON_PUBLIC(cJSON *) cJSON_CreateObjectReference(const cJSON * child); +CJSON_PUBLIC(cJSON *) cJSON_CreateArrayReference(const cJSON * child); + +/* These utilities create an Array of count items. */ +CJSON_PUBLIC(cJSON *) cJSON_CreateIntArray(const int *numbers, int count); +CJSON_PUBLIC(cJSON *) cJSON_CreateFloatArray(const float *numbers, int count); +CJSON_PUBLIC(cJSON *) cJSON_CreateDoubleArray(const double *numbers, int count); +CJSON_PUBLIC(cJSON *) cJSON_CreateStringArray(const char **strings, int count); + +/* Append item to the specified array/object. */ +CJSON_PUBLIC(void) cJSON_AddItemToArray(cJSON * array, cJSON * item); +CJSON_PUBLIC(void) cJSON_AddItemToObject(cJSON * object, + const char *string, + cJSON * item); + +/* Use this when string is definitely const (i.e. a literal, or as good as), and + * will definitely survive the cJSON object. + * WARNING: When this function was used, make sure to always check that + * (item->type & cJSON_StringIsConst) is zero before + * writing to `item->string` */ +CJSON_PUBLIC(void) cJSON_AddItemToObjectCS(cJSON * object, + const char *string, + cJSON * item); + +/* Append reference to item to the specified array/object. Use this when you + * want to add an existing cJSON to a new cJSON, but don't want to corrupt + * your existing cJSON. */ +CJSON_PUBLIC(void) cJSON_AddItemReferenceToArray(cJSON * array, cJSON * item); +CJSON_PUBLIC(void) cJSON_AddItemReferenceToObject(cJSON * object, + const char *string, + cJSON * item); + +/* Remove/Detach items from Arrays/Objects. */ +CJSON_PUBLIC(cJSON *) cJSON_DetachItemViaPointer(cJSON * parent, + cJSON * const item); +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromArray(cJSON * array, int which); +CJSON_PUBLIC(void) cJSON_DeleteItemFromArray(cJSON * array, int which); +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObject(cJSON * object, + const char *string); +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObjectCaseSensitive(cJSON * object, + const char *string); +CJSON_PUBLIC(void) cJSON_DeleteItemFromObject(cJSON * object, + const char *string); +CJSON_PUBLIC(void) cJSON_DeleteItemFromObjectCaseSensitive(cJSON * object, + const char *string); + +/* Update array items. */ +CJSON_PUBLIC(void) cJSON_InsertItemInArray(cJSON * array, + int which, + cJSON * newitem); /* + * Shifts + * pre-existing + * items to + * the right. + */ +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemViaPointer(cJSON * const parent, + cJSON * const item, + cJSON * replacement); +CJSON_PUBLIC(void) cJSON_ReplaceItemInArray(cJSON * array, + int which, + cJSON * newitem); +CJSON_PUBLIC(void) cJSON_ReplaceItemInObject(cJSON * object, + const char *string, + cJSON * newitem); +CJSON_PUBLIC(void) cJSON_ReplaceItemInObjectCaseSensitive(cJSON * object, + const char *string, + cJSON * newitem); + +/* Duplicate a cJSON item */ +CJSON_PUBLIC(cJSON *) cJSON_Duplicate(const cJSON * item, cJSON_bool recurse); + +/* Duplicate will create a new, identical cJSON item to the one you pass, in new + * memory that will + * need to be released. With recurse!=0, it will duplicate any children + * connected to the item. + * The item->next and ->prev pointers are always zero on return from + * Duplicate. */ +/* Recursively compare two cJSON items for equality. If either a or b is NULL or + * invalid, they will be considered unequal. + * case_sensitive determines if object keys are treated case sensitive (1) or + * case insensitive (0) */ +CJSON_PUBLIC(cJSON_bool) cJSON_Compare(const cJSON * const a, + const cJSON * const b, + const cJSON_bool case_sensitive); + +/* Minify a strings, remove blank characters(such as ' ', '\t', '\r', '\n') from + * strings */ + +/* The input pointer json cannot point to a read-only address area, such as a + * string constant, + * but should point to a readable and writable adress area. */ +CJSON_PUBLIC(void) cJSON_Minify(char *json); + +/* Helper functions for creating and adding items to an object at the same time. + * They return the added item or NULL on failure. */ +CJSON_PUBLIC(cJSON *) cJSON_AddNullToObject(cJSON * const object, + const char * const name); +CJSON_PUBLIC(cJSON *) cJSON_AddTrueToObject(cJSON * const object, + const char * const name); +CJSON_PUBLIC(cJSON *) cJSON_AddFalseToObject(cJSON * const object, + const char * const name); +CJSON_PUBLIC(cJSON *) cJSON_AddBoolToObject(cJSON * const object, + const char * const name, + const cJSON_bool boolean); +CJSON_PUBLIC(cJSON *) cJSON_AddNumberToObject(cJSON * const object, + const char * const name, + const double number); +CJSON_PUBLIC(cJSON *) cJSON_AddStringToObject(cJSON * const object, + const char * const name, + const char * const string); +CJSON_PUBLIC(cJSON *) cJSON_AddRawToObject(cJSON * const object, + const char * const name, + const char * const raw); +CJSON_PUBLIC(cJSON *) cJSON_AddObjectToObject(cJSON * const object, + const char * const name); +CJSON_PUBLIC(cJSON *) cJSON_AddArrayToObject(cJSON * const object, + const char * const name); + +/* When assigning an integer value, it needs to be propagated to valuedouble + * too. */ +#define cJSON_SetIntValue(object, number) \ + ((object) ? (object)->valueint = (object)->valuedouble = (number) : (number)) + +/* helper for the cJSON_SetNumberValue macro */ +CJSON_PUBLIC(double) cJSON_SetNumberHelper(cJSON * object, double number); +#define cJSON_SetNumberValue(object, number) \ + ((object != NULL) ? cJSON_SetNumberHelper(object, (double)number) : (number)) + +/* Macro for iterating over an array or object */ +#define cJSON_ArrayForEach(element, array) \ + for (element = (array != NULL) ? (array)->child : NULL; \ + element != NULL; element = element->next) + +/* malloc/free objects using the malloc/free functions that have been set with + * cJSON_InitHooks */ +CJSON_PUBLIC(void *) cJSON_malloc(size_t size); +CJSON_PUBLIC(void) cJSON_free(void *object); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/srv/json/json_2pc.c b/bluetooth_uwb_dw3000_slotted_twr/src/srv/json/json_2pc.c new file mode 100644 index 0000000..23799c6 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/srv/json/json_2pc.c @@ -0,0 +1,450 @@ +/** + * @file json_2pc.c + * @brief collection of JSON formatted functions which used + * to report from Node application to the PC + * + * @author Decawave + * + * @attention Copyright 2017 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ +#include +#include +#include "cmd_fn.h" +#include "node.h" +#include "usb_uart_tx.h" + +/* + * @brief function to report to PC a new tag was discovered + * + * 'JSxxxx{"NewTag": + * //address64, string + * }' + * + * */ +void signal_to_pc_new_tag_discovered(uint64_t addr64) +{ + char *str = CMD_MALLOC(MAX_STR_SIZE); + + if (str) { + int hlen; + + hlen = sprintf(str, "JS%04X", 0x5A5A); // reserve space for length of + // JS object + + sprintf(&str[strlen(str)], "{\"NewTag\":\"%08lX%08lX\"}", + (uint32_t)(addr64 >> 32), (uint32_t)addr64); + + sprintf(&str[2], "%04X", strlen(str) - hlen);// add formatted 4X of length, + // this will kill first '{' + str[hlen] = '{'; // restore the start bracket + + sprintf(&str[strlen(str)], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + + CMD_FREE(str); + } +} + +/* + * @brief function to report to PC the Listener data received + * + * 'JSxxxx{"LSTN":[RxBytes_hex,..,],"TS":"0xTimeStamp32_Hex","O":Offset_dec}' + * + * This fn() uses pseudo-JSON format, it appends "+" to the end of + * + * */ +#define MAX_PRINT_FAST_LISTENER (6) +error_e send_to_pc_listener_info(uint8_t *data, + uint8_t size, + uint8_t *ts, + int16_t cfo) +{ + error_e ret = _ERR_Cannot_Alloc_Memory; + + uint32_t cnt, flag_plus = 0; + uint16_t hlen; + int cfo_pphm; + char *str; + + if (app.listener_mode == 0) {// speed is a priority + if (size > MAX_PRINT_FAST_LISTENER) { + flag_plus = 1; + size = MAX_PRINT_FAST_LISTENER; + } + + str = CMD_MALLOC(MAX_STR_SIZE); + } else { + str = CMD_MALLOC(MAX_STR_SIZE + MAX_STR_SIZE); + } + + if (str) { + cfo_pphm = (int)((float)cfo * (CLOCK_OFFSET_PPM_TO_RATIO * 1e6 * 100)); + + hlen = sprintf(str, "JS%04X", 0x5A5A); // reserve space for length of + // JS object + sprintf(&str[strlen(str)], "{\"LSTN\":["); + + // Loop over the received data + for (cnt = 0; cnt < size; cnt++) + { + sprintf(&str[strlen(str)], "%02X,", data[cnt]); // Add the byte and the + // delimiter - "XX," + } + + if (flag_plus) { + sprintf(&str[strlen(str)], "+,"); + } + + sprintf(&str[strlen(str) - 1], + "],\"TS\":\"0x%02X%02X%02X%02X\",", + ts[3], + ts[2], + ts[1], + ts[0]); + sprintf(&str[strlen(str)], "\"O\":%d", cfo_pphm); + + sprintf(&str[strlen(str)], "%s", "}\r\n"); + sprintf(&str[2], "%04X", strlen(str) - hlen); // add formatted 4X of length, + // this will erase first '{' + str[hlen] = '{'; // restore the start bracket + ret = port_tx_msg((uint8_t *)str, strlen(str)); + + CMD_FREE(str); + } + + return (ret); +} + +/* + * @brief This is a report of twr to pc + * + * There are two modes of operation: JSON(long output) or plain(short output) + * JSON (default): + * 'JSxxxx{"TWR": + * { "a16":%04X, //addr16 + * "R":%d,//range num + * "T":%d,//sys timestamp of Final WRTO Node's SuperFrame start, us + * "D":%f,//distance + * "P":%f,//raw pdoa + * "Xcm":%f,//X, cm + * "Ycm":%f,//Y, cm + * "O":%f,//clock offset in hundreds part of ppm + * "V":%d //service message data from the tag: (stationary, etc) + * "X":%d //service message data from the tag: (stationary, etc) + * "Y":%d //service message data from the tag: (stationary, etc) + * "Z":%d //service message data from the tag: (stationary, etc) + * } + * }' + * + * Plain: + * used if any from below is true: + * diag, acc, + * */ +void send_to_pc_twr(result_t *pRes) +{ + char *str = CMD_MALLOC(MAX_STR_SIZE); + int hlen; + + if (str) { + if ((app.pConfig->s.accEn == 1) \ + || (app.pConfig->s.diagEn == 1) \ + || (app.pConfig->s.reportLevel > 1)) { + if (app.pConfig->s.reportLevel == 3) { + /* shortest "AR" output: 18 chars per location: ~640 locations per + * second + * */ + sprintf(str, "AR%04X%04X%08lX%08lX", + (uint16_t)(pRes->addr16), + (uint16_t) (pRes->rangeNum), + (long int)(pRes->x_cm), + (long int)(pRes->y_cm)); + } else { + /* optimum "RA" output: 58 chars per location: ~200 locations per second + * */ + sprintf(str, "RA%04X %04X %08lX %08lX %08lX %1X X:%04X Y:%04X Z:%04X", + (uint16_t)(pRes->addr16), + (uint16_t)(pRes->rangeNum), + (long int)(pRes->x_cm), + (long int)(pRes->y_cm), + (long int)(pRes->clockOffset_pphm), + (uint8_t) (pRes->flag), + (uint16_t)(pRes->acc_x), + (uint16_t)(pRes->acc_y), + (uint16_t)(pRes->acc_z)); + } + sprintf(&str[strlen(str)], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + } else if (app.pConfig->s.reportLevel == 1) { + /* use JSON type of output during a normal operation + * + * This is not very efficient, as one TWR location is ~110 chars, + * as per format below + * JS xx{"TWR": {"a16":"2E5C","R":3,"T":8605,"D":343,"P":1695,"Xcm":165, + * "Ycm":165,"O":14,"V":1,"X":53015,"Y":60972,"Z":10797}} + * + * For pure UART, with limit of 115200b/s, + * the channel can handle ~100 locations per second, + * i.e. 10 tags ranging on maximum rate of 10 times a second. + * For higher throughput cut the JSON TWR object + * or use plain output instead. + * + */ + + /* Floating point values are standard for JSON objects, + * however the floating point printing + * is not used in current application. + * If the floating point printing required, + * will need to add "-u _printf_float" to the + * linker string, to include "floating printf" + * to the nano.spec of the stdlib. + * This will increase the size of application by ~6kBytes + * and the floating printing also requires + * much more stack space. + * Use this with caution, + * as this might result unpredictable stack overflow / HardFault. + */ + hlen = sprintf(str, "JS%04X", 0x5A5A); // reserve space for length + // of JS object + + sprintf(&str[strlen(str)], "{\"TWR\": "); + + sprintf(&str[strlen(str)], + "{\"a16\":\"%04X\"," + "\"R\":%d," // range number + "\"T\":%d,", // sys timestamp of Final WRTO Node's + // SuperFrame start, us + (int)(pRes->addr16), + (int)(pRes->rangeNum), + (int)(pRes->resTime_us)); + + if (app.pConfig->s.debugEn) { + sprintf(&str[strlen(str)], + "\"Tm\":%d,", // Master's temperature, in degree + // centigrade + (int)(pRes->tMaster_C)); + } + + sprintf(&str[strlen(str)], + "\"D\":%d," // distance as int, in cm + "\"P\":%d," // pdoa as int in milli-radians + "\"P'\":%d," // pdoa from Poll message as int in + // milli-radians + "\"Xcm\":%d," // X distance wrt Node in cm + "\"Ycm\":%d," // Y distance wrt Node in cm + "\"Pdiffnm\":%d,", + (int)(pRes->dist_cm), + (int)(pRes->pdoa_raw_deg), + (int)(pRes->pdoa_raw_degP), + (int)(pRes->x_cm), + (int)(pRes->y_cm), + (int)(pRes->path_diff)); + + sprintf(&str[strlen(str)], + "\"O\":%d," // clock offset as int + "\"V\":%d," // service message data from the tag: (bitmask: + // bit0 = stationary, bit15 = zeroed + // pdoaOffset used; bit14 = zeroed rngOffset + // used) + "\"X\":%d," // Normalized accel data X from the Tag, mg + "\"Y\":%d," // Normalized accel data Y from the Tag, mg + "\"Z\":%d" // Normalized accel data Z from the Tag, mg + "}", + (int)(pRes->clockOffset_pphm), + (int)(pRes->flag), + (int)(pRes->acc_x), + (int)(pRes->acc_y), + (int)(pRes->acc_z)); + + sprintf(&str[strlen(str)], "}"); + + sprintf(&str[2], "%04X", strlen(str) - hlen); // add formatted 4X of + // length, this will + // kill first '{' + str[hlen] = '{'; // restore the start + // bracket + + sprintf(&str[strlen(str)], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + + if (0) {/* TWR PDoA mini Diag: SDTP-50 */ + hlen = sprintf(str, "JS%04X", 0x5A5A); // reserve space for length + // of JS object + + sprintf(&str[strlen(str)], "{\"TWR_DIAG\": "); + + sprintf(&str[strlen(str)], + "{\"a16\":\"%04X\"," + "\"R\":%d," // range number + "\"T\":%d," // sys timestamp of Final WRTO Node's + // SuperFrame start, us + , + (int)(pRes->addr16), + (int)(pRes->rangeNum), + (int)(pRes->resTime_us)); + + sprintf(&str[strlen(str)], + "\"pDTUNE5\":\"0x%02X\"," + "\"pCIA_TDOA_0\":\"0x%08X\"," + "\"pCIA_TDOA_1_PDOA\":\"0x%08X\"," + "\"pIP_DIAG_10\":\"0x%04X\"," + "\"pCY0_DIAG_10\":\"0x%04X\"," + "\"pCY0_TOA_HI\":\"0x%04X\"," + "\"pCY1_TOA_HI\":\"0x%04X\"," + , + (unsigned int)pRes->finalPDOA.mDiag.DTUNE5, + (unsigned int)pRes->finalPDOA.mDiag.CIA_TDOA_0, + (unsigned int)pRes->finalPDOA.mDiag.CIA_TDOA_1_PDOA, + (unsigned int)pRes->finalPDOA.mDiag.IP_DIAG_10, + (unsigned int)pRes->finalPDOA.mDiag.CY0_DIAG_10, + (unsigned int)pRes->finalPDOA.mDiag.CY0_TOA_HI, + (unsigned int)pRes->finalPDOA.mDiag.CY1_TOA_HI); + + sprintf(&str[strlen(str)], + "\"fDTUNE5\":\"0x%02X\"," + "\"fCIA_TDOA_0\":\"0x%08X\"," + "\"fCIA_TDOA_1_PDOA\":\"0x%08X\"," + "\"fIP_DIAG_10\":\"0x%04X\"," + "\"fCY0_DIAG_10\":\"0x%04X\"," + "\"fCY0_TOA_HI\":\"0x%04X\"," + "\"fCY1_TOA_HI\":\"0x%04X\"" + "}", + (unsigned int)pRes->finalPDOA.mDiag.DTUNE5, + (unsigned int)pRes->finalPDOA.mDiag.CIA_TDOA_0, + (unsigned int)pRes->finalPDOA.mDiag.CIA_TDOA_1_PDOA, + (unsigned int)pRes->finalPDOA.mDiag.IP_DIAG_10, + (unsigned int)pRes->finalPDOA.mDiag.CY0_DIAG_10, + (unsigned int)pRes->finalPDOA.mDiag.CY0_TOA_HI, + (unsigned int)pRes->finalPDOA.mDiag.CY1_TOA_HI); + + sprintf(&str[strlen(str)], "}"); + + sprintf(&str[2], "%04X", strlen(str) - hlen); // add formatted 4X of + // length, this will + // kill first '{' + str[hlen] = '{'; // restore the start + // bracket + + sprintf(&str[strlen(str)], "\r\n"); + port_tx_msg((uint8_t *)str, strlen(str)); + } + } else { + // no output + } + + CMD_FREE(str); + } +} + +/* @brief input "str" must be a null-terminated string with enough space in it. + * this is slow output of accumulator from the chip, starting with + * ACC_OFFSET value. + * To be used solely for debug purposes. + * */ +static void send_acc(char *str, + uint16_t maxLen, + uint8_t *pAcc) +{ + int n; + int16_t cmplex_m[2]; + + cmplex_m[0] = 0; + cmplex_m[1] = 0; + + n = strlen(str); + + for (int i = 1; i <= (FULL_ACC_LEN * 4); i += 4) + { + if (n >= (maxLen - 4)) { + while (port_tx_msg((uint8_t *)str, n) != _NO_ERR) + { + osThreadYield(); // force switch content + osDelay(5); // wait 5ms for Flush thread freed the buffer + } + n = 0; + } + + if (i > (ACC_OFFSET * 4)) { + memcpy(&cmplex_m[0], &pAcc[i], 4); + } + + n += + sprintf(&str[n], "%04X%04X", (cmplex_m[0] & 0xFFFF), + (cmplex_m[1] & 0xFFFF)); + } + + n += sprintf(&str[n], "\r\n"); + + while (port_tx_msg((uint8_t *)str, n) != _NO_ERR) + { + osThreadYield(); // force switch content + osDelay(5); // wait 5ms for Flush thread freed the buffer + } +} + +/* @brief input "str" must be a null-terminated string with enough space in it. + * To be used solely for debug purposes. + * */ +static void send_diag(char *str, + uint16_t maxLen, + uint8_t *pDiag, + int16_t pdoa) +{ + if ((strlen(str) + 2 * sizeof(dwt_rxdiag_t) + 1 + 10 + 6) < maxLen) { + for (size_t i = 0; i < sizeof(dwt_rxdiag_t); i++) + { + sprintf(&str[strlen(str)], "%02X", pDiag[i]); + } + + sprintf(&str[strlen(str)], " "); + + sprintf(&str[strlen(str)], "%04X", pdoa); + + while (port_tx_msg((uint8_t *)str, strlen(str)) != _NO_ERR) + { + osThreadYield(); // force switch content + osDelay(5); // wait 5ms for Flush thread freed the buffer + } + } +} + +/* @brief send acc & diagnostics information + * these are blocking operations + * + * */ +void send_to_pc_diag_acc(rx_mail_t *pRxMailPckt) +{ + static int logNum = 0; + + char *str = CMD_MALLOC(MAX_STR_SIZE); + uint8_t *p; + + if (str) { + // send the Accumulator information from the pRxMailPckt + if (app.pConfig->s.accEn == 1) { + /* "master chip" */ + p = (uint8_t *)&pRxMailPckt->acc; + + sprintf(str, + "\r\nAM%04X %02X CLKOFF: %d\r\n", + logNum, + pRxMailPckt->res.rangeNum, + (int)(pRxMailPckt->res.clockOffset_pphm)); + + send_acc(str, MAX_STR_SIZE, p); + } + + // send the Diagnostics information from the pRxMailPckt + if (app.pConfig->s.diagEn == 1) { + sprintf(str, "DM%04X %02X ", logNum, pRxMailPckt->res.rangeNum); + send_diag(str, MAX_STR_SIZE, + (uint8_t *)&pRxMailPckt->diag_dw3000, + pRxMailPckt->res.pdoa_raw_deg); // PDOA from the Final + } + + CMD_FREE(str); + } + + logNum++; +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/srv/json/json_interface.h b/bluetooth_uwb_dw3000_slotted_twr/src/srv/json/json_interface.h new file mode 100644 index 0000000..e120086 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/srv/json/json_interface.h @@ -0,0 +1,126 @@ +/** + * @file json_interface.h + * @brief specification for JSON interface in between Juniper and GUI + * + * + * @author Decawave and Applications team + * + * @attention Copyright 2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +/***************************************************************************//* + * General structure + * The general structure is JSON, incapsulated in the TLV format + * The type is 'JS', then 4bytes with the length of the JSON message + * + * The 4 bytes are in Hex? + * If not, if we need to read more than 9999 than we need 5 bytes(characters) + * + * 'JSxxxx{json_object}' + * + * json_object is standard + * + * The input object from GUI to ARM can be either command or command with value. + * the list of all JSON input commands object names can be found in the + * const command_t known_commands [] of cmd_fn.c file. + * + * 1. Commands to set system config, run-time and calibration variables + * Format: JSxxxx{"PARM": %d}, where %d is the integer value for a given PARM + * If need to specify "true" condition for PARM, then it is 1 and for "false" + * it is 0 + * + * 2. Anytime commands + * Format: JSxxxx{"PARM"} + * + * 3. Application start commands + * Format: JSxxxx{"START": "%s"} where %s is the NAME of Application, see + * known_commands [] section 3. + * + * 4. node application commands + * + * 5. service commands + * + * 6. TBD + */ + +/***************************************************************************//* + * Interface from GUI to Juniper + * + * EXAMPLES + * + *****************************************************************************/ + +/* Stop any running application + * + * 'JSxxxx{"STOP"}' + */ + +/* Get status of the connected application + * + * 'JSxxxx{"STAT"}' + */ + +/* Configure the UWB parameters + * + * 'JSxxxx{"UWB PARAM": {"CHAN": %d, "PRF": %d, "PLEN": %d, "PAC": %d, "TXCODE": + * %d, "DATARATE": %d, TBD } }' //TODO: add the rest for DW3000 + */ + +/* Get the UWB parameters (the output is the same as object "UWB PARAM") + * + * 'JSxxxx{"UWBCFG"}' + * + */ + +/* SAVE the UWB parameters to the FLASH + * + * 'JSxxxx{"SAVE"}' + */ + +/* Start the PDoA TWR application in the Node mode + * + * 'JSxxxx{"Start": "PDoA_Node" }' + */ + +/* Start the PDoA application in the Tag mode + * + * 'JSxxxx{"Start": "PDoA_Tag" }' + */ + +/***************************************************************************//* + * Interface from Juniper to GUI + * + * EXAMPLES + * + *****************************************************************************/ + +/* A new Tag was discovered + * + * 'JSxxxx{"NewTag": + * //address64, string + * }' + */ + +/* Report from PDoA Node Application + * + * 'JSxxxx{"TWR": + * { "a16":%04X, //addr16 + * "R":%d,//range num + * "T":%d,//sys timestamp of Final WRTO Node's SuperFrame start, us + * "D":%d,//distance + * "P":%d,//raw pdoa + * "Xcm":%d,//X, cm + * "Ycm":%d,//Y, cm + * "O":%d,//clock offset in hundreds part of ppm + * "V":%d //service message data from the tag: (stationary, etc) + * "X":%d //service message data from the tag: (stationary, etc) + * "Y":%d //service message data from the tag: (stationary, etc) + * "Z":%d //service message data from the tag: (stationary, etc) + * } + * }' + */ + +// TODO: some of the output has a non-JSON format, say Diag output or ACCUM, +// will be TBD diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/srv/msg_time/msg_time.c b/bluetooth_uwb_dw3000_slotted_twr/src/srv/msg_time/msg_time.c new file mode 100644 index 0000000..1db5825 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/srv/msg_time/msg_time.c @@ -0,0 +1,133 @@ +/* + * @file msg_time.c + * @brief used to calculate frames duration + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#include "msg_time.h" +#include +#include "util.h" +#include "port_common.h" + +/* @brief Calculates the length of message in us; sy; dt + * + * This is a function of data rate, preamble length, PRF and message data length + * */ +void calculate_msg_time(msg_t *msg, msg_time_t *msg_time) +{ + float datalen_ns = 0, phrlen_ns = 0, preamblelen = 0, stslen = 0; + int sfdlen, tmp; + + tmp = msg->msg_len; + datalen_ns = tmp * 8.0 + 48.0 * ceilf(tmp * 8.0 / 330.0);// number of bits + + // 48bits for each + // Reed-Solomon + // block (330bits + // or less) + + // pre-calculated : PHR length is 172308ns for 110k and 21539ns for 850k/6.81M + // Refer to "Frame Format" of DW1000-Datasheet.pdf + switch (msg->dataRate) + { + case (DWT_BR_850K): + { + datalen_ns *= 1025.64f; // length of data message in ns for + // data + phrlen_ns = (21 * 1025.64); // PHR header 850K 21bit*1025.64= + // 21539ns + break; + } + case (DWT_BR_6M8): + { + datalen_ns *= 128.21f; // length of data message in ns for + // data + phrlen_ns = (21 * 1025.64); // PHR header 6.8M 21bit*1025.64= + // 21539ns + break; + } + default: + { + assert(0); + break; + } + } + + /* sfd and preamble are in symbols */ + sfdlen = (msg->sfdType == DWT_SFD_DW_16) ? DWT_SFD_LEN16 : DWT_SFD_LEN8; + + // number of Symbols in preamble sequence + switch (msg->txPreambLength) + { + case DWT_PLEN_4096: preamblelen = 4096.0f; break; + case DWT_PLEN_2048: preamblelen = 2048.0f; break; + case DWT_PLEN_1536: preamblelen = 1536.0f; break; + case DWT_PLEN_1024: preamblelen = 1024.0f; break; + case DWT_PLEN_512: preamblelen = 512.0f; break; + case DWT_PLEN_256: preamblelen = 256.0f; break; + case DWT_PLEN_128: preamblelen = 128.0f; break; + case DWT_PLEN_64: preamblelen = 64.0f; break; + default: assert(0); break; + } + + if (msg->stsLength > 0) { + stslen = ((float)msg->stsLength) * 512.8e-3 * 2; // add STS length + } + + // convert Synchronisation Header (SHR)=PLEN+SFD to us + if (msg->txPcode < 9) { + preamblelen = (sfdlen + preamblelen) * 0.99359f; // us + } else if (msg->txPcode < 25) { + preamblelen = (sfdlen + preamblelen) * 1.01763f; // us + } else { + assert(0); + } + + msg_time->preamble_us = (uint32_t)(preamblelen); // length of + // Preamble + + // SFD + msg_time->sts_us = (uint32_t)(stslen); // length of STS + msg_time->phr_us = (uint32_t)(phrlen_ns / 1000); // length of PHR + msg_time->data_us = (uint32_t)(datalen_ns / 1000); // length of + // Data + msg_time->phrAndData_us = (uint32_t)((datalen_ns + phrlen_ns) / 1000); + + // length of the whole frame + msg_time->us = (uint32_t)(preamblelen + stslen + msg_time->phrAndData_us); + + /* + * in the application Symbol time and Device time is used + * + * 1. Conversion of values to SYMBOL TIME: 1sy = 1.0256us + * for wait4response timeouts you will need time in "Symbol" time: + * tmp = (int)((preamblelen + msgdatalen) / 1.0256); //message length in + * "Symbol" time + * + * 2. Conversion of values to DEVICE TIME: 1dt = + * (uint64_t)((double)(1us/(double)DWT_TIME_UNITS)/1e6) + * + * */ + + msg_time->sy = (uint32_t)util_us_to_sy(msg_time->us); // length of the + // whole frame + // in Symbol + // time units + msg_time->dt64 = util_us_to_dev_time(msg_time->us); // length of the + // whole frame + // in device + // time, + // uint64_t + + msg_time->dt[0] = (uint8_t)(msg_time->dt64 & 0xFF); // length of the + // whole frame + // in device + // TimeStamp + msg_time->dt[1] = (uint8_t)(msg_time->dt64 >> 8 & 0xFF); + msg_time->dt[2] = (uint8_t)(msg_time->dt64 >> 16 & 0xFF); + msg_time->dt[3] = (uint8_t)(msg_time->dt64 >> 24 & 0xFF); + msg_time->dt[4] = (uint8_t)(msg_time->dt64 >> 32 & 0xFF); +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/srv/msg_time/msg_time.h b/bluetooth_uwb_dw3000_slotted_twr/src/srv/msg_time/msg_time.h new file mode 100644 index 0000000..cdc05f4 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/srv/msg_time/msg_time.h @@ -0,0 +1,57 @@ +/** + * @file msg_time.h + * @brief used to calculate frames duration + * + * + * @author Decawave + * + * @attention Copyright 2017 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef __MSG_TIME__H__ +#define __MSG_TIME__H__ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#include "stdint.h" + +struct msg_s +{ + int dataRate; // Deca define: e.g. DWT_BR_850K + int txPreambLength; // Deca define: e.g. DWT_PLEN_4096 + int stsLength; // STS length in symbols + int sfdType; // 0-3 + int txPcode; // Preamble code + int msg_len; +}; + +typedef struct msg_s msg_t; + +struct msg_time_s +{ + uint16_t preamble_us; + uint16_t sts_us; + uint16_t phr_us; + uint16_t data_us; + uint16_t phrAndData_us; + + uint16_t us; + uint16_t sy; + uint64_t dt64; + uint8_t dt[5]; +}; + +typedef struct msg_time_s msg_time_t; + +/* exported functions prototypes */ +void calculate_msg_time(msg_t *msg, msg_time_t *msg_time); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/srv/tag_list/tag_list.c b/bluetooth_uwb_dw3000_slotted_twr/src/srv/tag_list/tag_list.c new file mode 100644 index 0000000..a281915 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/srv/tag_list/tag_list.c @@ -0,0 +1,288 @@ +/** + * @file tag_list.c + * @brief functions to manage + * + * @author Decawave + * + * @attention Copyright 2017 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ +#include +#include "app.h" +#include "port_common.h" +#include "tag_list.h" +// ---------------------------------------------------------------------------- +// There 2 Tag's lists in the Node: +// Discovered Tags List (DList) and Known Tags List (KList) +// +// DList[MAX_DISCOVERED_TAG_LIST_SIZE] is temporary list for harvesting of tags +// from the air. +// KList[MAX_KNOWN_TAG_LIST_SIZE] is permanent list and a part of NVM. +// +static uint64_t DList[MAX_DISCOVERED_TAG_LIST_SIZE]; + +/* array implementation of knownTagList: array is faster than linked list and it + * easier for "del" + * + * init_knownTagList + * get_knownTagList + * get_knownTagList_Size + * get_tag64_from_knownTagList + * get_tag16_from_knownTagList + * get_free_slot_from_knownTagList + * add_tag_to_knownTagList + * del_tag16_from_knownTagList + * del_tag64_from_knownTagList + * + * For PC<->NODE communication protocol see cmd_fn.c + * + * PC->NODE + * "getKList" : Node returns the knownTagList (long string)." + * " + * "getDList" : Node returns the knownTagList (long string)." + * " + * "addtag " + * + * NODE->PC + * JSON format strings: + * "JSxxxx{"New Tag": "} + * "JSxxxx{"TWR": ....}" + * + * */ + +/* @brief + * @return the pointer to the first element of knownTagList + * */ +tag_addr_slot_t *get_knownTagList(void) +{ + return (app.pConfig->knownTagList); +} + +/* @brief + * knownTagList can have gaps in the middle + * @return the numeber of elements in the knownTagList + * */ +uint16_t get_knownTagList_size(void) +{ + uint16_t size = 0; + tag_addr_slot_t *klist = get_knownTagList(); + + // KList can be with gaps, so need to scan it whole + for (int i = 0; i < MAX_KNOWN_TAG_LIST_SIZE; i++) + { + if (klist->slot != (uint16_t)(0)) { + size++; + } + + klist++; + } + + return (size); +} + +/* + * */ +void init_knownTagList(void) +{ + memset(app.pConfig->knownTagList, 0, sizeof(app.pConfig->knownTagList)); +} + +/* brief + * returns the address of the known tag if it is in known_list; + * otherwise returns NULL. + * + * */ +tag_addr_slot_t * +get_tag64_from_knownTagList(uint64_t addr64) +{ + int i; + int size = sizeof(app.pConfig->knownTagList) + / sizeof(app.pConfig->knownTagList[0]); + + tag_addr_slot_t *klist = get_knownTagList(); + + for (i = 0; i < size; i++) + { + if ((klist[i].addr64 == addr64) && (klist[i].slot != 0)) { + return &klist[i]; + } + } + return NULL; +} + +/* + * */ +tag_addr_slot_t * +get_tag16_from_knownTagList(uint16_t addr16) +{ + int i; + int size = sizeof(app.pConfig->knownTagList) + / sizeof(app.pConfig->knownTagList[0]); + + tag_addr_slot_t *klist = get_knownTagList(); + + for (i = 0; i < size; i++) + { + if ((klist[i].addr16 == addr16) && (klist[i].slot != 0)) { + return &klist[i]; + } + } + return NULL; +} + +/* + * return [1..(size+1)] for free slot + * + * return 0 if there no free slots found + * + * */ +uint16_t get_free_slot_from_knownTagList(void) +{ + int16_t i; + int size = sizeof(app.pConfig->knownTagList) + / sizeof(app.pConfig->knownTagList[0]); + + tag_addr_slot_t *klist = get_knownTagList(); + + for (i = 0; i < size; i++) + { + if (klist[i].slot == (uint16_t)(0)) { + return (i + 1); + } + } + return (0); +} + +/* + * @brief Checks and adds the tag to the list, if it's not in the list yet. + * + * returns the pointer to the tag in the list; + * returns NULL if the list is full; + * + * */ +tag_addr_slot_t *add_tag_to_knownTagList(uint64_t addr64, + uint16_t addr16, + uint16_t fast, + uint16_t slow, + uint16_t mode) +{ + int16_t slot; + tag_addr_slot_t *tag, *klist; + + int size = sizeof(app.pConfig->knownTagList) + / sizeof(app.pConfig->knownTagList[0]); + + tag = get_tag64_from_knownTagList(addr64); + + if (!tag) { + klist = get_knownTagList(); + + slot = get_free_slot_from_knownTagList(); + + if ((slot > 0) && (slot <= size)) { + /* Duplicate 16-bit addresses are not allowed + * use any next available address which is not + * in known addresses space instead + * */ + while (get_tag16_from_knownTagList(addr16)) + { + addr16++; + } + + /* add tag to KList */ + tag = &klist[slot - 1]; // slot=[1..MAX_KNOWN_TAG_LIST_SIZE] + tag->slot = slot; // klist[0].slot = 1; klist[1].slot = 2, + // etc. + tag->addr16 = addr16; + tag->addr64 = addr64; + + /* use default parameters for all tags : used in automatic adding of all + * tags : "d2k" command */ + tag->multFast = fast; + tag->multSlow = slow; + tag->mode = mode; + } + } + + return (tag); +} + +/* + * */ +void del_tag16_from_knownTagList(uint16_t addr16) +{ + tag_addr_slot_t *p; + + p = get_tag16_from_knownTagList(addr16); + + if (p) { + memset(p, 0, sizeof(tag_addr_slot_t)); + } +} + +/* + * */ +void del_tag64_from_knownTagList(uint64_t addr64) +{ + tag_addr_slot_t *p; + + p = get_tag64_from_knownTagList(addr64); + + if (p) { + memset(p, 0, sizeof(tag_addr_slot_t)); + } +} + +// ---------------------------------------------------------------------------- + +// ---------------------------------------------------------------------------- +// for harvesting Tags offline : we have limited space : use DList +// +uint16_t getDList_size(void) +{ + uint16_t len = 0; + for (size_t i = 0; i < sizeof(DList) / sizeof(DList[0]); i++) + { + if (!DList[i]) { + break; + } + len++; + } + return (len); +} + +uint64_t * getDList(void) +{ + return (DList); +} + +/* @brief + * return 1 : tag successfully added to discovered tag list / no space left in + * discovered tag list + * return 0 : already in the list + * */ +int addTagToDList(uint64_t addr64) +{ + int ret = 1; + + for (size_t i = 0; i < sizeof(DList) / sizeof(DList[0]); i++) + { + if (DList[i] == addr64) { + ret = 0; + break; + } + + if (!DList[i]) { + DList[i] = addr64; + ret = 1; + break; + } + } + return(ret); +} + +void initDList(void) +{ + memset(DList, 0, sizeof(DList)); +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/srv/tag_list/tag_list.h b/bluetooth_uwb_dw3000_slotted_twr/src/srv/tag_list/tag_list.h new file mode 100644 index 0000000..73d3c82 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/srv/tag_list/tag_list.h @@ -0,0 +1,80 @@ +/** + * @file tag_list.h + * + * @brief + * + * @author Decawave + * + * @attention Copyright 2017 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef __INC_TAG_LIST_H__ +#define __INC_TAG_LIST_H__ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +// #include + +// maximum is limited by NUM_SLOTS; FCONFIG_SIZE and available memory size, see +// default_config.h +#define MAX_KNOWN_TAG_LIST_SIZE (20) +#define MAX_DISCOVERED_TAG_LIST_SIZE (20) + +/* Tag list */ + +typedef struct +{ + uint16_t slot; + union { + uint8_t addrShort[2]; + uint16_t addr16; + }; + + union { + uint8_t addrLong[8]; + uint64_t addr64; + }; + + uint16_t multFast; + uint16_t multSlow; + uint16_t mode; // IMU = bit 0 + + union { + uint8_t req; + uint8_t reqUpdatePending : 1; // request to update Tag's + // configuration during range + // phase + }; +}__attribute__((packed)) +tag_addr_slot_t; + +tag_addr_slot_t *get_tag16_from_knownTagList(uint16_t addr16); +tag_addr_slot_t *get_tag64_from_knownTagList(uint64_t addr64); +tag_addr_slot_t *add_tag_to_knownTagList(uint64_t addr64, + uint16_t addr16, + uint16_t fast, + uint16_t slow, + uint16_t mode); +void del_tag64_from_knownTagList(uint64_t addr64); +void del_tag16_from_knownTagList(uint16_t addr16); + +int addTagToDList(uint64_t addr64); + +uint16_t getDList_size(void); +uint64_t *getDList(void); + +void init_knownTagList(void); +tag_addr_slot_t *get_knownTagList(void); +uint16_t get_knownTagList_size(void); + +void initDList(void); + +#ifdef __cplusplus +} +#endif + +#endif /* __INC_TAG_LIST_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/srv/translate/translate.c b/bluetooth_uwb_dw3000_slotted_twr/src/srv/translate/translate.c new file mode 100644 index 0000000..92a35a2 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/srv/translate/translate.c @@ -0,0 +1,191 @@ +/* @file deca_translate.c + * @brief translate DW1000 parameters from Deca to Human and from Human to + * Deca format + * + * return translated value or (-1) if out of allowed range + * + * @author Decawave + * @attention Copyright 2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + */ + +#include "deca_device_api.h" + +/* Channel */ +int chan_to_deca(int i) +{ + int ret = -1; + + if ((i == 5) || (i == 9)) { + ret = i; + } + + return (ret); +} + +int deca_to_chan(int i) +{ + return(chan_to_deca(i)); +} + +/* Bitrate */ +int bitrate_to_deca(int i) +{ + switch (i) + { + case 850: + return DWT_BR_850K; + case 6810: + return DWT_BR_6M8; + default: + return -1; + } +} + +int deca_to_bitrate(int i) +{ + switch (i) + { + case DWT_BR_850K: + return 850; + case DWT_BR_6M8: + return 6810; + default: + return -1; + } +} + +/* PAC */ +int pac_to_deca(int i) +{ + switch (i) + { + case 8: + return DWT_PAC8; + case 16: + return DWT_PAC16; + case 32: + return DWT_PAC32; + case 4: + return DWT_PAC4; + default: + return -1; + } +} + +int deca_to_pac(int i) +{ + switch (i) + { + case DWT_PAC8: + return 8; + case DWT_PAC16: + return 16; + case DWT_PAC32: + return 32; + case DWT_PAC4: + return 4; + default: + return -1; + } +} + +/* PLEN */ +int plen_to_deca(int i) +{ + switch (i) + { +// case 4096 : +// return DWT_PLEN_4096; + case 2048: + return DWT_PLEN_2048; + case 1536: + return DWT_PLEN_1536; + case 1024: + return DWT_PLEN_1024; + case 512: + return DWT_PLEN_512; + case 256: + return DWT_PLEN_256; + case 128: + return DWT_PLEN_128; + case 64: + return DWT_PLEN_64; + default: + return -1; + } +} + +int deca_to_plen(int i) +{ + switch (i) + { +// case DWT_PLEN_4096 : +// return 4096; + case DWT_PLEN_2048: + return 2048; + case DWT_PLEN_1536: + return 1536; + case DWT_PLEN_1024: + return 1024; + case DWT_PLEN_512: + return 512; + case DWT_PLEN_256: + return 256; + case DWT_PLEN_128: + return 128; + case DWT_PLEN_64: + return 64; + default: + return -1; + } +} + +/*STS Length*/ +int sts_length_to_deca(int i) +{ + switch (i) + { + case 2048: + return DWT_STS_LEN_2048; + case 1024: + return DWT_STS_LEN_1024; + case 512: + return DWT_STS_LEN_512; + case 256: + return DWT_STS_LEN_256; + case 128: + return DWT_STS_LEN_128; + case 64: + return DWT_STS_LEN_64; + case 32: + return DWT_STS_LEN_32; + default: + return -1; + } +} + +int deca_to_sts_length(int i) +{ + switch (i) + { + case DWT_STS_LEN_2048: + return 2048; + case DWT_STS_LEN_1024: + return 1024; + case DWT_STS_LEN_512: + return 512; + case DWT_STS_LEN_256: + return 256; + case DWT_STS_LEN_128: + return 128; + case DWT_STS_LEN_64: + return 64; + case DWT_STS_LEN_32: + return 32; + default: + return -1; + } +} + +/* END of translate */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/srv/translate/translate.h b/bluetooth_uwb_dw3000_slotted_twr/src/srv/translate/translate.h new file mode 100644 index 0000000..ffdded4 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/srv/translate/translate.h @@ -0,0 +1,48 @@ +/** + * @file translate.h + * + * @brief Header fiel for Decawave convertion + * + * @author Decawave + * + * @attention Copyright 2017-2019 (c) Decawave Ltd, Dublin, Ireland. + * All rights reserved. + * + */ + +#ifndef SRC_INC_TRANSLATE_H_ +#define SRC_INC_TRANSLATE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Channel */ +int chan_to_deca(int i); +int deca_to_chan(int i); + +/* Bitrate */ +int bitrate_to_deca(int i); +int deca_to_bitrate(int i); + +/* PRF */ +int prf_to_deca(int i); +int deca_to_prf(int i); + +/* PAC */ +int pac_to_deca(int i); +int deca_to_pac(int i); + +/* PLEN */ +int plen_to_deca(int i); +int deca_to_plen(int i); + +/*STS Length*/ +int sts_length_to_deca(int i); +int deca_to_sts_length(int i); + +#ifdef __cplusplus +} +#endif + +#endif /* SRC_INC_TRANSLATE_H_ */ diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/srv/util/util.c b/bluetooth_uwb_dw3000_slotted_twr/src/srv/util/util.c new file mode 100644 index 0000000..9cbb8d8 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/srv/util/util.c @@ -0,0 +1,135 @@ +/**--------------------------------------- + * @file util.c + * @brief utility functions: + * Seconds to/from DW1000 internal time conversions. + * 1sy = 1us / 1.0256 + * 1dt = 1s / 499.2e6 / 128.0 + * sfd timeout calculation + * + * @author Decawave + * + * Copyright 2017 (c) Decawave Ltd, Dublin, Ireland. + * + * All rights reserved. + * + * @author + */ + +#include "util.h" +#include "math.h" + +uint64_t util_us_to_dev_time(double microsecu) +{ + uint64_t dt; + long double dtime; + + dtime = (microsecu / (double) DWT_TIME_UNITS) / 1e6; + + dt = (uint64_t) (dtime); + + return (dt); +} + +double util_dev_time_to_sec(uint64_t dt) +{ + double f = 0; + + f = dt * DWT_TIME_UNITS; // seconds #define TIME_UNITS + // (1.0/499.2e6/128.0) = 15.65e-12 + + return (f); +} + +uint64_t util_sec_to_dev_time(double secu) +{ + uint64_t dt; + double dtime; + + dtime = (secu / (double) DWT_TIME_UNITS); + + dt = 0x0FFFFFFFFFULL & (uint64_t) (dtime); + + return (dt); +} + +double util_us_to_sy(double us) +{ + return (double)(us / 1.0256); +} + +/* @fn calc_sfd_to() + * @param caclulates SFDTimeout based on given dwt_config_t: + * sfdType, txPreambLength , rxPAC + * sfdto = {txPreambLength} + 1 + {SFDType} - {rxPAC} + * + * Calculation based on deca_device_api.h driver definition + * + * @return sfdto value + * + * */ +int16_t calc_sfd_to(void *p) +{ + dwt_config_t *pCfg = p; + + int16_t ret = 1; + + /* + length of SFD based on the SFD type */ + switch (pCfg->sfdType) + { + case 0x0: // IEEE standard 8 + case 0x1: // DW non-standard 8 + ret += 0x08; + break; + + case 0x2: // DW non-standard 16 + ret += 0x10; + break; + + default: // IEEE 4z 8 + ret += 0x08; + break; + } + + /* + {txPreambLength} */ + switch (pCfg->txPreambLength) + { + case DWT_PLEN_64: + case DWT_PLEN_128: + case DWT_PLEN_256: + case DWT_PLEN_512: + ret += (0x40 << (pCfg->txPreambLength >> 4)); + break; + + case DWT_PLEN_1024: + case DWT_PLEN_1536: + case DWT_PLEN_2048: + ret += (0x200 + (0x200 << (pCfg->txPreambLength >> 4))); + break; + + default: // Preamble length 4096 and any + ret += 0x1000; + break; + } + + /* - {rxPAC} */ + switch (pCfg->rxPAC) + { + case DWT_PAC4: + ret -= 4; + break; + + case DWT_PAC16: + ret -= 16; + break; + + case DWT_PAC32: + ret -= 32; + break; + + default: // PAC8 and any + ret -= 8; + break; + } + + return (ret); +} diff --git a/bluetooth_uwb_dw3000_slotted_twr/src/srv/util/util.h b/bluetooth_uwb_dw3000_slotted_twr/src/srv/util/util.h new file mode 100644 index 0000000..26e2184 --- /dev/null +++ b/bluetooth_uwb_dw3000_slotted_twr/src/srv/util/util.h @@ -0,0 +1,75 @@ +/** + * @file util.h + * + * @brief Decawave Application Layer utility functions & Macros + * + * @attention + * + * Copyright 2017 (c) Decawave Ltd, Dublin, Ireland. + * + * All rights reserved. + * + * @author + */ + +#ifndef __UTIL__H__ +#define __UTIL__H__ 1 + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "deca_device_api.h" + +#define AR2U32(x) (((uint32_t)x[3]) << 24 | \ + ((uint32_t)x[2]) << 16 | \ + ((uint32_t)x[1]) << 8 | \ + ((uint32_t)x[0])) + +#define AR2U16(x) ((x[1] << 8) | x[0]) + +#define TS2U64_MEMCPY(x, y) do{ \ + x = (uint64_t)(((uint64_t)y[4] << 32) | \ + ((uint64_t)y[3] << 24) | \ + ((uint64_t)y[2] << 16) | \ + ((uint64_t)y[1] << 8) | \ + y[0]); \ +}while (0) + +#define TS2TS_MEMCPY(x, y) do { \ + for (int i = 0; i < TS_40B_SIZE; i++) { x[i] = y[i]; } \ +}while (0) + +#define TS2TS_UWB_MEMCPY(x, y) do { \ + for (int i = 0; i < TS_UWB_SIZE; i++) { x[i] = y[i]; } \ +}while (0) + +#define U642TS_MEMCPY(x, y) do { \ + for (int i = 0; i < TS_40B_SIZE; \ + i++) { x[i] = (uint8_t)((y >> (i * 8) & 0xFF)); } \ +}while (0) + +#define U642TS_UWB_MEMCPY(x, y) do { \ + for (int i = 0; i < TS_UWB_SIZE; \ + i++) { x[i] = (uint8_t)((y >> (i * 8) & 0xFF)); } \ +}while (0) + +#define U32TOAR_MEMCPY(x, y) do { \ + for (int i = 0; i < 4; i++) { x[i] = (uint8_t)((y >> (i * 8) & 0xFF)); \ + } \ +}while (0) + +uint64_t util_us_to_dev_time (double us); +double util_dev_time_to_sec(uint64_t dt); +uint64_t util_sec_to_dev_time (double sec); +double util_us_to_sy(double us); + +int16_t calc_sfd_to(void *pCfg); +uint16_t calc_rx_to_sy(void *p, uint16_t len); + +#ifdef __cplusplus +} +#endif + +#endif /* __UTIL__H__ */ diff --git a/templates.xml b/templates.xml index c230c58..1d4b1cf 100644 --- a/templates.xml +++ b/templates.xml @@ -1109,4 +1109,21 @@ + + + + + + + + + + + + + + + +