Merge pull request #1098 from marios-stam/DTR_P2P_IN_FIRMWARE

DTR P2P in firmware

.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

Makefile

@@ -51,7 +51,7 @@ 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/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

docs/functional-areas/p2p_DTR_api.md

@@ -0,0 +1,132 @@
+---
+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 {
+	uint8_t packetSize;
+	uint8_t messageType;
+	uint8_t sourceId;
+	uint8_t targetId;
+	uint8_t dataSize;
+	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. 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;
+
+...
+
+void main() {
+  ...
+  // Start the token ring protocol
+  dtrEnableProtocol(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.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.
+   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 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 `dtrSendPacket`. 
+  ``` C
+  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 `.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;
+
+  // Fill the packet with the data
+  packet.dataSize = 3;
+  packet.data[0] = 0x01;
+  packet.data[1] = 0x02;
+  packet.data[2] = 0x03;
+  packet.targetId = 0xFF;
+  dtrSendPacket(&packet);
+
+  ```
+
+
+## Receive Data
+
+The user must call the function `dtrGetPacket` to receive a packet from the DTR. 
+``` C
+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;
+
+// Receive the packet
+while(1){
+  dtrGetPacket(&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");
+  }
+}
+```

examples/app_p2p_DTR/.gitignore

@@ -0,0 +1,2 @@
+bin/*
+cf2.*

examples/app_p2p_DTR/Kbuild

@@ -0,0 +1 @@
+obj-y += src/

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

examples/app_p2p_DTR/README.md

@@ -0,0 +1,25 @@
+# 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 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.
+
+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.
+
+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
+
+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_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
+make 
+make cload
+```
+
+If you want to compile the application elsewhere in your machine, make sure to update ```CRAZYFLIE_BASE``` in the **Makefile**.

examples/app_p2p_DTR/app-config

@@ -0,0 +1,3 @@
+CONFIG_APP_ENABLE=y
+CONFIG_APP_PRIORITY=1
+CONFIG_APP_STACKSIZE=350

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

examples/app_p2p_DTR/src/Kbuild

@@ -0,0 +1 @@
+obj-y += p2p_DTR_app.o

examples/app_p2p_DTR/src/p2p_DTR_app.c

@@ -0,0 +1,153 @@
+/**
+ * ,---------,       ____  _ __
+ * |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ * | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * | / ,--´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ *
+ * p2p_DTR_app.c - App layer application of simple demonstration of the 
+ * Dynamic Token Ring Protocol used on top of the P2P.
+ */
+
+
+#include <string.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdio.h>
+
+#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"
+
+#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
+
+static uint8_t my_id;
+static dtrTopology topology = NETWORK_TOPOLOGY;
+
+void loadTXPacketsForTesting(void){
+	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.targetId = 0xFF;
+	testSignal.packetSize = DTR_PACKET_HEADER_SIZE + testSignal.dataSize;
+	bool res;
+	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 loadResponse(void){
+	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.targetId = 0xFF;
+	testSignal.packetSize = DTR_PACKET_HEADER_SIZE + testSignal.dataSize;
+	bool res;
+	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){
+	// If the packet is a DTR service packet, then the handler will handle it.
+	// It returns true if the packet was handled.
+
+    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();
+	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);
+	vTaskDelay(2000);
+
+	// Register the callback function so that the CF can receive packets as well.
+	p2pRegisterCB(p2pcallbackHandler);
+
+	if (my_id == topology.devices_ids[0]){
+		DTR_DEBUG_PRINT("Starting communication...\n");
+		loadTXPacketsForTesting();
+	}
+
+	dtrPacket received_packet;
+	uint32_t start = T2M(xTaskGetTickCount());
+	while(1){
+		dtrGetPacket(&received_packet, portMAX_DELAY);
+		uint32_t dt = T2M(xTaskGetTickCount()) - start;
+
+		// 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.sourceId, data, dt);
+		start = T2M(xTaskGetTickCount());
+
+
+		if (strcmp(data, "Hello World") == 0){
+			loadResponse();
+		}
+	}
+}