forked from opendroneid/transmitter-linux
-
Notifications
You must be signed in to change notification settings - Fork 0
/
transmit.c
316 lines (277 loc) · 14.3 KB
/
transmit.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
/*
* Copyright (C) 2021, Soren Friis
*
* SPDX-License-Identifier: Apache-2.0
*
* Open Drone ID Linux transmitter example.
*
* Maintainer: Soren Friis
* friissoren2@gmail.com
*/
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <pthread.h>
#include <semaphore.h>
#include "ap_interface.h"
#include "bluetooth.h"
#include "wifi_beacon.h"
sem_t semaphore;
#define MINIMUM(a,b) (((a)<(b))?(a):(b))
#define BASIC_ID_POS_ZERO 0
#define BASIC_ID_POS_ONE 1
static void fill_example_data(struct ODID_UAS_Data *uasData) {
uasData->BasicID[BASIC_ID_POS_ZERO].UAType = ODID_UATYPE_HELICOPTER_OR_MULTIROTOR;
uasData->BasicID[BASIC_ID_POS_ZERO].IDType = ODID_IDTYPE_SERIAL_NUMBER;
char uas_id[] = "112624150A90E3AE1EC0";
strncpy(uasData->BasicID[BASIC_ID_POS_ZERO].UASID, uas_id,
MINIMUM(sizeof(uas_id), sizeof(uasData->BasicID[BASIC_ID_POS_ZERO].UASID)));
uasData->BasicID[BASIC_ID_POS_ONE].UAType = ODID_UATYPE_HELICOPTER_OR_MULTIROTOR;
uasData->BasicID[BASIC_ID_POS_ONE].IDType = ODID_IDTYPE_SPECIFIC_SESSION_ID;
char uas_caa_id[] = "FD3454B778E565C24B70";
strncpy(uasData->BasicID[BASIC_ID_POS_ONE].UASID, uas_caa_id,
MINIMUM(sizeof(uas_caa_id), sizeof(uasData->BasicID[BASIC_ID_POS_ONE].UASID)));
uasData->Location.Status = ODID_STATUS_AIRBORNE;
uasData->Location.Direction = 361.f;
uasData->Location.SpeedHorizontal = 0.0f;
uasData->Location.SpeedVertical = 0.35f;
uasData->Location.Latitude = 51.4791;
uasData->Location.Longitude = -0.0013;
uasData->Location.AltitudeBaro = 100;
uasData->Location.AltitudeGeo = 110;
uasData->Location.HeightType = ODID_HEIGHT_REF_OVER_GROUND;
uasData->Location.Height = 80;
uasData->Location.HorizAccuracy = createEnumHorizontalAccuracy(5.5f);
uasData->Location.VertAccuracy = createEnumVerticalAccuracy(9.5f);
uasData->Location.BaroAccuracy = createEnumVerticalAccuracy(0.5f);
uasData->Location.SpeedAccuracy = createEnumSpeedAccuracy(0.5f);
uasData->Location.TSAccuracy = createEnumTimestampAccuracy(0.1f);
uasData->Location.TimeStamp = 360.52f;
uasData->Auth[0].AuthType = ODID_AUTH_UAS_ID_SIGNATURE;
uasData->Auth[0].DataPage = 0;
uasData->Auth[0].LastPageIndex = 2;
uasData->Auth[0].Length = 63;
uasData->Auth[0].Timestamp = 28000000;
char auth0_data[] = "12345678901234567";
memcpy(uasData->Auth[0].AuthData, auth0_data,
MINIMUM(sizeof(auth0_data), sizeof(uasData->Auth[0].AuthData)));
uasData->Auth[1].AuthType = ODID_AUTH_UAS_ID_SIGNATURE;
uasData->Auth[1].DataPage = 1;
char auth1_data[] = "12345678901234567890123";
memcpy(uasData->Auth[1].AuthData, auth1_data,
MINIMUM(sizeof(auth1_data), sizeof(uasData->Auth[1].AuthData)));
uasData->Auth[2].AuthType = ODID_AUTH_UAS_ID_SIGNATURE;
uasData->Auth[2].DataPage = 2;
char auth2_data[] = "12345678901234567890123";
memcpy(uasData->Auth[2].AuthData, auth2_data,
MINIMUM(sizeof(auth2_data), sizeof(uasData->Auth[2].AuthData)));
uasData->SelfID.DescType = ODID_DESC_TYPE_TEXT;
char description[] = "Drone ID test flight---";
strncpy(uasData->SelfID.Desc, description,
MINIMUM(sizeof(description), sizeof(uasData->SelfID.Desc)));
uasData->System.OperatorLocationType = ODID_OPERATOR_LOCATION_TYPE_TAKEOFF;
uasData->System.ClassificationType = ODID_CLASSIFICATION_TYPE_EU;
uasData->System.OperatorLatitude = uasData->Location.Latitude + 0.001;
uasData->System.OperatorLongitude = uasData->Location.Longitude - 0.001;
uasData->System.AreaCount = 1;
uasData->System.AreaRadius = 0;
uasData->System.AreaCeiling = 0;
uasData->System.AreaFloor = 0;
uasData->System.CategoryEU = ODID_CATEGORY_EU_OPEN;
uasData->System.ClassEU = ODID_CLASS_EU_CLASS_1;
uasData->System.OperatorAltitudeGeo = 20.5f;
uasData->System.Timestamp = 28056789;
uasData->OperatorID.OperatorIdType = ODID_OPERATOR_ID;
char operatorId[] = "FIN87astrdge12k8";
strncpy(uasData->OperatorID.OperatorId, operatorId,
MINIMUM(sizeof(operatorId), sizeof(uasData->OperatorID.OperatorId)));
}
static void send_message(union ODID_Message_encoded *encoded, struct config_data *config, uint8_t msg_counter) {
if (config->use_btl)
send_bluetooth_message(encoded, msg_counter, config);
if (config->use_bt4 || config->use_bt5)
send_bluetooth_message_extended_api(encoded, msg_counter, config);
if (config->use_beacon)
send_beacon_message(encoded, msg_counter);
sleep(4);
}
// When using the WiFi Beacon transport method, the standards require that all messages are wrapped
// in a message pack and sent together. This single message send function is only for testing purposes.
static void send_single_messages(struct ODID_UAS_Data *uasData, struct config_data *config) {
union ODID_Message_encoded encoded;
memset(&encoded, 0, sizeof(union ODID_Message_encoded));
for (int i = 0; i < 10; i++) {
if (encodeBasicIDMessage((ODID_BasicID_encoded *) &encoded, &uasData->BasicID[BASIC_ID_POS_ZERO]) != ODID_SUCCESS)
printf("Error: Failed to encode Basic ID\n");
send_message(&encoded, config, config->msg_counters[ODID_MSG_COUNTER_BASIC_ID]++);
if (encodeBasicIDMessage((ODID_BasicID_encoded *) &encoded, &uasData->BasicID[BASIC_ID_POS_ONE]) != ODID_SUCCESS)
printf("Error: Failed to encode Basic ID\n");
send_message(&encoded, config, config->msg_counters[ODID_MSG_COUNTER_BASIC_ID]++);
if (encodeLocationMessage((ODID_Location_encoded *) &encoded, &uasData->Location) != ODID_SUCCESS)
printf("Error: Failed to encode Location\n");
send_message(&encoded, config, config->msg_counters[ODID_MSG_COUNTER_LOCATION]++);
if (encodeAuthMessage((ODID_Auth_encoded *) &encoded, &uasData->Auth[0]) != ODID_SUCCESS)
printf("Error: Failed to encode Auth 0\n");
send_message(&encoded, config, config->msg_counters[ODID_MSG_COUNTER_AUTH]++);
if (encodeAuthMessage((ODID_Auth_encoded *) &encoded, &uasData->Auth[1]) != ODID_SUCCESS)
printf("Error: Failed to encode Auth 1\n");
send_message(&encoded, config, config->msg_counters[ODID_MSG_COUNTER_AUTH]++);
if (encodeAuthMessage((ODID_Auth_encoded *) &encoded, &uasData->Auth[2]) != ODID_SUCCESS)
printf("Error: Failed to encode Auth 2\n");
send_message(&encoded, config, config->msg_counters[ODID_MSG_COUNTER_AUTH]++);
if (encodeSelfIDMessage((ODID_SelfID_encoded *) &encoded, &uasData->SelfID) != ODID_SUCCESS)
printf("Error: Failed to encode Self ID\n");
send_message(&encoded, config, config->msg_counters[ODID_MSG_COUNTER_SELF_ID]++);
if (encodeSystemMessage((ODID_System_encoded *) &encoded, &uasData->System) != ODID_SUCCESS)
printf("Error: Failed to encode System\n");
send_message(&encoded, config, config->msg_counters[ODID_MSG_COUNTER_SYSTEM]++);
if (encodeOperatorIDMessage((ODID_OperatorID_encoded *) &encoded, &uasData->OperatorID) != ODID_SUCCESS)
printf("Error: Failed to encode Operator ID\n");
send_message(&encoded, config, config->msg_counters[ODID_MSG_COUNTER_OPERATOR_ID]++);
}
}
static void create_message_pack(struct ODID_UAS_Data *uasData, struct ODID_MessagePack_encoded *pack_enc) {
union ODID_Message_encoded encoded = { 0 };
ODID_MessagePack_data pack_data = { 0 };
pack_data.SingleMessageSize = ODID_MESSAGE_SIZE;
pack_data.MsgPackSize = 9;
if (encodeBasicIDMessage((ODID_BasicID_encoded *) &encoded, &uasData->BasicID[BASIC_ID_POS_ZERO]) != ODID_SUCCESS)
printf("Error: Failed to encode Basic ID\n");
memcpy(&pack_data.Messages[0], &encoded, ODID_MESSAGE_SIZE);
if (encodeBasicIDMessage((ODID_BasicID_encoded *) &encoded, &uasData->BasicID[BASIC_ID_POS_ONE]) != ODID_SUCCESS)
printf("Error: Failed to encode Basic ID\n");
memcpy(&pack_data.Messages[1], &encoded, ODID_MESSAGE_SIZE);
if (encodeLocationMessage((ODID_Location_encoded *) &encoded, &uasData->Location) != ODID_SUCCESS)
printf("Error: Failed to encode Location\n");
memcpy(&pack_data.Messages[2], &encoded, ODID_MESSAGE_SIZE);
if (encodeAuthMessage((ODID_Auth_encoded *) &encoded, &uasData->Auth[0]) != ODID_SUCCESS)
printf("Error: Failed to encode Auth 0\n");
memcpy(&pack_data.Messages[3], &encoded, ODID_MESSAGE_SIZE);
if (encodeAuthMessage((ODID_Auth_encoded *) &encoded, &uasData->Auth[1]) != ODID_SUCCESS)
printf("Error: Failed to encode Auth 1\n");
memcpy(&pack_data.Messages[4], &encoded, ODID_MESSAGE_SIZE);
if (encodeAuthMessage((ODID_Auth_encoded *) &encoded, &uasData->Auth[2]) != ODID_SUCCESS)
printf("Error: Failed to encode Auth 2\n");
memcpy(&pack_data.Messages[5], &encoded, ODID_MESSAGE_SIZE);
if (encodeSelfIDMessage((ODID_SelfID_encoded *) &encoded, &uasData->SelfID) != ODID_SUCCESS)
printf("Error: Failed to encode Self ID\n");
memcpy(&pack_data.Messages[6], &encoded, ODID_MESSAGE_SIZE);
if (encodeSystemMessage((ODID_System_encoded *) &encoded, &uasData->System) != ODID_SUCCESS)
printf("Error: Failed to encode System\n");
memcpy(&pack_data.Messages[7], &encoded, ODID_MESSAGE_SIZE);
if (encodeOperatorIDMessage((ODID_OperatorID_encoded *) &encoded, &uasData->OperatorID) != ODID_SUCCESS)
printf("Error: Failed to encode Operator ID\n");
memcpy(&pack_data.Messages[8], &encoded, ODID_MESSAGE_SIZE);
if (encodeMessagePack(pack_enc, &pack_data) != ODID_SUCCESS)
printf("Error: Failed to encode message pack_data\n");
}
static void send_packs(struct ODID_UAS_Data *uasData, struct config_data *config) {
struct ODID_MessagePack_encoded pack_enc = { 0 };
create_message_pack(uasData, &pack_enc);
for (int i = 0; i < 10; i++) {
if (config->use_beacon)
send_beacon_message_pack(&pack_enc, config->msg_counters[ODID_MSG_COUNTER_PACKED]++);
if (config->use_bt5)
send_bluetooth_message_pack(&pack_enc, config->msg_counters[ODID_MSG_COUNTER_PACKED]++, config);
sleep(4);
}
}
void print_help() {
printf("Program for transmitting static drone ID data on Wi-Fi Beacon or Bluetooth.\n");
printf("Must be run with sudo rights in order to work.\n");
printf("Options: b Enable Wi-Fi Beacon transmission\n");
printf(" l Enable Bluetooth 4 Legacy Advertising transmission\n");
printf(" using the non-Extended Advertising HCI API commands\n");
printf(" 4 Enable Bluetooth 4 Legacy Advertising transmission\n");
printf(" using the Extended Advertising HCI API commands\n");
printf(" 5 Enable Bluetooth 5 Long Range + Extended Advertising transmission\n");
printf(" p Use message packs instead of single messages\n");
printf("E.g. sudo ./transmit b p\n\n");
printf("Wi-Fi Beacon transmit only works when running\n");
printf("\"sudo hostapd/hostapd/hostapd beacon.conf\" in a separate shell.\n");
printf("Disconnect from all Wi-Fi networks before starting Wi-Fi Beacon transmission.\n\n");
printf("If terminated abnormally, Beacon and Bluetooth broadcasts can remain on.\n");
printf(" - To stop Beacon broadcast, stop the hostapd instance.\n");
printf(" It can be difficult to stop the transmit instance.\n");
printf(" After stopping hostapd, use \"sudo pkill transmit\".\n");
printf(" - To stop Bluetooth, use \"sudo btmgmt power off\" and then\n");
printf(" \"sudo btmgmt power on\".\n");
}
static void parse_command_line(int argc, char *argv[], struct config_data *config) {
if (argc == 1) {
print_help();
exit(EXIT_SUCCESS);
}
for (int i = 1; i < argc; i++) {
switch (*argv[i]) {
case 'b':
config->use_beacon = true;
break;
case 'l':
config->use_btl = true;
break;
case '4':
config->use_bt4 = true;
break;
case '5':
config->use_bt5 = true;
break;
case 'p':
config->use_packs = true;
break;
default:
break;
}
}
if (config->use_beacon)
printf("\nReminder: Wi-Fi Beacon only works when running\n\"sudo hostapd/hostapd/hostapd beacon.conf\" in a separate shell.\n\n");
if (config->use_beacon && !config->use_packs)
printf("\nWarning: Transmitting single messages on Wi-Fi beacon is violating\nthe standards. Enable message packs.\n\n");
if (config->use_btl && (config->use_bt4 || config->use_bt5)) {
printf("\nError: Cannot use both old API and Extended Advertising API at the same time.\n\n");
exit(EXIT_FAILURE);
}
if ((config->use_btl || config->use_bt4) && config->use_packs) {
printf("\nError: BT4 cannot use message packs.\n\n");
exit(EXIT_FAILURE);
}
if (config->use_bt4 && config->use_bt5)
printf("\nWarning: Doing simultaneous BT4 and BT5 will not necessarily work.\n\n");
if (config->use_bt5 && !config->use_packs)
printf("\nWarning: Transmitting single messages on Bluetooth 5 Long Range is violating\nthe standards. Enable message packs.\n\n");
if (!config->use_beacon && !config->use_btl && !config->use_bt4 && !config->use_bt5) {
print_help();
exit(EXIT_SUCCESS);
}
}
int main(int argc, char *argv[])
{
pthread_t id;
struct config_data config = { 0 };
parse_command_line(argc, argv, &config);
config.handle_bt4 = 0; // The Extended Advertising set number used for BT4
config.handle_bt5 = 1; // The Extended Advertising set number used for BT5
if (config.use_beacon) {
sem_init(&semaphore,0,0);
pthread_create(&id, NULL, ap_interface_init, NULL);
sem_wait(&semaphore);
}
struct ODID_UAS_Data uasData;
odid_initUasData(&uasData);
fill_example_data(&uasData);
if (config.use_btl || config.use_bt4 || config.use_bt5)
init_bluetooth(&config);
if (config.use_packs)
send_packs(&uasData, &config);
else
send_single_messages(&uasData, &config);
if (config.use_btl || config.use_bt4 || config.use_bt5)
close_bluetooth(&config);
if (config.use_beacon) {
send_quit();
int *ptr;
pthread_join(id, (void **) &ptr);
printf("Return value from ap_interface_init: %i\n", *ptr);
sem_destroy(&semaphore);
}
}