Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
decawave_anchorless_communication.c
Go to the documentation of this file.
1/*
2 * Serial_Communication.c
3 *
4 * Created on: Jul 25, 2017
5 * Author: Steven van der Helm
6 */
7
8/*
9 * Copyright (C) C. DW
10 *
11 * This file is part of paparazzi
12 *
13 * paparazzi is free software; you can redistribute it and/or modify
14 * it under the terms of the GNU General Public License as published by
15 * the Free Software Foundation; either version 2, or (at your option)
16 * any later version.
17 *
18 * paparazzi is distributed in the hope that it will be useful,
19 * but WITHOUT ANY WARRANTY; without even the implied warranty of
20 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21 * GNU General Public License for more details.
22 *
23 * You should have received a copy of the GNU General Public License
24 * along with paparazzi; see the file COPYING. If not, see
25 * <http://www.gnu.org/licenses/>.
26 */
39
43#include "state.h"
44#include "mcu_periph/uart.h"
45#include "modules/core/abi.h"
46#include <stdio.h>
47
48#define UWB_SERIAL_PORT (&((SERIAL_UART).device))
49struct link_device *external_device = UWB_SERIAL_PORT;
50
51// Some meta data for serial communication
52#define UWB_SERIAL_COMM_MAX_MESSAGE 20
53#define UWB_SERIAL_COMM_END_MARKER 255
54#define UWB_SERIAL_COMM_SPECIAL_BYTE 253
55#define UWB_SERIAL_COMM_START_MARKER 254
56#define UWB_SERIAL_COMM_NODE_STATE_SIZE 7
57
58#define UWB_SERIAL_COMM_NUM_NODES 3 // How many nodes actually are in the network
59#define UWB_SERIAL_COMM_DIST_NUM_NODES UWB_SERIAL_COMM_NUM_NODES-1 // How many distant nodes are in the network (one less than the toal number of nodes)
60
61// Serial message
62
63#define UWB_SERIAL_COMM_RANGE 0
64#define UWB_SERIAL_COMM_VX 1
65#define UWB_SERIAL_COMM_VY 2
66#define UWB_SERIAL_COMM_Z 3
67#define UWB_SERIAL_COMM_AX 4
68#define UWB_SERIAL_COMM_AY 5
69#define UWB_SERIAL_COMM_YAWR 6
70
71struct nodeState {
73 float r;
74 float vx;
75 float vy;
76 float z;
77 float ax;
78 float ay;
79 float yawr;
81};
82
84
88static void handleNewStateValue(uint8_t nodeIndex, uint8_t msg_type, float value)
89{
90 struct nodeState *node = &states[nodeIndex];
91 switch (msg_type) {
93 node->r = value;
95 break;
97 node->vx = value;
99 break;
100 case UWB_SERIAL_COMM_VY :
101 node->vy = value;
102 node->state_updated[UWB_SERIAL_COMM_VY] = true;
103 break;
104 case UWB_SERIAL_COMM_Z :
105 node->z = value;
106 node->state_updated[UWB_SERIAL_COMM_Z] = true;
107 break;
108 case UWB_SERIAL_COMM_AX :
109 node->ax = value;
110 node->state_updated[UWB_SERIAL_COMM_AX] = true;
111 break;
112 case UWB_SERIAL_COMM_AY :
113 node->ay = value;
114 node->state_updated[UWB_SERIAL_COMM_AY] = true;
115 break;
117 node->yawr = value;
119 break;
120 }
121}
122
130static void decodeHighBytes(uint8_t bytes_received, uint8_t *received_message)
131{
132 static uint8_t receive_buffer[4];
133 float tempfloat;
134 uint8_t data_received_count = 0;
135 uint8_t this_address = received_message[1];
136 uint8_t msg_from = received_message[2];
137 uint8_t msg_type = received_message[3];
138 uint8_t nodeIndex = msg_from - 1 - (uint8_t)(this_address < msg_from);
139 for (uint8_t i = 4; i < bytes_received - 1; i++) {
140 // Skip the begin marker (0), this address (1), remote address (2), message type (3), and end marker (bytes_received-1)
141 uint8_t var_byte = received_message[i];
142 if (var_byte == UWB_SERIAL_COMM_SPECIAL_BYTE) {
143 i++;
144 var_byte = var_byte + received_message[i];
145 }
146 if (data_received_count <= 4) {
147 receive_buffer[data_received_count] = var_byte;
148 }
149 data_received_count++;
150 }
151 if (data_received_count == 4) {
152 // Move memory from integer buffer to float variable
153 memcpy(&tempfloat, &receive_buffer, 4);
154 // Set the variable to the appropriate type and store it in state
155 handleNewStateValue(nodeIndex, msg_type, tempfloat);
156 }
157}
158
164static void encodeHighBytes(uint8_t *send_data, uint8_t msg_size, uint8_t *data_send_buffer, uint8_t *data_total_send)
165{
166 uint8_t data_send_count = msg_size;
167 *data_total_send = 0;
168 for (uint8_t i = 0; i < data_send_count; i++) {
170 data_send_buffer[*data_total_send] = UWB_SERIAL_COMM_SPECIAL_BYTE;
171 (*data_total_send)++;
172 data_send_buffer[*data_total_send] = send_data[i] - UWB_SERIAL_COMM_SPECIAL_BYTE;
173 } else {
174 data_send_buffer[*data_total_send] = send_data[i];
175 }
176 (*data_total_send)++;
177 }
178}
179
184static void sendFloat(uint8_t msg_type, float data)
185{
186 static uint8_t data_send_buffer[UWB_SERIAL_COMM_MAX_MESSAGE];
187 static uint8_t data_total_send = 0;
188
189 // Make bytes of the float
190 uint8_t floatbyte[4];
191 memcpy(floatbyte, &data, 4);
192 encodeHighBytes(floatbyte, 4, data_send_buffer, &data_total_send);
193
195 UWB_SERIAL_PORT->put_byte(UWB_SERIAL_PORT->periph, 0, msg_type);
196
197 for (uint8_t i = 0; i < data_total_send; i++) {
198 UWB_SERIAL_PORT->put_byte(UWB_SERIAL_PORT->periph, 0, data_send_buffer[i]);
199 }
200
202}
203
207static void setNodeStatesFalse(uint8_t index)
208{
209 for (uint8_t j = 0; j < UWB_SERIAL_COMM_NODE_STATE_SIZE; j++) {
210 states[index].state_updated[j] = false;
211 }
212}
213
218static void checkStatesUpdated(void)
219{
220 bool checkbool;
221 for (uint8_t i = 0; i < UWB_SERIAL_COMM_DIST_NUM_NODES; i++) {
222 checkbool = true;
223 for (uint8_t j = 0; j < UWB_SERIAL_COMM_NODE_STATE_SIZE; j++) {
224 checkbool = checkbool && states[i].state_updated[j];
225 }
226 if (checkbool) {
227 AbiSendMsgUWB_COMMUNICATION(UWB_COMM_ID, i, states[i].r, states[i].vx, states[i].vy, states[i].z, states[i].ax, states[i].ay, states[i].yawr);
229 }
230 }
231}
232
239static void getSerialData(uint8_t *bytes_received)
240{
241 static bool in_progress = false;
242 static uint8_t var_byte;
243 static uint8_t received_message[UWB_SERIAL_COMM_MAX_MESSAGE];
244
245 while (external_device->char_available(external_device->periph)) {
246 var_byte = UWB_SERIAL_PORT->get_byte(UWB_SERIAL_PORT->periph);
247
248 if (var_byte == UWB_SERIAL_COMM_START_MARKER) {
249 (*bytes_received) = 0;
250 in_progress = true;
251 }
252
253 if (in_progress) {
254 if ((*bytes_received) < UWB_SERIAL_COMM_MAX_MESSAGE - 1) {
255 received_message[*bytes_received] = var_byte;
256 (*bytes_received)++;
257 } else {
258 in_progress = false;
259 }
260 }
261
262 if (var_byte == UWB_SERIAL_COMM_END_MARKER) {
263 in_progress = false;
264 decodeHighBytes(*bytes_received, received_message);
265 }
266 }
267}
268
273{
274 // Set all nodes to false
275 for (uint8_t i = 0; i < UWB_SERIAL_COMM_DIST_NUM_NODES; i++) {
277 }
278}
279
284{
285 // TODO: Right now floats are sent individually, but it would be nice to send all at once (requires integrating with UWB/Arduino side as well).
292}
293
299{
300 static uint8_t bytes_received;
301 getSerialData(&bytes_received);
303}
Main include for ABI (AirBorneInterface).
#define UWB_COMM_ID
static void send_data(uint32_t stamp)
static void checkStatesUpdated(void)
This function checks if all the states of all the distant nodes have at least once been updated.
#define UWB_SERIAL_COMM_NODE_STATE_SIZE
#define UWB_SERIAL_COMM_MAX_MESSAGE
void decawave_anchorless_communication_periodic(void)
This function periodically sends state data over the serial (which is received by the arduino).
static struct nodeState states[UWB_SERIAL_COMM_DIST_NUM_NODES]
bool state_updated[UWB_SERIAL_COMM_NODE_STATE_SIZE]
#define UWB_SERIAL_COMM_END_MARKER
void decawave_anchorless_communication_init(void)
Initialization functio.
struct link_device * external_device
#define UWB_SERIAL_COMM_YAWR
static void handleNewStateValue(uint8_t nodeIndex, uint8_t msg_type, float value)
Function that is called when over the serial a new state value from a remote node is received.
static void sendFloat(uint8_t msg_type, float data)
Function that will send a float over serial.
static void getSerialData(uint8_t *bytes_received)
Function for receiving serial data.
static void decodeHighBytes(uint8_t bytes_received, uint8_t *received_message)
Function for decoding the high bytes of received serial data and saving the message.
void decawave_anchorless_communication_event(void)
Event function currently checks for serial data and whether an update of states is available for a di...
#define UWB_SERIAL_COMM_DIST_NUM_NODES
#define UWB_SERIAL_COMM_RANGE
#define UWB_SERIAL_COMM_SPECIAL_BYTE
#define UWB_SERIAL_COMM_START_MARKER
static void encodeHighBytes(uint8_t *send_data, uint8_t msg_size, uint8_t *data_send_buffer, uint8_t *data_total_send)
Function that encodes the high bytes of the serial data to be sent.
static void setNodeStatesFalse(uint8_t index)
Helper function that sets the boolean that tells whether a remote drone has a new state update to fal...
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition state.h:1214
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:859
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition state.h:1392
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition state.h:1069
Generic interface for radio control modules.
API to get/set the generic vehicle states.
Periodic telemetry system header (includes downlink utility and generated code).
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.