From 9332356bb362d80e043d9a61a502a436b0eb7e0e Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Thu, 18 Aug 2022 15:10:43 +0200 Subject: [PATCH 01/19] DTR protocol implemented in firmware --- .vscode/settings.json | 10 +- Makefile | 2 +- examples/app_p2p_DTR/.gitignore | 2 + examples/app_p2p_DTR/Kbuild | 1 + examples/app_p2p_DTR/Makefile | 23 + examples/app_p2p_DTR/README.md | 23 + examples/app_p2p_DTR/app-config | 3 + examples/app_p2p_DTR/cload-all.sh | 27 ++ examples/app_p2p_DTR/src/Kbuild | 1 + examples/app_p2p_DTR/src/p2p_DTR_app.c | 109 +++++ src/utils/interface/debug.h | 1 + src/utils/interface/p2pDTR/DTR_timers.h | 65 +++ src/utils/interface/p2pDTR/DTR_types.h | 114 +++++ src/utils/interface/p2pDTR/p2p_interface.h | 47 ++ src/utils/interface/p2pDTR/queueing.h | 80 ++++ src/utils/interface/p2pDTR/token_ring.h | 100 ++++ src/utils/src/Kbuild | 6 + src/utils/src/p2pDTR/DTR_timers.c | 85 ++++ src/utils/src/p2pDTR/p2p_interface.c | 77 ++++ src/utils/src/p2pDTR/queueing.c | 142 ++++++ src/utils/src/p2pDTR/token_ring.c | 509 +++++++++++++++++++++ 21 files changed, 1424 insertions(+), 3 deletions(-) create mode 100644 examples/app_p2p_DTR/.gitignore create mode 100644 examples/app_p2p_DTR/Kbuild create mode 100644 examples/app_p2p_DTR/Makefile create mode 100644 examples/app_p2p_DTR/README.md create mode 100644 examples/app_p2p_DTR/app-config create mode 100755 examples/app_p2p_DTR/cload-all.sh create mode 100644 examples/app_p2p_DTR/src/Kbuild create mode 100644 examples/app_p2p_DTR/src/p2p_DTR_app.c create mode 100644 src/utils/interface/p2pDTR/DTR_timers.h create mode 100644 src/utils/interface/p2pDTR/DTR_types.h create mode 100644 src/utils/interface/p2pDTR/p2p_interface.h create mode 100644 src/utils/interface/p2pDTR/queueing.h create mode 100644 src/utils/interface/p2pDTR/token_ring.h create mode 100644 src/utils/src/p2pDTR/DTR_timers.c create mode 100644 src/utils/src/p2pDTR/p2p_interface.c create mode 100644 src/utils/src/p2pDTR/queueing.c create mode 100644 src/utils/src/p2pDTR/token_ring.c diff --git a/.vscode/settings.json b/.vscode/settings.json index 605386e7e4..fb62bf8110 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -5,6 +5,12 @@ "**/bower_components": true }, "files.associations": { - "bootloader.h": "c" - } + "bootloader.h": "c", + "token_ring.h": "c", + "p2p_interface.h": "c" + }, + "cSpell.words": [ + "Bitcraze", + "Zosimidis" + ] } diff --git a/Makefile b/Makefile index 81211f4e3f..1d84659fe6 100644 --- a/Makefile +++ b/Makefile @@ -52,7 +52,7 @@ INCLUDES += -I$(srctree)/src/drivers/interface -I$(srctree)/src/drivers/bosch/in INCLUDES += -I$(srctree)/src/drivers/esp32/interface INCLUDES += -I$(srctree)/src/hal/interface INCLUDES += -I$(srctree)/src/modules/interface -I$(srctree)/src/modules/interface/kalman_core -I$(srctree)/src/modules/interface/lighthouse -I$(srctree)/src/modules/interface/cpx -INCLUDES += -I$(srctree)/src/utils/interface -I$(srctree)/src/utils/interface/kve -I$(srctree)/src/utils/interface/lighthouse -I$(srctree)/src/utils/interface/tdoa +INCLUDES += -I$(srctree)/src/utils/interface -I$(srctree)/src/utils/interface/kve -I$(srctree)/src/utils/interface/lighthouse -I$(srctree)/src/utils/interface/tdoa -I$(srctree)/src/utils/interface/p2pDTR INCLUDES += -I$(LIB)/FatFS INCLUDES += -I$(LIB)/CMSIS/STM32F4xx/Include INCLUDES += -I$(LIB)/STM32_USB_Device_Library/Core/inc diff --git a/examples/app_p2p_DTR/.gitignore b/examples/app_p2p_DTR/.gitignore new file mode 100644 index 0000000000..a743f37a76 --- /dev/null +++ b/examples/app_p2p_DTR/.gitignore @@ -0,0 +1,2 @@ +bin/* +cf2.* diff --git a/examples/app_p2p_DTR/Kbuild b/examples/app_p2p_DTR/Kbuild new file mode 100644 index 0000000000..9d804337a0 --- /dev/null +++ b/examples/app_p2p_DTR/Kbuild @@ -0,0 +1 @@ +obj-y += src/ diff --git a/examples/app_p2p_DTR/Makefile b/examples/app_p2p_DTR/Makefile new file mode 100644 index 0000000000..d0a76e2ac3 --- /dev/null +++ b/examples/app_p2p_DTR/Makefile @@ -0,0 +1,23 @@ +# The firmware uses the Kbuild build system. There are 'Kbuild' files in this +# example that outlays what needs to be built. (check src/Kbuild). +# +# The firmware is configured using options in Kconfig files, the +# values of these end up in the .config file in the firmware directory. +# +# By setting the OOT_CONFIG (it is '$(PWD)/oot-config' by default) environment +# variable you can provide a custom configuration. It is important that you +# enable the app-layer. See app-config in this directory for example. + +# +# We want to execute the main Makefile for the firmware project, +# it will handle the build for us. +# +CRAZYFLIE_BASE := ../.. + +# +# We override the default OOT_CONFIG here, we could also name our config +# to oot-config and that would be the default. +# +OOT_CONFIG := $(PWD)/app-config + +include $(CRAZYFLIE_BASE)/tools/make/oot.mk diff --git a/examples/app_p2p_DTR/README.md b/examples/app_p2p_DTR/README.md new file mode 100644 index 0000000000..623b5a3e26 --- /dev/null +++ b/examples/app_p2p_DTR/README.md @@ -0,0 +1,23 @@ +# Peer to Peer App for Crazyflie 2.X + +This folder contains the app layer application for the Crazyflie to send and receive peer to peer messages. The debug messages of the received messages can be read in the console tab of the [cfclient](https://github.com/bitcraze/crazyflie-clients-python). Two Crazyflies need to be flashed with this program in order to work. Make sure that they are both on the same channel, and that they have different IDs. + +This example is going to blink the M4 LED when packets are sent and received, if you run 2 Crazyflies with this example you should see M4 blinking red and green in sequence indicating bidirectional P2P communication. + +You can find on Bitcraze's website the [API documentation for P2P](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/p2p_api/) as well as the [App layer API guide](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/userguides/app_layer/) + +## Limitations + +Since P2P communication happens asynchronously on the radio, this example does not work well when connecting a PC to the Crazyflies via the Radio. You should connect the Crazyflies using the USB port. This is a fundamental limitation of the current P2P implementation. + +## Build + +Make sure that you are in the app_peer_to_peer folder (not the main folder of the crazyflie firmware). Then type the following to build and flash it while the crazyflie is put into bootloader mode: + +``` +make clean +make +make cload +``` + +If you want to compile the application elsewhere in your machine, make sure to update ```CRAZYFLIE_BASE``` in the **Makefile**. diff --git a/examples/app_p2p_DTR/app-config b/examples/app_p2p_DTR/app-config new file mode 100644 index 0000000000..c86fe9169e --- /dev/null +++ b/examples/app_p2p_DTR/app-config @@ -0,0 +1,3 @@ +CONFIG_APP_ENABLE=y +CONFIG_APP_PRIORITY=1 +CONFIG_APP_STACKSIZE=350 diff --git a/examples/app_p2p_DTR/cload-all.sh b/examples/app_p2p_DTR/cload-all.sh new file mode 100755 index 0000000000..8b1c341d53 --- /dev/null +++ b/examples/app_p2p_DTR/cload-all.sh @@ -0,0 +1,27 @@ +#!/usr/bin/env bash +# Reset +COLOR_RESET='\033[0m' # Text Reset +YELLOW='\033[0;33m' # Yellow + +make -j 12 + +printf "${YELLOW}Flashing CF 00${COLOR_RESET}\n" +CLOAD_CMDS="-w radio://0/20/2M/E7E7E7E700" make cload +printf "${YELLOW}Flashing CF 01${COLOR_RESET}\n" +CLOAD_CMDS="-w radio://0/20/2M/E7E7E7E701" make cload +printf "${YELLOW}Flashing CF 02${COLOR_RESET}\n" +CLOAD_CMDS="-w radio://0/20/2M/E7E7E7E702" make cload +printf "${YELLOW}Flashing CF 03${COLOR_RESET}\n" +CLOAD_CMDS="-w radio://0/20/2M/E7E7E7E703" make cload +# printf "${YELLOW}Flashing CF 04${COLOR_RESET}\n" +# CLOAD_CMDS="-w radio://0/20/2M/E7E7E7E704" make cload +# printf "${YELLOW}Flashing CF 05${COLOR_RESET}\n" +# CLOAD_CMDS="-w radio://0/20/2M/E7E7E7E705" make cload +# printf "${YELLOW}Flashing CF 06${COLOR_RESET}\n" +# CLOAD_CMDS="-w radio://0/20/2M/E7E7E7E706" make cload +# printf "${YELLOW}Flashing CF 07${COLOR_RESET}\n" +# CLOAD_CMDS="-w radio://0/20/2M/E7E7E7E707" make cload +# printf "${YELLOW}Flashing CF 08${COLOR_RESET}\n" +# CLOAD_CMDS="-w radio://0/20/2M/E7E7E7E708" make cload +# printf "${YELLOW}Flashing CF 09${COLOR_RESET}\n" +# CLOAD_CMDS="-w radio://0/20/2M/E7E7E7E709" make cload diff --git a/examples/app_p2p_DTR/src/Kbuild b/examples/app_p2p_DTR/src/Kbuild new file mode 100644 index 0000000000..3fd0b0dd1f --- /dev/null +++ b/examples/app_p2p_DTR/src/Kbuild @@ -0,0 +1 @@ +obj-y += p2p_DTR_app.o diff --git a/examples/app_p2p_DTR/src/p2p_DTR_app.c b/examples/app_p2p_DTR/src/p2p_DTR_app.c new file mode 100644 index 0000000000..e9d3cf373b --- /dev/null +++ b/examples/app_p2p_DTR/src/p2p_DTR_app.c @@ -0,0 +1,109 @@ +/** + * ,---------, ____ _ __ + * | ,-^-, | / __ )(_) /_______________ _____ ___ + * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * | / ,--´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2019 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * + * peer_to_peer.c - App layer application of simple demonstartion peer to peer + * communication. Two crazyflies need this program in order to send and receive. + */ + + +#include +#include +#include +#include + +#include "app.h" + +#include "FreeRTOS.h" +#include "task.h" + +#include "radiolink.h" +#include "configblock.h" + +#define DEBUG_MODULE "P2P" +#include "debug.h" + +#include "token_ring.h" +#include "p2p_interface.h" + + +static uint8_t my_id; + +void loadTXPacketsForTesting(void){ + DTRpacket testSignal; + testSignal.message_type = DATA_FRAME; + testSignal.source_id = my_id; + testSignal.target_id = 1; + + uint8_t data_size = 25; + for (int i = 0; i < data_size; i++){ + testSignal.data[i] = i; + } + testSignal.dataSize = data_size; + testSignal.allToAllFlag = 1; + testSignal.packetSize = DTR_PACKET_HEADER_SIZE + testSignal.dataSize; + bool res; + for (int i = 0; i < TX_DATA_QUEUE_SIZE - 1; i++){ + testSignal.data[0] = 100+i; + res = insertPacketToQueue(&testSignal,TX_DATA_Q); + if (res){ + DTR_DEBUG_PRINT("TX Packet sent to TX_DATA Q\n"); + } + else{ + DEBUG_PRINT("Packet not sent to TX_DATA Q\n"); + } + } +} + +void appMain(){ + my_id = get_self_id(); + EnableDTRProtocol(); + vTaskDelay(2000); + + // Register the callback function so that the CF can receive packets as well. + p2pRegisterCB(DTRp2pcallbackHandler); + + if (my_id == 0){ + DTR_DEBUG_PRINT("Starting communication...\n"); + startRadioCommunication(); + loadTXPacketsForTesting(); + } + + DTRpacket received_packet; + uint32_t start = T2M(xTaskGetTickCount()); + while(1){ + getPacketFromQueue(&received_packet, RX_DATA_Q, portMAX_DELAY); + uint32_t dt = T2M(xTaskGetTickCount()) - start; + DEBUG_PRINT("Received data from %d : %d --> Time elapsed: %lu msec\n",received_packet.source_id, received_packet.data[0],dt); + start = T2M(xTaskGetTickCount()); + + if (my_id == 1 && received_packet.data[0] == 104){ + received_packet.source_id = my_id; + received_packet.data[0] = 123; + DEBUG_PRINT("Sending response...\n"); + insertPacketToQueue(&received_packet,TX_DATA_Q); + } + } +} + + diff --git a/src/utils/interface/debug.h b/src/utils/interface/debug.h index f6bb63b136..d2ee35b138 100644 --- a/src/utils/interface/debug.h +++ b/src/utils/interface/debug.h @@ -23,6 +23,7 @@ * * debug.h - Debugging utility functions */ +#pragma once // TODO: check if there is another way to fix this #include "config.h" #include "console.h" #include "autoconf.h" diff --git a/src/utils/interface/p2pDTR/DTR_timers.h b/src/utils/interface/p2pDTR/DTR_timers.h new file mode 100644 index 0000000000..09ea55fe57 --- /dev/null +++ b/src/utils/interface/p2pDTR/DTR_timers.h @@ -0,0 +1,65 @@ +/* + * MIT License + * + * Copyright (c) 2022 Christos Zosimidis + * + * 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. + * + * + * DTR_timers.h + * + * Created on: 14.02.2021 + * Author: Christos Zosimidis + * + * Modified for the P2P protocol by: Bitcraze AB + * + */ +#ifndef SRC_RADIO_DTR_TIMERS_H_ +#define SRC_RADIO_DTR_TIMERS_H_ + +#include "FreeRTOS.h" +#include "task.h" +#include + +#include "DTR_types.h" +#include "token_ring.h" +#include "timers.h" + +#define MAX_WAIT_TIME_FOR_RTS 10 // ms +#define MAX_WAIT_TIME_FOR_CTS 11 // ms +#define MAX_WAIT_TIME_FOR_DATA_ACK 12 // ms + +#define DTR_PROTOCOL_PERIOD 5 // ms (random value) + + +// DTR PROTOCOL TASK +#define DTR_PROTOCOL_TASK_STACK_SIZE 2 * configMINIMAL_STACK_SIZE +#define DTR_PROTOCOL_TASK_PRIORITY 1 //Higher number higher priority + +// DTR protocol Task +void startDTRProtocol(void); + +// ================ DTR sender timer ================== +void initDTRSenderTimer(void); + +void shutdownDTRSenderTimer(void); + +void startDTRSenderTimer(unsigned int time_out); + +#endif /* SRC_RADIO_DTR_TIMERS_H_ */ diff --git a/src/utils/interface/p2pDTR/DTR_types.h b/src/utils/interface/p2pDTR/DTR_types.h new file mode 100644 index 0000000000..ec4c914548 --- /dev/null +++ b/src/utils/interface/p2pDTR/DTR_types.h @@ -0,0 +1,114 @@ +/* + * MIT License + * + * Copyright (c) 2022 Christos Zosimidis + * + * 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. + * + * + * DTR_types.h + * + * Created on: 14.02.2021 + * Author: Christos Zosimidis + * + * Modified for the P2P protocol by: Bitcraze AB + * + */ + +#ifndef DTR_TYPES_H +#define DTR_TYPES_H + +#include "FreeRTOS.h" +#include "stdint.h" +#include "stdbool.h" +#include "radiolink.h" + + +#define DTR_PACKET_HEADER_SIZE 6 + +// max usable size for a packet is -1 byte for the port +#define P2P_MAX_USABLE_DATA_SIZE P2P_MAX_DATA_SIZE -1 + +#define MAXIMUM_DTR_PACKET_DATA_SIZE P2P_MAX_USABLE_DATA_SIZE - DTR_PACKET_HEADER_SIZE + +typedef enum tx_states_e { + TX_TOKEN, + TX_CTS, + TX_RTS, + TX_DATA_FRAME, + TX_DATA_ACK, +} TxStates; + +typedef enum rx_states_e { + RX_IDLE, + RX_WAIT_CTS, + RX_WAIT_RTS, + RX_WAIT_DATA_ACK, +} RxStates; + +enum message_types { + DATA_FRAME = 0, + TOKEN_FRAME = 1, + CTS_FRAME = 2, + RTS_FRAME = 3, + DATA_ACK_FRAME = 4, +}; + +// |--------------------| +// | PACKET FORMAT | +// |--------------------| +// | packetSize | (1 B) +// |--------------------| +// | message_type | (1 B) +// |--------------------| +// | source_id | (1 B) +// |--------------------| +// | dataSize | (1 B) +// |--------------------| +// | allToAllFlag | (1 b) +// |--------------------| +// | data | (dataSize B) +// |--------------------| +typedef struct radio_packet { + uint8_t packetSize; + uint8_t message_type; + uint8_t source_id; + uint8_t target_id; + uint8_t dataSize; + bool allToAllFlag; + uint8_t data[MAXIMUM_DTR_PACKET_DATA_SIZE]; +} DTRpacket; + +typedef struct radio_meta_info { + uint8_t deviceId; + + uint32_t failedCRC; + uint32_t failedRxQueueFull; + uint32_t failedTxQueueFull; + + uint32_t timeOutRTS; + uint32_t timeOutTOKEN; + uint32_t timeOutDATA; + + uint32_t sendPackets; + uint32_t receivedPackets; + +} RadioInfo; + +#endif //DTR_TYPES_H diff --git a/src/utils/interface/p2pDTR/p2p_interface.h b/src/utils/interface/p2pDTR/p2p_interface.h new file mode 100644 index 0000000000..09bf170eaf --- /dev/null +++ b/src/utils/interface/p2pDTR/p2p_interface.h @@ -0,0 +1,47 @@ +/** + * ,---------, ____ _ __ + * | ,-^-, | / __ )(_) /_______________ _____ ___ + * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * | / ,--´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2019 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * + * p2p_interface.h + * + */ + +#ifndef _P2P_INTERFACE_H_ +#define _P2P_INTERFACE_H_ + +#include "FreeRTOS.h" +#include "DTR_types.h" +#include "radiolink.h" +#include "token_ring.h" + +#define INCOMING_DTR_QUEUE_SIZE 10 + +// Broadcasts a DTR packet through the P2P network +void sendDTRpacket(const DTRpacket* packet) ; + +// Puts the DTR packet in the queue for the token ring to pick up. +void DTRp2pcallbackHandler(P2PPacket *p); + +void feedDTRPacketToProtocol(DTRpacket *incoming_DTR); + +#endif // _P2P_INTERFACE_H_ diff --git a/src/utils/interface/p2pDTR/queueing.h b/src/utils/interface/p2pDTR/queueing.h new file mode 100644 index 0000000000..7744ba2f29 --- /dev/null +++ b/src/utils/interface/p2pDTR/queueing.h @@ -0,0 +1,80 @@ +/* + * MIT License + * + * Copyright (c) 2022 Christos Zosimidis + * + * 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. + * + * + * queueing.h + * + * Created on: 14.02.2021 + * Author: Christos Zosimidis + * + * Modified for the P2P protocol by: Bitcraze AB + * + */ + +#ifndef _QUEUEING_H_ +#define _QUEUEING_H_ + +#include "FreeRTOS.h" +#include "queue.h" +#include "DTR_types.h" +#include "queuemonitor.h" +#include "static_mem.h" + +#include "token_ring.h" + +#include "debug.h" +#define TX_DATA_QUEUE_SIZE 10 +#define RX_SRV_QUEUE_SIZE 20 +#define RX_DATA_QUEUE_SIZE 10 + +#define TX_RECEIVED_WAIT_TIME 5// ms +#define RX_RECEIVED_WAIT_TIME 5// ms + +typedef enum queue_names_e { + TX_DATA_Q, + RX_SRV_Q, + RX_DATA_Q, +} DTRQueue_Names; + +void queueing_init(); + + +bool isPacketInQueueAvailable(DTRQueue_Names qName); + +uint8_t getNumberOfPacketsInQueue(DTRQueue_Names qName); + +bool getPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout); + +// Blocks to wait for a packet to be received for a given time +// new_packet_received --> True if a new packet has been received and False if the timeout has been reached +bool receivePacketWaitUntil(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received); + +bool insertPacketToQueue(DTRpacket *packet, DTRQueue_Names qName); + +bool releasePacketFromQueue(DTRQueue_Names qName); + +void emptyQueue(DTRQueue_Names qName); + +void emptyQueues(void); + +#endif /* _QUEUEING_H_ */ diff --git a/src/utils/interface/p2pDTR/token_ring.h b/src/utils/interface/p2pDTR/token_ring.h new file mode 100644 index 0000000000..e07f29b35a --- /dev/null +++ b/src/utils/interface/p2pDTR/token_ring.h @@ -0,0 +1,100 @@ +/* + * MIT License + * + * Copyright (c) 2022 Christos Zosimidis + * + * 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. + * + * + * token_ring.h + * + * Created on: 28.01.2021 + * Author: Christos Zosimidis + * + * Modified for the P2P protocol by: Bitcraze AB + */ + +#ifndef SRC_RADIO_RADIO_H_ +#define SRC_RADIO_RADIO_H_ + +#include "FreeRTOS.h" +#include "timers.h" + +#include "stdint.h" +#include "stdbool.h" +#include "string.h" +#include "radiolink.h" +#include "log.h" +#include "configblock.h" + + +#include "DTR_types.h" +#include "DTR_timers.h" +#include "p2p_interface.h" +#include "queueing.h" + +// #define UNUSED(...) ((void)sizeof((_Bool[]){__VA_ARGS__})); //Used to silence compiler warnings about unused parameters +#define UNUSED(...) ; //Used to silence compiler warnings about unused parameters + +// #define DEBUG_DTR_PROTOCOL // Enable debug prints for the DTR protocol (WARNING: May cause problems ) + +#ifdef DEBUG_DTR_PROTOCOL + #define DTR_DEBUG_PRINT(fmt, ... ) DEBUG_PRINT(DEBUG_FMT(fmt), ##__VA_ARGS__) +#else + //do nothing + #define DTR_DEBUG_PRINT( fmt, ... ) (void) (fmt) +#endif + + +#define NETWORK_SIZE 4 +#define PROTOCOL_TIMEOUT_MS 4 * 1000.0f // ms + + +void initRadio(uint8_t networkSize, uint8_t device_id); + +void initTokenRing(uint8_t networkSize, uint8_t device_id); + +void setDeviceRadioAddress (uint8_t radio_address); + +uint8_t getDeviceRadioAddress(); + +void timeOutCallBack(xTimerHandle timer); + +void startRadioCommunication(); + +void DTRInterruptHandler(void *param); + +const RadioInfo* getRadioInfo(); + +void resetRadioMetaInfo(); + +void printDTRPacket(DTRpacket* packet); + +const char* getMessageType(uint8_t message_type); + +const char* getRXState(uint8_t rx_state); + +const char* getTXState(uint8_t tx_state); + +uint8_t get_self_id(void); + +// Starts the task of the Dynamic Token Ring Protocol (DTR) and initializes the protocol +void EnableDTRProtocol(void); + +#endif /* SRC_RADIO_RADIO_H_ */ diff --git a/src/utils/src/Kbuild b/src/utils/src/Kbuild index e943b1f01b..63d29118c1 100644 --- a/src/utils/src/Kbuild +++ b/src/utils/src/Kbuild @@ -32,6 +32,12 @@ obj-$(CONFIG_DECK_LIGHTHOUSE) += lighthouse/ootx_decoder.o obj-$(CONFIG_DECK_LIGHTHOUSE) += lighthouse/pulse_processor.o obj-$(CONFIG_DECK_LIGHTHOUSE) += lighthouse/pulse_processor_v1.o obj-$(CONFIG_DECK_LIGHTHOUSE) += lighthouse/pulse_processor_v2.o + +obj-y += p2pDTR/DTR_timers.o +obj-y += p2pDTR/p2p_interface.o +obj-y += p2pDTR/queueing.o +obj-y += p2pDTR/token_ring.o + obj-y += num.o obj-y += rateSupervisor.o obj-y += sleepus.o diff --git a/src/utils/src/p2pDTR/DTR_timers.c b/src/utils/src/p2pDTR/DTR_timers.c new file mode 100644 index 0000000000..8444959c69 --- /dev/null +++ b/src/utils/src/p2pDTR/DTR_timers.c @@ -0,0 +1,85 @@ +/* + * MIT License + * + * Copyright (c) 2022 Christos Zosimidis + * + * 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. + * + * + * DTR_timers.c + * + * Created on: 14.02.2021 + * Author: Christos Zosimidis + * + * Modified for the P2P protocol by: Bitcraze AB + * + */ + + +#include "DTR_timers.h" + +static xTimerHandle sender_timer; +// static xTimerHandle protocol_timer; + +static bool sender_timer_running = false; + +static char type_to_spam[15]; + +void startDTRProtocol(void){ + xTaskCreate(DTRInterruptHandler, "DTR_P2P", DTR_PROTOCOL_TASK_STACK_SIZE, NULL,DTR_PROTOCOL_TASK_PRIORITY, NULL); +} + +void initDTRSenderTimer(void) { + sender_timer = xTimerCreate("DTRSenderTimer", M2T(20), pdTRUE, NULL, timeOutCallBack); +} + +void shutdownDTRSenderTimer(void) { + if (xTimerIsTimerActive(sender_timer)==pdTRUE) { + xTimerStop(sender_timer, 0); + DTR_DEBUG_PRINT("Stopped spamming messages\n"); + sender_timer_running = false; + }else{ + DEBUG_PRINT("Radio timer not running\n"); + } +} + + +void startDTRSenderTimer(unsigned int time_out) { + + if(time_out == MAX_WAIT_TIME_FOR_RTS ){ + strcpy(type_to_spam, "RTS"); + }else if (time_out == MAX_WAIT_TIME_FOR_CTS ){ + strcpy(type_to_spam, "CTS"); + }else if (time_out == MAX_WAIT_TIME_FOR_DATA_ACK ){ + strcpy(type_to_spam, "DATA"); + } + + if (sender_timer_running){ + DTR_DEBUG_PRINT("Radio timer already running\n"); + }else{ + #ifdef DEBUG_DTR_PROTOCOL + DTR_DEBUG_PRINT("Started spamming %s\n", type_to_spam); + #endif + + xTimerStart(sender_timer, 20); + // xTimerChangePeriod(sender_timer, M2T(time_out), 0); + + sender_timer_running = true; + } +} diff --git a/src/utils/src/p2pDTR/p2p_interface.c b/src/utils/src/p2pDTR/p2p_interface.c new file mode 100644 index 0000000000..00bc4ebec3 --- /dev/null +++ b/src/utils/src/p2pDTR/p2p_interface.c @@ -0,0 +1,77 @@ +/** + * ,---------, ____ _ __ + * | ,-^-, | / __ )(_) /_______________ _____ ___ + * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * | / ,--´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2019 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * + * p2p_interface.c + * + */ + + + +#include "p2p_interface.h" + +static P2PPacket p2p_TXpacket; +static DTRpacket prev_received = {0}; + +void sendDTRpacket(const DTRpacket* packet) { + p2p_TXpacket.port=0x00; + + memcpy(&p2p_TXpacket.data[0], packet, packet->packetSize); + p2p_TXpacket.size = packet->packetSize; + + radiolinkSendP2PPacketBroadcast(&p2p_TXpacket); +} + +void DTRp2pcallbackHandler(P2PPacket *p){ + + DTRpacket incoming_DTR; + + uint8_t DTRpacket_size = p->data[0]; + + memcpy(&incoming_DTR, &(p->data[0]), DTRpacket_size); + feedDTRPacketToProtocol(&incoming_DTR); +} + + +void feedDTRPacketToProtocol(DTRpacket *incoming_DTR) { + + bool same_packet_received = incoming_DTR->message_type == prev_received.message_type && + incoming_DTR->target_id == prev_received.target_id && + incoming_DTR->source_id == prev_received.source_id; + + // if there are packets in the queue and the new packet is the same as the previous one, ignore it + DTR_DEBUG_PRINT("Packets in RX_SRV queue: %d\n", getNumberOfPacketsInQueue(RX_SRV_Q) ); + if ( getNumberOfPacketsInQueue(RX_SRV_Q) !=0 && same_packet_received ) { + DTR_DEBUG_PRINT("Duplicate packet received\n"); + DTR_DEBUG_PRINT("Message type: %d\n", incoming_DTR->message_type); + DTR_DEBUG_PRINT("Target id: %d\n", incoming_DTR->target_id); + + return; + } + + prev_received.message_type = incoming_DTR->message_type; + prev_received.target_id = incoming_DTR->target_id; + prev_received.source_id = incoming_DTR->source_id; + + insertPacketToQueue(incoming_DTR, RX_SRV_Q); +} diff --git a/src/utils/src/p2pDTR/queueing.c b/src/utils/src/p2pDTR/queueing.c new file mode 100644 index 0000000000..45670831fe --- /dev/null +++ b/src/utils/src/p2pDTR/queueing.c @@ -0,0 +1,142 @@ +/* + * MIT License + * + * Copyright (c) 2022 Christos Zosimidis + * + * 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. + * + * + * queueing.c + * + * Created on: 14.02.2021 + * Author: Christos Zosimidis + * + * Modified for the P2P protocol by: Bitcraze AB + * + */ + + +#include "queueing.h" + +// TX SRV packet queue +static xQueueHandle TX_DATA_queue; + +// RX SRV packet queue +static xQueueHandle RX_SRV_queue; + +// RX DATA packet queue +static xQueueHandle RX_DATA_queue; + + +xQueueHandle *getQueueHandler(DTRQueue_Names qName){ + switch(qName){ + case TX_DATA_Q: + return &TX_DATA_queue; + case RX_SRV_Q: + return &RX_SRV_queue; + case RX_DATA_Q: + return &RX_DATA_queue; + default: + // The q enum is not valid + DEBUG_PRINT("Invalid queue name\n"); + ASSERT(0); + return NULL; + } +} + + +void queueing_init(){ + // TX SRV queue + STATIC_MEM_QUEUE_ALLOC(TX_DATA_queue, TX_DATA_QUEUE_SIZE, sizeof(DTRpacket)); + TX_DATA_queue = STATIC_MEM_QUEUE_CREATE(TX_DATA_queue); + DEBUG_QUEUE_MONITOR_REGISTER(TX_DATA_queue); + + // RX SRV queue + STATIC_MEM_QUEUE_ALLOC(RX_SRV_queue, RX_SRV_QUEUE_SIZE, sizeof(DTRpacket)); + RX_SRV_queue = STATIC_MEM_QUEUE_CREATE(RX_SRV_queue); + DEBUG_QUEUE_MONITOR_REGISTER(RX_SRV_queue); + + // RX DATA queue + STATIC_MEM_QUEUE_ALLOC(RX_DATA_queue, RX_DATA_QUEUE_SIZE, sizeof(DTRpacket)); + RX_DATA_queue = STATIC_MEM_QUEUE_CREATE(RX_DATA_queue); + DEBUG_QUEUE_MONITOR_REGISTER(RX_DATA_queue); + +} + +uint8_t getNumberOfPacketsInQueue(DTRQueue_Names qName){ + return (uint8_t) uxQueueMessagesWaiting(*getQueueHandler(qName)); +} + +bool isPacketInQueueAvailable(DTRQueue_Names qName) { + return uxQueueMessagesWaiting(*getQueueHandler(qName)) > 0; +} + +bool getPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout){ + // notice that xQueuePeek is used instead of xQueueReceive, because the packet is not removed from the queue + //TODO: make a separate function for this + bool received_success; + switch (qName) + { + case TX_DATA_Q: + received_success = xQueuePeek(TX_DATA_queue, packet, timeout) == pdTRUE; + break; + + case RX_SRV_Q: + received_success = xQueueReceive(RX_SRV_queue, packet, timeout) == pdTRUE; + break; + case RX_DATA_Q: + received_success = xQueueReceive(RX_DATA_queue, packet, timeout) == pdTRUE; + break; + + default: + DEBUG_PRINT("Invalid queue name\n"); + received_success = false; + break; + } + return received_success; +} + +bool receivePacketWaitUntil(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received){ + *new_packet_received = xQueueReceive(*getQueueHandler(qName), packet, M2T(timeout_ms)) == pdTRUE; + return true; +} + +bool insertPacketToQueue(DTRpacket *packet, DTRQueue_Names qName) { + bool res = xQueueSend(*getQueueHandler(qName),(void *) packet, 0) == pdTRUE; + if (!res) { + DEBUG_PRINT("TX_DATA queue busy\n"); + } + + return res; +} + +bool releasePacketFromQueue(DTRQueue_Names qName) { + DTRpacket packet; + return xQueueReceive(*getQueueHandler(qName), &packet, M2T(TX_RECEIVED_WAIT_TIME)) == pdTRUE; +} + +void emptyQueue(DTRQueue_Names qName) { + xQueueReset(*getQueueHandler(qName)); +} + +void emptyQueues(void){ + emptyQueue(TX_DATA_Q); + emptyQueue(RX_SRV_Q); + emptyQueue(RX_DATA_Q); +} diff --git a/src/utils/src/p2pDTR/token_ring.c b/src/utils/src/p2pDTR/token_ring.c new file mode 100644 index 0000000000..0dcc0c520e --- /dev/null +++ b/src/utils/src/p2pDTR/token_ring.c @@ -0,0 +1,509 @@ +/* + * MIT License + * + * Copyright (c) 2022 Christos Zosimidis + * + * 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. + * + * + * token_ring.c + * + * Created on: 28.01.2021 + * Author: Christos Zosimidis + * + * Modified for the P2P protocol by: Bitcraze AB + */ + + +#include "token_ring.h" + +#define DEBUG_MODULE "TOK_RING" +#include "debug.h" + + +static uint8_t MAX_NETWORK_SIZE = 0; +static uint8_t node_id = 0; +static uint8_t next_node_id = 0; +static uint8_t prev_node_id = 0; +static uint8_t next_target_id = 0; +static uint8_t last_packet_source_id = 0; + +static DTRpacket* timerDTRpacket; + +static DTRpacket startPacket; // not sure if this is needed + +static DTRpacket servicePk = { + .dataSize = 0, + .packetSize = DTR_PACKET_HEADER_SIZE, + .allToAllFlag = false, +}; +static TxStates tx_state, timerRadioTxState; +static RxStates rx_state; + + +static RadioInfo radioMetaInfo; +static uint32_t protocol_timeout_ms; + +static uint8_t my_id ; + +static void setNodeIds(uint8_t networkSize, uint8_t device_id) { + MAX_NETWORK_SIZE = networkSize; + node_id = device_id; + servicePk.source_id = node_id; + last_packet_source_id = node_id; + next_node_id = (node_id + 1) % MAX_NETWORK_SIZE; + prev_node_id = (node_id + (MAX_NETWORK_SIZE - 1)) % MAX_NETWORK_SIZE; +} + +void initTokenRing(uint8_t networkSize, uint8_t device_id) { + my_id = get_self_id(); + + /* Network node configuration*/ + setNodeIds(networkSize, my_id); + + rx_state = RX_IDLE; +} + +static void setupRadioTx(DTRpacket* packet, TxStates txState) { + + tx_state = txState; + DTR_DEBUG_PRINT("Setting up radio tx with state: %s \n", getTXState(tx_state)); + + switch (tx_state) { + + case TX_DATA_ACK: + // set the receiver to IDLE, after packet is sent + rx_state = RX_IDLE; + break; + + case TX_CTS: + // set the receiver to IDLE, after packet is sent + rx_state = RX_IDLE; + break; + + case TX_RTS: + // set the receiver to WAIT_FOR_CTS, after packet is sent + rx_state = RX_WAIT_CTS; + + // set the radio timer to "spam" RTS messages + // setDTRSenderTimer(MAX_WAIT_TIME_FOR_CTS); + timerDTRpacket = packet; + startDTRSenderTimer(MAX_WAIT_TIME_FOR_CTS); + break; + + case TX_TOKEN: + // set the receiver to RX_WAIT_RTS, after packet is sent + rx_state = RX_WAIT_RTS; + + // set the radio timer to "spam" token messages + // setDTRSenderTimer(MAX_WAIT_TIME_FOR_RTS); + timerDTRpacket = packet; + startDTRSenderTimer(MAX_WAIT_TIME_FOR_RTS); + break; + + case TX_DATA_FRAME: + // set the receiver to WAIT_DATA_ACK, after packet is sent + rx_state = RX_WAIT_DATA_ACK; + + // set the radio timer to "spam" the same DATA frame + // setDTRSenderTimer(MAX_WAIT_TIME_FOR_DATA_ACK); + timerDTRpacket = packet; + startDTRSenderTimer(MAX_WAIT_TIME_FOR_DATA_ACK); + break; + + default: + DEBUG_PRINT("\nRadio transmitter state not set correctly!!\n"); + return; + } + sendDTRpacket(packet); + radioMetaInfo.sendPackets++; + + +} + +static void resetProtocol(void){ + DTR_DEBUG_PRINT("\nResetting protocol\n"); + rx_state = RX_IDLE; + protocol_timeout_ms = T2M(xTaskGetTickCount()) + PROTOCOL_TIMEOUT_MS; + emptyQueues(); // Maybe not all queues, but only the RX_SRV since the data are going to be lost saved in DATA queues + shutdownDTRSenderTimer(); + last_packet_source_id = 255; +} + +static uint8_t send_data_to_peer_counter = 0; + +void DTRInterruptHandler(void *param) { + + DTRpacket* rxPk, *txPk; + + //TODO: bad implementation, should be fixed + DTRpacket _rxPk ; + protocol_timeout_ms = T2M(xTaskGetTickCount()) + PROTOCOL_TIMEOUT_MS; + bool new_packet_received; + + DEBUG_PRINT("\nDTRInterruptHandler Task called...\n"); + while ( receivePacketWaitUntil(&_rxPk, RX_SRV_Q, PROTOCOL_TIMEOUT_MS, &new_packet_received) ){ + if (!new_packet_received) { + DTR_DEBUG_PRINT("\nPROTOCOL TIMEOUT!\n"); + if (my_id != 0){ + resetProtocol(); + } + + continue; + } + + rxPk = &_rxPk; + + radioMetaInfo.receivedPackets++; + + DTR_DEBUG_PRINT("===============================================================\n"); + DTR_DEBUG_PRINT("=\n"); + DTR_DEBUG_PRINT("TX_DATA Q Empty: %d\n", !isPacketInQueueAvailable(TX_DATA_Q)); + + + DTR_DEBUG_PRINT("rx_state: %s\n", getRXState(rx_state)); + + // printDTRPacket(rxPk); + DTR_DEBUG_PRINT("Received packet type: %d\n", rxPk->message_type); + DTR_DEBUG_PRINT("Received packet target id: %d\n", rxPk->target_id); + DTR_DEBUG_PRINT("my_id: %d\n", node_id); + + switch (rx_state) { + + case RX_IDLE: + /* if packet is DATA packet and received from previous node, + * then it can be handled. */ + if (rxPk->message_type == DATA_FRAME && rxPk->target_id == node_id) { + DTR_DEBUG_PRINT("\nReceived DATA packet from prev\n"); + if (rxPk->source_id != last_packet_source_id) { + last_packet_source_id = rxPk->source_id; + /* if packet is relevant and receiver queue is not full, then + * push packet in the queue and prepare queue for next packet. */ + bool queueFull = insertPacketToQueue(rxPk, RX_DATA_Q); + if (queueFull){ + radioMetaInfo.failedRxQueueFull++; + } + } + /* Acknowledge the data */ + DTR_DEBUG_PRINT("Received DATA %d\n", rxPk->data[0]); + DTR_DEBUG_PRINT("\nSending ACK packet\n"); + servicePk.message_type = DATA_ACK_FRAME; + servicePk.target_id = rxPk->source_id; + setupRadioTx(&servicePk, TX_DATA_ACK); + continue; + } + + /* if packet is TOKEN packet and received from previous node, then + * send a TOKEN_ACK packet and prepare the packet for the timer. */ + if (rxPk->message_type == TOKEN_FRAME && rxPk->source_id == prev_node_id) { + DTR_DEBUG_PRINT("\nReceived TOKEN from prev\n"); + servicePk.message_type = RTS_FRAME; + setupRadioTx(&servicePk, TX_RTS); + continue; + } + + /* if packet is RTS packet and received from next node, then send + * a CTS packet to next node. */ + if (rxPk->message_type == RTS_FRAME && rxPk->source_id == next_node_id) { + DTR_DEBUG_PRINT("\nReceived RTS from next -> CTS to next\n"); + servicePk.message_type = CTS_FRAME; + setupRadioTx(&servicePk, TX_CTS); + continue; + } + + DTR_DEBUG_PRINT("\nRECEIVED PACKET NOT HANDLED\n"); + /* drop all other packets and restart receiver */ + break; + + case RX_WAIT_CTS: + + /* if packet is CTS and received from previous node, then node can + * send its next DATA packet. */ + if (rxPk->message_type == CTS_FRAME && rxPk->source_id == prev_node_id) { + shutdownDTRSenderTimer(); + last_packet_source_id = node_id; + DTR_DEBUG_PRINT("\nRcvd CTS from prev,send DATA to next\n"); + /* check if there is a DATA packet. If yes, prepare it and + * send it, otherwise forward the token to the next node. */ + + DTRpacket _txPk;//TODO: bad implementation, should be fixed + if (getPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME))) { + txPk = &_txPk; + DTR_DEBUG_PRINT("TX DATA Packet exists (%d), sending it\n",txPk->data[0]); + if(txPk->allToAllFlag) { + txPk->target_id = next_node_id; + } + if (txPk->target_id >= MAX_NETWORK_SIZE) { + DTR_DEBUG_PRINT("Releasing TX DATA Packet because target > default:\n"); + + DTR_DEBUG_PRINT("Is Queue Empty: %d\n", !isPacketInQueueAvailable(TX_DATA_Q)); + + releasePacketFromQueue(TX_DATA_Q); + txPk = &servicePk; + txPk->message_type = TOKEN_FRAME; + tx_state = TX_TOKEN; + } else { + txPk->packetSize = DTR_PACKET_HEADER_SIZE + txPk->dataSize; + txPk->source_id = node_id; + txPk->message_type = DATA_FRAME; + tx_state = TX_DATA_FRAME; + } + } else { + DTR_DEBUG_PRINT("No TX DATA,forwarding token to next\n"); + txPk = &servicePk; + txPk->message_type = TOKEN_FRAME; + tx_state = TX_TOKEN; + } + + setupRadioTx(txPk, tx_state); + continue; + } + + /* drop all other packets and restart receiver */ + break; + + case RX_WAIT_RTS: + + /* if packet is TOKEN_ACK and received from next node, then + * a CTS packet can be sent. */ + if (rxPk->message_type == RTS_FRAME && rxPk->source_id == next_node_id) { + DTR_DEBUG_PRINT("\nReceived TOKEN_ACK from next->sending CTS \n"); + shutdownDTRSenderTimer(); + servicePk.message_type = CTS_FRAME; + setupRadioTx(&servicePk, TX_CTS); + continue; + } + + /* drop all other packets and restart receiver */ + break; + + case RX_WAIT_DATA_ACK: + if (rxPk->message_type == DATA_ACK_FRAME && rxPk->target_id == node_id) { + shutdownDTRSenderTimer(); + + //TODO: bad implementation, should be fixed + DTRpacket _txPk; + getPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME)); + txPk = &_txPk; + + next_target_id = (txPk->target_id + send_data_to_peer_counter + 1) % MAX_NETWORK_SIZE; + DTR_DEBUG_PRINT("next_target_id: %d\n", next_target_id); + + if (!txPk->allToAllFlag || next_target_id == node_id) { + DTR_DEBUG_PRINT("Releasing TX DATA:\n"); + // printDTRPacket(txPk); + + DTR_DEBUG_PRINT("Is Q Empty: %d\n", !isPacketInQueueAvailable(TX_DATA_Q)); + + releasePacketFromQueue(TX_DATA_Q); + txPk = &servicePk; + txPk->message_type = TOKEN_FRAME; + tx_state = TX_TOKEN; + send_data_to_peer_counter = 0; + } else { + txPk->target_id = next_target_id; + send_data_to_peer_counter++; + DTR_DEBUG_PRINT("Sending DATA to next peer.."); + DTR_DEBUG_PRINT("with target id: %d\n", txPk->target_id); + tx_state = TX_DATA_FRAME; + } + setupRadioTx(txPk, tx_state); + continue; + } + + /* drop all other packets and restart receiver */ + break; + + default: + DEBUG_PRINT("\nRadio receiver state not set correctly!!\n"); + continue; + } + + // TODO: There are cases that the packet received from queue + // is the same to the one that was previously received and already processed. + // Maybe when releasing the packet, also release the ones that are similar to it in the queue. + + // DEBUG_PRINT("Packet Received cannot be processed\n"); + // DEBUG_PRINT("State: %d\n", rx_state); + // DEBUG_PRINT("Packet type: %d\n", rxPk->message_type); + + // resetProtocol(); //TODO: test if it makes sense + } + +} + +void setDeviceRadioAddress(uint8_t radio_address) { + node_id = radio_address; +} + +uint8_t getDeviceRadioAddress() { + return node_id; +} + +void timeOutCallBack(xTimerHandle timer) { + #ifdef DEBUG_DTR + DTR_DEBUG_PRINT("Sending packet after timeout: %s",getMessageType(timerDTRpacket->message_type)); + if (timerDTRpacket->message_type == DATA_FRAME) { + for (size_t i = 0; i dataSize; i++) + { + DTR_DEBUG_PRINT(" %d ",timerDTRpacket->data[i]); + } + } + DTR_DEBUG_PRINT("\n"); + #endif + + sendDTRpacket(timerDTRpacket); + radioMetaInfo.sendPackets++; + + switch(tx_state) { + case TX_RTS: + radioMetaInfo.timeOutRTS++; + break; + case TX_TOKEN: + radioMetaInfo.timeOutTOKEN++; + break; + case TX_DATA_FRAME: + radioMetaInfo.timeOutDATA++; + break; + default: + break; + } +} + +const RadioInfo* getRadioInfo() { + radioMetaInfo.deviceId = node_id; + return &radioMetaInfo; +} + +void resetRadioMetaInfo() { + memset(&radioMetaInfo, 0, sizeof(radioMetaInfo)); +} + +void startRadioCommunication() { + DTRpacket* startSignal = &startPacket; + + startSignal->source_id = node_id; + startSignal->target_id = next_node_id; + startSignal->data[0] = 69; + startSignal->dataSize = 1; + startSignal->packetSize = DTR_PACKET_HEADER_SIZE + startSignal->dataSize; + startSignal->message_type = DATA_FRAME; + timerDTRpacket = startSignal; + + timerRadioTxState = TX_DATA_FRAME; + rx_state = RX_WAIT_DATA_ACK; + DTR_DEBUG_PRINT("Spamming DATA\n"); + startDTRSenderTimer(MAX_WAIT_TIME_FOR_DATA_ACK); +} + +// get the message name from the message type +const char* getMessageType(uint8_t message_type){ + switch(message_type){ + case DATA_FRAME: + return "DATA"; + case TOKEN_FRAME: + return "TOKEN"; + case CTS_FRAME: + return "CTS"; + case RTS_FRAME: + return "RTS"; + case DATA_ACK_FRAME: + return "DATA_ACK"; + default: + return "UNKNOWN"; + } +} + +const char* getRXState(uint8_t rx_state){ + switch(rx_state){ + case RX_IDLE: + return "RX_IDLE"; + case RX_WAIT_CTS: + return "RX_WAIT_CTS"; + case RX_WAIT_RTS: + return "RX_WAIT_RTS"; + case RX_WAIT_DATA_ACK: + return "RX_WAIT_DATA_ACK"; + default: + return "UNKNOWN"; + } +} + +const char* getTXState(uint8_t tx_state){ + switch(tx_state){ + case TX_TOKEN: + return "TX_TOKEN"; + case TX_RTS: + return "TX_RTS"; + case TX_CTS: + return "TX_CTS"; + case TX_DATA_FRAME: + return "TX_DATA_FRAME"; + case TX_DATA_ACK: + return "TX_DATA_ACK"; + default: + return "UNKNOWN"; + } +} + +void printDTRPacket(DTRpacket* packet){ + DTR_DEBUG_PRINT("\nDTR Packet Received: %s\n", getMessageType(packet->message_type)); + DTR_DEBUG_PRINT("Packet Size: %d\n", packet->packetSize); + DTR_DEBUG_PRINT("Message Type: %s\n", getMessageType(packet->message_type)); + DTR_DEBUG_PRINT("Source ID: %d\n", packet->source_id); + DTR_DEBUG_PRINT("Target ID: %d\n", packet->target_id); + DTR_DEBUG_PRINT("AllToAll Flag: %d\n", packet->allToAllFlag); + if (packet->dataSize > 0) { + DEBUG_PRINT("Data: "); + for (int i = 0; i < packet->dataSize; i++) { + DEBUG_PRINT("%d ", packet->data[i]); + } + DEBUG_PRINT("\n"); + } + + DTR_DEBUG_PRINT("\n\n"); +} + +uint8_t get_self_id(void){ + // Get the current address of the crazyflie and + //keep the last 2 digits as the id of the crazyflie. + uint64_t address = configblockGetRadioAddress(); + uint8_t my_id = (uint8_t)((address)&0x00000000ff); + return my_id; +} + +void EnableDTRProtocol(void){ + DTR_DEBUG_PRINT("Initializing queues ...\n"); + queueing_init(); + + DTR_DEBUG_PRINT("Initializing token ring ...\n"); + initTokenRing(NETWORK_SIZE, my_id); + + DTR_DEBUG_PRINT("Initializing DTR Sender ...\n"); + initDTRSenderTimer(); + + DTR_DEBUG_PRINT("Starting protocol timer ...\n"); + startDTRProtocol(); +} + + +LOG_GROUP_START(DTR_P2P) + LOG_ADD(LOG_UINT8, rx_state, &rx_state) + LOG_ADD(LOG_UINT8, tx_state, &tx_state) +LOG_GROUP_STOP(DTR_P2P) From 1b32b153dd94d927b5195b2bab156709809ccec9 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Thu, 18 Aug 2022 15:19:43 +0200 Subject: [PATCH 02/19] Moved DTR protocol into modules folder --- .vscode/settings.json | 1 + Makefile | 4 ++-- examples/app_p2p_DTR/src/p2p_DTR_app.c | 2 +- src/{utils => modules}/interface/p2pDTR/DTR_timers.h | 0 src/{utils => modules}/interface/p2pDTR/DTR_types.h | 0 src/{utils => modules}/interface/p2pDTR/p2p_interface.h | 0 src/{utils => modules}/interface/p2pDTR/queueing.h | 0 src/{utils => modules}/interface/p2pDTR/token_ring.h | 0 src/modules/src/Kbuild | 4 ++++ src/{utils => modules}/src/p2pDTR/DTR_timers.c | 0 src/{utils => modules}/src/p2pDTR/p2p_interface.c | 0 src/{utils => modules}/src/p2pDTR/queueing.c | 0 src/{utils => modules}/src/p2pDTR/token_ring.c | 0 src/utils/src/Kbuild | 5 ----- 14 files changed, 8 insertions(+), 8 deletions(-) rename src/{utils => modules}/interface/p2pDTR/DTR_timers.h (100%) rename src/{utils => modules}/interface/p2pDTR/DTR_types.h (100%) rename src/{utils => modules}/interface/p2pDTR/p2p_interface.h (100%) rename src/{utils => modules}/interface/p2pDTR/queueing.h (100%) rename src/{utils => modules}/interface/p2pDTR/token_ring.h (100%) rename src/{utils => modules}/src/p2pDTR/DTR_timers.c (100%) rename src/{utils => modules}/src/p2pDTR/p2p_interface.c (100%) rename src/{utils => modules}/src/p2pDTR/queueing.c (100%) rename src/{utils => modules}/src/p2pDTR/token_ring.c (100%) diff --git a/.vscode/settings.json b/.vscode/settings.json index fb62bf8110..c29eeee3c3 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -11,6 +11,7 @@ }, "cSpell.words": [ "Bitcraze", + "pcallback", "Zosimidis" ] } diff --git a/Makefile b/Makefile index 1d84659fe6..d3a501639a 100644 --- a/Makefile +++ b/Makefile @@ -51,8 +51,8 @@ INCLUDES += -I$(srctree)/src/deck/interface -I$(srctree)/src/deck/drivers/interf INCLUDES += -I$(srctree)/src/drivers/interface -I$(srctree)/src/drivers/bosch/interface INCLUDES += -I$(srctree)/src/drivers/esp32/interface INCLUDES += -I$(srctree)/src/hal/interface -INCLUDES += -I$(srctree)/src/modules/interface -I$(srctree)/src/modules/interface/kalman_core -I$(srctree)/src/modules/interface/lighthouse -I$(srctree)/src/modules/interface/cpx -INCLUDES += -I$(srctree)/src/utils/interface -I$(srctree)/src/utils/interface/kve -I$(srctree)/src/utils/interface/lighthouse -I$(srctree)/src/utils/interface/tdoa -I$(srctree)/src/utils/interface/p2pDTR +INCLUDES += -I$(srctree)/src/modules/interface -I$(srctree)/src/modules/interface/kalman_core -I$(srctree)/src/modules/interface/lighthouse -I$(srctree)/src/modules/interface/cpx -I$(srctree)/src/modules/interface/p2pDTR +INCLUDES += -I$(srctree)/src/utils/interface -I$(srctree)/src/utils/interface/kve -I$(srctree)/src/utils/interface/lighthouse -I$(srctree)/src/utils/interface/tdoa INCLUDES += -I$(LIB)/FatFS INCLUDES += -I$(LIB)/CMSIS/STM32F4xx/Include INCLUDES += -I$(LIB)/STM32_USB_Device_Library/Core/inc diff --git a/examples/app_p2p_DTR/src/p2p_DTR_app.c b/examples/app_p2p_DTR/src/p2p_DTR_app.c index e9d3cf373b..dd8673aeed 100644 --- a/examples/app_p2p_DTR/src/p2p_DTR_app.c +++ b/examples/app_p2p_DTR/src/p2p_DTR_app.c @@ -22,7 +22,7 @@ * along with this program. If not, see . * * - * peer_to_peer.c - App layer application of simple demonstartion peer to peer + * peer_to_peer.c - App layer application of simple demonstration peer to peer * communication. Two crazyflies need this program in order to send and receive. */ diff --git a/src/utils/interface/p2pDTR/DTR_timers.h b/src/modules/interface/p2pDTR/DTR_timers.h similarity index 100% rename from src/utils/interface/p2pDTR/DTR_timers.h rename to src/modules/interface/p2pDTR/DTR_timers.h diff --git a/src/utils/interface/p2pDTR/DTR_types.h b/src/modules/interface/p2pDTR/DTR_types.h similarity index 100% rename from src/utils/interface/p2pDTR/DTR_types.h rename to src/modules/interface/p2pDTR/DTR_types.h diff --git a/src/utils/interface/p2pDTR/p2p_interface.h b/src/modules/interface/p2pDTR/p2p_interface.h similarity index 100% rename from src/utils/interface/p2pDTR/p2p_interface.h rename to src/modules/interface/p2pDTR/p2p_interface.h diff --git a/src/utils/interface/p2pDTR/queueing.h b/src/modules/interface/p2pDTR/queueing.h similarity index 100% rename from src/utils/interface/p2pDTR/queueing.h rename to src/modules/interface/p2pDTR/queueing.h diff --git a/src/utils/interface/p2pDTR/token_ring.h b/src/modules/interface/p2pDTR/token_ring.h similarity index 100% rename from src/utils/interface/p2pDTR/token_ring.h rename to src/modules/interface/p2pDTR/token_ring.h diff --git a/src/modules/src/Kbuild b/src/modules/src/Kbuild index 6834e23c02..62c38451f1 100644 --- a/src/modules/src/Kbuild +++ b/src/modules/src/Kbuild @@ -62,6 +62,10 @@ obj-$(CONFIG_ENABLE_CPX_ON_UART2) += cpx/cpx_uart_transport.o obj-$(CONFIG_ENABLE_CPX) += cpx/cpxlink.o obj-$(CONFIG_ENABLE_CPX) += cpx/cpx.o +obj-y += p2pDTR/DTR_timers.o +obj-y += p2pDTR/p2p_interface.o +obj-y += p2pDTR/queueing.o +obj-y += p2pDTR/token_ring.o # Kalman core obj-$(CONFIG_ESTIMATOR_KALMAN_ENABLE) += kalman_core/ diff --git a/src/utils/src/p2pDTR/DTR_timers.c b/src/modules/src/p2pDTR/DTR_timers.c similarity index 100% rename from src/utils/src/p2pDTR/DTR_timers.c rename to src/modules/src/p2pDTR/DTR_timers.c diff --git a/src/utils/src/p2pDTR/p2p_interface.c b/src/modules/src/p2pDTR/p2p_interface.c similarity index 100% rename from src/utils/src/p2pDTR/p2p_interface.c rename to src/modules/src/p2pDTR/p2p_interface.c diff --git a/src/utils/src/p2pDTR/queueing.c b/src/modules/src/p2pDTR/queueing.c similarity index 100% rename from src/utils/src/p2pDTR/queueing.c rename to src/modules/src/p2pDTR/queueing.c diff --git a/src/utils/src/p2pDTR/token_ring.c b/src/modules/src/p2pDTR/token_ring.c similarity index 100% rename from src/utils/src/p2pDTR/token_ring.c rename to src/modules/src/p2pDTR/token_ring.c diff --git a/src/utils/src/Kbuild b/src/utils/src/Kbuild index 63d29118c1..bda1fea08c 100644 --- a/src/utils/src/Kbuild +++ b/src/utils/src/Kbuild @@ -33,11 +33,6 @@ obj-$(CONFIG_DECK_LIGHTHOUSE) += lighthouse/pulse_processor.o obj-$(CONFIG_DECK_LIGHTHOUSE) += lighthouse/pulse_processor_v1.o obj-$(CONFIG_DECK_LIGHTHOUSE) += lighthouse/pulse_processor_v2.o -obj-y += p2pDTR/DTR_timers.o -obj-y += p2pDTR/p2p_interface.o -obj-y += p2pDTR/queueing.o -obj-y += p2pDTR/token_ring.o - obj-y += num.o obj-y += rateSupervisor.o obj-y += sleepus.o From 7e905db985e62d60b926d8d42847b7dd104b0847 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Thu, 18 Aug 2022 16:57:54 +0200 Subject: [PATCH 03/19] DTR protocol special P2P port --- examples/app_p2p_DTR/src/p2p_DTR_app.c | 15 +++++++++++---- src/modules/interface/p2pDTR/p2p_interface.h | 2 +- src/modules/interface/p2pDTR/token_ring.h | 2 +- src/modules/src/p2pDTR/p2p_interface.c | 7 +++++-- 4 files changed, 18 insertions(+), 8 deletions(-) diff --git a/examples/app_p2p_DTR/src/p2p_DTR_app.c b/examples/app_p2p_DTR/src/p2p_DTR_app.c index dd8673aeed..d0d8c0847c 100644 --- a/examples/app_p2p_DTR/src/p2p_DTR_app.c +++ b/examples/app_p2p_DTR/src/p2p_DTR_app.c @@ -46,6 +46,7 @@ #include "token_ring.h" #include "p2p_interface.h" +#define INTERESTING_DATA 104 static uint8_t my_id; @@ -75,13 +76,21 @@ void loadTXPacketsForTesting(void){ } } +void p2pcallbackHandler(P2PPacket *p){ + // DEBUG_PRINT("P2P callback at port: %d\n", p->port); + // If the packet is a DTR service packet, then the handler will handle it. + DTRp2pIncomingHandler(p); + + // Then write your own code below ... +} + void appMain(){ my_id = get_self_id(); EnableDTRProtocol(); vTaskDelay(2000); // Register the callback function so that the CF can receive packets as well. - p2pRegisterCB(DTRp2pcallbackHandler); + p2pRegisterCB(p2pcallbackHandler); if (my_id == 0){ DTR_DEBUG_PRINT("Starting communication...\n"); @@ -97,7 +106,7 @@ void appMain(){ DEBUG_PRINT("Received data from %d : %d --> Time elapsed: %lu msec\n",received_packet.source_id, received_packet.data[0],dt); start = T2M(xTaskGetTickCount()); - if (my_id == 1 && received_packet.data[0] == 104){ + if (my_id == 1 && received_packet.data[0] == INTERESTING_DATA){ received_packet.source_id = my_id; received_packet.data[0] = 123; DEBUG_PRINT("Sending response...\n"); @@ -105,5 +114,3 @@ void appMain(){ } } } - - diff --git a/src/modules/interface/p2pDTR/p2p_interface.h b/src/modules/interface/p2pDTR/p2p_interface.h index 09bf170eaf..a488df206b 100644 --- a/src/modules/interface/p2pDTR/p2p_interface.h +++ b/src/modules/interface/p2pDTR/p2p_interface.h @@ -40,7 +40,7 @@ void sendDTRpacket(const DTRpacket* packet) ; // Puts the DTR packet in the queue for the token ring to pick up. -void DTRp2pcallbackHandler(P2PPacket *p); +void DTRp2pIncomingHandler(P2PPacket *p); void feedDTRPacketToProtocol(DTRpacket *incoming_DTR); diff --git a/src/modules/interface/p2pDTR/token_ring.h b/src/modules/interface/p2pDTR/token_ring.h index e07f29b35a..857a56bb2b 100644 --- a/src/modules/interface/p2pDTR/token_ring.h +++ b/src/modules/interface/p2pDTR/token_ring.h @@ -64,7 +64,7 @@ #define NETWORK_SIZE 4 #define PROTOCOL_TIMEOUT_MS 4 * 1000.0f // ms - +#define DTR_P2P_PORT 15 // between 0 and 15(4 bits) void initRadio(uint8_t networkSize, uint8_t device_id); diff --git a/src/modules/src/p2pDTR/p2p_interface.c b/src/modules/src/p2pDTR/p2p_interface.c index 00bc4ebec3..e6fa4481d3 100644 --- a/src/modules/src/p2pDTR/p2p_interface.c +++ b/src/modules/src/p2pDTR/p2p_interface.c @@ -34,7 +34,7 @@ static P2PPacket p2p_TXpacket; static DTRpacket prev_received = {0}; void sendDTRpacket(const DTRpacket* packet) { - p2p_TXpacket.port=0x00; + p2p_TXpacket.port = DTR_P2P_PORT; memcpy(&p2p_TXpacket.data[0], packet, packet->packetSize); p2p_TXpacket.size = packet->packetSize; @@ -42,7 +42,10 @@ void sendDTRpacket(const DTRpacket* packet) { radiolinkSendP2PPacketBroadcast(&p2p_TXpacket); } -void DTRp2pcallbackHandler(P2PPacket *p){ +void DTRp2pIncomingHandler(P2PPacket *p){ + if (p->port != DTR_P2P_PORT ){ + return; + } DTRpacket incoming_DTR; From b3df693b2f0980de5e9f25f28d8214ebe05ea6bf Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Fri, 19 Aug 2022 10:11:05 +0200 Subject: [PATCH 04/19] Renaming queueing functions and disabling DTR task --- examples/app_p2p_DTR/src/p2p_DTR_app.c | 6 ++-- src/modules/interface/p2pDTR/DTR_timers.h | 4 ++- src/modules/interface/p2pDTR/queueing.h | 18 +++++------ src/modules/interface/p2pDTR/token_ring.h | 2 ++ src/modules/src/p2pDTR/DTR_timers.c | 10 ++++-- src/modules/src/p2pDTR/p2p_interface.c | 6 ++-- src/modules/src/p2pDTR/queueing.c | 26 ++++++++-------- src/modules/src/p2pDTR/token_ring.c | 38 ++++++++++++++++------- 8 files changed, 66 insertions(+), 44 deletions(-) diff --git a/examples/app_p2p_DTR/src/p2p_DTR_app.c b/examples/app_p2p_DTR/src/p2p_DTR_app.c index d0d8c0847c..6bde80bf27 100644 --- a/examples/app_p2p_DTR/src/p2p_DTR_app.c +++ b/examples/app_p2p_DTR/src/p2p_DTR_app.c @@ -66,7 +66,7 @@ void loadTXPacketsForTesting(void){ bool res; for (int i = 0; i < TX_DATA_QUEUE_SIZE - 1; i++){ testSignal.data[0] = 100+i; - res = insertPacketToQueue(&testSignal,TX_DATA_Q); + res = insertDTRPacketToQueue(&testSignal,TX_DATA_Q); if (res){ DTR_DEBUG_PRINT("TX Packet sent to TX_DATA Q\n"); } @@ -101,7 +101,7 @@ void appMain(){ DTRpacket received_packet; uint32_t start = T2M(xTaskGetTickCount()); while(1){ - getPacketFromQueue(&received_packet, RX_DATA_Q, portMAX_DELAY); + getDTRPacketFromQueue(&received_packet, RX_DATA_Q, portMAX_DELAY); uint32_t dt = T2M(xTaskGetTickCount()) - start; DEBUG_PRINT("Received data from %d : %d --> Time elapsed: %lu msec\n",received_packet.source_id, received_packet.data[0],dt); start = T2M(xTaskGetTickCount()); @@ -110,7 +110,7 @@ void appMain(){ received_packet.source_id = my_id; received_packet.data[0] = 123; DEBUG_PRINT("Sending response...\n"); - insertPacketToQueue(&received_packet,TX_DATA_Q); + insertDTRPacketToQueue(&received_packet,TX_DATA_Q); } } } diff --git a/src/modules/interface/p2pDTR/DTR_timers.h b/src/modules/interface/p2pDTR/DTR_timers.h index 09ea55fe57..e85a4b0f68 100644 --- a/src/modules/interface/p2pDTR/DTR_timers.h +++ b/src/modules/interface/p2pDTR/DTR_timers.h @@ -53,7 +53,9 @@ #define DTR_PROTOCOL_TASK_PRIORITY 1 //Higher number higher priority // DTR protocol Task -void startDTRProtocol(void); +void startDTRProtocolTask(void); + +void stopDTRProtocolTask(void); // ================ DTR sender timer ================== void initDTRSenderTimer(void); diff --git a/src/modules/interface/p2pDTR/queueing.h b/src/modules/interface/p2pDTR/queueing.h index 7744ba2f29..70df6da75e 100644 --- a/src/modules/interface/p2pDTR/queueing.h +++ b/src/modules/interface/p2pDTR/queueing.h @@ -56,25 +56,25 @@ typedef enum queue_names_e { RX_DATA_Q, } DTRQueue_Names; -void queueing_init(); +void DTRqueueingInit(); -bool isPacketInQueueAvailable(DTRQueue_Names qName); +bool isDTRPacketInQueueAvailable(DTRQueue_Names qName); -uint8_t getNumberOfPacketsInQueue(DTRQueue_Names qName); +uint8_t getNumberOfDTRPacketsInQueue(DTRQueue_Names qName); -bool getPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout); +bool getDTRPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout); // Blocks to wait for a packet to be received for a given time // new_packet_received --> True if a new packet has been received and False if the timeout has been reached -bool receivePacketWaitUntil(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received); +bool receiveDTRPacketWaitUntil(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received); -bool insertPacketToQueue(DTRpacket *packet, DTRQueue_Names qName); +bool insertDTRPacketToQueue(DTRpacket *packet, DTRQueue_Names qName); -bool releasePacketFromQueue(DTRQueue_Names qName); +bool releaseDTRPacketFromQueue(DTRQueue_Names qName); -void emptyQueue(DTRQueue_Names qName); +void emptyDTRQueue(DTRQueue_Names qName); -void emptyQueues(void); +void emptyDTRQueues(void); #endif /* _QUEUEING_H_ */ diff --git a/src/modules/interface/p2pDTR/token_ring.h b/src/modules/interface/p2pDTR/token_ring.h index 857a56bb2b..5e5d92ac7d 100644 --- a/src/modules/interface/p2pDTR/token_ring.h +++ b/src/modules/interface/p2pDTR/token_ring.h @@ -97,4 +97,6 @@ uint8_t get_self_id(void); // Starts the task of the Dynamic Token Ring Protocol (DTR) and initializes the protocol void EnableDTRProtocol(void); +void DisableDTRProtocol(void); + #endif /* SRC_RADIO_RADIO_H_ */ diff --git a/src/modules/src/p2pDTR/DTR_timers.c b/src/modules/src/p2pDTR/DTR_timers.c index 8444959c69..f1720a73f1 100644 --- a/src/modules/src/p2pDTR/DTR_timers.c +++ b/src/modules/src/p2pDTR/DTR_timers.c @@ -35,14 +35,18 @@ #include "DTR_timers.h" static xTimerHandle sender_timer; -// static xTimerHandle protocol_timer; +static TaskHandle_t DTRtaskHandler = NULL; static bool sender_timer_running = false; static char type_to_spam[15]; -void startDTRProtocol(void){ - xTaskCreate(DTRInterruptHandler, "DTR_P2P", DTR_PROTOCOL_TASK_STACK_SIZE, NULL,DTR_PROTOCOL_TASK_PRIORITY, NULL); +void startDTRProtocolTask(void){ + xTaskCreate(DTRInterruptHandler, "DTR_P2P", DTR_PROTOCOL_TASK_STACK_SIZE, NULL,DTR_PROTOCOL_TASK_PRIORITY, &DTRtaskHandler); +} + +void stopDTRProtocolTask(void){ + vTaskDelete(DTRtaskHandler); } void initDTRSenderTimer(void) { diff --git a/src/modules/src/p2pDTR/p2p_interface.c b/src/modules/src/p2pDTR/p2p_interface.c index e6fa4481d3..6a03816dda 100644 --- a/src/modules/src/p2pDTR/p2p_interface.c +++ b/src/modules/src/p2pDTR/p2p_interface.c @@ -63,8 +63,8 @@ void feedDTRPacketToProtocol(DTRpacket *incoming_DTR) { incoming_DTR->source_id == prev_received.source_id; // if there are packets in the queue and the new packet is the same as the previous one, ignore it - DTR_DEBUG_PRINT("Packets in RX_SRV queue: %d\n", getNumberOfPacketsInQueue(RX_SRV_Q) ); - if ( getNumberOfPacketsInQueue(RX_SRV_Q) !=0 && same_packet_received ) { + DTR_DEBUG_PRINT("Packets in RX_SRV queue: %d\n", getNumberOfDTRPacketsInQueue(RX_SRV_Q) ); + if ( getNumberOfDTRPacketsInQueue(RX_SRV_Q) !=0 && same_packet_received ) { DTR_DEBUG_PRINT("Duplicate packet received\n"); DTR_DEBUG_PRINT("Message type: %d\n", incoming_DTR->message_type); DTR_DEBUG_PRINT("Target id: %d\n", incoming_DTR->target_id); @@ -76,5 +76,5 @@ void feedDTRPacketToProtocol(DTRpacket *incoming_DTR) { prev_received.target_id = incoming_DTR->target_id; prev_received.source_id = incoming_DTR->source_id; - insertPacketToQueue(incoming_DTR, RX_SRV_Q); + insertDTRPacketToQueue(incoming_DTR, RX_SRV_Q); } diff --git a/src/modules/src/p2pDTR/queueing.c b/src/modules/src/p2pDTR/queueing.c index 45670831fe..4bee9eacc9 100644 --- a/src/modules/src/p2pDTR/queueing.c +++ b/src/modules/src/p2pDTR/queueing.c @@ -44,7 +44,7 @@ static xQueueHandle RX_SRV_queue; static xQueueHandle RX_DATA_queue; -xQueueHandle *getQueueHandler(DTRQueue_Names qName){ +static xQueueHandle *getQueueHandler(DTRQueue_Names qName){ switch(qName){ case TX_DATA_Q: return &TX_DATA_queue; @@ -61,7 +61,7 @@ xQueueHandle *getQueueHandler(DTRQueue_Names qName){ } -void queueing_init(){ +void DTRqueueingInit(){ // TX SRV queue STATIC_MEM_QUEUE_ALLOC(TX_DATA_queue, TX_DATA_QUEUE_SIZE, sizeof(DTRpacket)); TX_DATA_queue = STATIC_MEM_QUEUE_CREATE(TX_DATA_queue); @@ -79,15 +79,15 @@ void queueing_init(){ } -uint8_t getNumberOfPacketsInQueue(DTRQueue_Names qName){ +uint8_t getNumberOfDTRPacketsInQueue(DTRQueue_Names qName){ return (uint8_t) uxQueueMessagesWaiting(*getQueueHandler(qName)); } -bool isPacketInQueueAvailable(DTRQueue_Names qName) { +bool isDTRPacketInQueueAvailable(DTRQueue_Names qName) { return uxQueueMessagesWaiting(*getQueueHandler(qName)) > 0; } -bool getPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout){ +bool getDTRPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout){ // notice that xQueuePeek is used instead of xQueueReceive, because the packet is not removed from the queue //TODO: make a separate function for this bool received_success; @@ -112,12 +112,12 @@ bool getPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeou return received_success; } -bool receivePacketWaitUntil(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received){ +bool receiveDTRPacketWaitUntil(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received){ *new_packet_received = xQueueReceive(*getQueueHandler(qName), packet, M2T(timeout_ms)) == pdTRUE; return true; } -bool insertPacketToQueue(DTRpacket *packet, DTRQueue_Names qName) { +bool insertDTRPacketToQueue(DTRpacket *packet, DTRQueue_Names qName) { bool res = xQueueSend(*getQueueHandler(qName),(void *) packet, 0) == pdTRUE; if (!res) { DEBUG_PRINT("TX_DATA queue busy\n"); @@ -126,17 +126,17 @@ bool insertPacketToQueue(DTRpacket *packet, DTRQueue_Names qName) { return res; } -bool releasePacketFromQueue(DTRQueue_Names qName) { +bool releaseDTRPacketFromQueue(DTRQueue_Names qName) { DTRpacket packet; return xQueueReceive(*getQueueHandler(qName), &packet, M2T(TX_RECEIVED_WAIT_TIME)) == pdTRUE; } -void emptyQueue(DTRQueue_Names qName) { +void emptyDTRQueue(DTRQueue_Names qName) { xQueueReset(*getQueueHandler(qName)); } -void emptyQueues(void){ - emptyQueue(TX_DATA_Q); - emptyQueue(RX_SRV_Q); - emptyQueue(RX_DATA_Q); +void emptyDTRQueues(void){ + emptyDTRQueue(TX_DATA_Q); + emptyDTRQueue(RX_SRV_Q); + emptyDTRQueue(RX_DATA_Q); } diff --git a/src/modules/src/p2pDTR/token_ring.c b/src/modules/src/p2pDTR/token_ring.c index 0dcc0c520e..3beeff1eb5 100644 --- a/src/modules/src/p2pDTR/token_ring.c +++ b/src/modules/src/p2pDTR/token_ring.c @@ -141,7 +141,7 @@ static void resetProtocol(void){ DTR_DEBUG_PRINT("\nResetting protocol\n"); rx_state = RX_IDLE; protocol_timeout_ms = T2M(xTaskGetTickCount()) + PROTOCOL_TIMEOUT_MS; - emptyQueues(); // Maybe not all queues, but only the RX_SRV since the data are going to be lost saved in DATA queues + emptyDTRQueues(); // Maybe not all queues, but only the RX_SRV since the data are going to be lost saved in DATA queues shutdownDTRSenderTimer(); last_packet_source_id = 255; } @@ -158,7 +158,7 @@ void DTRInterruptHandler(void *param) { bool new_packet_received; DEBUG_PRINT("\nDTRInterruptHandler Task called...\n"); - while ( receivePacketWaitUntil(&_rxPk, RX_SRV_Q, PROTOCOL_TIMEOUT_MS, &new_packet_received) ){ + while ( receiveDTRPacketWaitUntil(&_rxPk, RX_SRV_Q, PROTOCOL_TIMEOUT_MS, &new_packet_received) ){ if (!new_packet_received) { DTR_DEBUG_PRINT("\nPROTOCOL TIMEOUT!\n"); if (my_id != 0){ @@ -174,7 +174,7 @@ void DTRInterruptHandler(void *param) { DTR_DEBUG_PRINT("===============================================================\n"); DTR_DEBUG_PRINT("=\n"); - DTR_DEBUG_PRINT("TX_DATA Q Empty: %d\n", !isPacketInQueueAvailable(TX_DATA_Q)); + DTR_DEBUG_PRINT("TX_DATA Q Empty: %d\n", !isDTRPacketInQueueAvailable(TX_DATA_Q)); DTR_DEBUG_PRINT("rx_state: %s\n", getRXState(rx_state)); @@ -195,7 +195,7 @@ void DTRInterruptHandler(void *param) { last_packet_source_id = rxPk->source_id; /* if packet is relevant and receiver queue is not full, then * push packet in the queue and prepare queue for next packet. */ - bool queueFull = insertPacketToQueue(rxPk, RX_DATA_Q); + bool queueFull = insertDTRPacketToQueue(rxPk, RX_DATA_Q); if (queueFull){ radioMetaInfo.failedRxQueueFull++; } @@ -243,7 +243,7 @@ void DTRInterruptHandler(void *param) { * send it, otherwise forward the token to the next node. */ DTRpacket _txPk;//TODO: bad implementation, should be fixed - if (getPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME))) { + if (getDTRPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME))) { txPk = &_txPk; DTR_DEBUG_PRINT("TX DATA Packet exists (%d), sending it\n",txPk->data[0]); if(txPk->allToAllFlag) { @@ -252,9 +252,9 @@ void DTRInterruptHandler(void *param) { if (txPk->target_id >= MAX_NETWORK_SIZE) { DTR_DEBUG_PRINT("Releasing TX DATA Packet because target > default:\n"); - DTR_DEBUG_PRINT("Is Queue Empty: %d\n", !isPacketInQueueAvailable(TX_DATA_Q)); + DTR_DEBUG_PRINT("Is Queue Empty: %d\n", !isDTRPacketInQueueAvailable(TX_DATA_Q)); - releasePacketFromQueue(TX_DATA_Q); + releaseDTRPacketFromQueue(TX_DATA_Q); txPk = &servicePk; txPk->message_type = TOKEN_FRAME; tx_state = TX_TOKEN; @@ -299,7 +299,7 @@ void DTRInterruptHandler(void *param) { //TODO: bad implementation, should be fixed DTRpacket _txPk; - getPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME)); + getDTRPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME)); txPk = &_txPk; next_target_id = (txPk->target_id + send_data_to_peer_counter + 1) % MAX_NETWORK_SIZE; @@ -309,9 +309,9 @@ void DTRInterruptHandler(void *param) { DTR_DEBUG_PRINT("Releasing TX DATA:\n"); // printDTRPacket(txPk); - DTR_DEBUG_PRINT("Is Q Empty: %d\n", !isPacketInQueueAvailable(TX_DATA_Q)); + DTR_DEBUG_PRINT("Is Q Empty: %d\n", !isDTRPacketInQueueAvailable(TX_DATA_Q)); - releasePacketFromQueue(TX_DATA_Q); + releaseDTRPacketFromQueue(TX_DATA_Q); txPk = &servicePk; txPk->message_type = TOKEN_FRAME; tx_state = TX_TOKEN; @@ -490,7 +490,7 @@ uint8_t get_self_id(void){ void EnableDTRProtocol(void){ DTR_DEBUG_PRINT("Initializing queues ...\n"); - queueing_init(); + DTRqueueingInit(); DTR_DEBUG_PRINT("Initializing token ring ...\n"); initTokenRing(NETWORK_SIZE, my_id); @@ -499,7 +499,21 @@ void EnableDTRProtocol(void){ initDTRSenderTimer(); DTR_DEBUG_PRINT("Starting protocol timer ...\n"); - startDTRProtocol(); + startDTRProtocolTask(); +} + +void DisableDTRProtocol(void){ + DTR_DEBUG_PRINT("Stopping protocol timer ...\n"); + stopDTRProtocolTask(); + + DTR_DEBUG_PRINT("Stopping DTR Sender ...\n"); + shutdownDTRSenderTimer(); + + DTR_DEBUG_PRINT("Resetting token ring ...\n"); + resetProtocol(); + + DTR_DEBUG_PRINT("Stopping queues ...\n"); + emptyDTRQueues(); } From d2512169dbc3397d00294c67f8a7f9849a290797 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Fri, 19 Aug 2022 13:39:03 +0200 Subject: [PATCH 05/19] User defined network topology --- examples/app_p2p_DTR/src/p2p_DTR_app.c | 12 +++- src/modules/interface/p2pDTR/DTR_types.h | 7 +- src/modules/interface/p2pDTR/token_ring.h | 12 ++-- src/modules/src/p2pDTR/token_ring.c | 87 +++++++++++++++++------ 4 files changed, 86 insertions(+), 32 deletions(-) diff --git a/examples/app_p2p_DTR/src/p2p_DTR_app.c b/examples/app_p2p_DTR/src/p2p_DTR_app.c index 6bde80bf27..029c72bdc0 100644 --- a/examples/app_p2p_DTR/src/p2p_DTR_app.c +++ b/examples/app_p2p_DTR/src/p2p_DTR_app.c @@ -48,7 +48,11 @@ #define INTERESTING_DATA 104 +// define the ids of each node in the network +#define NETWORK_TOPOLOGY {.size = 2, .devices_ids = {1, 0, 2, 3} } // Maximum size of the network is 20 by default + static uint8_t my_id; +static DTRtopology topology = NETWORK_TOPOLOGY; void loadTXPacketsForTesting(void){ DTRpacket testSignal; @@ -86,7 +90,13 @@ void p2pcallbackHandler(P2PPacket *p){ void appMain(){ my_id = get_self_id(); - EnableDTRProtocol(); + DEBUG_PRINT("Network Topology: %d", topology.size); + for (int i = 0; i < topology.size; i++){ + DEBUG_PRINT("%d ", topology.devices_ids[i]); + } + DEBUG_PRINT("\n"); + + EnableDTRProtocol(topology); vTaskDelay(2000); // Register the callback function so that the CF can receive packets as well. diff --git a/src/modules/interface/p2pDTR/DTR_types.h b/src/modules/interface/p2pDTR/DTR_types.h index ec4c914548..38d0ee1a90 100644 --- a/src/modules/interface/p2pDTR/DTR_types.h +++ b/src/modules/interface/p2pDTR/DTR_types.h @@ -39,7 +39,7 @@ #include "stdbool.h" #include "radiolink.h" - +#define MAX_NETWORK_SIZE 20 #define DTR_PACKET_HEADER_SIZE 6 // max usable size for a packet is -1 byte for the port @@ -111,4 +111,9 @@ typedef struct radio_meta_info { } RadioInfo; +typedef struct DTR_network_topology { + uint8_t size; // number of nodes in the network + uint8_t devices_ids[MAX_NETWORK_SIZE]; // array that contains the device id of each node in the network +} DTRtopology; + #endif //DTR_TYPES_H diff --git a/src/modules/interface/p2pDTR/token_ring.h b/src/modules/interface/p2pDTR/token_ring.h index 5e5d92ac7d..9733468902 100644 --- a/src/modules/interface/p2pDTR/token_ring.h +++ b/src/modules/interface/p2pDTR/token_ring.h @@ -52,7 +52,8 @@ // #define UNUSED(...) ((void)sizeof((_Bool[]){__VA_ARGS__})); //Used to silence compiler warnings about unused parameters #define UNUSED(...) ; //Used to silence compiler warnings about unused parameters -// #define DEBUG_DTR_PROTOCOL // Enable debug prints for the DTR protocol (WARNING: May cause problems ) +// Enable debug prints for the DTR protocol (WARNING: May cause problems ) +// #define DEBUG_DTR_PROTOCOL #ifdef DEBUG_DTR_PROTOCOL #define DTR_DEBUG_PRINT(fmt, ... ) DEBUG_PRINT(DEBUG_FMT(fmt), ##__VA_ARGS__) @@ -62,15 +63,10 @@ #endif -#define NETWORK_SIZE 4 #define PROTOCOL_TIMEOUT_MS 4 * 1000.0f // ms #define DTR_P2P_PORT 15 // between 0 and 15(4 bits) -void initRadio(uint8_t networkSize, uint8_t device_id); - -void initTokenRing(uint8_t networkSize, uint8_t device_id); - -void setDeviceRadioAddress (uint8_t radio_address); +void initTokenRing(DTRtopology topology, uint8_t device_id); uint8_t getDeviceRadioAddress(); @@ -95,7 +91,7 @@ const char* getTXState(uint8_t tx_state); uint8_t get_self_id(void); // Starts the task of the Dynamic Token Ring Protocol (DTR) and initializes the protocol -void EnableDTRProtocol(void); +void EnableDTRProtocol(DTRtopology topology); void DisableDTRProtocol(void); diff --git a/src/modules/src/p2pDTR/token_ring.c b/src/modules/src/p2pDTR/token_ring.c index 3beeff1eb5..5341fea664 100644 --- a/src/modules/src/p2pDTR/token_ring.c +++ b/src/modules/src/p2pDTR/token_ring.c @@ -37,7 +37,6 @@ #include "debug.h" -static uint8_t MAX_NETWORK_SIZE = 0; static uint8_t node_id = 0; static uint8_t next_node_id = 0; static uint8_t prev_node_id = 0; @@ -62,20 +61,62 @@ static uint32_t protocol_timeout_ms; static uint8_t my_id ; -static void setNodeIds(uint8_t networkSize, uint8_t device_id) { - MAX_NETWORK_SIZE = networkSize; +static DTRtopology networkTopology; + +static uint8_t getIndexInTopology(uint8_t id){ + for(uint8_t i = 0; i < networkTopology.size; i++){ + if(networkTopology.devices_ids[i] == id){ + return i; + } + } + return 255; +} + +static uint8_t getNextNodeId(uint8_t id){ + uint8_t index = getIndexInTopology(id); + bool index_is_last = index == networkTopology.size - 1; + uint8_t next_index = index_is_last ? 0 : index + 1; + + return networkTopology.devices_ids[next_index]; +} + +static uint8_t getPrevNodeId(uint8_t id){ + uint8_t index = getIndexInTopology(id); + + bool index_is_first = index == 0; + uint8_t prev_index = index_is_first ? networkTopology.size - 1 : index - 1; + return networkTopology.devices_ids[prev_index]; +} + +static bool IdExistsInTopology(uint8_t id){ + for(uint8_t i = 0; i < networkTopology.size; i++){ + if(networkTopology.devices_ids[i] == id){ + return true; + } + } + return false; +} + +static void setNodeIds(DTRtopology topology, uint8_t device_id) { + networkTopology = topology; node_id = device_id; + servicePk.source_id = node_id; last_packet_source_id = node_id; - next_node_id = (node_id + 1) % MAX_NETWORK_SIZE; - prev_node_id = (node_id + (MAX_NETWORK_SIZE - 1)) % MAX_NETWORK_SIZE; + + next_node_id = getNextNodeId(node_id); + prev_node_id = getPrevNodeId(node_id); + + DTR_DEBUG_PRINT("self node_id: %d\n", node_id); + DTR_DEBUG_PRINT("next node_id: %d\n", next_node_id); + DTR_DEBUG_PRINT("prev node_id: %d\n", prev_node_id); } -void initTokenRing(uint8_t networkSize, uint8_t device_id) { +void initTokenRing(DTRtopology topology, uint8_t device_id) { my_id = get_self_id(); /* Network node configuration*/ - setNodeIds(networkSize, my_id); + setNodeIds(topology, my_id); rx_state = RX_IDLE; } @@ -146,7 +187,8 @@ static void resetProtocol(void){ last_packet_source_id = 255; } -static uint8_t send_data_to_peer_counter = 0; +// static uint8_t send_data_to_peer_counter = 0; +static uint8_t next_sender_id = 255; void DTRInterruptHandler(void *param) { @@ -249,10 +291,10 @@ void DTRInterruptHandler(void *param) { if(txPk->allToAllFlag) { txPk->target_id = next_node_id; } - if (txPk->target_id >= MAX_NETWORK_SIZE) { - DTR_DEBUG_PRINT("Releasing TX DATA Packet because target > default:\n"); + if (!IdExistsInTopology(txPk->target_id)) { + DEBUG_PRINT("Releasing TX DATA Packet because target > default:\n"); - DTR_DEBUG_PRINT("Is Queue Empty: %d\n", !isDTRPacketInQueueAvailable(TX_DATA_Q)); + DEBUG_PRINT("Is Queue Empty: %d\n", !isDTRPacketInQueueAvailable(TX_DATA_Q)); releaseDTRPacketFromQueue(TX_DATA_Q); txPk = &servicePk; @@ -302,9 +344,14 @@ void DTRInterruptHandler(void *param) { getDTRPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME)); txPk = &_txPk; - next_target_id = (txPk->target_id + send_data_to_peer_counter + 1) % MAX_NETWORK_SIZE; - DTR_DEBUG_PRINT("next_target_id: %d\n", next_target_id); + if (next_sender_id == 255){ + next_target_id = getNextNodeId(node_id); + } + else { + next_target_id = getNextNodeId(next_sender_id); + } + DTR_DEBUG_PRINT("next_target_id: %d\n", next_target_id); if (!txPk->allToAllFlag || next_target_id == node_id) { DTR_DEBUG_PRINT("Releasing TX DATA:\n"); // printDTRPacket(txPk); @@ -315,10 +362,10 @@ void DTRInterruptHandler(void *param) { txPk = &servicePk; txPk->message_type = TOKEN_FRAME; tx_state = TX_TOKEN; - send_data_to_peer_counter = 0; + next_sender_id = 255; } else { txPk->target_id = next_target_id; - send_data_to_peer_counter++; + next_sender_id = next_target_id; DTR_DEBUG_PRINT("Sending DATA to next peer.."); DTR_DEBUG_PRINT("with target id: %d\n", txPk->target_id); tx_state = TX_DATA_FRAME; @@ -348,10 +395,6 @@ void DTRInterruptHandler(void *param) { } -void setDeviceRadioAddress(uint8_t radio_address) { - node_id = radio_address; -} - uint8_t getDeviceRadioAddress() { return node_id; } @@ -400,7 +443,7 @@ void startRadioCommunication() { startSignal->source_id = node_id; startSignal->target_id = next_node_id; - startSignal->data[0] = 69; + startSignal->data[0] = 111; startSignal->dataSize = 1; startSignal->packetSize = DTR_PACKET_HEADER_SIZE + startSignal->dataSize; startSignal->message_type = DATA_FRAME; @@ -488,12 +531,12 @@ uint8_t get_self_id(void){ return my_id; } -void EnableDTRProtocol(void){ +void EnableDTRProtocol(DTRtopology topology){ DTR_DEBUG_PRINT("Initializing queues ...\n"); DTRqueueingInit(); DTR_DEBUG_PRINT("Initializing token ring ...\n"); - initTokenRing(NETWORK_SIZE, my_id); + initTokenRing(topology, my_id); DTR_DEBUG_PRINT("Initializing DTR Sender ...\n"); initDTRSenderTimer(); From 1bc1d45273236d7160550ed5eb5cf59eed93bd3a Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Fri, 19 Aug 2022 13:53:49 +0200 Subject: [PATCH 06/19] Deciding which node initiates the DTR P2P protocol --- examples/app_p2p_DTR/src/p2p_DTR_app.c | 2 +- src/modules/src/p2pDTR/token_ring.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/app_p2p_DTR/src/p2p_DTR_app.c b/examples/app_p2p_DTR/src/p2p_DTR_app.c index 029c72bdc0..6f65b8643d 100644 --- a/examples/app_p2p_DTR/src/p2p_DTR_app.c +++ b/examples/app_p2p_DTR/src/p2p_DTR_app.c @@ -102,7 +102,7 @@ void appMain(){ // Register the callback function so that the CF can receive packets as well. p2pRegisterCB(p2pcallbackHandler); - if (my_id == 0){ + if (my_id == networkTopology.devices_ids[0]){ DTR_DEBUG_PRINT("Starting communication...\n"); startRadioCommunication(); loadTXPacketsForTesting(); diff --git a/src/modules/src/p2pDTR/token_ring.c b/src/modules/src/p2pDTR/token_ring.c index 5341fea664..bde0425fef 100644 --- a/src/modules/src/p2pDTR/token_ring.c +++ b/src/modules/src/p2pDTR/token_ring.c @@ -203,7 +203,7 @@ void DTRInterruptHandler(void *param) { while ( receiveDTRPacketWaitUntil(&_rxPk, RX_SRV_Q, PROTOCOL_TIMEOUT_MS, &new_packet_received) ){ if (!new_packet_received) { DTR_DEBUG_PRINT("\nPROTOCOL TIMEOUT!\n"); - if (my_id != 0){ + if (my_id != networkTopology.devices_ids[0]) { resetProtocol(); } From 11fec4e113a893206dc8e171ec8a933738853081 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Fri, 19 Aug 2022 14:24:02 +0200 Subject: [PATCH 07/19] Fixed unsilenced prints in DTR --- examples/app_p2p_DTR/src/p2p_DTR_app.c | 4 ++-- src/modules/src/p2pDTR/DTR_timers.c | 2 +- src/modules/src/p2pDTR/queueing.c | 4 ++-- src/modules/src/p2pDTR/token_ring.c | 7 +++---- 4 files changed, 8 insertions(+), 9 deletions(-) diff --git a/examples/app_p2p_DTR/src/p2p_DTR_app.c b/examples/app_p2p_DTR/src/p2p_DTR_app.c index 6f65b8643d..049d74dc0a 100644 --- a/examples/app_p2p_DTR/src/p2p_DTR_app.c +++ b/examples/app_p2p_DTR/src/p2p_DTR_app.c @@ -49,7 +49,7 @@ #define INTERESTING_DATA 104 // define the ids of each node in the network -#define NETWORK_TOPOLOGY {.size = 2, .devices_ids = {1, 0, 2, 3} } // Maximum size of the network is 20 by default +#define NETWORK_TOPOLOGY {.size = 4, .devices_ids = {1, 0, 2, 3} } // Maximum size of network is 20 by default static uint8_t my_id; static DTRtopology topology = NETWORK_TOPOLOGY; @@ -102,7 +102,7 @@ void appMain(){ // Register the callback function so that the CF can receive packets as well. p2pRegisterCB(p2pcallbackHandler); - if (my_id == networkTopology.devices_ids[0]){ + if (my_id == topology.devices_ids[0]){ DTR_DEBUG_PRINT("Starting communication...\n"); startRadioCommunication(); loadTXPacketsForTesting(); diff --git a/src/modules/src/p2pDTR/DTR_timers.c b/src/modules/src/p2pDTR/DTR_timers.c index f1720a73f1..ac601d6a1f 100644 --- a/src/modules/src/p2pDTR/DTR_timers.c +++ b/src/modules/src/p2pDTR/DTR_timers.c @@ -59,7 +59,7 @@ void shutdownDTRSenderTimer(void) { DTR_DEBUG_PRINT("Stopped spamming messages\n"); sender_timer_running = false; }else{ - DEBUG_PRINT("Radio timer not running\n"); + DTR_DEBUG_PRINT("Radio timer not running\n"); } } diff --git a/src/modules/src/p2pDTR/queueing.c b/src/modules/src/p2pDTR/queueing.c index 4bee9eacc9..0bafa32e03 100644 --- a/src/modules/src/p2pDTR/queueing.c +++ b/src/modules/src/p2pDTR/queueing.c @@ -54,7 +54,7 @@ static xQueueHandle *getQueueHandler(DTRQueue_Names qName){ return &RX_DATA_queue; default: // The q enum is not valid - DEBUG_PRINT("Invalid queue name\n"); + DEBUG_PRINT("Invalid DTR queue name\n"); ASSERT(0); return NULL; } @@ -105,7 +105,7 @@ bool getDTRPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t tim break; default: - DEBUG_PRINT("Invalid queue name\n"); + DEBUG_PRINT("Invalid DTR queue name\n"); received_success = false; break; } diff --git a/src/modules/src/p2pDTR/token_ring.c b/src/modules/src/p2pDTR/token_ring.c index bde0425fef..1f308f371f 100644 --- a/src/modules/src/p2pDTR/token_ring.c +++ b/src/modules/src/p2pDTR/token_ring.c @@ -199,7 +199,7 @@ void DTRInterruptHandler(void *param) { protocol_timeout_ms = T2M(xTaskGetTickCount()) + PROTOCOL_TIMEOUT_MS; bool new_packet_received; - DEBUG_PRINT("\nDTRInterruptHandler Task called...\n"); + DTR_DEBUG_PRINT("\nDTRInterruptHandler Task called...\n"); while ( receiveDTRPacketWaitUntil(&_rxPk, RX_SRV_Q, PROTOCOL_TIMEOUT_MS, &new_packet_received) ){ if (!new_packet_received) { DTR_DEBUG_PRINT("\nPROTOCOL TIMEOUT!\n"); @@ -292,9 +292,8 @@ void DTRInterruptHandler(void *param) { txPk->target_id = next_node_id; } if (!IdExistsInTopology(txPk->target_id)) { - DEBUG_PRINT("Releasing TX DATA Packet because target > default:\n"); - - DEBUG_PRINT("Is Queue Empty: %d\n", !isDTRPacketInQueueAvailable(TX_DATA_Q)); + DEBUG_PRINT("Releasing DTR TX packet,target is not in topology.\n"); + DTR_DEBUG_PRINT("Is Queue Empty: %d\n", !isDTRPacketInQueueAvailable(TX_DATA_Q)); releaseDTRPacketFromQueue(TX_DATA_Q); txPk = &servicePk; From a909453d65028fb18d137518f711f46000fcbcd5 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Fri, 19 Aug 2022 14:33:00 +0200 Subject: [PATCH 08/19] Minor changes --- examples/app_p2p_DTR/src/p2p_DTR_app.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/app_p2p_DTR/src/p2p_DTR_app.c b/examples/app_p2p_DTR/src/p2p_DTR_app.c index 049d74dc0a..3fc9b43d92 100644 --- a/examples/app_p2p_DTR/src/p2p_DTR_app.c +++ b/examples/app_p2p_DTR/src/p2p_DTR_app.c @@ -116,7 +116,7 @@ void appMain(){ DEBUG_PRINT("Received data from %d : %d --> Time elapsed: %lu msec\n",received_packet.source_id, received_packet.data[0],dt); start = T2M(xTaskGetTickCount()); - if (my_id == 1 && received_packet.data[0] == INTERESTING_DATA){ + if (my_id == topology.devices_ids[1] && received_packet.data[0] == INTERESTING_DATA){ received_packet.source_id = my_id; received_packet.data[0] = 123; DEBUG_PRINT("Sending response...\n"); From 6bbe08890d027798ac1c98d462336084c17ce430 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Fri, 19 Aug 2022 14:48:09 +0200 Subject: [PATCH 09/19] Renaming files and fixed debug.h includes --- .../interface/p2pDTR/{DTR_timers.h => DTR_handlers.h} | 8 ++++---- src/modules/interface/p2pDTR/queueing.h | 1 - src/modules/interface/p2pDTR/token_ring.h | 2 +- src/modules/src/Kbuild | 2 +- src/modules/src/p2pDTR/{DTR_timers.c => DTR_handlers.c} | 7 +++++-- src/modules/src/p2pDTR/p2p_interface.c | 3 +++ src/modules/src/p2pDTR/queueing.c | 2 ++ src/utils/interface/debug.h | 1 - src/utils/src/Kbuild | 1 - 9 files changed, 16 insertions(+), 11 deletions(-) rename src/modules/interface/p2pDTR/{DTR_timers.h => DTR_handlers.h} (94%) rename src/modules/src/p2pDTR/{DTR_timers.c => DTR_handlers.c} (96%) diff --git a/src/modules/interface/p2pDTR/DTR_timers.h b/src/modules/interface/p2pDTR/DTR_handlers.h similarity index 94% rename from src/modules/interface/p2pDTR/DTR_timers.h rename to src/modules/interface/p2pDTR/DTR_handlers.h index e85a4b0f68..6e85b01b4e 100644 --- a/src/modules/interface/p2pDTR/DTR_timers.h +++ b/src/modules/interface/p2pDTR/DTR_handlers.h @@ -22,7 +22,7 @@ * SOFTWARE. * * - * DTR_timers.h + * DTR_handlers.h * * Created on: 14.02.2021 * Author: Christos Zosimidis @@ -30,8 +30,8 @@ * Modified for the P2P protocol by: Bitcraze AB * */ -#ifndef SRC_RADIO_DTR_TIMERS_H_ -#define SRC_RADIO_DTR_TIMERS_H_ +#ifndef SRC_RADIO_DTR_HANDLERS_H_ +#define SRC_RADIO_DTR_HANDLERS_H_ #include "FreeRTOS.h" #include "task.h" @@ -64,4 +64,4 @@ void shutdownDTRSenderTimer(void); void startDTRSenderTimer(unsigned int time_out); -#endif /* SRC_RADIO_DTR_TIMERS_H_ */ +#endif /* SRC_RADIO_DTR_HANDLERS_H_ */ diff --git a/src/modules/interface/p2pDTR/queueing.h b/src/modules/interface/p2pDTR/queueing.h index 70df6da75e..8add907d90 100644 --- a/src/modules/interface/p2pDTR/queueing.h +++ b/src/modules/interface/p2pDTR/queueing.h @@ -42,7 +42,6 @@ #include "token_ring.h" -#include "debug.h" #define TX_DATA_QUEUE_SIZE 10 #define RX_SRV_QUEUE_SIZE 20 #define RX_DATA_QUEUE_SIZE 10 diff --git a/src/modules/interface/p2pDTR/token_ring.h b/src/modules/interface/p2pDTR/token_ring.h index 9733468902..eba7d5570b 100644 --- a/src/modules/interface/p2pDTR/token_ring.h +++ b/src/modules/interface/p2pDTR/token_ring.h @@ -45,7 +45,7 @@ #include "DTR_types.h" -#include "DTR_timers.h" +#include "DTR_handlers.h" #include "p2p_interface.h" #include "queueing.h" diff --git a/src/modules/src/Kbuild b/src/modules/src/Kbuild index 62c38451f1..49dc737ab0 100644 --- a/src/modules/src/Kbuild +++ b/src/modules/src/Kbuild @@ -62,7 +62,7 @@ obj-$(CONFIG_ENABLE_CPX_ON_UART2) += cpx/cpx_uart_transport.o obj-$(CONFIG_ENABLE_CPX) += cpx/cpxlink.o obj-$(CONFIG_ENABLE_CPX) += cpx/cpx.o -obj-y += p2pDTR/DTR_timers.o +obj-y += p2pDTR/DTR_handlers.o obj-y += p2pDTR/p2p_interface.o obj-y += p2pDTR/queueing.o obj-y += p2pDTR/token_ring.o diff --git a/src/modules/src/p2pDTR/DTR_timers.c b/src/modules/src/p2pDTR/DTR_handlers.c similarity index 96% rename from src/modules/src/p2pDTR/DTR_timers.c rename to src/modules/src/p2pDTR/DTR_handlers.c index ac601d6a1f..ea02d959d4 100644 --- a/src/modules/src/p2pDTR/DTR_timers.c +++ b/src/modules/src/p2pDTR/DTR_handlers.c @@ -22,7 +22,7 @@ * SOFTWARE. * * - * DTR_timers.c + * DTR_handlers.c * * Created on: 14.02.2021 * Author: Christos Zosimidis @@ -32,7 +32,10 @@ */ -#include "DTR_timers.h" +#include "DTR_handlers.h" + +#define DEBUG_MODULE "DTR_HANDL" +#include "debug.h" static xTimerHandle sender_timer; static TaskHandle_t DTRtaskHandler = NULL; diff --git a/src/modules/src/p2pDTR/p2p_interface.c b/src/modules/src/p2pDTR/p2p_interface.c index 6a03816dda..d95cc18fb9 100644 --- a/src/modules/src/p2pDTR/p2p_interface.c +++ b/src/modules/src/p2pDTR/p2p_interface.c @@ -30,6 +30,9 @@ #include "p2p_interface.h" +#define DEBUG_MODULE "DTR_P2P" +#include "debug.h" + static P2PPacket p2p_TXpacket; static DTRpacket prev_received = {0}; diff --git a/src/modules/src/p2pDTR/queueing.c b/src/modules/src/p2pDTR/queueing.c index 0bafa32e03..ac166cb06a 100644 --- a/src/modules/src/p2pDTR/queueing.c +++ b/src/modules/src/p2pDTR/queueing.c @@ -34,6 +34,8 @@ #include "queueing.h" +#include "debug.h" + // TX SRV packet queue static xQueueHandle TX_DATA_queue; diff --git a/src/utils/interface/debug.h b/src/utils/interface/debug.h index d2ee35b138..f6bb63b136 100644 --- a/src/utils/interface/debug.h +++ b/src/utils/interface/debug.h @@ -23,7 +23,6 @@ * * debug.h - Debugging utility functions */ -#pragma once // TODO: check if there is another way to fix this #include "config.h" #include "console.h" #include "autoconf.h" diff --git a/src/utils/src/Kbuild b/src/utils/src/Kbuild index bda1fea08c..e943b1f01b 100644 --- a/src/utils/src/Kbuild +++ b/src/utils/src/Kbuild @@ -32,7 +32,6 @@ obj-$(CONFIG_DECK_LIGHTHOUSE) += lighthouse/ootx_decoder.o obj-$(CONFIG_DECK_LIGHTHOUSE) += lighthouse/pulse_processor.o obj-$(CONFIG_DECK_LIGHTHOUSE) += lighthouse/pulse_processor_v1.o obj-$(CONFIG_DECK_LIGHTHOUSE) += lighthouse/pulse_processor_v2.o - obj-y += num.o obj-y += rateSupervisor.o obj-y += sleepus.o From 3c9b4b404cdccf12e1c889312247d32d7b6f062a Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Fri, 19 Aug 2022 14:51:22 +0200 Subject: [PATCH 10/19] Revert settings.json to old stage --- .vscode/settings.json | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index c29eeee3c3..8620dcdc9d 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -5,13 +5,6 @@ "**/bower_components": true }, "files.associations": { - "bootloader.h": "c", - "token_ring.h": "c", - "p2p_interface.h": "c" - }, - "cSpell.words": [ - "Bitcraze", - "pcallback", - "Zosimidis" - ] -} + "bootloader.h": "c" + } +} \ No newline at end of file From 78b34cf6299759eb592b9c41aa64452afebf4800 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Fri, 19 Aug 2022 17:16:03 +0200 Subject: [PATCH 11/19] DTR Documentation --- docs/functional-areas/p2p_DTR_api.md | 129 ++++++++++++++++++++++ examples/app_p2p_DTR/README.md | 12 +- examples/app_p2p_DTR/src/p2p_DTR_app.c | 10 +- src/modules/interface/p2pDTR/token_ring.h | 17 +++ src/modules/src/p2pDTR/p2p_interface.c | 2 +- src/modules/src/p2pDTR/token_ring.c | 17 +++ 6 files changed, 176 insertions(+), 11 deletions(-) create mode 100644 docs/functional-areas/p2p_DTR_api.md diff --git a/docs/functional-areas/p2p_DTR_api.md b/docs/functional-areas/p2p_DTR_api.md new file mode 100644 index 0000000000..8066ff3e6c --- /dev/null +++ b/docs/functional-areas/p2p_DTR_api.md @@ -0,0 +1,129 @@ +--- +title: Token Ring-P2P API +page_id: DTR_p2p_api +--- + +## Introduction +An extra layer of communication is added to the Crazyflie API to allow for robust and more reliable peer to peer communication. This is done by implementing a token ring protocol on top of the peer to peer API.The Token Ring Protocol is assuming that the nodes are static and the communication is not changing. The protocol is implemented by using a token ring structure. The token ring structure is a circular linked list of nodes. The nodes are connected by a ring of links. The ring is formed by the nodes in the order they are connected. The first node is connected to the last node and the last node is connected to the first node. This protocol is used to ensure that each time only one Crazyflie broadcasts data which leads to less packet collisions and losses. It also provides a way to ensure that the transmitted data will be sent to the other copters since it receives an acknowledgement from each receiver. + +Currently, peer to peer communication on the Crazyflie along with Token Ring Protocol are **in development**. An API is made available to send and receive packets to and from other Crazyflies by letting the protocol automatically handle the lower level interactions with P2P API. The protocol runs as a separate task and utilizes a separate port of the P2P API. Thus the user is given the freedom to use both the P2P API and the DTR protocol at the same time,depending on the use case and the problem to be solved. + +The interface with the Token Ring Protocol is achieved by using 2 queues. One queue is used to send messages to the protocol and the other queue is used to receive messages from the protocol. In that sense ,the execution of the protocol won't block the execution of the rest of the user code and the user can send and receive messages asynchronously. + + +## Using the Token Ring API +Functions and structures are defined in the header files `src/modules/interface/p2pDTR/DTR_types.h` and `src/modules/interface/p2pDTR/token_ring.h`. There is also an app layer example (`/app_p2p_DTR/`) available in the example folder of the repository. + +Each packet has the following structure: + +``` C +typedef struct radio_packet { + uint8_t packetSize; + uint8_t message_type; + uint8_t source_id; + uint8_t target_id; + uint8_t dataSize; + bool allToAllFlag; + uint8_t data[MAXIMUM_DTR_PACKET_DATA_SIZE]; +} DTRpacket; +``` + +Where PacketSize is the size of the packet in bytes, MessageType is the type of the message, SourceID is the ID of the source, TargetID is the ID of the target, DataSize is the size of the data in bytes, AllToAllFlag is a flag indicating if the message is to be sent to all the other Crazyflies or not, and Data is the data of the message.The maximum size of the data is 53 since the data for the P2P is 60 and the protocol occupies 7 of them for operation. + +## Setting up the Token Ring Protocol +Since the nodes od the token ring are static, the user has to define the topology of the network. The topology is defined by the number of nodes and their ids in the order they are connected. The topology is defined like following: + +``` C +#define NETWORK_TOPOLOGY {.size = 4, .devices_ids = {1, 0, 2, 3} } // Maximum size of network is 20 by default +static DTRtopology topology = NETWORK_TOPOLOGY; + +... + +void main() { + ... + // Start the token ring protocol + EnableDTRProtocol(topology); + ... +} +``` + +In the example above, the topology is defined as having 4 nodes and their ids are 1, 0, 2, 3. + +## Feeding incoming packets to the protocol +In order for the protocol to work, the user must feed the protocol with incoming packets. This is done by calling the function `DTRp2pIncomingHandler` which in the interface for receiving P2P packets. As mentioned above the protocol uses the port 15 of the P2P API, so it automatically checks if the packet is coming from the port 15 and if it is, it handles it. Otherwise it is discarded. Though, the user can implement their own code to handle the rest incoming packets if they want to. + +Example usage : +``` C +void p2pcallbackHandler(P2PPacket *p){ + // If the packet is a DTR service packet, then the handler will handle it. + DTRp2pIncomingHandler(p); + + // User code for handling other packets ... +} + +... + + +void main(){ + ... + // Register the callback function to handle incoming packets. + p2pRegisterCallback(p2pcallbackHandler); + ... +} + +``` + + +## Data Broadcast + +To send data through the protocol, the user must call the function `sendPacketToDTR`. + ``` C + bool sendPacketToDTR(DTRpacket* packet) + ``` +This function takes the packet to be sent as a parameter. The packet must be filled wit the data the user wants to send and by defining the size of them. The function returns true if the packet was sent successfully to the DTR (**not to the receiver copter**) and false otherwise.Keep in mind that the packet is sent asynchronously and the user can continue to use the P2P API while the packet is being sent.It is not necessary to fill the `.source_id`, `message_type`, `packet_size` fields of the packet since they are automatically filled by the API function. After the execution of the function, the new packet is inserted in the queue of the protocol responsible for all the packets to be sent. + + + + Example Usage: + ``` C + // Initialize the packet + DTRpacket packet; + + // Fill the packet with the data + packet.dataSize = 3; + packet.data[0] = 0x01; + packet.data[1] = 0x02; + packet.data[2] = 0x03; + packet.allToAllFlag = 1; + sendPacketToDTR(&packet); + + ``` + + +## Receive Data + +The user must call the function `getPacketFromDTR` to receive a packet from the DTR. +``` C +bool getPacketFromDTR(DTRpacket* packet, uint32_t timeout); +``` + +The function blocks for the specified time (in milliseconds) until a packet is received.If the user wants to block indefinitely, the timeout parameter must be set to `portMAX_DELAY`. The function returns true if a packet was received and false otherwise. If a packet is received, the packet is filled with the data received. In case it was received, the packet is filled with the data received and the corresponding packet is released from the queue responsible for the reception of the DTR packets. +Example Usage: +``` C +// Initialize the packet +DTRpacket packet; + +// Receive the packet +while(1){ + getPacketFromDTR(&packet, portMAX_DELAY); + + if(packet.dataSize > 0){ + // Do something with the packet + for (int i = 0; i < packet.dataSize; i++){ + printf("%d ", packet.data[i]); + } + + printf("\n"); + } +} +``` diff --git a/examples/app_p2p_DTR/README.md b/examples/app_p2p_DTR/README.md index 623b5a3e26..ed7c175c20 100644 --- a/examples/app_p2p_DTR/README.md +++ b/examples/app_p2p_DTR/README.md @@ -1,10 +1,12 @@ -# Peer to Peer App for Crazyflie 2.X +# Dynamic Token Ring Protocol for Crazyflie 2.X -This folder contains the app layer application for the Crazyflie to send and receive peer to peer messages. The debug messages of the received messages can be read in the console tab of the [cfclient](https://github.com/bitcraze/crazyflie-clients-python). Two Crazyflies need to be flashed with this program in order to work. Make sure that they are both on the same channel, and that they have different IDs. +This folder contains the app layer application for the Crazyflie to send and receive peer to peer messages while utilising the DTR(Dynamic Token Ring) protocol. This protocol is used to ensure that each time only one Crazyflie broadcasts data which leads to less packet collisions and losses. It also provides a way to ensure that the transmitted data will be sent to the other copters since it receives an acknowledgement from each receiver. -This example is going to blink the M4 LED when packets are sent and received, if you run 2 Crazyflies with this example you should see M4 blinking red and green in sequence indicating bidirectional P2P communication. +The debug messages of the received messages can be read in the console tab of the [cfclient](https://github.com/bitcraze/crazyflie-clients-python). Four Crazyflies with ids 0-3 need to be flashed with this program in order to work. Make sure that they are both on the same channel, and that they have different IDs. -You can find on Bitcraze's website the [API documentation for P2P](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/p2p_api/) as well as the [App layer API guide](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/userguides/app_layer/) +This example is going to be used to send and receive messages from the Crazyflie with ID 0 to all the other Crazyflies with ID 1-3. When ID 1 receives the message with the first byte of data being 104, it will send a reply message with the same data but the first byte being 123. The verification of the received messages can be done by looking at the console tab of the client and the amount of time each broadcast took. + +You can find on Bitcraze's website the [API documentation for Token Ring Protocol](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/DTR_p2p_api/) as well as the [App layer API guide](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/userguides/app_layer/) ## Limitations @@ -12,7 +14,7 @@ Since P2P communication happens asynchronously on the radio, this example does n ## Build -Make sure that you are in the app_peer_to_peer folder (not the main folder of the crazyflie firmware). Then type the following to build and flash it while the crazyflie is put into bootloader mode: +Make sure that you are in the app_p2p_DTR folder (not the main folder of the crazyflie firmware). Then type the following to build and flash it while the crazyflie is put into bootloader mode: ``` make clean diff --git a/examples/app_p2p_DTR/src/p2p_DTR_app.c b/examples/app_p2p_DTR/src/p2p_DTR_app.c index 3fc9b43d92..f362354c76 100644 --- a/examples/app_p2p_DTR/src/p2p_DTR_app.c +++ b/examples/app_p2p_DTR/src/p2p_DTR_app.c @@ -49,7 +49,7 @@ #define INTERESTING_DATA 104 // define the ids of each node in the network -#define NETWORK_TOPOLOGY {.size = 4, .devices_ids = {1, 0, 2, 3} } // Maximum size of network is 20 by default +#define NETWORK_TOPOLOGY {.size = 4, .devices_ids = {0, 1, 2, 3} } // Maximum size of network is 20 by default static uint8_t my_id; static DTRtopology topology = NETWORK_TOPOLOGY; @@ -70,12 +70,12 @@ void loadTXPacketsForTesting(void){ bool res; for (int i = 0; i < TX_DATA_QUEUE_SIZE - 1; i++){ testSignal.data[0] = 100+i; - res = insertDTRPacketToQueue(&testSignal,TX_DATA_Q); + res = sendPacketToDTR(&testSignal); if (res){ - DTR_DEBUG_PRINT("TX Packet sent to TX_DATA Q\n"); + DTR_DEBUG_PRINT("Packet sent to DTR protocol\n"); } else{ - DEBUG_PRINT("Packet not sent to TX_DATA Q\n"); + DEBUG_PRINT("Packet not sent to DTR protocol\n"); } } } @@ -111,7 +111,7 @@ void appMain(){ DTRpacket received_packet; uint32_t start = T2M(xTaskGetTickCount()); while(1){ - getDTRPacketFromQueue(&received_packet, RX_DATA_Q, portMAX_DELAY); + getPacketFromDTR(&received_packet, portMAX_DELAY); uint32_t dt = T2M(xTaskGetTickCount()) - start; DEBUG_PRINT("Received data from %d : %d --> Time elapsed: %lu msec\n",received_packet.source_id, received_packet.data[0],dt); start = T2M(xTaskGetTickCount()); diff --git a/src/modules/interface/p2pDTR/token_ring.h b/src/modules/interface/p2pDTR/token_ring.h index eba7d5570b..39fb519f76 100644 --- a/src/modules/interface/p2pDTR/token_ring.h +++ b/src/modules/interface/p2pDTR/token_ring.h @@ -90,9 +90,26 @@ const char* getTXState(uint8_t tx_state); uint8_t get_self_id(void); + +// =========================== DTR API =========================== + // Starts the task of the Dynamic Token Ring Protocol (DTR) and initializes the protocol +// @param topology The topology of the network (see DTR_types.h) void EnableDTRProtocol(DTRtopology topology); +// Stops the task of the Dynamic Token Ring Protocol (DTR) and deinitializes the protocol void DisableDTRProtocol(void); +// Sends a packet to the DTR protocol +// @param packet The packet to be sent +// @return true if the packet was sent successfully to the DTR (not the final receiver), false otherwise +bool sendPacketToDTR(DTRpacket* packet); + +// Receives a packet from the DTR Protocol +// Blocks for the specified timeout if no packet is received +// @param packet: the packet to be received +// @param timeout: the timeout in milliseconds +// @return true if the packet was received, false otherwise +bool getPacketFromDTR(DTRpacket* packet, uint32_t timeout); + #endif /* SRC_RADIO_RADIO_H_ */ diff --git a/src/modules/src/p2pDTR/p2p_interface.c b/src/modules/src/p2pDTR/p2p_interface.c index d95cc18fb9..468c475cf0 100644 --- a/src/modules/src/p2pDTR/p2p_interface.c +++ b/src/modules/src/p2pDTR/p2p_interface.c @@ -46,7 +46,7 @@ void sendDTRpacket(const DTRpacket* packet) { } void DTRp2pIncomingHandler(P2PPacket *p){ - if (p->port != DTR_P2P_PORT ){ + if (p->port != DTR_P2P_PORT){ return; } diff --git a/src/modules/src/p2pDTR/token_ring.c b/src/modules/src/p2pDTR/token_ring.c index 1f308f371f..3a08df933f 100644 --- a/src/modules/src/p2pDTR/token_ring.c +++ b/src/modules/src/p2pDTR/token_ring.c @@ -559,6 +559,23 @@ void DisableDTRProtocol(void){ } +bool sendPacketToDTR(DTRpacket* packet){ + packet->message_type = DATA_FRAME; + packet->source_id = node_id; + packet->packetSize = DTR_PACKET_HEADER_SIZE + packet->dataSize; + return insertDTRPacketToQueue(packet,TX_DATA_Q); +} + + +bool getPacketFromDTR(DTRpacket* packet, uint32_t timeout){ + if (timeout != portMAX_DELAY){ + timeout = M2T(timeout); + } + + return getDTRPacketFromQueue(packet,RX_DATA_Q,timeout); +} + + LOG_GROUP_START(DTR_P2P) LOG_ADD(LOG_UINT8, rx_state, &rx_state) LOG_ADD(LOG_UINT8, tx_state, &tx_state) From f7389c4152ae21ceb23769f7fe6a1bb3755d03fe Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Mon, 22 Aug 2022 11:53:32 +0200 Subject: [PATCH 12/19] Functions Renaming --- .vscode/settings.json | 2 +- docs/functional-areas/p2p_DTR_api.md | 16 +- examples/app_p2p_DTR/src/p2p_DTR_app.c | 12 +- src/modules/interface/p2pDTR/DTR_handlers.h | 10 +- src/modules/interface/p2pDTR/p2p_interface.h | 4 +- src/modules/interface/p2pDTR/queueing.h | 10 +- src/modules/interface/p2pDTR/token_ring.h | 30 ++- src/modules/src/p2pDTR/DTR_handlers.c | 12 +- src/modules/src/p2pDTR/p2p_interface.c | 12 +- src/modules/src/p2pDTR/queueing.c | 10 +- src/modules/src/p2pDTR/token_ring.c | 181 ++++++++++--------- 11 files changed, 149 insertions(+), 150 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 8620dcdc9d..605386e7e4 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -7,4 +7,4 @@ "files.associations": { "bootloader.h": "c" } -} \ No newline at end of file +} diff --git a/docs/functional-areas/p2p_DTR_api.md b/docs/functional-areas/p2p_DTR_api.md index 8066ff3e6c..f46de5e368 100644 --- a/docs/functional-areas/p2p_DTR_api.md +++ b/docs/functional-areas/p2p_DTR_api.md @@ -34,7 +34,7 @@ Where PacketSize is the size of the packet in bytes, MessageType is the type of Since the nodes od the token ring are static, the user has to define the topology of the network. The topology is defined by the number of nodes and their ids in the order they are connected. The topology is defined like following: ``` C -#define NETWORK_TOPOLOGY {.size = 4, .devices_ids = {1, 0, 2, 3} } // Maximum size of network is 20 by default +#define NETWORK_TOPOLOGY {.size = 4, .devices_ids = {0, 1, 2, 3} } // Maximum size of network is 20 by default static DTRtopology topology = NETWORK_TOPOLOGY; ... @@ -42,7 +42,7 @@ static DTRtopology topology = NETWORK_TOPOLOGY; void main() { ... // Start the token ring protocol - EnableDTRProtocol(topology); + DTRenableProtocol(topology); ... } ``` @@ -76,9 +76,9 @@ void main(){ ## Data Broadcast -To send data through the protocol, the user must call the function `sendPacketToDTR`. +To send data through the protocol, the user must call the function `DTRsendPacket`. ``` C - bool sendPacketToDTR(DTRpacket* packet) + bool DTRsendPacket(DTRpacket* packet) ``` This function takes the packet to be sent as a parameter. The packet must be filled wit the data the user wants to send and by defining the size of them. The function returns true if the packet was sent successfully to the DTR (**not to the receiver copter**) and false otherwise.Keep in mind that the packet is sent asynchronously and the user can continue to use the P2P API while the packet is being sent.It is not necessary to fill the `.source_id`, `message_type`, `packet_size` fields of the packet since they are automatically filled by the API function. After the execution of the function, the new packet is inserted in the queue of the protocol responsible for all the packets to be sent. @@ -95,16 +95,16 @@ This function takes the packet to be sent as a parameter. The packet must be fil packet.data[1] = 0x02; packet.data[2] = 0x03; packet.allToAllFlag = 1; - sendPacketToDTR(&packet); + DTRsendPacket(&packet); ``` ## Receive Data -The user must call the function `getPacketFromDTR` to receive a packet from the DTR. +The user must call the function `DTRgetPacket` to receive a packet from the DTR. ``` C -bool getPacketFromDTR(DTRpacket* packet, uint32_t timeout); +bool DTRgetPacket(DTRpacket* packet, uint32_t timeout); ``` The function blocks for the specified time (in milliseconds) until a packet is received.If the user wants to block indefinitely, the timeout parameter must be set to `portMAX_DELAY`. The function returns true if a packet was received and false otherwise. If a packet is received, the packet is filled with the data received. In case it was received, the packet is filled with the data received and the corresponding packet is released from the queue responsible for the reception of the DTR packets. @@ -115,7 +115,7 @@ DTRpacket packet; // Receive the packet while(1){ - getPacketFromDTR(&packet, portMAX_DELAY); + DTRgetPacket(&packet, portMAX_DELAY); if(packet.dataSize > 0){ // Do something with the packet diff --git a/examples/app_p2p_DTR/src/p2p_DTR_app.c b/examples/app_p2p_DTR/src/p2p_DTR_app.c index f362354c76..a4135fa437 100644 --- a/examples/app_p2p_DTR/src/p2p_DTR_app.c +++ b/examples/app_p2p_DTR/src/p2p_DTR_app.c @@ -70,7 +70,7 @@ void loadTXPacketsForTesting(void){ bool res; for (int i = 0; i < TX_DATA_QUEUE_SIZE - 1; i++){ testSignal.data[0] = 100+i; - res = sendPacketToDTR(&testSignal); + res = DTRsendPacket(&testSignal); if (res){ DTR_DEBUG_PRINT("Packet sent to DTR protocol\n"); } @@ -89,14 +89,14 @@ void p2pcallbackHandler(P2PPacket *p){ } void appMain(){ - my_id = get_self_id(); + my_id = DTRgetSelfId(); DEBUG_PRINT("Network Topology: %d", topology.size); for (int i = 0; i < topology.size; i++){ DEBUG_PRINT("%d ", topology.devices_ids[i]); } DEBUG_PRINT("\n"); - EnableDTRProtocol(topology); + DTRenableProtocol(topology); vTaskDelay(2000); // Register the callback function so that the CF can receive packets as well. @@ -104,14 +104,14 @@ void appMain(){ if (my_id == topology.devices_ids[0]){ DTR_DEBUG_PRINT("Starting communication...\n"); - startRadioCommunication(); + DTRstartCommunication(); loadTXPacketsForTesting(); } DTRpacket received_packet; uint32_t start = T2M(xTaskGetTickCount()); while(1){ - getPacketFromDTR(&received_packet, portMAX_DELAY); + DTRgetPacket(&received_packet, portMAX_DELAY); uint32_t dt = T2M(xTaskGetTickCount()) - start; DEBUG_PRINT("Received data from %d : %d --> Time elapsed: %lu msec\n",received_packet.source_id, received_packet.data[0],dt); start = T2M(xTaskGetTickCount()); @@ -120,7 +120,7 @@ void appMain(){ received_packet.source_id = my_id; received_packet.data[0] = 123; DEBUG_PRINT("Sending response...\n"); - insertDTRPacketToQueue(&received_packet,TX_DATA_Q); + DTRinsertPacketToQueue(&received_packet,TX_DATA_Q); } } } diff --git a/src/modules/interface/p2pDTR/DTR_handlers.h b/src/modules/interface/p2pDTR/DTR_handlers.h index 6e85b01b4e..99e91ca3bc 100644 --- a/src/modules/interface/p2pDTR/DTR_handlers.h +++ b/src/modules/interface/p2pDTR/DTR_handlers.h @@ -53,15 +53,15 @@ #define DTR_PROTOCOL_TASK_PRIORITY 1 //Higher number higher priority // DTR protocol Task -void startDTRProtocolTask(void); +void DTRstartProtocolTask(void); -void stopDTRProtocolTask(void); +void DTRstopProtocolTask(void); // ================ DTR sender timer ================== -void initDTRSenderTimer(void); +void DTRinitSenderTimer(void); -void shutdownDTRSenderTimer(void); +void DTRshutdownSenderTimer(void); -void startDTRSenderTimer(unsigned int time_out); +void DTRstartSenderTimer(unsigned int time_out); #endif /* SRC_RADIO_DTR_HANDLERS_H_ */ diff --git a/src/modules/interface/p2pDTR/p2p_interface.h b/src/modules/interface/p2pDTR/p2p_interface.h index a488df206b..84a7deec8b 100644 --- a/src/modules/interface/p2pDTR/p2p_interface.h +++ b/src/modules/interface/p2pDTR/p2p_interface.h @@ -37,11 +37,11 @@ #define INCOMING_DTR_QUEUE_SIZE 10 // Broadcasts a DTR packet through the P2P network -void sendDTRpacket(const DTRpacket* packet) ; +void DTRsendP2Ppacket(const DTRpacket* packet) ; // Puts the DTR packet in the queue for the token ring to pick up. void DTRp2pIncomingHandler(P2PPacket *p); -void feedDTRPacketToProtocol(DTRpacket *incoming_DTR); +void DTRfeedPacketToProtocol(DTRpacket *incoming_DTR); #endif // _P2P_INTERFACE_H_ diff --git a/src/modules/interface/p2pDTR/queueing.h b/src/modules/interface/p2pDTR/queueing.h index 8add907d90..c0da1e5256 100644 --- a/src/modules/interface/p2pDTR/queueing.h +++ b/src/modules/interface/p2pDTR/queueing.h @@ -58,17 +58,17 @@ typedef enum queue_names_e { void DTRqueueingInit(); -bool isDTRPacketInQueueAvailable(DTRQueue_Names qName); +bool DTRisPacketInQueueAvailable(DTRQueue_Names qName); -uint8_t getNumberOfDTRPacketsInQueue(DTRQueue_Names qName); +uint8_t DTRgetNumberOfPacketsInQueue(DTRQueue_Names qName); -bool getDTRPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout); +bool DTRgetPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout); // Blocks to wait for a packet to be received for a given time // new_packet_received --> True if a new packet has been received and False if the timeout has been reached -bool receiveDTRPacketWaitUntil(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received); +bool DTRreceivePacketWaitUntil(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received); -bool insertDTRPacketToQueue(DTRpacket *packet, DTRQueue_Names qName); +bool DTRinsertPacketToQueue(DTRpacket *packet, DTRQueue_Names qName); bool releaseDTRPacketFromQueue(DTRQueue_Names qName); diff --git a/src/modules/interface/p2pDTR/token_ring.h b/src/modules/interface/p2pDTR/token_ring.h index 39fb519f76..1043619294 100644 --- a/src/modules/interface/p2pDTR/token_ring.h +++ b/src/modules/interface/p2pDTR/token_ring.h @@ -66,36 +66,28 @@ #define PROTOCOL_TIMEOUT_MS 4 * 1000.0f // ms #define DTR_P2P_PORT 15 // between 0 and 15(4 bits) -void initTokenRing(DTRtopology topology, uint8_t device_id); +// void initTokenRing(DTRtopology topology, uint8_t device_id); -uint8_t getDeviceRadioAddress(); +uint8_t DTRgetDeviceAddress(); -void timeOutCallBack(xTimerHandle timer); +void DTRtimeOutCallBack(xTimerHandle timer); -void startRadioCommunication(); +void DTRstartCommunication(); void DTRInterruptHandler(void *param); -const RadioInfo* getRadioInfo(); +const RadioInfo* DTRgetRadioInfo(); -void resetRadioMetaInfo(); - -void printDTRPacket(DTRpacket* packet); - -const char* getMessageType(uint8_t message_type); - -const char* getRXState(uint8_t rx_state); - -const char* getTXState(uint8_t tx_state); - -uint8_t get_self_id(void); +void DTRresetRadioMetaInfo(); +void DTRprintPacket(DTRpacket* packet); +uint8_t DTRgetSelfId(void); // =========================== DTR API =========================== // Starts the task of the Dynamic Token Ring Protocol (DTR) and initializes the protocol // @param topology The topology of the network (see DTR_types.h) -void EnableDTRProtocol(DTRtopology topology); +void DTRenableProtocol(DTRtopology topology); // Stops the task of the Dynamic Token Ring Protocol (DTR) and deinitializes the protocol void DisableDTRProtocol(void); @@ -103,13 +95,13 @@ void DisableDTRProtocol(void); // Sends a packet to the DTR protocol // @param packet The packet to be sent // @return true if the packet was sent successfully to the DTR (not the final receiver), false otherwise -bool sendPacketToDTR(DTRpacket* packet); +bool DTRsendPacket(DTRpacket* packet); // Receives a packet from the DTR Protocol // Blocks for the specified timeout if no packet is received // @param packet: the packet to be received // @param timeout: the timeout in milliseconds // @return true if the packet was received, false otherwise -bool getPacketFromDTR(DTRpacket* packet, uint32_t timeout); +bool DTRgetPacket(DTRpacket* packet, uint32_t timeout); #endif /* SRC_RADIO_RADIO_H_ */ diff --git a/src/modules/src/p2pDTR/DTR_handlers.c b/src/modules/src/p2pDTR/DTR_handlers.c index ea02d959d4..d649ab6713 100644 --- a/src/modules/src/p2pDTR/DTR_handlers.c +++ b/src/modules/src/p2pDTR/DTR_handlers.c @@ -44,19 +44,19 @@ static bool sender_timer_running = false; static char type_to_spam[15]; -void startDTRProtocolTask(void){ +void DTRstartProtocolTask(void){ xTaskCreate(DTRInterruptHandler, "DTR_P2P", DTR_PROTOCOL_TASK_STACK_SIZE, NULL,DTR_PROTOCOL_TASK_PRIORITY, &DTRtaskHandler); } -void stopDTRProtocolTask(void){ +void DTRstopProtocolTask(void){ vTaskDelete(DTRtaskHandler); } -void initDTRSenderTimer(void) { - sender_timer = xTimerCreate("DTRSenderTimer", M2T(20), pdTRUE, NULL, timeOutCallBack); +void DTRinitSenderTimer(void) { + sender_timer = xTimerCreate("DTRSenderTimer", M2T(20), pdTRUE, NULL, DTRtimeOutCallBack); } -void shutdownDTRSenderTimer(void) { +void DTRshutdownSenderTimer(void) { if (xTimerIsTimerActive(sender_timer)==pdTRUE) { xTimerStop(sender_timer, 0); DTR_DEBUG_PRINT("Stopped spamming messages\n"); @@ -67,7 +67,7 @@ void shutdownDTRSenderTimer(void) { } -void startDTRSenderTimer(unsigned int time_out) { +void DTRstartSenderTimer(unsigned int time_out) { if(time_out == MAX_WAIT_TIME_FOR_RTS ){ strcpy(type_to_spam, "RTS"); diff --git a/src/modules/src/p2pDTR/p2p_interface.c b/src/modules/src/p2pDTR/p2p_interface.c index 468c475cf0..9affbac46d 100644 --- a/src/modules/src/p2pDTR/p2p_interface.c +++ b/src/modules/src/p2pDTR/p2p_interface.c @@ -36,7 +36,7 @@ static P2PPacket p2p_TXpacket; static DTRpacket prev_received = {0}; -void sendDTRpacket(const DTRpacket* packet) { +void DTRsendP2Ppacket(const DTRpacket* packet) { p2p_TXpacket.port = DTR_P2P_PORT; memcpy(&p2p_TXpacket.data[0], packet, packet->packetSize); @@ -55,19 +55,19 @@ void DTRp2pIncomingHandler(P2PPacket *p){ uint8_t DTRpacket_size = p->data[0]; memcpy(&incoming_DTR, &(p->data[0]), DTRpacket_size); - feedDTRPacketToProtocol(&incoming_DTR); + DTRfeedPacketToProtocol(&incoming_DTR); } -void feedDTRPacketToProtocol(DTRpacket *incoming_DTR) { +void DTRfeedPacketToProtocol(DTRpacket *incoming_DTR) { bool same_packet_received = incoming_DTR->message_type == prev_received.message_type && incoming_DTR->target_id == prev_received.target_id && incoming_DTR->source_id == prev_received.source_id; // if there are packets in the queue and the new packet is the same as the previous one, ignore it - DTR_DEBUG_PRINT("Packets in RX_SRV queue: %d\n", getNumberOfDTRPacketsInQueue(RX_SRV_Q) ); - if ( getNumberOfDTRPacketsInQueue(RX_SRV_Q) !=0 && same_packet_received ) { + DTR_DEBUG_PRINT("Packets in RX_SRV queue: %d\n", DTRgetNumberOfPacketsInQueue(RX_SRV_Q) ); + if ( DTRgetNumberOfPacketsInQueue(RX_SRV_Q) !=0 && same_packet_received ) { DTR_DEBUG_PRINT("Duplicate packet received\n"); DTR_DEBUG_PRINT("Message type: %d\n", incoming_DTR->message_type); DTR_DEBUG_PRINT("Target id: %d\n", incoming_DTR->target_id); @@ -79,5 +79,5 @@ void feedDTRPacketToProtocol(DTRpacket *incoming_DTR) { prev_received.target_id = incoming_DTR->target_id; prev_received.source_id = incoming_DTR->source_id; - insertDTRPacketToQueue(incoming_DTR, RX_SRV_Q); + DTRinsertPacketToQueue(incoming_DTR, RX_SRV_Q); } diff --git a/src/modules/src/p2pDTR/queueing.c b/src/modules/src/p2pDTR/queueing.c index ac166cb06a..6d92b1b4e0 100644 --- a/src/modules/src/p2pDTR/queueing.c +++ b/src/modules/src/p2pDTR/queueing.c @@ -81,15 +81,15 @@ void DTRqueueingInit(){ } -uint8_t getNumberOfDTRPacketsInQueue(DTRQueue_Names qName){ +uint8_t DTRgetNumberOfPacketsInQueue(DTRQueue_Names qName){ return (uint8_t) uxQueueMessagesWaiting(*getQueueHandler(qName)); } -bool isDTRPacketInQueueAvailable(DTRQueue_Names qName) { +bool DTRisPacketInQueueAvailable(DTRQueue_Names qName) { return uxQueueMessagesWaiting(*getQueueHandler(qName)) > 0; } -bool getDTRPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout){ +bool DTRgetPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout){ // notice that xQueuePeek is used instead of xQueueReceive, because the packet is not removed from the queue //TODO: make a separate function for this bool received_success; @@ -114,12 +114,12 @@ bool getDTRPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t tim return received_success; } -bool receiveDTRPacketWaitUntil(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received){ +bool DTRreceivePacketWaitUntil(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received){ *new_packet_received = xQueueReceive(*getQueueHandler(qName), packet, M2T(timeout_ms)) == pdTRUE; return true; } -bool insertDTRPacketToQueue(DTRpacket *packet, DTRQueue_Names qName) { +bool DTRinsertPacketToQueue(DTRpacket *packet, DTRQueue_Names qName) { bool res = xQueueSend(*getQueueHandler(qName),(void *) packet, 0) == pdTRUE; if (!res) { DEBUG_PRINT("TX_DATA queue busy\n"); diff --git a/src/modules/src/p2pDTR/token_ring.c b/src/modules/src/p2pDTR/token_ring.c index 3a08df933f..ece6c0bb28 100644 --- a/src/modules/src/p2pDTR/token_ring.c +++ b/src/modules/src/p2pDTR/token_ring.c @@ -63,6 +63,62 @@ static uint8_t my_id ; static DTRtopology networkTopology; + +// DEBUGGING FUNCTIONS +#ifdef DEBUG_DTR_PROTOCOL + +// get the message name from the message type +static const char* getMessageType(uint8_t message_type){ + switch(message_type){ + case DATA_FRAME: + return "DATA"; + case TOKEN_FRAME: + return "TOKEN"; + case CTS_FRAME: + return "CTS"; + case RTS_FRAME: + return "RTS"; + case DATA_ACK_FRAME: + return "DATA_ACK"; + default: + return "UNKNOWN"; + } +} + +static const char* getRXState(uint8_t rx_state){ + switch(rx_state){ + case RX_IDLE: + return "RX_IDLE"; + case RX_WAIT_CTS: + return "RX_WAIT_CTS"; + case RX_WAIT_RTS: + return "RX_WAIT_RTS"; + case RX_WAIT_DATA_ACK: + return "RX_WAIT_DATA_ACK"; + default: + return "UNKNOWN"; + } +} + +static const char* getTXState(uint8_t tx_state){ + switch(tx_state){ + case TX_TOKEN: + return "TX_TOKEN"; + case TX_RTS: + return "TX_RTS"; + case TX_CTS: + return "TX_CTS"; + case TX_DATA_FRAME: + return "TX_DATA_FRAME"; + case TX_DATA_ACK: + return "TX_DATA_ACK"; + default: + return "UNKNOWN"; + } +} + +#endif + static uint8_t getIndexInTopology(uint8_t id){ for(uint8_t i = 0; i < networkTopology.size; i++){ if(networkTopology.devices_ids[i] == id){ @@ -112,8 +168,8 @@ static void setNodeIds(DTRtopology topology, uint8_t device_id) { DTR_DEBUG_PRINT("prev node_id: %d\n", prev_node_id); } -void initTokenRing(DTRtopology topology, uint8_t device_id) { - my_id = get_self_id(); +static void initTokenRing(DTRtopology topology, uint8_t device_id) { + my_id = DTRgetSelfId(); /* Network node configuration*/ setNodeIds(topology, my_id); @@ -145,7 +201,7 @@ static void setupRadioTx(DTRpacket* packet, TxStates txState) { // set the radio timer to "spam" RTS messages // setDTRSenderTimer(MAX_WAIT_TIME_FOR_CTS); timerDTRpacket = packet; - startDTRSenderTimer(MAX_WAIT_TIME_FOR_CTS); + DTRstartSenderTimer(MAX_WAIT_TIME_FOR_CTS); break; case TX_TOKEN: @@ -155,7 +211,7 @@ static void setupRadioTx(DTRpacket* packet, TxStates txState) { // set the radio timer to "spam" token messages // setDTRSenderTimer(MAX_WAIT_TIME_FOR_RTS); timerDTRpacket = packet; - startDTRSenderTimer(MAX_WAIT_TIME_FOR_RTS); + DTRstartSenderTimer(MAX_WAIT_TIME_FOR_RTS); break; case TX_DATA_FRAME: @@ -165,14 +221,14 @@ static void setupRadioTx(DTRpacket* packet, TxStates txState) { // set the radio timer to "spam" the same DATA frame // setDTRSenderTimer(MAX_WAIT_TIME_FOR_DATA_ACK); timerDTRpacket = packet; - startDTRSenderTimer(MAX_WAIT_TIME_FOR_DATA_ACK); + DTRstartSenderTimer(MAX_WAIT_TIME_FOR_DATA_ACK); break; default: DEBUG_PRINT("\nRadio transmitter state not set correctly!!\n"); return; } - sendDTRpacket(packet); + DTRsendP2Ppacket(packet); radioMetaInfo.sendPackets++; @@ -183,7 +239,7 @@ static void resetProtocol(void){ rx_state = RX_IDLE; protocol_timeout_ms = T2M(xTaskGetTickCount()) + PROTOCOL_TIMEOUT_MS; emptyDTRQueues(); // Maybe not all queues, but only the RX_SRV since the data are going to be lost saved in DATA queues - shutdownDTRSenderTimer(); + DTRshutdownSenderTimer(); last_packet_source_id = 255; } @@ -200,7 +256,7 @@ void DTRInterruptHandler(void *param) { bool new_packet_received; DTR_DEBUG_PRINT("\nDTRInterruptHandler Task called...\n"); - while ( receiveDTRPacketWaitUntil(&_rxPk, RX_SRV_Q, PROTOCOL_TIMEOUT_MS, &new_packet_received) ){ + while ( DTRreceivePacketWaitUntil(&_rxPk, RX_SRV_Q, PROTOCOL_TIMEOUT_MS, &new_packet_received) ){ if (!new_packet_received) { DTR_DEBUG_PRINT("\nPROTOCOL TIMEOUT!\n"); if (my_id != networkTopology.devices_ids[0]) { @@ -216,12 +272,12 @@ void DTRInterruptHandler(void *param) { DTR_DEBUG_PRINT("===============================================================\n"); DTR_DEBUG_PRINT("=\n"); - DTR_DEBUG_PRINT("TX_DATA Q Empty: %d\n", !isDTRPacketInQueueAvailable(TX_DATA_Q)); + DTR_DEBUG_PRINT("TX_DATA Q Empty: %d\n", !DTRisPacketInQueueAvailable(TX_DATA_Q)); DTR_DEBUG_PRINT("rx_state: %s\n", getRXState(rx_state)); - // printDTRPacket(rxPk); + // DTRprintPacket(rxPk); DTR_DEBUG_PRINT("Received packet type: %d\n", rxPk->message_type); DTR_DEBUG_PRINT("Received packet target id: %d\n", rxPk->target_id); DTR_DEBUG_PRINT("my_id: %d\n", node_id); @@ -237,7 +293,7 @@ void DTRInterruptHandler(void *param) { last_packet_source_id = rxPk->source_id; /* if packet is relevant and receiver queue is not full, then * push packet in the queue and prepare queue for next packet. */ - bool queueFull = insertDTRPacketToQueue(rxPk, RX_DATA_Q); + bool queueFull = DTRinsertPacketToQueue(rxPk, RX_DATA_Q); if (queueFull){ radioMetaInfo.failedRxQueueFull++; } @@ -278,14 +334,14 @@ void DTRInterruptHandler(void *param) { /* if packet is CTS and received from previous node, then node can * send its next DATA packet. */ if (rxPk->message_type == CTS_FRAME && rxPk->source_id == prev_node_id) { - shutdownDTRSenderTimer(); + DTRshutdownSenderTimer(); last_packet_source_id = node_id; DTR_DEBUG_PRINT("\nRcvd CTS from prev,send DATA to next\n"); /* check if there is a DATA packet. If yes, prepare it and * send it, otherwise forward the token to the next node. */ DTRpacket _txPk;//TODO: bad implementation, should be fixed - if (getDTRPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME))) { + if (DTRgetPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME))) { txPk = &_txPk; DTR_DEBUG_PRINT("TX DATA Packet exists (%d), sending it\n",txPk->data[0]); if(txPk->allToAllFlag) { @@ -293,7 +349,7 @@ void DTRInterruptHandler(void *param) { } if (!IdExistsInTopology(txPk->target_id)) { DEBUG_PRINT("Releasing DTR TX packet,target is not in topology.\n"); - DTR_DEBUG_PRINT("Is Queue Empty: %d\n", !isDTRPacketInQueueAvailable(TX_DATA_Q)); + DTR_DEBUG_PRINT("Is Queue Empty: %d\n", !DTRisPacketInQueueAvailable(TX_DATA_Q)); releaseDTRPacketFromQueue(TX_DATA_Q); txPk = &servicePk; @@ -325,7 +381,7 @@ void DTRInterruptHandler(void *param) { * a CTS packet can be sent. */ if (rxPk->message_type == RTS_FRAME && rxPk->source_id == next_node_id) { DTR_DEBUG_PRINT("\nReceived TOKEN_ACK from next->sending CTS \n"); - shutdownDTRSenderTimer(); + DTRshutdownSenderTimer(); servicePk.message_type = CTS_FRAME; setupRadioTx(&servicePk, TX_CTS); continue; @@ -336,11 +392,11 @@ void DTRInterruptHandler(void *param) { case RX_WAIT_DATA_ACK: if (rxPk->message_type == DATA_ACK_FRAME && rxPk->target_id == node_id) { - shutdownDTRSenderTimer(); + DTRshutdownSenderTimer(); //TODO: bad implementation, should be fixed DTRpacket _txPk; - getDTRPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME)); + DTRgetPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME)); txPk = &_txPk; if (next_sender_id == 255){ @@ -353,9 +409,9 @@ void DTRInterruptHandler(void *param) { DTR_DEBUG_PRINT("next_target_id: %d\n", next_target_id); if (!txPk->allToAllFlag || next_target_id == node_id) { DTR_DEBUG_PRINT("Releasing TX DATA:\n"); - // printDTRPacket(txPk); + // DTRprintPacket(txPk); - DTR_DEBUG_PRINT("Is Q Empty: %d\n", !isDTRPacketInQueueAvailable(TX_DATA_Q)); + DTR_DEBUG_PRINT("Is Q Empty: %d\n", !DTRisPacketInQueueAvailable(TX_DATA_Q)); releaseDTRPacketFromQueue(TX_DATA_Q); txPk = &servicePk; @@ -394,12 +450,12 @@ void DTRInterruptHandler(void *param) { } -uint8_t getDeviceRadioAddress() { +uint8_t DTRgetDeviceAddress() { return node_id; } -void timeOutCallBack(xTimerHandle timer) { - #ifdef DEBUG_DTR +void DTRtimeOutCallBack(xTimerHandle timer) { + #ifdef DEBUG_DTR_PROTOCOL DTR_DEBUG_PRINT("Sending packet after timeout: %s",getMessageType(timerDTRpacket->message_type)); if (timerDTRpacket->message_type == DATA_FRAME) { for (size_t i = 0; i dataSize; i++) @@ -410,7 +466,7 @@ void timeOutCallBack(xTimerHandle timer) { DTR_DEBUG_PRINT("\n"); #endif - sendDTRpacket(timerDTRpacket); + DTRsendP2Ppacket(timerDTRpacket); radioMetaInfo.sendPackets++; switch(tx_state) { @@ -428,16 +484,16 @@ void timeOutCallBack(xTimerHandle timer) { } } -const RadioInfo* getRadioInfo() { +const RadioInfo* DTRgetRadioInfo() { radioMetaInfo.deviceId = node_id; return &radioMetaInfo; } -void resetRadioMetaInfo() { +void DTRresetRadioMetaInfo() { memset(&radioMetaInfo, 0, sizeof(radioMetaInfo)); } -void startRadioCommunication() { +void DTRstartCommunication() { DTRpacket* startSignal = &startPacket; startSignal->source_id = node_id; @@ -451,60 +507,11 @@ void startRadioCommunication() { timerRadioTxState = TX_DATA_FRAME; rx_state = RX_WAIT_DATA_ACK; DTR_DEBUG_PRINT("Spamming DATA\n"); - startDTRSenderTimer(MAX_WAIT_TIME_FOR_DATA_ACK); -} - -// get the message name from the message type -const char* getMessageType(uint8_t message_type){ - switch(message_type){ - case DATA_FRAME: - return "DATA"; - case TOKEN_FRAME: - return "TOKEN"; - case CTS_FRAME: - return "CTS"; - case RTS_FRAME: - return "RTS"; - case DATA_ACK_FRAME: - return "DATA_ACK"; - default: - return "UNKNOWN"; - } -} - -const char* getRXState(uint8_t rx_state){ - switch(rx_state){ - case RX_IDLE: - return "RX_IDLE"; - case RX_WAIT_CTS: - return "RX_WAIT_CTS"; - case RX_WAIT_RTS: - return "RX_WAIT_RTS"; - case RX_WAIT_DATA_ACK: - return "RX_WAIT_DATA_ACK"; - default: - return "UNKNOWN"; - } + DTRstartSenderTimer(MAX_WAIT_TIME_FOR_DATA_ACK); } -const char* getTXState(uint8_t tx_state){ - switch(tx_state){ - case TX_TOKEN: - return "TX_TOKEN"; - case TX_RTS: - return "TX_RTS"; - case TX_CTS: - return "TX_CTS"; - case TX_DATA_FRAME: - return "TX_DATA_FRAME"; - case TX_DATA_ACK: - return "TX_DATA_ACK"; - default: - return "UNKNOWN"; - } -} -void printDTRPacket(DTRpacket* packet){ +void DTRprintPacket(DTRpacket* packet){ DTR_DEBUG_PRINT("\nDTR Packet Received: %s\n", getMessageType(packet->message_type)); DTR_DEBUG_PRINT("Packet Size: %d\n", packet->packetSize); DTR_DEBUG_PRINT("Message Type: %s\n", getMessageType(packet->message_type)); @@ -522,7 +529,7 @@ void printDTRPacket(DTRpacket* packet){ DTR_DEBUG_PRINT("\n\n"); } -uint8_t get_self_id(void){ +uint8_t DTRgetSelfId(void){ // Get the current address of the crazyflie and //keep the last 2 digits as the id of the crazyflie. uint64_t address = configblockGetRadioAddress(); @@ -530,7 +537,7 @@ uint8_t get_self_id(void){ return my_id; } -void EnableDTRProtocol(DTRtopology topology){ +void DTRenableProtocol(DTRtopology topology){ DTR_DEBUG_PRINT("Initializing queues ...\n"); DTRqueueingInit(); @@ -538,18 +545,18 @@ void EnableDTRProtocol(DTRtopology topology){ initTokenRing(topology, my_id); DTR_DEBUG_PRINT("Initializing DTR Sender ...\n"); - initDTRSenderTimer(); + DTRinitSenderTimer(); DTR_DEBUG_PRINT("Starting protocol timer ...\n"); - startDTRProtocolTask(); + DTRstartProtocolTask(); } void DisableDTRProtocol(void){ DTR_DEBUG_PRINT("Stopping protocol timer ...\n"); - stopDTRProtocolTask(); + DTRstopProtocolTask(); DTR_DEBUG_PRINT("Stopping DTR Sender ...\n"); - shutdownDTRSenderTimer(); + DTRshutdownSenderTimer(); DTR_DEBUG_PRINT("Resetting token ring ...\n"); resetProtocol(); @@ -559,20 +566,20 @@ void DisableDTRProtocol(void){ } -bool sendPacketToDTR(DTRpacket* packet){ +bool DTRsendPacket(DTRpacket* packet){ packet->message_type = DATA_FRAME; packet->source_id = node_id; packet->packetSize = DTR_PACKET_HEADER_SIZE + packet->dataSize; - return insertDTRPacketToQueue(packet,TX_DATA_Q); + return DTRinsertPacketToQueue(packet,TX_DATA_Q); } -bool getPacketFromDTR(DTRpacket* packet, uint32_t timeout){ +bool DTRgetPacket(DTRpacket* packet, uint32_t timeout){ if (timeout != portMAX_DELAY){ timeout = M2T(timeout); } - return getDTRPacketFromQueue(packet,RX_DATA_Q,timeout); + return DTRgetPacketFromQueue(packet,RX_DATA_Q,timeout); } From a6728a6cd0ef0ce023afbdbe0c76660bede05b30 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Mon, 22 Aug 2022 12:41:31 +0200 Subject: [PATCH 13/19] Start the DTR communication internally --- examples/app_p2p_DTR/src/p2p_DTR_app.c | 1 - src/modules/src/p2pDTR/token_ring.c | 5 +++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/examples/app_p2p_DTR/src/p2p_DTR_app.c b/examples/app_p2p_DTR/src/p2p_DTR_app.c index a4135fa437..d87835a16c 100644 --- a/examples/app_p2p_DTR/src/p2p_DTR_app.c +++ b/examples/app_p2p_DTR/src/p2p_DTR_app.c @@ -104,7 +104,6 @@ void appMain(){ if (my_id == topology.devices_ids[0]){ DTR_DEBUG_PRINT("Starting communication...\n"); - DTRstartCommunication(); loadTXPacketsForTesting(); } diff --git a/src/modules/src/p2pDTR/token_ring.c b/src/modules/src/p2pDTR/token_ring.c index ece6c0bb28..375b5f5f4e 100644 --- a/src/modules/src/p2pDTR/token_ring.c +++ b/src/modules/src/p2pDTR/token_ring.c @@ -549,6 +549,11 @@ void DTRenableProtocol(DTRtopology topology){ DTR_DEBUG_PRINT("Starting protocol timer ...\n"); DTRstartProtocolTask(); + + // The first node starts the protocol communication + if (my_id == topology.devices_ids[0]) { + DTRstartCommunication(); + } } void DisableDTRProtocol(void){ From a35f5bfe145763782c0f995e8726c4d3d4b497e1 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Wed, 24 Aug 2022 14:57:38 +0200 Subject: [PATCH 14/19] DTR functions renaming and formatting --- .github/workflows/CI.yml | 1 + docs/functional-areas/p2p_DTR_api.md | 28 ++--- examples/app_p2p_DTR/src/p2p_DTR_app.c | 86 ++++++++++------ src/modules/interface/p2pDTR/DTR_handlers.h | 12 +-- src/modules/interface/p2pDTR/DTR_types.h | 2 +- src/modules/interface/p2pDTR/p2p_interface.h | 12 +-- src/modules/interface/p2pDTR/queueing.h | 19 ++-- src/modules/interface/p2pDTR/token_ring.h | 26 ++--- src/modules/src/p2pDTR/DTR_handlers.c | 14 +-- src/modules/src/p2pDTR/p2p_interface.c | 41 ++++---- src/modules/src/p2pDTR/queueing.c | 28 ++--- src/modules/src/p2pDTR/token_ring.c | 102 +++++++++---------- 12 files changed, 200 insertions(+), 171 deletions(-) diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index f350d5b235..572a599226 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -142,6 +142,7 @@ jobs: - examples/app_hello_world-cpp - examples/app_hello_file_tree - examples/app_peer_to_peer + - examples/app_p2p_DTR - examples/demos/app_push_demo - examples/demos/swarm_demo - examples/demos/app_wall_following_demo diff --git a/docs/functional-areas/p2p_DTR_api.md b/docs/functional-areas/p2p_DTR_api.md index f46de5e368..40fd7e75ad 100644 --- a/docs/functional-areas/p2p_DTR_api.md +++ b/docs/functional-areas/p2p_DTR_api.md @@ -42,7 +42,7 @@ static DTRtopology topology = NETWORK_TOPOLOGY; void main() { ... // Start the token ring protocol - DTRenableProtocol(topology); + dtrEnableProtocol(topology); ... } ``` @@ -50,15 +50,19 @@ void main() { In the example above, the topology is defined as having 4 nodes and their ids are 1, 0, 2, 3. ## Feeding incoming packets to the protocol -In order for the protocol to work, the user must feed the protocol with incoming packets. This is done by calling the function `DTRp2pIncomingHandler` which in the interface for receiving P2P packets. As mentioned above the protocol uses the port 15 of the P2P API, so it automatically checks if the packet is coming from the port 15 and if it is, it handles it. Otherwise it is discarded. Though, the user can implement their own code to handle the rest incoming packets if they want to. +In order for the protocol to work, the user must feed the protocol with incoming packets. This is done by calling the function `dtrP2PIncomingHandler` which in the interface for receiving P2P packets. As mentioned above the protocol uses the port 15 of the P2P API, so it automatically checks if the packet is coming from the port 15 and if it is, it handles it. Otherwise it is discarded.The function returns true if the packet was handled and false otherwise. + Example usage : ``` C void p2pcallbackHandler(P2PPacket *p){ // If the packet is a DTR service packet, then the handler will handle it. - DTRp2pIncomingHandler(p); - - // User code for handling other packets ... + if (!dtrP2PIncomingHandler(p)){ + // If packet was not handled from DTR , then it is a normal packet + // that user has to handle. + + // Write your own code below ... + } } ... @@ -76,9 +80,9 @@ void main(){ ## Data Broadcast -To send data through the protocol, the user must call the function `DTRsendPacket`. +To send data through the protocol, the user must call the function `dtrSendPacket`. ``` C - bool DTRsendPacket(DTRpacket* packet) + bool dtrSendPacket(DTRpacket* packet) ``` This function takes the packet to be sent as a parameter. The packet must be filled wit the data the user wants to send and by defining the size of them. The function returns true if the packet was sent successfully to the DTR (**not to the receiver copter**) and false otherwise.Keep in mind that the packet is sent asynchronously and the user can continue to use the P2P API while the packet is being sent.It is not necessary to fill the `.source_id`, `message_type`, `packet_size` fields of the packet since they are automatically filled by the API function. After the execution of the function, the new packet is inserted in the queue of the protocol responsible for all the packets to be sent. @@ -95,19 +99,19 @@ This function takes the packet to be sent as a parameter. The packet must be fil packet.data[1] = 0x02; packet.data[2] = 0x03; packet.allToAllFlag = 1; - DTRsendPacket(&packet); + dtrSendPacket(&packet); ``` ## Receive Data -The user must call the function `DTRgetPacket` to receive a packet from the DTR. +The user must call the function `dtrGetPacket` to receive a packet from the DTR. ``` C -bool DTRgetPacket(DTRpacket* packet, uint32_t timeout); +bool dtrGetPacket(DTRpacket* packet, uint32_t timeout); ``` -The function blocks for the specified time (in milliseconds) until a packet is received.If the user wants to block indefinitely, the timeout parameter must be set to `portMAX_DELAY`. The function returns true if a packet was received and false otherwise. If a packet is received, the packet is filled with the data received. In case it was received, the packet is filled with the data received and the corresponding packet is released from the queue responsible for the reception of the DTR packets. +The function blocks for the specified time (in ticks) until a packet is received. If the user wants to block indefinitely, the timeout parameter must be set to `portMAX_DELAY`. The function returns true if a packet was received and false otherwise. If a packet is received, the packet is filled with the data received. In case it was received, the packet is filled with the data received and the corresponding packet is released from the queue responsible for the reception of the DTR packets. Example Usage: ``` C // Initialize the packet @@ -115,7 +119,7 @@ DTRpacket packet; // Receive the packet while(1){ - DTRgetPacket(&packet, portMAX_DELAY); + dtrGetPacket(&packet, portMAX_DELAY); if(packet.dataSize > 0){ // Do something with the packet diff --git a/examples/app_p2p_DTR/src/p2p_DTR_app.c b/examples/app_p2p_DTR/src/p2p_DTR_app.c index d87835a16c..3ec0879598 100644 --- a/examples/app_p2p_DTR/src/p2p_DTR_app.c +++ b/examples/app_p2p_DTR/src/p2p_DTR_app.c @@ -7,7 +7,7 @@ * * Crazyflie control firmware * - * Copyright (C) 2019 Bitcraze AB + * Copyright (C) 2022 Bitcraze AB * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -22,8 +22,8 @@ * along with this program. If not, see . * * - * peer_to_peer.c - App layer application of simple demonstration peer to peer - * communication. Two crazyflies need this program in order to send and receive. + * p2p_DTR_app.c - App layer application of simple demonstration of the + * Dynamic Token Ring Protocol used on top of the P2P. */ @@ -48,6 +48,9 @@ #define INTERESTING_DATA 104 +#define STARTING_MESSAGE "Hello World" +#define STARTING_MESSAGE_SIZE 11 + // define the ids of each node in the network #define NETWORK_TOPOLOGY {.size = 4, .devices_ids = {0, 1, 2, 3} } // Maximum size of network is 20 by default @@ -58,45 +61,64 @@ void loadTXPacketsForTesting(void){ DTRpacket testSignal; testSignal.message_type = DATA_FRAME; testSignal.source_id = my_id; - testSignal.target_id = 1; - uint8_t data_size = 25; - for (int i = 0; i < data_size; i++){ - testSignal.data[i] = i; + const char testMessage[STARTING_MESSAGE_SIZE] = "Hello World"; + strcpy(testSignal.data, testMessage); + testSignal.dataSize = STARTING_MESSAGE_SIZE; + testSignal.allToAllFlag = 1; + testSignal.packetSize = DTR_PACKET_HEADER_SIZE + testSignal.dataSize; + bool res; + res = dtrSendPacket(&testSignal); + if (res){ + DTR_DEBUG_PRINT("Packet sent to DTR protocol\n"); } - testSignal.dataSize = data_size; + else{ + DEBUG_PRINT("Packet not sent to DTR protocol\n"); + } +} + +void loadResponse(void){ + DTRpacket testSignal; + testSignal.message_type = DATA_FRAME; + testSignal.source_id = my_id; + testSignal.target_id = 1; + + const char testMessage[25] = "Hello from the other side"; + strcpy(testSignal.data, testMessage); + testSignal.dataSize = 25; testSignal.allToAllFlag = 1; testSignal.packetSize = DTR_PACKET_HEADER_SIZE + testSignal.dataSize; bool res; - for (int i = 0; i < TX_DATA_QUEUE_SIZE - 1; i++){ - testSignal.data[0] = 100+i; - res = DTRsendPacket(&testSignal); - if (res){ - DTR_DEBUG_PRINT("Packet sent to DTR protocol\n"); - } - else{ - DEBUG_PRINT("Packet not sent to DTR protocol\n"); - } + res = dtrSendPacket(&testSignal); + if (res){ + DTR_DEBUG_PRINT("Packet sent to DTR protocol\n"); + } + else{ + DEBUG_PRINT("Packet not sent to DTR protocol\n"); } } void p2pcallbackHandler(P2PPacket *p){ - // DEBUG_PRINT("P2P callback at port: %d\n", p->port); // If the packet is a DTR service packet, then the handler will handle it. - DTRp2pIncomingHandler(p); + // It returns true if the packet was handled. - // Then write your own code below ... + if (!dtrP2PIncomingHandler(p)){ + // If packet was not handled from DTR , then it is a normal packet + // that user has to handle. + + // Write your own code below ... + } } void appMain(){ - my_id = DTRgetSelfId(); + my_id = dtrGetSelfId(); DEBUG_PRINT("Network Topology: %d", topology.size); for (int i = 0; i < topology.size; i++){ DEBUG_PRINT("%d ", topology.devices_ids[i]); } DEBUG_PRINT("\n"); - DTRenableProtocol(topology); + dtrEnableProtocol(topology); vTaskDelay(2000); // Register the callback function so that the CF can receive packets as well. @@ -110,16 +132,22 @@ void appMain(){ DTRpacket received_packet; uint32_t start = T2M(xTaskGetTickCount()); while(1){ - DTRgetPacket(&received_packet, portMAX_DELAY); + dtrGetPacket(&received_packet, portMAX_DELAY); uint32_t dt = T2M(xTaskGetTickCount()) - start; - DEBUG_PRINT("Received data from %d : %d --> Time elapsed: %lu msec\n",received_packet.source_id, received_packet.data[0],dt); + + // uint8_t array to string conversion + char data[received_packet.dataSize + 1]; + for (int i = 0; i < received_packet.dataSize; i++){ + data[i] = received_packet.data[i]; + } + data[received_packet.dataSize] = '\0'; + + DEBUG_PRINT("Received data from %d : %s --> Time elapsed: %lu msec\n",received_packet.source_id, data, dt); start = T2M(xTaskGetTickCount()); - if (my_id == topology.devices_ids[1] && received_packet.data[0] == INTERESTING_DATA){ - received_packet.source_id = my_id; - received_packet.data[0] = 123; - DEBUG_PRINT("Sending response...\n"); - DTRinsertPacketToQueue(&received_packet,TX_DATA_Q); + + if (strcmp(data, "Hello World") == 0){ + loadResponse(); } } } diff --git a/src/modules/interface/p2pDTR/DTR_handlers.h b/src/modules/interface/p2pDTR/DTR_handlers.h index 99e91ca3bc..f89866c4a2 100644 --- a/src/modules/interface/p2pDTR/DTR_handlers.h +++ b/src/modules/interface/p2pDTR/DTR_handlers.h @@ -49,19 +49,19 @@ // DTR PROTOCOL TASK -#define DTR_PROTOCOL_TASK_STACK_SIZE 2 * configMINIMAL_STACK_SIZE +#define DTR_PROTOCOL_TASK_STACK_SIZE (2 * configMINIMAL_STACK_SIZE) #define DTR_PROTOCOL_TASK_PRIORITY 1 //Higher number higher priority // DTR protocol Task -void DTRstartProtocolTask(void); +void dtrStartProtocolTask(void); -void DTRstopProtocolTask(void); +void dtrStopProtocolTask(void); // ================ DTR sender timer ================== -void DTRinitSenderTimer(void); +void dtrInitSenderTimer(void); -void DTRshutdownSenderTimer(void); +void dtrShutdownSenderTimer(void); -void DTRstartSenderTimer(unsigned int time_out); +void dtrStartSenderTimer(unsigned int time_out); #endif /* SRC_RADIO_DTR_HANDLERS_H_ */ diff --git a/src/modules/interface/p2pDTR/DTR_types.h b/src/modules/interface/p2pDTR/DTR_types.h index 38d0ee1a90..48391ae2cb 100644 --- a/src/modules/interface/p2pDTR/DTR_types.h +++ b/src/modules/interface/p2pDTR/DTR_types.h @@ -111,7 +111,7 @@ typedef struct radio_meta_info { } RadioInfo; -typedef struct DTR_network_topology { +typedef struct { uint8_t size; // number of nodes in the network uint8_t devices_ids[MAX_NETWORK_SIZE]; // array that contains the device id of each node in the network } DTRtopology; diff --git a/src/modules/interface/p2pDTR/p2p_interface.h b/src/modules/interface/p2pDTR/p2p_interface.h index 84a7deec8b..d3c8521f79 100644 --- a/src/modules/interface/p2pDTR/p2p_interface.h +++ b/src/modules/interface/p2pDTR/p2p_interface.h @@ -7,7 +7,7 @@ * * Crazyflie control firmware * - * Copyright (C) 2019 Bitcraze AB + * Copyright (C) 2022 Bitcraze AB * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -26,8 +26,7 @@ * */ -#ifndef _P2P_INTERFACE_H_ -#define _P2P_INTERFACE_H_ +#pragma once #include "FreeRTOS.h" #include "DTR_types.h" @@ -37,11 +36,8 @@ #define INCOMING_DTR_QUEUE_SIZE 10 // Broadcasts a DTR packet through the P2P network -void DTRsendP2Ppacket(const DTRpacket* packet) ; +void dtrSendP2Ppacket(const DTRpacket* packet) ; // Puts the DTR packet in the queue for the token ring to pick up. -void DTRp2pIncomingHandler(P2PPacket *p); +bool dtrP2PIncomingHandler(P2PPacket *p); -void DTRfeedPacketToProtocol(DTRpacket *incoming_DTR); - -#endif // _P2P_INTERFACE_H_ diff --git a/src/modules/interface/p2pDTR/queueing.h b/src/modules/interface/p2pDTR/queueing.h index c0da1e5256..1b5203d5dc 100644 --- a/src/modules/interface/p2pDTR/queueing.h +++ b/src/modules/interface/p2pDTR/queueing.h @@ -55,25 +55,24 @@ typedef enum queue_names_e { RX_DATA_Q, } DTRQueue_Names; -void DTRqueueingInit(); +void dtrQueueingInit(); +bool dtrIsPacketInQueueAvailable(DTRQueue_Names qName); -bool DTRisPacketInQueueAvailable(DTRQueue_Names qName); +uint8_t dtrGetNumberOfPacketsInQueue(DTRQueue_Names qName); -uint8_t DTRgetNumberOfPacketsInQueue(DTRQueue_Names qName); - -bool DTRgetPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout); +bool dtrGetPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout); // Blocks to wait for a packet to be received for a given time // new_packet_received --> True if a new packet has been received and False if the timeout has been reached -bool DTRreceivePacketWaitUntil(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received); +bool dtrReceivePacketWaitUntil(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received); -bool DTRinsertPacketToQueue(DTRpacket *packet, DTRQueue_Names qName); +bool dtrInsertPacketToQueue(DTRpacket *packet, DTRQueue_Names qName); -bool releaseDTRPacketFromQueue(DTRQueue_Names qName); +bool dtrReleasePacketFromQueue(DTRQueue_Names qName); -void emptyDTRQueue(DTRQueue_Names qName); +void dtrEmptyQueue(DTRQueue_Names qName); -void emptyDTRQueues(void); +void dtrEmptyQueues(void); #endif /* _QUEUEING_H_ */ diff --git a/src/modules/interface/p2pDTR/token_ring.h b/src/modules/interface/p2pDTR/token_ring.h index 1043619294..51911719f7 100644 --- a/src/modules/interface/p2pDTR/token_ring.h +++ b/src/modules/interface/p2pDTR/token_ring.h @@ -66,28 +66,28 @@ #define PROTOCOL_TIMEOUT_MS 4 * 1000.0f // ms #define DTR_P2P_PORT 15 // between 0 and 15(4 bits) -// void initTokenRing(DTRtopology topology, uint8_t device_id); +#define START_PACKET 0xBCCF -uint8_t DTRgetDeviceAddress(); +// void initTokenRing(DTRtopology topology, uint8_t device_id); -void DTRtimeOutCallBack(xTimerHandle timer); +uint8_t dtrGetDeviceAddress(); -void DTRstartCommunication(); +void dtrTimeOutCallBack(xTimerHandle timer); -void DTRInterruptHandler(void *param); +void dtrTaskHandler(void *param); -const RadioInfo* DTRgetRadioInfo(); +const RadioInfo* dtrGetRadioInfo(); -void DTRresetRadioMetaInfo(); +void dtrResetRadioMetaInfo(); -void DTRprintPacket(DTRpacket* packet); +void dtrPrintPacket(DTRpacket* packet); -uint8_t DTRgetSelfId(void); +uint8_t dtrGetSelfId(void); // =========================== DTR API =========================== // Starts the task of the Dynamic Token Ring Protocol (DTR) and initializes the protocol // @param topology The topology of the network (see DTR_types.h) -void DTRenableProtocol(DTRtopology topology); +void dtrEnableProtocol(DTRtopology topology); // Stops the task of the Dynamic Token Ring Protocol (DTR) and deinitializes the protocol void DisableDTRProtocol(void); @@ -95,13 +95,13 @@ void DisableDTRProtocol(void); // Sends a packet to the DTR protocol // @param packet The packet to be sent // @return true if the packet was sent successfully to the DTR (not the final receiver), false otherwise -bool DTRsendPacket(DTRpacket* packet); +bool dtrSendPacket(DTRpacket* packet); // Receives a packet from the DTR Protocol // Blocks for the specified timeout if no packet is received // @param packet: the packet to be received -// @param timeout: the timeout in milliseconds +// @param timeout: the timeout in ticks // @return true if the packet was received, false otherwise -bool DTRgetPacket(DTRpacket* packet, uint32_t timeout); +bool dtrGetPacket(DTRpacket* packet, uint32_t timeout); #endif /* SRC_RADIO_RADIO_H_ */ diff --git a/src/modules/src/p2pDTR/DTR_handlers.c b/src/modules/src/p2pDTR/DTR_handlers.c index d649ab6713..6aa6eb24a6 100644 --- a/src/modules/src/p2pDTR/DTR_handlers.c +++ b/src/modules/src/p2pDTR/DTR_handlers.c @@ -44,19 +44,19 @@ static bool sender_timer_running = false; static char type_to_spam[15]; -void DTRstartProtocolTask(void){ - xTaskCreate(DTRInterruptHandler, "DTR_P2P", DTR_PROTOCOL_TASK_STACK_SIZE, NULL,DTR_PROTOCOL_TASK_PRIORITY, &DTRtaskHandler); +void dtrStartProtocolTask(void){ + xTaskCreate(dtrTaskHandler, "DTR_P2P", DTR_PROTOCOL_TASK_STACK_SIZE, NULL,DTR_PROTOCOL_TASK_PRIORITY, &DTRtaskHandler); } -void DTRstopProtocolTask(void){ +void dtrStopProtocolTask(void){ vTaskDelete(DTRtaskHandler); } -void DTRinitSenderTimer(void) { - sender_timer = xTimerCreate("DTRSenderTimer", M2T(20), pdTRUE, NULL, DTRtimeOutCallBack); +void dtrInitSenderTimer(void) { + sender_timer = xTimerCreate("DTRSenderTimer", M2T(20), pdTRUE, NULL, dtrTimeOutCallBack); } -void DTRshutdownSenderTimer(void) { +void dtrShutdownSenderTimer(void) { if (xTimerIsTimerActive(sender_timer)==pdTRUE) { xTimerStop(sender_timer, 0); DTR_DEBUG_PRINT("Stopped spamming messages\n"); @@ -67,7 +67,7 @@ void DTRshutdownSenderTimer(void) { } -void DTRstartSenderTimer(unsigned int time_out) { +void dtrStartSenderTimer(unsigned int time_out) { if(time_out == MAX_WAIT_TIME_FOR_RTS ){ strcpy(type_to_spam, "RTS"); diff --git a/src/modules/src/p2pDTR/p2p_interface.c b/src/modules/src/p2pDTR/p2p_interface.c index 9affbac46d..d695203202 100644 --- a/src/modules/src/p2pDTR/p2p_interface.c +++ b/src/modules/src/p2pDTR/p2p_interface.c @@ -7,7 +7,7 @@ * * Crazyflie control firmware * - * Copyright (C) 2019 Bitcraze AB + * Copyright (C) 2022 Bitcraze AB * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -36,7 +36,7 @@ static P2PPacket p2p_TXpacket; static DTRpacket prev_received = {0}; -void DTRsendP2Ppacket(const DTRpacket* packet) { +void dtrSendP2Ppacket(const DTRpacket* packet) { p2p_TXpacket.port = DTR_P2P_PORT; memcpy(&p2p_TXpacket.data[0], packet, packet->packetSize); @@ -45,29 +45,15 @@ void DTRsendP2Ppacket(const DTRpacket* packet) { radiolinkSendP2PPacketBroadcast(&p2p_TXpacket); } -void DTRp2pIncomingHandler(P2PPacket *p){ - if (p->port != DTR_P2P_PORT){ - return; - } - - DTRpacket incoming_DTR; - - uint8_t DTRpacket_size = p->data[0]; - - memcpy(&incoming_DTR, &(p->data[0]), DTRpacket_size); - DTRfeedPacketToProtocol(&incoming_DTR); -} - - -void DTRfeedPacketToProtocol(DTRpacket *incoming_DTR) { +static void dtrFeedPacketToProtocol(DTRpacket *incoming_DTR) { bool same_packet_received = incoming_DTR->message_type == prev_received.message_type && incoming_DTR->target_id == prev_received.target_id && incoming_DTR->source_id == prev_received.source_id; // if there are packets in the queue and the new packet is the same as the previous one, ignore it - DTR_DEBUG_PRINT("Packets in RX_SRV queue: %d\n", DTRgetNumberOfPacketsInQueue(RX_SRV_Q) ); - if ( DTRgetNumberOfPacketsInQueue(RX_SRV_Q) !=0 && same_packet_received ) { + DTR_DEBUG_PRINT("Packets in RX_SRV queue: %d\n", dtrGetNumberOfPacketsInQueue(RX_SRV_Q) ); + if ( dtrGetNumberOfPacketsInQueue(RX_SRV_Q) !=0 && same_packet_received ) { DTR_DEBUG_PRINT("Duplicate packet received\n"); DTR_DEBUG_PRINT("Message type: %d\n", incoming_DTR->message_type); DTR_DEBUG_PRINT("Target id: %d\n", incoming_DTR->target_id); @@ -79,5 +65,20 @@ void DTRfeedPacketToProtocol(DTRpacket *incoming_DTR) { prev_received.target_id = incoming_DTR->target_id; prev_received.source_id = incoming_DTR->source_id; - DTRinsertPacketToQueue(incoming_DTR, RX_SRV_Q); + dtrInsertPacketToQueue(incoming_DTR, RX_SRV_Q); +} + +bool dtrP2PIncomingHandler(P2PPacket *p){ + if (p->port != DTR_P2P_PORT){ + return false; + } + + DTRpacket incoming_DTR; + + uint8_t DTRpacket_size = p->data[0]; + + memcpy(&incoming_DTR, &(p->data[0]), DTRpacket_size); + dtrFeedPacketToProtocol(&incoming_DTR); + + return true; } diff --git a/src/modules/src/p2pDTR/queueing.c b/src/modules/src/p2pDTR/queueing.c index 6d92b1b4e0..3f2d64f4a5 100644 --- a/src/modules/src/p2pDTR/queueing.c +++ b/src/modules/src/p2pDTR/queueing.c @@ -63,7 +63,7 @@ static xQueueHandle *getQueueHandler(DTRQueue_Names qName){ } -void DTRqueueingInit(){ +void dtrQueueingInit(){ // TX SRV queue STATIC_MEM_QUEUE_ALLOC(TX_DATA_queue, TX_DATA_QUEUE_SIZE, sizeof(DTRpacket)); TX_DATA_queue = STATIC_MEM_QUEUE_CREATE(TX_DATA_queue); @@ -81,18 +81,18 @@ void DTRqueueingInit(){ } -uint8_t DTRgetNumberOfPacketsInQueue(DTRQueue_Names qName){ +uint8_t dtrGetNumberOfPacketsInQueue(DTRQueue_Names qName){ return (uint8_t) uxQueueMessagesWaiting(*getQueueHandler(qName)); } -bool DTRisPacketInQueueAvailable(DTRQueue_Names qName) { +bool dtrIsPacketInQueueAvailable(DTRQueue_Names qName) { return uxQueueMessagesWaiting(*getQueueHandler(qName)) > 0; } -bool DTRgetPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout){ +bool dtrGetPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout){ // notice that xQueuePeek is used instead of xQueueReceive, because the packet is not removed from the queue //TODO: make a separate function for this - bool received_success; + bool received_success = false; switch (qName) { case TX_DATA_Q: @@ -114,13 +114,13 @@ bool DTRgetPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t tim return received_success; } -bool DTRreceivePacketWaitUntil(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received){ +bool dtrReceivePacketWaitUntil(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received){ *new_packet_received = xQueueReceive(*getQueueHandler(qName), packet, M2T(timeout_ms)) == pdTRUE; return true; } -bool DTRinsertPacketToQueue(DTRpacket *packet, DTRQueue_Names qName) { - bool res = xQueueSend(*getQueueHandler(qName),(void *) packet, 0) == pdTRUE; +bool dtrInsertPacketToQueue(DTRpacket *packet, DTRQueue_Names qName) { + bool res = (xQueueSend(*getQueueHandler(qName),(void *) packet, 0) == pdTRUE); if (!res) { DEBUG_PRINT("TX_DATA queue busy\n"); } @@ -128,17 +128,17 @@ bool DTRinsertPacketToQueue(DTRpacket *packet, DTRQueue_Names qName) { return res; } -bool releaseDTRPacketFromQueue(DTRQueue_Names qName) { +bool dtrReleasePacketFromQueue(DTRQueue_Names qName) { DTRpacket packet; return xQueueReceive(*getQueueHandler(qName), &packet, M2T(TX_RECEIVED_WAIT_TIME)) == pdTRUE; } -void emptyDTRQueue(DTRQueue_Names qName) { +void dtrEmptyQueue(DTRQueue_Names qName) { xQueueReset(*getQueueHandler(qName)); } -void emptyDTRQueues(void){ - emptyDTRQueue(TX_DATA_Q); - emptyDTRQueue(RX_SRV_Q); - emptyDTRQueue(RX_DATA_Q); +void dtrEmptyQueues(void){ + dtrEmptyQueue(TX_DATA_Q); + dtrEmptyQueue(RX_SRV_Q); + dtrEmptyQueue(RX_DATA_Q); } diff --git a/src/modules/src/p2pDTR/token_ring.c b/src/modules/src/p2pDTR/token_ring.c index 375b5f5f4e..a04a92e5a6 100644 --- a/src/modules/src/p2pDTR/token_ring.c +++ b/src/modules/src/p2pDTR/token_ring.c @@ -169,7 +169,7 @@ static void setNodeIds(DTRtopology topology, uint8_t device_id) { } static void initTokenRing(DTRtopology topology, uint8_t device_id) { - my_id = DTRgetSelfId(); + my_id = dtrGetSelfId(); /* Network node configuration*/ setNodeIds(topology, my_id); @@ -201,7 +201,7 @@ static void setupRadioTx(DTRpacket* packet, TxStates txState) { // set the radio timer to "spam" RTS messages // setDTRSenderTimer(MAX_WAIT_TIME_FOR_CTS); timerDTRpacket = packet; - DTRstartSenderTimer(MAX_WAIT_TIME_FOR_CTS); + dtrStartSenderTimer(MAX_WAIT_TIME_FOR_CTS); break; case TX_TOKEN: @@ -211,7 +211,7 @@ static void setupRadioTx(DTRpacket* packet, TxStates txState) { // set the radio timer to "spam" token messages // setDTRSenderTimer(MAX_WAIT_TIME_FOR_RTS); timerDTRpacket = packet; - DTRstartSenderTimer(MAX_WAIT_TIME_FOR_RTS); + dtrStartSenderTimer(MAX_WAIT_TIME_FOR_RTS); break; case TX_DATA_FRAME: @@ -221,14 +221,14 @@ static void setupRadioTx(DTRpacket* packet, TxStates txState) { // set the radio timer to "spam" the same DATA frame // setDTRSenderTimer(MAX_WAIT_TIME_FOR_DATA_ACK); timerDTRpacket = packet; - DTRstartSenderTimer(MAX_WAIT_TIME_FOR_DATA_ACK); + dtrStartSenderTimer(MAX_WAIT_TIME_FOR_DATA_ACK); break; default: DEBUG_PRINT("\nRadio transmitter state not set correctly!!\n"); return; } - DTRsendP2Ppacket(packet); + dtrSendP2Ppacket(packet); radioMetaInfo.sendPackets++; @@ -238,15 +238,15 @@ static void resetProtocol(void){ DTR_DEBUG_PRINT("\nResetting protocol\n"); rx_state = RX_IDLE; protocol_timeout_ms = T2M(xTaskGetTickCount()) + PROTOCOL_TIMEOUT_MS; - emptyDTRQueues(); // Maybe not all queues, but only the RX_SRV since the data are going to be lost saved in DATA queues - DTRshutdownSenderTimer(); + dtrEmptyQueues(); // Maybe not all queues, but only the RX_SRV since the data are going to be lost saved in DATA queues + dtrShutdownSenderTimer(); last_packet_source_id = 255; } // static uint8_t send_data_to_peer_counter = 0; static uint8_t next_sender_id = 255; -void DTRInterruptHandler(void *param) { +void dtrTaskHandler(void *param) { DTRpacket* rxPk, *txPk; @@ -256,7 +256,7 @@ void DTRInterruptHandler(void *param) { bool new_packet_received; DTR_DEBUG_PRINT("\nDTRInterruptHandler Task called...\n"); - while ( DTRreceivePacketWaitUntil(&_rxPk, RX_SRV_Q, PROTOCOL_TIMEOUT_MS, &new_packet_received) ){ + while ( dtrReceivePacketWaitUntil(&_rxPk, RX_SRV_Q, PROTOCOL_TIMEOUT_MS, &new_packet_received) ){ if (!new_packet_received) { DTR_DEBUG_PRINT("\nPROTOCOL TIMEOUT!\n"); if (my_id != networkTopology.devices_ids[0]) { @@ -272,12 +272,12 @@ void DTRInterruptHandler(void *param) { DTR_DEBUG_PRINT("===============================================================\n"); DTR_DEBUG_PRINT("=\n"); - DTR_DEBUG_PRINT("TX_DATA Q Empty: %d\n", !DTRisPacketInQueueAvailable(TX_DATA_Q)); + DTR_DEBUG_PRINT("TX_DATA Q Empty: %d\n", !dtrIsPacketInQueueAvailable(TX_DATA_Q)); DTR_DEBUG_PRINT("rx_state: %s\n", getRXState(rx_state)); - // DTRprintPacket(rxPk); + // dtrPrintPacket(rxPk); DTR_DEBUG_PRINT("Received packet type: %d\n", rxPk->message_type); DTR_DEBUG_PRINT("Received packet target id: %d\n", rxPk->target_id); DTR_DEBUG_PRINT("my_id: %d\n", node_id); @@ -289,11 +289,13 @@ void DTRInterruptHandler(void *param) { * then it can be handled. */ if (rxPk->message_type == DATA_FRAME && rxPk->target_id == node_id) { DTR_DEBUG_PRINT("\nReceived DATA packet from prev\n"); - if (rxPk->source_id != last_packet_source_id) { + bool received_starting_packet = (rxPk->data[0] == (uint8_t) (START_PACKET >> 8)) + && (rxPk->data[1] == (uint8_t) START_PACKET) ; + if (rxPk->source_id != last_packet_source_id && !received_starting_packet) { last_packet_source_id = rxPk->source_id; /* if packet is relevant and receiver queue is not full, then * push packet in the queue and prepare queue for next packet. */ - bool queueFull = DTRinsertPacketToQueue(rxPk, RX_DATA_Q); + bool queueFull = !dtrInsertPacketToQueue(rxPk, RX_DATA_Q); if (queueFull){ radioMetaInfo.failedRxQueueFull++; } @@ -334,24 +336,24 @@ void DTRInterruptHandler(void *param) { /* if packet is CTS and received from previous node, then node can * send its next DATA packet. */ if (rxPk->message_type == CTS_FRAME && rxPk->source_id == prev_node_id) { - DTRshutdownSenderTimer(); + dtrShutdownSenderTimer(); last_packet_source_id = node_id; DTR_DEBUG_PRINT("\nRcvd CTS from prev,send DATA to next\n"); /* check if there is a DATA packet. If yes, prepare it and * send it, otherwise forward the token to the next node. */ DTRpacket _txPk;//TODO: bad implementation, should be fixed - if (DTRgetPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME))) { + if (dtrGetPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME))) { txPk = &_txPk; - DTR_DEBUG_PRINT("TX DATA Packet exists (%d), sending it\n",txPk->data[0]); + DTR_DEBUG_PRINT("TX DATA Packet exists (dataSize: %d), sending it\n",txPk->dataSize); if(txPk->allToAllFlag) { txPk->target_id = next_node_id; } if (!IdExistsInTopology(txPk->target_id)) { DEBUG_PRINT("Releasing DTR TX packet,target is not in topology.\n"); - DTR_DEBUG_PRINT("Is Queue Empty: %d\n", !DTRisPacketInQueueAvailable(TX_DATA_Q)); + DTR_DEBUG_PRINT("Is Queue Empty: %d\n", !dtrIsPacketInQueueAvailable(TX_DATA_Q)); - releaseDTRPacketFromQueue(TX_DATA_Q); + dtrReleasePacketFromQueue(TX_DATA_Q); txPk = &servicePk; txPk->message_type = TOKEN_FRAME; tx_state = TX_TOKEN; @@ -381,7 +383,7 @@ void DTRInterruptHandler(void *param) { * a CTS packet can be sent. */ if (rxPk->message_type == RTS_FRAME && rxPk->source_id == next_node_id) { DTR_DEBUG_PRINT("\nReceived TOKEN_ACK from next->sending CTS \n"); - DTRshutdownSenderTimer(); + dtrShutdownSenderTimer(); servicePk.message_type = CTS_FRAME; setupRadioTx(&servicePk, TX_CTS); continue; @@ -392,11 +394,11 @@ void DTRInterruptHandler(void *param) { case RX_WAIT_DATA_ACK: if (rxPk->message_type == DATA_ACK_FRAME && rxPk->target_id == node_id) { - DTRshutdownSenderTimer(); + dtrShutdownSenderTimer(); //TODO: bad implementation, should be fixed DTRpacket _txPk; - DTRgetPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME)); + dtrGetPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME)); txPk = &_txPk; if (next_sender_id == 255){ @@ -409,11 +411,11 @@ void DTRInterruptHandler(void *param) { DTR_DEBUG_PRINT("next_target_id: %d\n", next_target_id); if (!txPk->allToAllFlag || next_target_id == node_id) { DTR_DEBUG_PRINT("Releasing TX DATA:\n"); - // DTRprintPacket(txPk); + // dtrPrintPacket(txPk); - DTR_DEBUG_PRINT("Is Q Empty: %d\n", !DTRisPacketInQueueAvailable(TX_DATA_Q)); + DTR_DEBUG_PRINT("Is Q Empty: %d\n", !dtrIsPacketInQueueAvailable(TX_DATA_Q)); - releaseDTRPacketFromQueue(TX_DATA_Q); + dtrReleasePacketFromQueue(TX_DATA_Q); txPk = &servicePk; txPk->message_type = TOKEN_FRAME; tx_state = TX_TOKEN; @@ -450,11 +452,11 @@ void DTRInterruptHandler(void *param) { } -uint8_t DTRgetDeviceAddress() { +uint8_t dtrGetDeviceAddress() { return node_id; } -void DTRtimeOutCallBack(xTimerHandle timer) { +void dtrTimeOutCallBack(xTimerHandle timer) { #ifdef DEBUG_DTR_PROTOCOL DTR_DEBUG_PRINT("Sending packet after timeout: %s",getMessageType(timerDTRpacket->message_type)); if (timerDTRpacket->message_type == DATA_FRAME) { @@ -466,7 +468,7 @@ void DTRtimeOutCallBack(xTimerHandle timer) { DTR_DEBUG_PRINT("\n"); #endif - DTRsendP2Ppacket(timerDTRpacket); + dtrSendP2Ppacket(timerDTRpacket); radioMetaInfo.sendPackets++; switch(tx_state) { @@ -484,22 +486,23 @@ void DTRtimeOutCallBack(xTimerHandle timer) { } } -const RadioInfo* DTRgetRadioInfo() { +const RadioInfo* dtrGetRadioInfo() { radioMetaInfo.deviceId = node_id; return &radioMetaInfo; } -void DTRresetRadioMetaInfo() { +void dtrResetRadioMetaInfo() { memset(&radioMetaInfo, 0, sizeof(radioMetaInfo)); } -void DTRstartCommunication() { +static void dtrStartCommunication() { DTRpacket* startSignal = &startPacket; startSignal->source_id = node_id; startSignal->target_id = next_node_id; - startSignal->data[0] = 111; - startSignal->dataSize = 1; + startSignal->data[0] = (uint8_t) (START_PACKET >> 8); + startSignal->data[1] = (uint8_t) START_PACKET; + startSignal->dataSize = 2; startSignal->packetSize = DTR_PACKET_HEADER_SIZE + startSignal->dataSize; startSignal->message_type = DATA_FRAME; timerDTRpacket = startSignal; @@ -507,11 +510,12 @@ void DTRstartCommunication() { timerRadioTxState = TX_DATA_FRAME; rx_state = RX_WAIT_DATA_ACK; DTR_DEBUG_PRINT("Spamming DATA\n"); - DTRstartSenderTimer(MAX_WAIT_TIME_FOR_DATA_ACK); + dtrSendPacket(startSignal); + dtrStartSenderTimer(MAX_WAIT_TIME_FOR_DATA_ACK); } -void DTRprintPacket(DTRpacket* packet){ +void dtrPrintPacket(DTRpacket* packet){ DTR_DEBUG_PRINT("\nDTR Packet Received: %s\n", getMessageType(packet->message_type)); DTR_DEBUG_PRINT("Packet Size: %d\n", packet->packetSize); DTR_DEBUG_PRINT("Message Type: %s\n", getMessageType(packet->message_type)); @@ -529,7 +533,7 @@ void DTRprintPacket(DTRpacket* packet){ DTR_DEBUG_PRINT("\n\n"); } -uint8_t DTRgetSelfId(void){ +uint8_t dtrGetSelfId(void){ // Get the current address of the crazyflie and //keep the last 2 digits as the id of the crazyflie. uint64_t address = configblockGetRadioAddress(); @@ -537,54 +541,50 @@ uint8_t DTRgetSelfId(void){ return my_id; } -void DTRenableProtocol(DTRtopology topology){ +void dtrEnableProtocol(DTRtopology topology){ DTR_DEBUG_PRINT("Initializing queues ...\n"); - DTRqueueingInit(); + dtrQueueingInit(); DTR_DEBUG_PRINT("Initializing token ring ...\n"); initTokenRing(topology, my_id); DTR_DEBUG_PRINT("Initializing DTR Sender ...\n"); - DTRinitSenderTimer(); + dtrInitSenderTimer(); DTR_DEBUG_PRINT("Starting protocol timer ...\n"); - DTRstartProtocolTask(); + dtrStartProtocolTask(); // The first node starts the protocol communication if (my_id == topology.devices_ids[0]) { - DTRstartCommunication(); + dtrStartCommunication(); } } void DisableDTRProtocol(void){ DTR_DEBUG_PRINT("Stopping protocol timer ...\n"); - DTRstopProtocolTask(); + dtrStopProtocolTask(); DTR_DEBUG_PRINT("Stopping DTR Sender ...\n"); - DTRshutdownSenderTimer(); + dtrShutdownSenderTimer(); DTR_DEBUG_PRINT("Resetting token ring ...\n"); resetProtocol(); DTR_DEBUG_PRINT("Stopping queues ...\n"); - emptyDTRQueues(); + dtrEmptyQueues(); } -bool DTRsendPacket(DTRpacket* packet){ +bool dtrSendPacket(DTRpacket* packet){ packet->message_type = DATA_FRAME; packet->source_id = node_id; packet->packetSize = DTR_PACKET_HEADER_SIZE + packet->dataSize; - return DTRinsertPacketToQueue(packet,TX_DATA_Q); + return dtrInsertPacketToQueue(packet,TX_DATA_Q); } -bool DTRgetPacket(DTRpacket* packet, uint32_t timeout){ - if (timeout != portMAX_DELAY){ - timeout = M2T(timeout); - } - - return DTRgetPacketFromQueue(packet,RX_DATA_Q,timeout); +bool dtrGetPacket(DTRpacket* packet, uint32_t timeout){ + return dtrGetPacketFromQueue(packet,RX_DATA_Q,timeout); } From c6897b094a476f4f6d22f48dd2eb917922f7eb59 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Wed, 24 Aug 2022 14:57:58 +0200 Subject: [PATCH 15/19] Include swarm flashing script in utils --- tools/utils/cload-all.sh | 46 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100755 tools/utils/cload-all.sh diff --git a/tools/utils/cload-all.sh b/tools/utils/cload-all.sh new file mode 100755 index 0000000000..4966e321ef --- /dev/null +++ b/tools/utils/cload-all.sh @@ -0,0 +1,46 @@ +#!/usr/bin/env bash +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2022 Bitcraze AB +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, in version 3. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +# This script is used to flash a firmware to all Crazyflies listed in the +# uris array.Keep in mind that the file must be at the same location as the +# firmware/app you want to flash. + +COLOR_RESET='\033[0m' # Text Reset +YELLOW='\033[0;33m' # Yellow + +# define array of uris +uris=( + "radio://0/10/2M/E7E7E7E701" + "radio://0/10/2M/E7E7E7E702" + "radio://0/10/2M/E7E7E7E703" + "radio://0/10/2M/E7E7E7E704" + "radio://0/10/2M/E7E7E7E705" + "radio://0/10/2M/E7E7E7E706" + "radio://0/10/2M/E7E7E7E707" + "radio://0/10/2M/E7E7E7E708" + "radio://0/10/2M/E7E7E7E709" +) + +for uri in "${uris[@]}" ; do + printf "${YELLOW}Flashing CF $uri ${COLOR_RESET}\n" + CLOAD_CMDS="-w ${uri}" make cload +done From 99bc50ec9beb6b529799c36c1a5e5a78467c7bf4 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Wed, 24 Aug 2022 16:24:17 +0200 Subject: [PATCH 16/19] Remove allToAll flag --- docs/functional-areas/p2p_DTR_api.md | 5 ++--- examples/app_p2p_DTR/src/p2p_DTR_app.c | 4 ++-- src/modules/interface/p2pDTR/DTR_types.h | 5 +---- src/modules/src/p2pDTR/token_ring.c | 7 +++---- 4 files changed, 8 insertions(+), 13 deletions(-) diff --git a/docs/functional-areas/p2p_DTR_api.md b/docs/functional-areas/p2p_DTR_api.md index 40fd7e75ad..2acbbaf586 100644 --- a/docs/functional-areas/p2p_DTR_api.md +++ b/docs/functional-areas/p2p_DTR_api.md @@ -23,12 +23,11 @@ typedef struct radio_packet { uint8_t source_id; uint8_t target_id; uint8_t dataSize; - bool allToAllFlag; uint8_t data[MAXIMUM_DTR_PACKET_DATA_SIZE]; } DTRpacket; ``` -Where PacketSize is the size of the packet in bytes, MessageType is the type of the message, SourceID is the ID of the source, TargetID is the ID of the target, DataSize is the size of the data in bytes, AllToAllFlag is a flag indicating if the message is to be sent to all the other Crazyflies or not, and Data is the data of the message.The maximum size of the data is 53 since the data for the P2P is 60 and the protocol occupies 7 of them for operation. +Where PacketSize is the size of the packet in bytes, MessageType is the type of the message, SourceID is the ID of the source, TargetID is the ID of the target, DataSize is the size of the data in bytes, Data is the data of the message.The maximum size of the data is 55 since the data for the P2P is 60 and the protocol occupies 5 of them for operation. ## Setting up the Token Ring Protocol Since the nodes od the token ring are static, the user has to define the topology of the network. The topology is defined by the number of nodes and their ids in the order they are connected. The topology is defined like following: @@ -98,7 +97,7 @@ This function takes the packet to be sent as a parameter. The packet must be fil packet.data[0] = 0x01; packet.data[1] = 0x02; packet.data[2] = 0x03; - packet.allToAllFlag = 1; + packet.target_id = 0xFF; dtrSendPacket(&packet); ``` diff --git a/examples/app_p2p_DTR/src/p2p_DTR_app.c b/examples/app_p2p_DTR/src/p2p_DTR_app.c index 3ec0879598..f570f7b261 100644 --- a/examples/app_p2p_DTR/src/p2p_DTR_app.c +++ b/examples/app_p2p_DTR/src/p2p_DTR_app.c @@ -65,7 +65,7 @@ void loadTXPacketsForTesting(void){ const char testMessage[STARTING_MESSAGE_SIZE] = "Hello World"; strcpy(testSignal.data, testMessage); testSignal.dataSize = STARTING_MESSAGE_SIZE; - testSignal.allToAllFlag = 1; + testSignal.target_id = 0xFF; testSignal.packetSize = DTR_PACKET_HEADER_SIZE + testSignal.dataSize; bool res; res = dtrSendPacket(&testSignal); @@ -86,7 +86,7 @@ void loadResponse(void){ const char testMessage[25] = "Hello from the other side"; strcpy(testSignal.data, testMessage); testSignal.dataSize = 25; - testSignal.allToAllFlag = 1; + testSignal.target_id = 0xFF; testSignal.packetSize = DTR_PACKET_HEADER_SIZE + testSignal.dataSize; bool res; res = dtrSendPacket(&testSignal); diff --git a/src/modules/interface/p2pDTR/DTR_types.h b/src/modules/interface/p2pDTR/DTR_types.h index 48391ae2cb..b5b3f4de62 100644 --- a/src/modules/interface/p2pDTR/DTR_types.h +++ b/src/modules/interface/p2pDTR/DTR_types.h @@ -40,7 +40,7 @@ #include "radiolink.h" #define MAX_NETWORK_SIZE 20 -#define DTR_PACKET_HEADER_SIZE 6 +#define DTR_PACKET_HEADER_SIZE 5 // max usable size for a packet is -1 byte for the port #define P2P_MAX_USABLE_DATA_SIZE P2P_MAX_DATA_SIZE -1 @@ -81,8 +81,6 @@ enum message_types { // |--------------------| // | dataSize | (1 B) // |--------------------| -// | allToAllFlag | (1 b) -// |--------------------| // | data | (dataSize B) // |--------------------| typedef struct radio_packet { @@ -91,7 +89,6 @@ typedef struct radio_packet { uint8_t source_id; uint8_t target_id; uint8_t dataSize; - bool allToAllFlag; uint8_t data[MAXIMUM_DTR_PACKET_DATA_SIZE]; } DTRpacket; diff --git a/src/modules/src/p2pDTR/token_ring.c b/src/modules/src/p2pDTR/token_ring.c index a04a92e5a6..9b8dd8c037 100644 --- a/src/modules/src/p2pDTR/token_ring.c +++ b/src/modules/src/p2pDTR/token_ring.c @@ -50,7 +50,7 @@ static DTRpacket startPacket; // not sure if this is needed static DTRpacket servicePk = { .dataSize = 0, .packetSize = DTR_PACKET_HEADER_SIZE, - .allToAllFlag = false, + .target_id = 254, }; static TxStates tx_state, timerRadioTxState; static RxStates rx_state; @@ -346,7 +346,7 @@ void dtrTaskHandler(void *param) { if (dtrGetPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME))) { txPk = &_txPk; DTR_DEBUG_PRINT("TX DATA Packet exists (dataSize: %d), sending it\n",txPk->dataSize); - if(txPk->allToAllFlag) { + if(txPk->target_id == 0xFF) { txPk->target_id = next_node_id; } if (!IdExistsInTopology(txPk->target_id)) { @@ -409,7 +409,7 @@ void dtrTaskHandler(void *param) { } DTR_DEBUG_PRINT("next_target_id: %d\n", next_target_id); - if (!txPk->allToAllFlag || next_target_id == node_id) { + if (!(txPk->target_id == 0xFF) || next_target_id == node_id) { DTR_DEBUG_PRINT("Releasing TX DATA:\n"); // dtrPrintPacket(txPk); @@ -521,7 +521,6 @@ void dtrPrintPacket(DTRpacket* packet){ DTR_DEBUG_PRINT("Message Type: %s\n", getMessageType(packet->message_type)); DTR_DEBUG_PRINT("Source ID: %d\n", packet->source_id); DTR_DEBUG_PRINT("Target ID: %d\n", packet->target_id); - DTR_DEBUG_PRINT("AllToAll Flag: %d\n", packet->allToAllFlag); if (packet->dataSize > 0) { DEBUG_PRINT("Data: "); for (int i = 0; i < packet->dataSize; i++) { From 9d967507c4c524d38c53f91ea72fc7048faf9b19 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Thu, 25 Aug 2022 09:58:15 +0200 Subject: [PATCH 17/19] DTR types renaming --- docs/functional-areas/p2p_DTR_api.md | 30 ++--- examples/app_p2p_DTR/src/p2p_DTR_app.c | 24 ++-- src/modules/interface/p2pDTR/DTR_types.h | 26 ++-- src/modules/interface/p2pDTR/p2p_interface.h | 2 +- src/modules/interface/p2pDTR/queueing.h | 6 +- src/modules/interface/p2pDTR/token_ring.h | 12 +- src/modules/src/p2pDTR/p2p_interface.c | 24 ++-- src/modules/src/p2pDTR/queueing.c | 14 +-- src/modules/src/p2pDTR/token_ring.c | 124 +++++++++---------- 9 files changed, 130 insertions(+), 132 deletions(-) diff --git a/docs/functional-areas/p2p_DTR_api.md b/docs/functional-areas/p2p_DTR_api.md index 2acbbaf586..149bd2c9c8 100644 --- a/docs/functional-areas/p2p_DTR_api.md +++ b/docs/functional-areas/p2p_DTR_api.md @@ -17,24 +17,24 @@ Functions and structures are defined in the header files `src/modules/interface/ Each packet has the following structure: ``` C -typedef struct radio_packet { +typedef struct { uint8_t packetSize; - uint8_t message_type; - uint8_t source_id; - uint8_t target_id; + uint8_t messageType; + uint8_t sourceId; + uint8_t targetId; uint8_t dataSize; uint8_t data[MAXIMUM_DTR_PACKET_DATA_SIZE]; -} DTRpacket; +} dtrPacket; ``` -Where PacketSize is the size of the packet in bytes, MessageType is the type of the message, SourceID is the ID of the source, TargetID is the ID of the target, DataSize is the size of the data in bytes, Data is the data of the message.The maximum size of the data is 55 since the data for the P2P is 60 and the protocol occupies 5 of them for operation. +Where `packetSize` is the size of the packet in bytes, `messageType` is the type of the message, `sourceID` is the ID of the source, `targetId` is the ID of the target. Keep in mind that due to the nature of the P2P protocol even if one node is targeted the data may be received from the others before it as well, the difference is that the sending of data stops as soon as it is received from the desired node. If broadcast in all nodes is demanded, `targetId` must be set to `0xFF`. `dataSize` is the size of the data in bytes, Data is the data of the message.The maximum size of the data is 55 since the data for the P2P is 60 and the protocol occupies 5 of them for operation. ## Setting up the Token Ring Protocol Since the nodes od the token ring are static, the user has to define the topology of the network. The topology is defined by the number of nodes and their ids in the order they are connected. The topology is defined like following: ``` C #define NETWORK_TOPOLOGY {.size = 4, .devices_ids = {0, 1, 2, 3} } // Maximum size of network is 20 by default -static DTRtopology topology = NETWORK_TOPOLOGY; +static dtrTopology topology = NETWORK_TOPOLOGY; ... @@ -54,7 +54,7 @@ In order for the protocol to work, the user must feed the protocol with incoming Example usage : ``` C -void p2pcallbackHandler(P2PPacket *p){ +void p2pCallbackHandler(P2PPacket *p){ // If the packet is a DTR service packet, then the handler will handle it. if (!dtrP2PIncomingHandler(p)){ // If packet was not handled from DTR , then it is a normal packet @@ -70,7 +70,7 @@ void p2pcallbackHandler(P2PPacket *p){ void main(){ ... // Register the callback function to handle incoming packets. - p2pRegisterCallback(p2pcallbackHandler); + p2pRegisterCallback(p2pCallbackHandler); ... } @@ -81,23 +81,23 @@ void main(){ To send data through the protocol, the user must call the function `dtrSendPacket`. ``` C - bool dtrSendPacket(DTRpacket* packet) + bool dtrSendPacket(dtrPacket* packet) ``` -This function takes the packet to be sent as a parameter. The packet must be filled wit the data the user wants to send and by defining the size of them. The function returns true if the packet was sent successfully to the DTR (**not to the receiver copter**) and false otherwise.Keep in mind that the packet is sent asynchronously and the user can continue to use the P2P API while the packet is being sent.It is not necessary to fill the `.source_id`, `message_type`, `packet_size` fields of the packet since they are automatically filled by the API function. After the execution of the function, the new packet is inserted in the queue of the protocol responsible for all the packets to be sent. +This function takes the packet to be sent as a parameter. The packet must be filled wit the data the user wants to send and by defining the size of them. The function returns true if the packet was sent successfully to the DTR (**not to the receiver copter**) and false otherwise.Keep in mind that the packet is sent asynchronously and the user can continue to use the P2P API while the packet is being sent.It is not necessary to fill the `.sourceId`, `messageType`, `packetSize` fields of the packet since they are automatically filled by the API function. After the execution of the function, the new packet is inserted in the queue of the protocol responsible for all the packets to be sent. Example Usage: ``` C // Initialize the packet - DTRpacket packet; + dtrPacket packet; // Fill the packet with the data packet.dataSize = 3; packet.data[0] = 0x01; packet.data[1] = 0x02; packet.data[2] = 0x03; - packet.target_id = 0xFF; + packet.targetId = 0xFF; dtrSendPacket(&packet); ``` @@ -107,14 +107,14 @@ This function takes the packet to be sent as a parameter. The packet must be fil The user must call the function `dtrGetPacket` to receive a packet from the DTR. ``` C -bool dtrGetPacket(DTRpacket* packet, uint32_t timeout); +bool dtrGetPacket(dtrPacket* packet, uint32_t timeout); ``` The function blocks for the specified time (in ticks) until a packet is received. If the user wants to block indefinitely, the timeout parameter must be set to `portMAX_DELAY`. The function returns true if a packet was received and false otherwise. If a packet is received, the packet is filled with the data received. In case it was received, the packet is filled with the data received and the corresponding packet is released from the queue responsible for the reception of the DTR packets. Example Usage: ``` C // Initialize the packet -DTRpacket packet; +dtrPacket packet; // Receive the packet while(1){ diff --git a/examples/app_p2p_DTR/src/p2p_DTR_app.c b/examples/app_p2p_DTR/src/p2p_DTR_app.c index f570f7b261..7c0dc1a2e1 100644 --- a/examples/app_p2p_DTR/src/p2p_DTR_app.c +++ b/examples/app_p2p_DTR/src/p2p_DTR_app.c @@ -55,17 +55,17 @@ #define NETWORK_TOPOLOGY {.size = 4, .devices_ids = {0, 1, 2, 3} } // Maximum size of network is 20 by default static uint8_t my_id; -static DTRtopology topology = NETWORK_TOPOLOGY; +static dtrTopology topology = NETWORK_TOPOLOGY; void loadTXPacketsForTesting(void){ - DTRpacket testSignal; - testSignal.message_type = DATA_FRAME; - testSignal.source_id = my_id; + dtrPacket testSignal; + testSignal.messageType = DATA_FRAME; + testSignal.sourceId = my_id; const char testMessage[STARTING_MESSAGE_SIZE] = "Hello World"; strcpy(testSignal.data, testMessage); testSignal.dataSize = STARTING_MESSAGE_SIZE; - testSignal.target_id = 0xFF; + testSignal.targetId = 0xFF; testSignal.packetSize = DTR_PACKET_HEADER_SIZE + testSignal.dataSize; bool res; res = dtrSendPacket(&testSignal); @@ -78,15 +78,15 @@ void loadTXPacketsForTesting(void){ } void loadResponse(void){ - DTRpacket testSignal; - testSignal.message_type = DATA_FRAME; - testSignal.source_id = my_id; - testSignal.target_id = 1; + dtrPacket testSignal; + testSignal.messageType = DATA_FRAME; + testSignal.sourceId = my_id; + testSignal.targetId = 1; const char testMessage[25] = "Hello from the other side"; strcpy(testSignal.data, testMessage); testSignal.dataSize = 25; - testSignal.target_id = 0xFF; + testSignal.targetId = 0xFF; testSignal.packetSize = DTR_PACKET_HEADER_SIZE + testSignal.dataSize; bool res; res = dtrSendPacket(&testSignal); @@ -129,7 +129,7 @@ void appMain(){ loadTXPacketsForTesting(); } - DTRpacket received_packet; + dtrPacket received_packet; uint32_t start = T2M(xTaskGetTickCount()); while(1){ dtrGetPacket(&received_packet, portMAX_DELAY); @@ -142,7 +142,7 @@ void appMain(){ } data[received_packet.dataSize] = '\0'; - DEBUG_PRINT("Received data from %d : %s --> Time elapsed: %lu msec\n",received_packet.source_id, data, dt); + DEBUG_PRINT("Received data from %d : %s --> Time elapsed: %lu msec\n",received_packet.sourceId, data, dt); start = T2M(xTaskGetTickCount()); diff --git a/src/modules/interface/p2pDTR/DTR_types.h b/src/modules/interface/p2pDTR/DTR_types.h index b5b3f4de62..e002013c98 100644 --- a/src/modules/interface/p2pDTR/DTR_types.h +++ b/src/modules/interface/p2pDTR/DTR_types.h @@ -53,16 +53,16 @@ typedef enum tx_states_e { TX_RTS, TX_DATA_FRAME, TX_DATA_ACK, -} TxStates; +} dtrTxStates; typedef enum rx_states_e { RX_IDLE, RX_WAIT_CTS, RX_WAIT_RTS, RX_WAIT_DATA_ACK, -} RxStates; +} dtrRxStates; -enum message_types { +enum dtrMessageTypes { DATA_FRAME = 0, TOKEN_FRAME = 1, CTS_FRAME = 2, @@ -75,24 +75,24 @@ enum message_types { // |--------------------| // | packetSize | (1 B) // |--------------------| -// | message_type | (1 B) +// | messageType | (1 B) // |--------------------| -// | source_id | (1 B) +// | sourceId | (1 B) // |--------------------| // | dataSize | (1 B) // |--------------------| // | data | (dataSize B) // |--------------------| -typedef struct radio_packet { +typedef struct { uint8_t packetSize; - uint8_t message_type; - uint8_t source_id; - uint8_t target_id; + uint8_t messageType; + uint8_t sourceId; + uint8_t targetId; uint8_t dataSize; uint8_t data[MAXIMUM_DTR_PACKET_DATA_SIZE]; -} DTRpacket; +} dtrPacket; -typedef struct radio_meta_info { +typedef struct { uint8_t deviceId; uint32_t failedCRC; @@ -106,11 +106,11 @@ typedef struct radio_meta_info { uint32_t sendPackets; uint32_t receivedPackets; -} RadioInfo; +} dtrRadioInfo; typedef struct { uint8_t size; // number of nodes in the network uint8_t devices_ids[MAX_NETWORK_SIZE]; // array that contains the device id of each node in the network -} DTRtopology; +} dtrTopology; #endif //DTR_TYPES_H diff --git a/src/modules/interface/p2pDTR/p2p_interface.h b/src/modules/interface/p2pDTR/p2p_interface.h index d3c8521f79..5c72046b51 100644 --- a/src/modules/interface/p2pDTR/p2p_interface.h +++ b/src/modules/interface/p2pDTR/p2p_interface.h @@ -36,7 +36,7 @@ #define INCOMING_DTR_QUEUE_SIZE 10 // Broadcasts a DTR packet through the P2P network -void dtrSendP2Ppacket(const DTRpacket* packet) ; +void dtrSendP2Ppacket(const dtrPacket* packet) ; // Puts the DTR packet in the queue for the token ring to pick up. bool dtrP2PIncomingHandler(P2PPacket *p); diff --git a/src/modules/interface/p2pDTR/queueing.h b/src/modules/interface/p2pDTR/queueing.h index 1b5203d5dc..23213a2a50 100644 --- a/src/modules/interface/p2pDTR/queueing.h +++ b/src/modules/interface/p2pDTR/queueing.h @@ -61,13 +61,13 @@ bool dtrIsPacketInQueueAvailable(DTRQueue_Names qName); uint8_t dtrGetNumberOfPacketsInQueue(DTRQueue_Names qName); -bool dtrGetPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout); +bool dtrGetPacketFromQueue(dtrPacket *packet, DTRQueue_Names qName, uint32_t timeout); // Blocks to wait for a packet to be received for a given time // new_packet_received --> True if a new packet has been received and False if the timeout has been reached -bool dtrReceivePacketWaitUntil(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received); +bool dtrReceivePacketWaitUntil(dtrPacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received); -bool dtrInsertPacketToQueue(DTRpacket *packet, DTRQueue_Names qName); +bool dtrInsertPacketToQueue(dtrPacket *packet, DTRQueue_Names qName); bool dtrReleasePacketFromQueue(DTRQueue_Names qName); diff --git a/src/modules/interface/p2pDTR/token_ring.h b/src/modules/interface/p2pDTR/token_ring.h index 51911719f7..28f5000a9c 100644 --- a/src/modules/interface/p2pDTR/token_ring.h +++ b/src/modules/interface/p2pDTR/token_ring.h @@ -68,26 +68,24 @@ #define START_PACKET 0xBCCF -// void initTokenRing(DTRtopology topology, uint8_t device_id); - uint8_t dtrGetDeviceAddress(); void dtrTimeOutCallBack(xTimerHandle timer); void dtrTaskHandler(void *param); -const RadioInfo* dtrGetRadioInfo(); +const dtrRadioInfo* dtrGetRadioInfo(); void dtrResetRadioMetaInfo(); -void dtrPrintPacket(DTRpacket* packet); +void dtrPrintPacket(dtrPacket* packet); uint8_t dtrGetSelfId(void); // =========================== DTR API =========================== // Starts the task of the Dynamic Token Ring Protocol (DTR) and initializes the protocol // @param topology The topology of the network (see DTR_types.h) -void dtrEnableProtocol(DTRtopology topology); +void dtrEnableProtocol(dtrTopology topology); // Stops the task of the Dynamic Token Ring Protocol (DTR) and deinitializes the protocol void DisableDTRProtocol(void); @@ -95,13 +93,13 @@ void DisableDTRProtocol(void); // Sends a packet to the DTR protocol // @param packet The packet to be sent // @return true if the packet was sent successfully to the DTR (not the final receiver), false otherwise -bool dtrSendPacket(DTRpacket* packet); +bool dtrSendPacket(dtrPacket* packet); // Receives a packet from the DTR Protocol // Blocks for the specified timeout if no packet is received // @param packet: the packet to be received // @param timeout: the timeout in ticks // @return true if the packet was received, false otherwise -bool dtrGetPacket(DTRpacket* packet, uint32_t timeout); +bool dtrGetPacket(dtrPacket* packet, uint32_t timeout); #endif /* SRC_RADIO_RADIO_H_ */ diff --git a/src/modules/src/p2pDTR/p2p_interface.c b/src/modules/src/p2pDTR/p2p_interface.c index d695203202..b34520a1b1 100644 --- a/src/modules/src/p2pDTR/p2p_interface.c +++ b/src/modules/src/p2pDTR/p2p_interface.c @@ -34,9 +34,9 @@ #include "debug.h" static P2PPacket p2p_TXpacket; -static DTRpacket prev_received = {0}; +static dtrPacket prev_received = {0}; -void dtrSendP2Ppacket(const DTRpacket* packet) { +void dtrSendP2Ppacket(const dtrPacket* packet) { p2p_TXpacket.port = DTR_P2P_PORT; memcpy(&p2p_TXpacket.data[0], packet, packet->packetSize); @@ -45,25 +45,25 @@ void dtrSendP2Ppacket(const DTRpacket* packet) { radiolinkSendP2PPacketBroadcast(&p2p_TXpacket); } -static void dtrFeedPacketToProtocol(DTRpacket *incoming_DTR) { +static void dtrFeedPacketToProtocol(dtrPacket *incoming_DTR) { - bool same_packet_received = incoming_DTR->message_type == prev_received.message_type && - incoming_DTR->target_id == prev_received.target_id && - incoming_DTR->source_id == prev_received.source_id; + bool same_packet_received = incoming_DTR->messageType == prev_received.messageType && + incoming_DTR->targetId == prev_received.targetId && + incoming_DTR->sourceId == prev_received.sourceId; // if there are packets in the queue and the new packet is the same as the previous one, ignore it DTR_DEBUG_PRINT("Packets in RX_SRV queue: %d\n", dtrGetNumberOfPacketsInQueue(RX_SRV_Q) ); if ( dtrGetNumberOfPacketsInQueue(RX_SRV_Q) !=0 && same_packet_received ) { DTR_DEBUG_PRINT("Duplicate packet received\n"); - DTR_DEBUG_PRINT("Message type: %d\n", incoming_DTR->message_type); - DTR_DEBUG_PRINT("Target id: %d\n", incoming_DTR->target_id); + DTR_DEBUG_PRINT("Message type: %d\n", incoming_DTR->messageType); + DTR_DEBUG_PRINT("Target id: %d\n", incoming_DTR->targetId); return; } - prev_received.message_type = incoming_DTR->message_type; - prev_received.target_id = incoming_DTR->target_id; - prev_received.source_id = incoming_DTR->source_id; + prev_received.messageType = incoming_DTR->messageType; + prev_received.targetId = incoming_DTR->targetId; + prev_received.sourceId = incoming_DTR->sourceId; dtrInsertPacketToQueue(incoming_DTR, RX_SRV_Q); } @@ -73,7 +73,7 @@ bool dtrP2PIncomingHandler(P2PPacket *p){ return false; } - DTRpacket incoming_DTR; + dtrPacket incoming_DTR; uint8_t DTRpacket_size = p->data[0]; diff --git a/src/modules/src/p2pDTR/queueing.c b/src/modules/src/p2pDTR/queueing.c index 3f2d64f4a5..e2e42757ac 100644 --- a/src/modules/src/p2pDTR/queueing.c +++ b/src/modules/src/p2pDTR/queueing.c @@ -65,17 +65,17 @@ static xQueueHandle *getQueueHandler(DTRQueue_Names qName){ void dtrQueueingInit(){ // TX SRV queue - STATIC_MEM_QUEUE_ALLOC(TX_DATA_queue, TX_DATA_QUEUE_SIZE, sizeof(DTRpacket)); + STATIC_MEM_QUEUE_ALLOC(TX_DATA_queue, TX_DATA_QUEUE_SIZE, sizeof(dtrPacket)); TX_DATA_queue = STATIC_MEM_QUEUE_CREATE(TX_DATA_queue); DEBUG_QUEUE_MONITOR_REGISTER(TX_DATA_queue); // RX SRV queue - STATIC_MEM_QUEUE_ALLOC(RX_SRV_queue, RX_SRV_QUEUE_SIZE, sizeof(DTRpacket)); + STATIC_MEM_QUEUE_ALLOC(RX_SRV_queue, RX_SRV_QUEUE_SIZE, sizeof(dtrPacket)); RX_SRV_queue = STATIC_MEM_QUEUE_CREATE(RX_SRV_queue); DEBUG_QUEUE_MONITOR_REGISTER(RX_SRV_queue); // RX DATA queue - STATIC_MEM_QUEUE_ALLOC(RX_DATA_queue, RX_DATA_QUEUE_SIZE, sizeof(DTRpacket)); + STATIC_MEM_QUEUE_ALLOC(RX_DATA_queue, RX_DATA_QUEUE_SIZE, sizeof(dtrPacket)); RX_DATA_queue = STATIC_MEM_QUEUE_CREATE(RX_DATA_queue); DEBUG_QUEUE_MONITOR_REGISTER(RX_DATA_queue); @@ -89,7 +89,7 @@ bool dtrIsPacketInQueueAvailable(DTRQueue_Names qName) { return uxQueueMessagesWaiting(*getQueueHandler(qName)) > 0; } -bool dtrGetPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout){ +bool dtrGetPacketFromQueue(dtrPacket *packet, DTRQueue_Names qName, uint32_t timeout){ // notice that xQueuePeek is used instead of xQueueReceive, because the packet is not removed from the queue //TODO: make a separate function for this bool received_success = false; @@ -114,12 +114,12 @@ bool dtrGetPacketFromQueue(DTRpacket *packet, DTRQueue_Names qName, uint32_t tim return received_success; } -bool dtrReceivePacketWaitUntil(DTRpacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received){ +bool dtrReceivePacketWaitUntil(dtrPacket *packet, DTRQueue_Names qName, uint32_t timeout_ms, bool *new_packet_received){ *new_packet_received = xQueueReceive(*getQueueHandler(qName), packet, M2T(timeout_ms)) == pdTRUE; return true; } -bool dtrInsertPacketToQueue(DTRpacket *packet, DTRQueue_Names qName) { +bool dtrInsertPacketToQueue(dtrPacket *packet, DTRQueue_Names qName) { bool res = (xQueueSend(*getQueueHandler(qName),(void *) packet, 0) == pdTRUE); if (!res) { DEBUG_PRINT("TX_DATA queue busy\n"); @@ -129,7 +129,7 @@ bool dtrInsertPacketToQueue(DTRpacket *packet, DTRQueue_Names qName) { } bool dtrReleasePacketFromQueue(DTRQueue_Names qName) { - DTRpacket packet; + dtrPacket packet; return xQueueReceive(*getQueueHandler(qName), &packet, M2T(TX_RECEIVED_WAIT_TIME)) == pdTRUE; } diff --git a/src/modules/src/p2pDTR/token_ring.c b/src/modules/src/p2pDTR/token_ring.c index 9b8dd8c037..03ed54f2ca 100644 --- a/src/modules/src/p2pDTR/token_ring.c +++ b/src/modules/src/p2pDTR/token_ring.c @@ -43,33 +43,33 @@ static uint8_t prev_node_id = 0; static uint8_t next_target_id = 0; static uint8_t last_packet_source_id = 0; -static DTRpacket* timerDTRpacket; +static dtrPacket* timerDTRpacket; -static DTRpacket startPacket; // not sure if this is needed +static dtrPacket startPacket; // not sure if this is needed -static DTRpacket servicePk = { +static dtrPacket servicePk = { .dataSize = 0, .packetSize = DTR_PACKET_HEADER_SIZE, - .target_id = 254, + .targetId = 254, }; -static TxStates tx_state, timerRadioTxState; -static RxStates rx_state; +static dtrTxStates tx_state, timerRadioTxState; +static dtrRxStates rx_state; -static RadioInfo radioMetaInfo; +static dtrRadioInfo radioMetaInfo; static uint32_t protocol_timeout_ms; static uint8_t my_id ; -static DTRtopology networkTopology; +static dtrTopology networkTopology; // DEBUGGING FUNCTIONS #ifdef DEBUG_DTR_PROTOCOL // get the message name from the message type -static const char* getMessageType(uint8_t message_type){ - switch(message_type){ +static const char* getMessageType(uint8_t messageType){ + switch(messageType){ case DATA_FRAME: return "DATA"; case TOKEN_FRAME: @@ -153,11 +153,11 @@ static bool IdExistsInTopology(uint8_t id){ return false; } -static void setNodeIds(DTRtopology topology, uint8_t device_id) { +static void setNodeIds(dtrTopology topology, uint8_t device_id) { networkTopology = topology; node_id = device_id; - servicePk.source_id = node_id; + servicePk.sourceId = node_id; last_packet_source_id = node_id; next_node_id = getNextNodeId(node_id); @@ -168,7 +168,7 @@ static void setNodeIds(DTRtopology topology, uint8_t device_id) { DTR_DEBUG_PRINT("prev node_id: %d\n", prev_node_id); } -static void initTokenRing(DTRtopology topology, uint8_t device_id) { +static void initTokenRing(dtrTopology topology, uint8_t device_id) { my_id = dtrGetSelfId(); /* Network node configuration*/ @@ -177,7 +177,7 @@ static void initTokenRing(DTRtopology topology, uint8_t device_id) { rx_state = RX_IDLE; } -static void setupRadioTx(DTRpacket* packet, TxStates txState) { +static void setupRadioTx(dtrPacket* packet, dtrTxStates txState) { tx_state = txState; DTR_DEBUG_PRINT("Setting up radio tx with state: %s \n", getTXState(tx_state)); @@ -248,10 +248,10 @@ static uint8_t next_sender_id = 255; void dtrTaskHandler(void *param) { - DTRpacket* rxPk, *txPk; + dtrPacket* rxPk, *txPk; //TODO: bad implementation, should be fixed - DTRpacket _rxPk ; + dtrPacket _rxPk ; protocol_timeout_ms = T2M(xTaskGetTickCount()) + PROTOCOL_TIMEOUT_MS; bool new_packet_received; @@ -278,8 +278,8 @@ void dtrTaskHandler(void *param) { DTR_DEBUG_PRINT("rx_state: %s\n", getRXState(rx_state)); // dtrPrintPacket(rxPk); - DTR_DEBUG_PRINT("Received packet type: %d\n", rxPk->message_type); - DTR_DEBUG_PRINT("Received packet target id: %d\n", rxPk->target_id); + DTR_DEBUG_PRINT("Received packet type: %d\n", rxPk->messageType); + DTR_DEBUG_PRINT("Received packet target id: %d\n", rxPk->targetId); DTR_DEBUG_PRINT("my_id: %d\n", node_id); switch (rx_state) { @@ -287,12 +287,12 @@ void dtrTaskHandler(void *param) { case RX_IDLE: /* if packet is DATA packet and received from previous node, * then it can be handled. */ - if (rxPk->message_type == DATA_FRAME && rxPk->target_id == node_id) { + if (rxPk->messageType == DATA_FRAME && rxPk->targetId == node_id) { DTR_DEBUG_PRINT("\nReceived DATA packet from prev\n"); bool received_starting_packet = (rxPk->data[0] == (uint8_t) (START_PACKET >> 8)) && (rxPk->data[1] == (uint8_t) START_PACKET) ; - if (rxPk->source_id != last_packet_source_id && !received_starting_packet) { - last_packet_source_id = rxPk->source_id; + if (rxPk->sourceId != last_packet_source_id && !received_starting_packet) { + last_packet_source_id = rxPk->sourceId; /* if packet is relevant and receiver queue is not full, then * push packet in the queue and prepare queue for next packet. */ bool queueFull = !dtrInsertPacketToQueue(rxPk, RX_DATA_Q); @@ -303,26 +303,26 @@ void dtrTaskHandler(void *param) { /* Acknowledge the data */ DTR_DEBUG_PRINT("Received DATA %d\n", rxPk->data[0]); DTR_DEBUG_PRINT("\nSending ACK packet\n"); - servicePk.message_type = DATA_ACK_FRAME; - servicePk.target_id = rxPk->source_id; + servicePk.messageType = DATA_ACK_FRAME; + servicePk.targetId = rxPk->sourceId; setupRadioTx(&servicePk, TX_DATA_ACK); continue; } /* if packet is TOKEN packet and received from previous node, then * send a TOKEN_ACK packet and prepare the packet for the timer. */ - if (rxPk->message_type == TOKEN_FRAME && rxPk->source_id == prev_node_id) { + if (rxPk->messageType == TOKEN_FRAME && rxPk->sourceId == prev_node_id) { DTR_DEBUG_PRINT("\nReceived TOKEN from prev\n"); - servicePk.message_type = RTS_FRAME; + servicePk.messageType = RTS_FRAME; setupRadioTx(&servicePk, TX_RTS); continue; } /* if packet is RTS packet and received from next node, then send * a CTS packet to next node. */ - if (rxPk->message_type == RTS_FRAME && rxPk->source_id == next_node_id) { + if (rxPk->messageType == RTS_FRAME && rxPk->sourceId == next_node_id) { DTR_DEBUG_PRINT("\nReceived RTS from next -> CTS to next\n"); - servicePk.message_type = CTS_FRAME; + servicePk.messageType = CTS_FRAME; setupRadioTx(&servicePk, TX_CTS); continue; } @@ -335,38 +335,38 @@ void dtrTaskHandler(void *param) { /* if packet is CTS and received from previous node, then node can * send its next DATA packet. */ - if (rxPk->message_type == CTS_FRAME && rxPk->source_id == prev_node_id) { + if (rxPk->messageType == CTS_FRAME && rxPk->sourceId == prev_node_id) { dtrShutdownSenderTimer(); last_packet_source_id = node_id; DTR_DEBUG_PRINT("\nRcvd CTS from prev,send DATA to next\n"); /* check if there is a DATA packet. If yes, prepare it and * send it, otherwise forward the token to the next node. */ - DTRpacket _txPk;//TODO: bad implementation, should be fixed + dtrPacket _txPk;//TODO: bad implementation, should be fixed if (dtrGetPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME))) { txPk = &_txPk; DTR_DEBUG_PRINT("TX DATA Packet exists (dataSize: %d), sending it\n",txPk->dataSize); - if(txPk->target_id == 0xFF) { - txPk->target_id = next_node_id; + if(txPk->targetId == 0xFF) { + txPk->targetId = next_node_id; } - if (!IdExistsInTopology(txPk->target_id)) { + if (!IdExistsInTopology(txPk->targetId)) { DEBUG_PRINT("Releasing DTR TX packet,target is not in topology.\n"); DTR_DEBUG_PRINT("Is Queue Empty: %d\n", !dtrIsPacketInQueueAvailable(TX_DATA_Q)); dtrReleasePacketFromQueue(TX_DATA_Q); txPk = &servicePk; - txPk->message_type = TOKEN_FRAME; + txPk->messageType = TOKEN_FRAME; tx_state = TX_TOKEN; } else { txPk->packetSize = DTR_PACKET_HEADER_SIZE + txPk->dataSize; - txPk->source_id = node_id; - txPk->message_type = DATA_FRAME; + txPk->sourceId = node_id; + txPk->messageType = DATA_FRAME; tx_state = TX_DATA_FRAME; } } else { DTR_DEBUG_PRINT("No TX DATA,forwarding token to next\n"); txPk = &servicePk; - txPk->message_type = TOKEN_FRAME; + txPk->messageType = TOKEN_FRAME; tx_state = TX_TOKEN; } @@ -381,10 +381,10 @@ void dtrTaskHandler(void *param) { /* if packet is TOKEN_ACK and received from next node, then * a CTS packet can be sent. */ - if (rxPk->message_type == RTS_FRAME && rxPk->source_id == next_node_id) { + if (rxPk->messageType == RTS_FRAME && rxPk->sourceId == next_node_id) { DTR_DEBUG_PRINT("\nReceived TOKEN_ACK from next->sending CTS \n"); dtrShutdownSenderTimer(); - servicePk.message_type = CTS_FRAME; + servicePk.messageType = CTS_FRAME; setupRadioTx(&servicePk, TX_CTS); continue; } @@ -393,11 +393,11 @@ void dtrTaskHandler(void *param) { break; case RX_WAIT_DATA_ACK: - if (rxPk->message_type == DATA_ACK_FRAME && rxPk->target_id == node_id) { + if (rxPk->messageType == DATA_ACK_FRAME && rxPk->targetId == node_id) { dtrShutdownSenderTimer(); //TODO: bad implementation, should be fixed - DTRpacket _txPk; + dtrPacket _txPk; dtrGetPacketFromQueue(&_txPk, TX_DATA_Q, M2T(TX_RECEIVED_WAIT_TIME)); txPk = &_txPk; @@ -409,7 +409,7 @@ void dtrTaskHandler(void *param) { } DTR_DEBUG_PRINT("next_target_id: %d\n", next_target_id); - if (!(txPk->target_id == 0xFF) || next_target_id == node_id) { + if (!(txPk->targetId == 0xFF) || next_target_id == node_id) { DTR_DEBUG_PRINT("Releasing TX DATA:\n"); // dtrPrintPacket(txPk); @@ -417,14 +417,14 @@ void dtrTaskHandler(void *param) { dtrReleasePacketFromQueue(TX_DATA_Q); txPk = &servicePk; - txPk->message_type = TOKEN_FRAME; + txPk->messageType = TOKEN_FRAME; tx_state = TX_TOKEN; next_sender_id = 255; } else { - txPk->target_id = next_target_id; + txPk->targetId = next_target_id; next_sender_id = next_target_id; DTR_DEBUG_PRINT("Sending DATA to next peer.."); - DTR_DEBUG_PRINT("with target id: %d\n", txPk->target_id); + DTR_DEBUG_PRINT("with target id: %d\n", txPk->targetId); tx_state = TX_DATA_FRAME; } setupRadioTx(txPk, tx_state); @@ -445,7 +445,7 @@ void dtrTaskHandler(void *param) { // DEBUG_PRINT("Packet Received cannot be processed\n"); // DEBUG_PRINT("State: %d\n", rx_state); - // DEBUG_PRINT("Packet type: %d\n", rxPk->message_type); + // DEBUG_PRINT("Packet type: %d\n", rxPk->messageType); // resetProtocol(); //TODO: test if it makes sense } @@ -458,8 +458,8 @@ uint8_t dtrGetDeviceAddress() { void dtrTimeOutCallBack(xTimerHandle timer) { #ifdef DEBUG_DTR_PROTOCOL - DTR_DEBUG_PRINT("Sending packet after timeout: %s",getMessageType(timerDTRpacket->message_type)); - if (timerDTRpacket->message_type == DATA_FRAME) { + DTR_DEBUG_PRINT("Sending packet after timeout: %s",getMessageType(timerDTRpacket->messageType)); + if (timerDTRpacket->messageType == DATA_FRAME) { for (size_t i = 0; i dataSize; i++) { DTR_DEBUG_PRINT(" %d ",timerDTRpacket->data[i]); @@ -486,7 +486,7 @@ void dtrTimeOutCallBack(xTimerHandle timer) { } } -const RadioInfo* dtrGetRadioInfo() { +const dtrRadioInfo* dtrGetRadioInfo() { radioMetaInfo.deviceId = node_id; return &radioMetaInfo; } @@ -496,15 +496,15 @@ void dtrResetRadioMetaInfo() { } static void dtrStartCommunication() { - DTRpacket* startSignal = &startPacket; + dtrPacket* startSignal = &startPacket; - startSignal->source_id = node_id; - startSignal->target_id = next_node_id; + startSignal->sourceId = node_id; + startSignal->targetId = next_node_id; startSignal->data[0] = (uint8_t) (START_PACKET >> 8); startSignal->data[1] = (uint8_t) START_PACKET; startSignal->dataSize = 2; startSignal->packetSize = DTR_PACKET_HEADER_SIZE + startSignal->dataSize; - startSignal->message_type = DATA_FRAME; + startSignal->messageType = DATA_FRAME; timerDTRpacket = startSignal; timerRadioTxState = TX_DATA_FRAME; @@ -515,12 +515,12 @@ static void dtrStartCommunication() { } -void dtrPrintPacket(DTRpacket* packet){ - DTR_DEBUG_PRINT("\nDTR Packet Received: %s\n", getMessageType(packet->message_type)); +void dtrPrintPacket(dtrPacket* packet){ + DTR_DEBUG_PRINT("\nDTR Packet Received: %s\n", getMessageType(packet->messageType)); DTR_DEBUG_PRINT("Packet Size: %d\n", packet->packetSize); - DTR_DEBUG_PRINT("Message Type: %s\n", getMessageType(packet->message_type)); - DTR_DEBUG_PRINT("Source ID: %d\n", packet->source_id); - DTR_DEBUG_PRINT("Target ID: %d\n", packet->target_id); + DTR_DEBUG_PRINT("Message Type: %s\n", getMessageType(packet->messageType)); + DTR_DEBUG_PRINT("Source ID: %d\n", packet->sourceId); + DTR_DEBUG_PRINT("Target ID: %d\n", packet->targetId); if (packet->dataSize > 0) { DEBUG_PRINT("Data: "); for (int i = 0; i < packet->dataSize; i++) { @@ -540,7 +540,7 @@ uint8_t dtrGetSelfId(void){ return my_id; } -void dtrEnableProtocol(DTRtopology topology){ +void dtrEnableProtocol(dtrTopology topology){ DTR_DEBUG_PRINT("Initializing queues ...\n"); dtrQueueingInit(); @@ -574,15 +574,15 @@ void DisableDTRProtocol(void){ } -bool dtrSendPacket(DTRpacket* packet){ - packet->message_type = DATA_FRAME; - packet->source_id = node_id; +bool dtrSendPacket(dtrPacket* packet){ + packet->messageType = DATA_FRAME; + packet->sourceId = node_id; packet->packetSize = DTR_PACKET_HEADER_SIZE + packet->dataSize; return dtrInsertPacketToQueue(packet,TX_DATA_Q); } -bool dtrGetPacket(DTRpacket* packet, uint32_t timeout){ +bool dtrGetPacket(dtrPacket* packet, uint32_t timeout){ return dtrGetPacketFromQueue(packet,RX_DATA_Q,timeout); } From 01268ff5df368f4708b213c10f26a927972c6c51 Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Thu, 25 Aug 2022 10:23:11 +0200 Subject: [PATCH 18/19] Fixed DTR early stop of data sending --- src/modules/src/p2pDTR/token_ring.c | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/modules/src/p2pDTR/token_ring.c b/src/modules/src/p2pDTR/token_ring.c index 03ed54f2ca..4875d8dc49 100644 --- a/src/modules/src/p2pDTR/token_ring.c +++ b/src/modules/src/p2pDTR/token_ring.c @@ -409,10 +409,19 @@ void dtrTaskHandler(void *param) { } DTR_DEBUG_PRINT("next_target_id: %d\n", next_target_id); - if (!(txPk->targetId == 0xFF) || next_target_id == node_id) { + + bool not_in_broadcast_mode = !(txPk->targetId == 0xFF); + bool reached_desired_node = not_in_broadcast_mode && (txPk->targetId == next_target_id) ; + + bool reached_self = (next_target_id == node_id); + + if ( reached_desired_node || reached_self ) { DTR_DEBUG_PRINT("Releasing TX DATA:\n"); // dtrPrintPacket(txPk); - + + if (reached_desired_node){ + DTR_DEBUG_PRINT("Reached desired node (%d), stop data sending earlier...\n", txPk->targetId); + } DTR_DEBUG_PRINT("Is Q Empty: %d\n", !dtrIsPacketInQueueAvailable(TX_DATA_Q)); dtrReleasePacketFromQueue(TX_DATA_Q); @@ -420,11 +429,11 @@ void dtrTaskHandler(void *param) { txPk->messageType = TOKEN_FRAME; tx_state = TX_TOKEN; next_sender_id = 255; - } else { + } else { txPk->targetId = next_target_id; next_sender_id = next_target_id; - DTR_DEBUG_PRINT("Sending DATA to next peer.."); - DTR_DEBUG_PRINT("with target id: %d\n", txPk->targetId); + DEBUG_PRINT("Sending DATA to next peer.."); + DEBUG_PRINT("with target id: %d\n", txPk->targetId); tx_state = TX_DATA_FRAME; } setupRadioTx(txPk, tx_state); From 7bb56cec30e54492ade4fd1345bf376842eff8ea Mon Sep 17 00:00:00 2001 From: Marios Stamatopoulos Date: Thu, 25 Aug 2022 10:27:01 +0200 Subject: [PATCH 19/19] DTR silencing debug prints --- src/modules/src/p2pDTR/token_ring.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/src/p2pDTR/token_ring.c b/src/modules/src/p2pDTR/token_ring.c index 4875d8dc49..689454820e 100644 --- a/src/modules/src/p2pDTR/token_ring.c +++ b/src/modules/src/p2pDTR/token_ring.c @@ -432,8 +432,8 @@ void dtrTaskHandler(void *param) { } else { txPk->targetId = next_target_id; next_sender_id = next_target_id; - DEBUG_PRINT("Sending DATA to next peer.."); - DEBUG_PRINT("with target id: %d\n", txPk->targetId); + DTR_DEBUG_PRINT("Sending DATA to next peer.."); + DTR_DEBUG_PRINT("with target id: %d\n", txPk->targetId); tx_state = TX_DATA_FRAME; } setupRadioTx(txPk, tx_state);