Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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  */
43 #include "state.h"
44 #include "mcu_periph/uart.h"
45 #include "subsystems/abi.h"
46 #include <stdio.h>
47 
48 #define UWB_SERIAL_PORT (&((SERIAL_UART).device))
49 struct 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 
71 struct 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 
88 static 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;
96  case UWB_SERIAL_COMM_VX :
97  node->vx = value;
98  node->state_updated[UWB_SERIAL_COMM_VX] = true;
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;
116  case UWB_SERIAL_COMM_YAWR :
117  node->yawr = value;
118  node->state_updated[UWB_SERIAL_COMM_YAWR] = true;
119  break;
120  }
121 }
122 
130 static 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 
164 static 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++) {
169  if (send_data[i] >= UWB_SERIAL_COMM_SPECIAL_BYTE) {
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 
184 static 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 
207 static 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 
218 static 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 
239 static 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 }
radio_control.h
nodeState::r
float r
Definition: decawave_anchorless_communication.c:73
checkStatesUpdated
static void checkStatesUpdated(void)
This function checks if all the states of all the distant nodes have at least once been updated.
Definition: decawave_anchorless_communication.c:218
states
static struct nodeState states[UWB_SERIAL_COMM_DIST_NUM_NODES]
Definition: decawave_anchorless_communication.c:83
decodeHighBytes
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.
Definition: decawave_anchorless_communication.c:130
sendFloat
static void sendFloat(uint8_t msg_type, float data)
Function that will send a float over serial.
Definition: decawave_anchorless_communication.c:184
decawave_anchorless_communication_periodic
void decawave_anchorless_communication_periodic(void)
This function periodically sends state data over the serial (which is received by the arduino)
Definition: decawave_anchorless_communication.c:283
encodeHighBytes
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.
Definition: decawave_anchorless_communication.c:164
decawave_anchorless_communication_init
void decawave_anchorless_communication_init(void)
Initialization functio.
Definition: decawave_anchorless_communication.c:272
abi.h
getSerialData
static void getSerialData(uint8_t *bytes_received)
Function for receiving serial data.
Definition: decawave_anchorless_communication.c:239
nodeState::yawr
float yawr
Definition: decawave_anchorless_communication.c:79
UWB_SERIAL_COMM_SPECIAL_BYTE
#define UWB_SERIAL_COMM_SPECIAL_BYTE
Definition: decawave_anchorless_communication.c:54
stateGetAccelNed_f
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition: state.h:1038
UWB_SERIAL_COMM_AX
#define UWB_SERIAL_COMM_AX
Definition: decawave_anchorless_communication.c:67
decawave_anchorless_communication_event
void decawave_anchorless_communication_event(void)
Event function currently checks for serial data and whether an update of states is available for a di...
Definition: decawave_anchorless_communication.c:298
UWB_SERIAL_COMM_START_MARKER
#define UWB_SERIAL_COMM_START_MARKER
Definition: decawave_anchorless_communication.c:55
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
stateGetBodyRates_f
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
UWB_SERIAL_COMM_END_MARKER
#define UWB_SERIAL_COMM_END_MARKER
Definition: decawave_anchorless_communication.c:53
nodeState::ax
float ax
Definition: decawave_anchorless_communication.c:77
telemetry.h
UWB_SERIAL_COMM_RANGE
#define UWB_SERIAL_COMM_RANGE
Definition: decawave_anchorless_communication.c:63
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
setNodeStatesFalse
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...
Definition: decawave_anchorless_communication.c:207
UWB_SERIAL_COMM_VX
#define UWB_SERIAL_COMM_VX
Definition: decawave_anchorless_communication.c:64
nodeState::vy
float vy
Definition: decawave_anchorless_communication.c:75
UWB_SERIAL_COMM_Z
#define UWB_SERIAL_COMM_Z
Definition: decawave_anchorless_communication.c:66
uint8_t
unsigned char uint8_t
Definition: types.h:14
UWB_SERIAL_COMM_AY
#define UWB_SERIAL_COMM_AY
Definition: decawave_anchorless_communication.c:68
UWB_SERIAL_COMM_VY
#define UWB_SERIAL_COMM_VY
Definition: decawave_anchorless_communication.c:65
UWB_SERIAL_COMM_YAWR
#define UWB_SERIAL_COMM_YAWR
Definition: decawave_anchorless_communication.c:69
UWB_SERIAL_COMM_MAX_MESSAGE
#define UWB_SERIAL_COMM_MAX_MESSAGE
Definition: decawave_anchorless_communication.c:52
nodeState::z
float z
Definition: decawave_anchorless_communication.c:76
nodeState::state_updated
bool state_updated[UWB_SERIAL_COMM_NODE_STATE_SIZE]
Definition: decawave_anchorless_communication.c:80
UWB_SERIAL_PORT
#define UWB_SERIAL_PORT
Definition: decawave_anchorless_communication.c:48
stateGetSpeedEnu_f
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
nodeState::nodeAddress
uint8_t nodeAddress
Definition: decawave_anchorless_communication.c:72
nodeState::ay
float ay
Definition: decawave_anchorless_communication.c:78
nodeState::vx
float vx
Definition: decawave_anchorless_communication.c:74
nodeState
Definition: decawave_anchorless_communication.c:71
state.h
external_device
struct link_device * external_device
Definition: decawave_anchorless_communication.c:49
UWB_COMM_ID
#define UWB_COMM_ID
Definition: abi_sender_ids.h:438
decawave_anchorless_communication.h
UWB_SERIAL_COMM_NODE_STATE_SIZE
#define UWB_SERIAL_COMM_NODE_STATE_SIZE
Definition: decawave_anchorless_communication.c:56
handleNewStateValue
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.
Definition: decawave_anchorless_communication.c:88
UWB_SERIAL_COMM_DIST_NUM_NODES
#define UWB_SERIAL_COMM_DIST_NUM_NODES
Definition: decawave_anchorless_communication.c:59