mirror of
https://github.com/jjshoots/RemoteIDSpoofer.git
synced 2026-01-21 02:18:05 +00:00
first commit
This commit is contained in:
Jet
commit
2a749eade0
16
README.md
Normal file
16
README.md
Normal file
@ -0,0 +1,16 @@
|
||||
# ID Open
|
||||
An Arduino/ESP32 class to act as a wrapper around opendroneid.
|
||||
|
||||
Supports BLE 4, WiFi NAN and WiFi beacon.
|
||||
|
||||
Runs on a cheap ESP32 dev board.
|
||||
|
||||
Needs opendroneid.c, opendroneid.h, odid_wifi.h and wifi.c from [opendroneid](https://github.com/opendroneid/opendroneid-core-c/tree/master/libopendroneid) to be copied into the id_open directory.
|
||||
|
||||
Last tested with opendroneid release 2.0.
|
||||
|
||||
If you are thinking of using this to make remote IDs for use in the US or EU, there are problems.
|
||||
* It looks like both of these jurisdictions are going to require IDs to transmit ANSI/CTA serial numbers. (See the table at the bottom of [this page](https://github.com/opendroneid/opendroneid-core-c/).)
|
||||
* The FAA are requiring remote IDs to be tamper resistant (see their [acceptance of the ASTM Means of Compliance](https://www.federalregister.gov/documents/2022/08/11/2022-16997/accepted-means-of-compliance-remote-identification-of-unmanned-aircraft) ). I don't see how this can be done with an open source, home built ID.
|
||||
|
||||
There is a report of an ESP32 that will not simultaneously do WiFi and Bluetooth remote ID (see issue #18). If your ESP32 goes into a reboot loop when both are enabled, try one or the other. This may be related to the ESP IDF version.
|
||||
131
RemoteIDSpoofer.ino
Normal file
131
RemoteIDSpoofer.ino
Normal file
@ -0,0 +1,131 @@
|
||||
// ESP8266 RemoteID spoofer
|
||||
// Heavily adapted from https://github.com/sxjack/uav_electronic_ids
|
||||
|
||||
#pragma GCC diagnostic warning "-Wunused-variable"
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <math.h>
|
||||
#include <time.h>
|
||||
#include <sys/time.h>
|
||||
|
||||
#include "id_open.h"
|
||||
|
||||
static ID_OpenDrone squitter;
|
||||
static UTM_Utilities utm_utils;
|
||||
|
||||
static struct UTM_parameters utm_parameters;
|
||||
static struct UTM_data utm_data;
|
||||
|
||||
static int speed_kn = 40;
|
||||
static float x = 0.0, y = 0.0, z = 0.0, speed_m_x, max_dir_change = 75.0;
|
||||
static double deg2rad = 0.0, m_deg_lat = 0.0, m_deg_long = 0.0;
|
||||
|
||||
void setup() {
|
||||
|
||||
char text[64];
|
||||
double lat_d, long_d;
|
||||
time_t time_2;
|
||||
struct tm clock_tm;
|
||||
struct timeval tv = {0,0};
|
||||
struct timezone utc = {0,0};
|
||||
|
||||
Serial.begin(115200);
|
||||
Serial.print("\nSerial\n\n\r");
|
||||
|
||||
deg2rad = (4.0 * atan(1.0)) / 180.0;
|
||||
|
||||
memset(&clock_tm,0,sizeof(struct tm));
|
||||
|
||||
clock_tm.tm_hour = 10;
|
||||
clock_tm.tm_mday = 16;
|
||||
clock_tm.tm_mon = 11;
|
||||
clock_tm.tm_year = 122;
|
||||
|
||||
tv.tv_sec =
|
||||
time_2 = mktime(&clock_tm);
|
||||
|
||||
settimeofday(&tv,&utc);
|
||||
|
||||
delay(500);
|
||||
|
||||
Serial.print(ctime(&time_2));
|
||||
|
||||
memset(&utm_parameters,0,sizeof(utm_parameters));
|
||||
|
||||
strcpy(utm_parameters.UAS_operator,"SUKONDEEZ");
|
||||
utm_parameters.region = 1;
|
||||
utm_parameters.EU_category = 1;
|
||||
utm_parameters.EU_class = 5;
|
||||
|
||||
squitter.init(&utm_parameters);
|
||||
|
||||
memset(&utm_data,0,sizeof(utm_data));
|
||||
|
||||
// 52° 24' 24.4404" -1° 29' 36.564"W
|
||||
lat_d =
|
||||
utm_data.latitude_d =
|
||||
utm_data.base_latitude = 52.0 + (24.0 / 60.0) + (24.4404 / 3600.0);
|
||||
long_d =
|
||||
utm_data.longitude_d =
|
||||
utm_data.base_longitude = -1.0 - (29.0 / 60.0) - (36.564 / 3600.0);
|
||||
utm_data.base_alt_m = 42.0;
|
||||
|
||||
utm_data.alt_msl_m = utm_data.base_alt_m + z;
|
||||
utm_data.alt_agl_m = z;
|
||||
|
||||
utm_data.speed_kn = speed_kn;
|
||||
utm_data.satellites = 8;
|
||||
utm_data.base_valid = 1;
|
||||
|
||||
speed_m_x = ((float) speed_kn) * 0.514444 / 5.0; // Because we update every 200 ms.
|
||||
|
||||
utm_utils.calc_m_per_deg(lat_d,&m_deg_lat,&m_deg_long);
|
||||
|
||||
Serial.print("\r\n");
|
||||
|
||||
sprintf(text,"%d degrees/radian\r\n",(int) (1.0 / deg2rad));
|
||||
Serial.print(text);
|
||||
|
||||
sprintf(text,"%d m per degree latitude\r\n",(int) m_deg_lat);
|
||||
Serial.print(text);
|
||||
|
||||
sprintf(text,"%d m per degree longitude\r\n",(int) m_deg_long);
|
||||
Serial.print(text);
|
||||
|
||||
Serial.print("\r\n");
|
||||
|
||||
srand(micros());
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
int dir_change;
|
||||
char text[64], lat_s[16], long_s[16];
|
||||
float rads, ranf;
|
||||
uint32_t msecs;
|
||||
static uint32_t last_update = 0;
|
||||
|
||||
void loop() {
|
||||
if ((millis() - last_update) < 200) {
|
||||
return;
|
||||
}
|
||||
|
||||
ranf = 0.001 * (float) (((int) rand() % 1000) - 500);
|
||||
dir_change = (int) (max_dir_change * ranf);
|
||||
utm_data.heading = (utm_data.heading + dir_change + 360) % 360;
|
||||
|
||||
x += speed_m_x * sin(rads = (deg2rad * (float) utm_data.heading));
|
||||
y += speed_m_x * cos(rads);
|
||||
|
||||
utm_data.latitude_d = utm_data.base_latitude + (y / m_deg_lat);
|
||||
utm_data.longitude_d = utm_data.base_longitude + (x / m_deg_long);
|
||||
|
||||
dtostrf(utm_data.latitude_d, 10, 7, lat_s);
|
||||
dtostrf(utm_data.longitude_d, 10, 7, long_s);
|
||||
|
||||
// sprintf(text,"%s,%s,%d,%d,%d\r\n", lat_s,long_s,utm_data.heading,utm_data.speed_kn,dir_change);
|
||||
// Serial.print(text);
|
||||
|
||||
squitter.transmit(&utm_data);
|
||||
last_update = millis();
|
||||
}
|
||||
119
alt_unix_time.c
Normal file
119
alt_unix_time.c
Normal file
@ -0,0 +1,119 @@
|
||||
/* -*- tab-width: 2; mode: c; -*-
|
||||
*
|
||||
* Seconds since 1/1/1970 for systems that don't have the unix time() function.
|
||||
*
|
||||
* The Julian day algorithm is from Jean Meeus's Astronomical Algorithms
|
||||
* as are the two test dates.
|
||||
*
|
||||
*/
|
||||
|
||||
#if defined(ARDUINO)
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#else
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <time.h>
|
||||
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
uint64_t alt_unix_secs(int,int,int,int,int,int);
|
||||
static double julian_day(int,int,float,int);
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef ARDUINO
|
||||
|
||||
int main(int argc,char *argv[]) {
|
||||
|
||||
uint64_t alt_secs;
|
||||
time_t unix_secs;
|
||||
struct tm *gmt;
|
||||
|
||||
time(&unix_secs);
|
||||
|
||||
gmt = gmtime(&unix_secs);
|
||||
|
||||
alt_secs = alt_unix_secs(1900 + gmt->tm_year,1 + gmt->tm_mon,gmt->tm_mday,
|
||||
gmt->tm_hour,gmt->tm_min,gmt->tm_sec);
|
||||
|
||||
printf("\nunix: %10lu\n",(unsigned long int) unix_secs);
|
||||
printf( "alt: %10lu\n",(unsigned long int) alt_secs);
|
||||
printf( " %10d\n",(int) ((int64_t) unix_secs - (int64_t) alt_secs));
|
||||
|
||||
printf("\nJD 27/1/333: %10.2f\n",julian_day(333,1,27.5,0));
|
||||
printf( "JD Sputnik: %10.2f\n",julian_day(1957,10,4.81,1));
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
uint64_t alt_unix_secs(int year,int month,int mday,
|
||||
int hour,int minute,int second) {
|
||||
|
||||
uint64_t secs = 0;
|
||||
static uint64_t jd_1970 = 0;
|
||||
|
||||
if (!jd_1970) {
|
||||
|
||||
jd_1970 = (uint64_t) julian_day(1970,1,1,1) * (uint64_t) 86400;
|
||||
}
|
||||
|
||||
secs = (uint64_t) julian_day(year,month,mday,1) * (uint64_t) 86400;
|
||||
secs += (uint64_t) (((uint32_t) hour * 3600) +
|
||||
((uint32_t) minute * 60) +
|
||||
((uint32_t) second));
|
||||
secs -= jd_1970;
|
||||
|
||||
return secs;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
double julian_day(int year,int month,float mday,int gregorian) {
|
||||
|
||||
int a;
|
||||
double y, m, d, jday = 0.0, b = 0.0;
|
||||
|
||||
if (month < 3) {
|
||||
|
||||
--year;
|
||||
month += 12;
|
||||
}
|
||||
|
||||
if (gregorian) {
|
||||
a = year / 100;
|
||||
b = 2.0 - (double) a + (double) (a/4);
|
||||
}
|
||||
|
||||
y = (double) (year + 4716);
|
||||
m = (double) (month + 1);
|
||||
d = (double) mday;
|
||||
|
||||
jday = floor(365.25 * y) +
|
||||
floor(30.6001 * m) +
|
||||
d + b - 1524.5;
|
||||
|
||||
return jday;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
923
id_open.cpp
Normal file
923
id_open.cpp
Normal file
@ -0,0 +1,923 @@
|
||||
/* -*- tab-width: 2; mode: c; -*-
|
||||
*
|
||||
* C++ class for Arduino to function as a wrapper around opendroneid.
|
||||
*
|
||||
* Copyright (c) 2020-2022, Steve Jack.
|
||||
*
|
||||
* Jan. '23: Function to set the self ID.
|
||||
*
|
||||
* Nov. '22: Moved the processor specific code to a separate file.
|
||||
* Had another attempt to get beacon to work.
|
||||
* Tidied up the scheduler.
|
||||
*
|
||||
* May '22: opendroneid 2.0.
|
||||
*
|
||||
* Nov. '21: Removed some redundant code.
|
||||
* Added option to use the new odid_wifi_build_message_pack_beacon_frame() function.
|
||||
*
|
||||
* Oct. '21: Updated for opendroneid release 1.0.
|
||||
*
|
||||
* May '21: Packed WiFi.
|
||||
*
|
||||
* April '21: Added support for beacon frames (untested).
|
||||
* Minor tidying up.
|
||||
*
|
||||
* January '21: Modified initialisation of BasicID.
|
||||
* Authenication codes.
|
||||
*
|
||||
*
|
||||
* MIT licence.
|
||||
*
|
||||
* NOTES
|
||||
*
|
||||
* When porting to a different processor, check the time() function.
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
#define DIAGNOSTICS 0
|
||||
|
||||
//
|
||||
|
||||
#pragma GCC diagnostic warning "-Wunused-variable"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#include <time.h>
|
||||
#include <sys/time.h>
|
||||
|
||||
extern "C" {
|
||||
int clock_gettime(clockid_t,struct timespec *);
|
||||
uint64_t alt_unix_secs(int,int,int,int,int,int);
|
||||
}
|
||||
|
||||
#include "id_open.h"
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
ID_OpenDrone::ID_OpenDrone() {
|
||||
|
||||
int i;
|
||||
static const char *dummy = "";
|
||||
|
||||
//
|
||||
|
||||
UAS_operator = (char *) dummy;
|
||||
|
||||
#if ID_OD_WIFI
|
||||
|
||||
memset(WiFi_mac_addr,0,6);
|
||||
memset(ssid,0,sizeof(ssid));
|
||||
|
||||
strcpy(ssid,"UAS_ID_OPEN");
|
||||
|
||||
beacon_interval = (BEACON_INTERVAL) ? BEACON_INTERVAL: 500;
|
||||
|
||||
#if ID_OD_WIFI_BEACON
|
||||
|
||||
// If ODID_PACK_MAX_MESSAGES == 10, then the potential size of the beacon message is > 255.
|
||||
|
||||
#if ODID_PACK_MAX_MESSAGES > 9
|
||||
#undef ODID_PACK_MAX_MESSAGES
|
||||
#define ODID_PACK_MAX_MESSAGES 9
|
||||
#endif
|
||||
|
||||
memset(beacon_frame,0,BEACON_FRAME_SIZE);
|
||||
|
||||
#if !USE_BEACON_FUNC
|
||||
|
||||
beacon_counter =
|
||||
beacon_length =
|
||||
beacon_timestamp =
|
||||
beacon_seq =
|
||||
beacon_payload = beacon_frame;
|
||||
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
memset(msg_counter,0,sizeof(msg_counter));
|
||||
|
||||
//
|
||||
// Below '// 0' indicates where we are setting 0 to 0 for clarity.
|
||||
//
|
||||
|
||||
memset(&UAS_data,0,sizeof(ODID_UAS_Data));
|
||||
|
||||
basicID_data = &UAS_data.BasicID[0];
|
||||
location_data = &UAS_data.Location;
|
||||
selfID_data = &UAS_data.SelfID;
|
||||
system_data = &UAS_data.System;
|
||||
operatorID_data = &UAS_data.OperatorID;
|
||||
|
||||
for (i = 0; i < ODID_AUTH_MAX_PAGES; ++i) {
|
||||
|
||||
auth_data[i] = &UAS_data.Auth[i];
|
||||
|
||||
auth_data[i]->DataPage = i;
|
||||
auth_data[i]->AuthType = ODID_AUTH_NONE; // 0
|
||||
}
|
||||
|
||||
UAS_data.BasicID[0].IDType = ODID_IDTYPE_NONE; // 0
|
||||
UAS_data.BasicID[0].UAType = ODID_UATYPE_NONE; // 0
|
||||
UAS_data.BasicID[1].IDType = ODID_IDTYPE_NONE; // 0
|
||||
UAS_data.BasicID[1].UAType = ODID_UATYPE_NONE; // 0
|
||||
|
||||
odid_initLocationData(location_data);
|
||||
|
||||
location_data->Status = ODID_STATUS_UNDECLARED; // 0
|
||||
location_data->SpeedVertical = INV_SPEED_V;
|
||||
location_data->HeightType = ODID_HEIGHT_REF_OVER_TAKEOFF;
|
||||
location_data->HorizAccuracy = ODID_HOR_ACC_10_METER;
|
||||
location_data->VertAccuracy = ODID_VER_ACC_10_METER;
|
||||
location_data->BaroAccuracy = ODID_VER_ACC_10_METER;
|
||||
location_data->SpeedAccuracy = ODID_SPEED_ACC_10_METERS_PER_SECOND;
|
||||
location_data->TSAccuracy = ODID_TIME_ACC_1_5_SECOND;
|
||||
|
||||
selfID_data->DescType = ODID_DESC_TYPE_TEXT;
|
||||
strcpy(selfID_data->Desc,"Recreational");
|
||||
|
||||
odid_initSystemData(system_data);
|
||||
|
||||
system_data->OperatorLocationType = ODID_OPERATOR_LOCATION_TYPE_TAKEOFF;
|
||||
system_data->ClassificationType = ODID_CLASSIFICATION_TYPE_EU;
|
||||
system_data->AreaCount = 1;
|
||||
system_data->AreaRadius = 500;
|
||||
system_data->AreaCeiling =
|
||||
system_data->AreaFloor = -1000.0;
|
||||
system_data->CategoryEU = ODID_CATEGORY_EU_SPECIFIC;
|
||||
system_data->ClassEU = ODID_CLASS_EU_UNDECLARED;
|
||||
system_data->OperatorAltitudeGeo = -1000.0;
|
||||
|
||||
operatorID_data->OperatorIdType = ODID_OPERATOR_ID;
|
||||
|
||||
//
|
||||
|
||||
construct2();
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
void ID_OpenDrone::init(UTM_parameters *parameters) {
|
||||
|
||||
int status, i;
|
||||
char text[128];
|
||||
|
||||
status = 0;
|
||||
text[0] = text[63] = 0;
|
||||
|
||||
#if DIAGNOSTICS
|
||||
Debug_Serial = &Serial;
|
||||
#endif
|
||||
|
||||
// operator
|
||||
|
||||
UAS_operator = parameters->UAS_operator;
|
||||
|
||||
strncpy(operatorID_data->OperatorId,parameters->UAS_operator,ODID_ID_SIZE);
|
||||
operatorID_data->OperatorId[sizeof(operatorID_data->OperatorId) - 1] = 0;
|
||||
|
||||
// basic
|
||||
|
||||
UAS_data.BasicID[0].UAType = (ODID_uatype_t) parameters->UA_type;
|
||||
UAS_data.BasicID[1].UAType = (ODID_uatype_t) parameters->UA_type;
|
||||
|
||||
#if ID_NATIONAL
|
||||
|
||||
init_national(parameters);
|
||||
|
||||
#else
|
||||
|
||||
UAS_data.BasicID[0].IDType = (ODID_idtype_t) parameters->ID_type;
|
||||
UAS_data.BasicID[1].IDType = (ODID_idtype_t) parameters->ID_type2;
|
||||
|
||||
switch(basicID_data->IDType) {
|
||||
|
||||
case ODID_IDTYPE_SERIAL_NUMBER:
|
||||
|
||||
strncpy(basicID_data->UASID,parameters->UAV_id,ODID_ID_SIZE);
|
||||
break;
|
||||
|
||||
case ODID_IDTYPE_CAA_REGISTRATION_ID:
|
||||
|
||||
strncpy(basicID_data->UASID,parameters->UAS_operator,ODID_ID_SIZE);
|
||||
break;
|
||||
}
|
||||
|
||||
basicID_data->UASID[sizeof(basicID_data->UASID) - 1] = 0;
|
||||
|
||||
#endif
|
||||
|
||||
// system
|
||||
|
||||
if (parameters->region < 2) {
|
||||
|
||||
system_data->ClassificationType = (ODID_classification_type_t) parameters->region;
|
||||
}
|
||||
|
||||
if (parameters->EU_category < 4) {
|
||||
|
||||
system_data->CategoryEU = (ODID_category_EU_t) parameters->EU_category;
|
||||
}
|
||||
|
||||
if (parameters->EU_class < 8) {
|
||||
|
||||
system_data->ClassEU = (ODID_class_EU_t) parameters->EU_class;
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
encodeBasicIDMessage(&basicID_enc[0],&UAS_data.BasicID[0]);
|
||||
encodeBasicIDMessage(&basicID_enc[1],&UAS_data.BasicID[1]);
|
||||
encodeLocationMessage(&location_enc,location_data);
|
||||
encodeAuthMessage(&auth_enc,auth_data[0]);
|
||||
encodeSelfIDMessage(&selfID_enc,selfID_data);
|
||||
encodeSystemMessage(&system_enc,system_data);
|
||||
encodeOperatorIDMessage(&operatorID_enc,operatorID_data);
|
||||
|
||||
//
|
||||
|
||||
if (UAS_operator[0]) {
|
||||
|
||||
strncpy(ssid,UAS_operator,i = sizeof(ssid)); ssid[i - 1] = 0;
|
||||
}
|
||||
|
||||
ssid_length = strlen(ssid);
|
||||
|
||||
init2(ssid,ssid_length,WiFi_mac_addr,wifi_channel);
|
||||
|
||||
#if ID_OD_WIFI
|
||||
|
||||
#if ID_OD_WIFI_BEACON && !USE_BEACON_FUNC
|
||||
|
||||
init_beacon();
|
||||
|
||||
// payload
|
||||
beacon_payload = &beacon_frame[beacon_offset];
|
||||
beacon_offset += 7;
|
||||
|
||||
*beacon_payload++ = 0xdd;
|
||||
beacon_length = beacon_payload++;
|
||||
|
||||
*beacon_payload++ = 0xfa;
|
||||
*beacon_payload++ = 0x0b;
|
||||
*beacon_payload++ = 0xbc;
|
||||
|
||||
*beacon_payload++ = 0x0d;
|
||||
beacon_counter = beacon_payload++;
|
||||
|
||||
beacon_max_packed = BEACON_FRAME_SIZE - beacon_offset - 2;
|
||||
|
||||
if (beacon_max_packed > (ODID_PACK_MAX_MESSAGES * ODID_MESSAGE_SIZE)) {
|
||||
|
||||
beacon_max_packed = (ODID_PACK_MAX_MESSAGES * ODID_MESSAGE_SIZE);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
void ID_OpenDrone::set_self_id(char *self_id) {
|
||||
|
||||
memset(selfID_data->Desc,0,ODID_STR_SIZE + 1);
|
||||
strncpy(selfID_data->Desc,self_id,ODID_STR_SIZE);
|
||||
|
||||
encodeSelfIDMessage(&selfID_enc,selfID_data);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* These authentication functions need reviewing to make sure that they
|
||||
* comply with opendroneid release 2.0.
|
||||
*/
|
||||
|
||||
void ID_OpenDrone::set_auth(char *auth) {
|
||||
|
||||
set_auth((uint8_t *) auth,strlen(auth),0x0a);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
void ID_OpenDrone::set_auth(uint8_t *auth,short int len,uint8_t type) {
|
||||
|
||||
int i, j;
|
||||
char text[160];
|
||||
uint8_t check[32];
|
||||
|
||||
auth_page_count = 1;
|
||||
|
||||
if (len > MAX_AUTH_LENGTH) {
|
||||
|
||||
len = MAX_AUTH_LENGTH;
|
||||
auth[len] = 0;
|
||||
}
|
||||
|
||||
auth_data[0]->AuthType = (ODID_authtype_t) type;
|
||||
|
||||
for (i = 0; (i < 17)&&(auth[i]); ++i) {
|
||||
|
||||
check[i] =
|
||||
auth_data[0]->AuthData[i] = auth[i];
|
||||
}
|
||||
|
||||
check[i] =
|
||||
auth_data[0]->AuthData[i] = 0;
|
||||
|
||||
if (Debug_Serial) {
|
||||
|
||||
sprintf(text,"Auth. Code \'%s\' (%d)\r\n",auth,len);
|
||||
Debug_Serial->print(text);
|
||||
|
||||
sprintf(text,"Page 0 \'%s\'\r\n",check);
|
||||
Debug_Serial->print(text);
|
||||
}
|
||||
|
||||
if (len > 16) {
|
||||
|
||||
for (auth_page_count = 1; (auth_page_count < ODID_AUTH_MAX_PAGES)&&(i < len); ++auth_page_count) {
|
||||
|
||||
auth_data[auth_page_count]->AuthType = (ODID_authtype_t) type;
|
||||
|
||||
for (j = 0; (j < 23)&&(i < len); ++i, ++j) {
|
||||
|
||||
check[j] =
|
||||
auth_data[auth_page_count]->AuthData[j] = auth[i];
|
||||
}
|
||||
|
||||
if (j < 23) {
|
||||
|
||||
auth_data[auth_page_count]->AuthData[j] = 0;
|
||||
}
|
||||
|
||||
check[j] = 0;
|
||||
|
||||
if (Debug_Serial) {
|
||||
|
||||
sprintf(text,"Page %d \'%s\'\r\n",auth_page_count,check);
|
||||
Debug_Serial->print(text);
|
||||
}
|
||||
}
|
||||
|
||||
len = i;
|
||||
}
|
||||
|
||||
auth_data[0]->LastPageIndex = (auth_page_count) ? auth_page_count - 1: 0;
|
||||
auth_data[0]->Length = len;
|
||||
|
||||
#if not defined(ARDUINO_ARCH_NRF52)
|
||||
time_t secs;
|
||||
|
||||
time(&secs);
|
||||
|
||||
auth_data[0]->Timestamp = (uint32_t) (secs - ID_OD_AUTH_DATUM);
|
||||
#else
|
||||
auth_data[0]->Timestamp = 0;
|
||||
#endif
|
||||
|
||||
if (Debug_Serial) {
|
||||
|
||||
sprintf(text,"%d pages\r\n",auth_page_count);
|
||||
Debug_Serial->print(text);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
int ID_OpenDrone::transmit(struct UTM_data *utm_data) {
|
||||
|
||||
int i, status;
|
||||
char text[128];
|
||||
uint32_t msecs;
|
||||
time_t secs = 0;
|
||||
static int phase = 0;
|
||||
static uint32_t last_msecs = 2000;
|
||||
|
||||
//
|
||||
|
||||
i = 0;
|
||||
text[0] = 0;
|
||||
msecs = millis();
|
||||
|
||||
// For the ODID 2.0 and auth timestamps.
|
||||
#if defined(ARDUINO_ARCH_NRF52) || defined(ARDUINO_ARCH_ESP8266)
|
||||
secs = alt_unix_secs(utm_data->years,utm_data->months,utm_data->days,
|
||||
utm_data->hours,utm_data->minutes,utm_data->seconds);
|
||||
// secs = ID_OD_AUTH_DATUM;
|
||||
#elif 0
|
||||
struct tm clock_tm;
|
||||
|
||||
clock_tm.tm_year = utm_data->years - 1900;
|
||||
clock_tm.tm_mon = utm_data->months - 1;
|
||||
clock_tm.tm_mday = utm_data->days;
|
||||
clock_tm.tm_hour = utm_data->hours;
|
||||
clock_tm.tm_min = utm_data->minutes;
|
||||
clock_tm.tm_sec = utm_data->seconds;
|
||||
|
||||
secs = mktime(&clock_tm);
|
||||
#else
|
||||
time(&secs);
|
||||
#endif
|
||||
|
||||
//
|
||||
|
||||
if ((!system_data->OperatorLatitude)&&(utm_data->base_valid)) {
|
||||
|
||||
system_data->OperatorLatitude = utm_data->base_latitude;
|
||||
system_data->OperatorLongitude = utm_data->base_longitude;
|
||||
system_data->OperatorAltitudeGeo = utm_data->base_alt_m;
|
||||
|
||||
system_data->Timestamp = (uint32_t) (secs - ID_OD_AUTH_DATUM);
|
||||
|
||||
encodeSystemMessage(&system_enc,system_data);
|
||||
}
|
||||
|
||||
// Periodically encode live data and advertise using Bluetooth.
|
||||
|
||||
if ((msecs > last_msecs)&&
|
||||
((msecs - last_msecs) > 74)) {
|
||||
|
||||
last_msecs += 75;
|
||||
|
||||
switch (phase) {
|
||||
|
||||
case 0: case 8: case 16: case 24: case 32:
|
||||
case 4: case 12: case 20: case 28: case 36: // Every 300 ms.
|
||||
|
||||
if (utm_data->satellites >= SATS_LEVEL_2) {
|
||||
|
||||
location_data->Status = ODID_STATUS_UNDECLARED;
|
||||
location_data->Direction = (float) utm_data->heading;
|
||||
location_data->SpeedHorizontal = 0.514444 * (float) utm_data->speed_kn;
|
||||
location_data->SpeedVertical = INV_SPEED_V;
|
||||
location_data->Latitude = utm_data->latitude_d;
|
||||
location_data->Longitude = utm_data->longitude_d;
|
||||
location_data->Height = utm_data->alt_agl_m;
|
||||
location_data->AltitudeGeo = utm_data->alt_msl_m;
|
||||
|
||||
location_data->TimeStamp = (float) ((utm_data->minutes * 60) + utm_data->seconds) +
|
||||
0.01 * (float) utm_data->csecs;
|
||||
UAS_data.LocationValid = 1;
|
||||
|
||||
} else {
|
||||
|
||||
location_data->Status = ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
|
||||
}
|
||||
|
||||
if ((status = encodeLocationMessage(&location_enc,location_data)) == ODID_SUCCESS) {
|
||||
|
||||
transmit_ble((uint8_t *) &location_enc,sizeof(location_enc));
|
||||
|
||||
} else if (Debug_Serial) {
|
||||
|
||||
sprintf(text,"ID_OpenDrone::%s, encodeLocationMessage returned %d\r\n",
|
||||
__func__,status);
|
||||
Debug_Serial->print(text);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case 6: case 14: case 22: case 30: case 38: // Every 600 ms.
|
||||
|
||||
if (secs > ID_OD_AUTH_DATUM) {
|
||||
|
||||
system_data->Timestamp = (uint32_t) (secs - ID_OD_AUTH_DATUM);
|
||||
encodeSystemMessage(&system_enc,system_data);
|
||||
}
|
||||
|
||||
transmit_ble((uint8_t *) &system_enc,sizeof(system_enc));
|
||||
|
||||
break;
|
||||
|
||||
case 2:
|
||||
|
||||
if (UAS_data.BasicID[0].IDType) {
|
||||
|
||||
transmit_ble((uint8_t *) &basicID_enc[0],sizeof(ODID_BasicID_encoded));
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case 10:
|
||||
|
||||
if (UAS_data.BasicID[1].IDType) {
|
||||
|
||||
transmit_ble((uint8_t *) &basicID_enc[1],sizeof(ODID_BasicID_encoded));
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case 18:
|
||||
|
||||
transmit_ble((uint8_t *) &selfID_enc,sizeof(selfID_enc));
|
||||
break;
|
||||
|
||||
case 26:
|
||||
|
||||
transmit_ble((uint8_t *) &operatorID_enc,sizeof(operatorID_enc));
|
||||
break;
|
||||
|
||||
case 34:
|
||||
|
||||
if (auth_page_count) {
|
||||
|
||||
// Refresh the timestamp on page 0?
|
||||
|
||||
encodeAuthMessage(&auth_enc,auth_data[auth_page]);
|
||||
|
||||
transmit_ble((uint8_t *) &auth_enc,sizeof(auth_enc));
|
||||
|
||||
if (++auth_page >= auth_page_count) {
|
||||
|
||||
auth_page = 0;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
if (++phase > 39) {
|
||||
|
||||
phase = 0;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
#if ID_OD_WIFI
|
||||
|
||||
// Pack and transmit the WiFi data.
|
||||
|
||||
static uint8_t wifi_toggle = 1;
|
||||
static uint32_t last_wifi = 0;
|
||||
|
||||
if ((msecs - last_wifi) >= beacon_interval) {
|
||||
|
||||
last_wifi = msecs;
|
||||
|
||||
if (wifi_toggle ^= 1) { // Basic IDs, operator, system and location.
|
||||
|
||||
UAS_data.SystemValid = 1;
|
||||
|
||||
if (UAS_data.BasicID[0].UASID[0]) {
|
||||
UAS_data.BasicIDValid[0] = 1;
|
||||
}
|
||||
|
||||
if (UAS_data.BasicID[1].UASID[0]) {
|
||||
UAS_data.BasicIDValid[1] = 1;
|
||||
}
|
||||
|
||||
if (UAS_data.OperatorID.OperatorId[0]) {
|
||||
UAS_data.OperatorIDValid = 1;
|
||||
}
|
||||
|
||||
status = transmit_wifi(utm_data,0);
|
||||
|
||||
UAS_data.BasicIDValid[0] =
|
||||
UAS_data.BasicIDValid[1] =
|
||||
UAS_data.LocationValid =
|
||||
UAS_data.SystemValid =
|
||||
UAS_data.OperatorIDValid = 0;
|
||||
|
||||
} else {
|
||||
|
||||
#if ID_NATIONAL
|
||||
|
||||
UAS_data.Auth[0].Timestamp = system_data->Timestamp;
|
||||
|
||||
// memset(UAS_data.Auth[0].AuthData,0,12);
|
||||
encodeAuthMessage(&auth_enc,&UAS_data.Auth[0]);
|
||||
|
||||
status = transmit_wifi(utm_data,pack_encrypt_national(beacon_payload));
|
||||
|
||||
#else // Self ID, authentication and location.
|
||||
|
||||
if (UAS_data.SelfID.Desc[0]) {
|
||||
UAS_data.SelfIDValid = 1;
|
||||
}
|
||||
|
||||
for (i = 0; (i < auth_page_count)&&(i < ODID_AUTH_MAX_PAGES); ++i) {
|
||||
|
||||
UAS_data.AuthValid[i] = 1;
|
||||
}
|
||||
|
||||
status = transmit_wifi(utm_data,0);
|
||||
|
||||
UAS_data.LocationValid =
|
||||
UAS_data.SelfIDValid = 0;
|
||||
|
||||
for (i = 0; (i < auth_page_count)&&(i < ODID_AUTH_MAX_PAGES); ++i) {
|
||||
|
||||
UAS_data.AuthValid[i] = 0;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
#endif // ID_OD_WIFI
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
int ID_OpenDrone::transmit_wifi(struct UTM_data *utm_data,int prepacked) {
|
||||
|
||||
#if ID_OD_WIFI
|
||||
|
||||
int length = 0, wifi_status = 0;
|
||||
uint32_t msecs;
|
||||
uint64_t usecs = 0;
|
||||
static uint32_t last_wifi = 0;
|
||||
char text[128];
|
||||
|
||||
text[0] = 0;
|
||||
|
||||
//
|
||||
|
||||
if (++sequence > 0xffffff) {
|
||||
|
||||
sequence = 1;
|
||||
}
|
||||
|
||||
msecs = millis();
|
||||
wifi_interval = msecs - last_wifi;
|
||||
last_wifi = msecs;
|
||||
|
||||
#if not (defined(ARDUINO_ARCH_RP2040) || defined(ARDUINO_ARCH_NRF52))
|
||||
struct timespec ts;
|
||||
|
||||
clock_gettime(CLOCK_REALTIME,&ts);
|
||||
usecs = (uint64_t)((double) ts.tv_sec * 1e6 + (double) ts.tv_nsec * 1e-3);
|
||||
#else
|
||||
usecs = micros();
|
||||
#endif
|
||||
|
||||
#if ID_OD_WIFI_NAN
|
||||
|
||||
uint8_t buffer[1024];
|
||||
static uint8_t send_counter = 0;
|
||||
|
||||
if ((length = odid_wifi_build_nan_sync_beacon_frame((char *) WiFi_mac_addr,
|
||||
buffer,sizeof(buffer))) > 0) {
|
||||
|
||||
wifi_status = transmit_wifi2(buffer,length);
|
||||
}
|
||||
|
||||
if ((Debug_Serial)&&((length < 0)||(wifi_status != 0))) {
|
||||
|
||||
sprintf(text,"odid_wifi_build_nan_sync_beacon_frame() = %d, transmit_wifi2() = %d\r\n",
|
||||
length,(int) wifi_status);
|
||||
Debug_Serial->print(text);
|
||||
}
|
||||
|
||||
if ((length = odid_wifi_build_message_pack_nan_action_frame(&UAS_data,(char *) WiFi_mac_addr,
|
||||
++send_counter,
|
||||
buffer,sizeof(buffer))) > 0) {
|
||||
|
||||
wifi_status = transmit_wifi2(buffer,length);
|
||||
}
|
||||
|
||||
if (Debug_Serial) {
|
||||
|
||||
if ((length < 0)||(wifi_status != 0)) {
|
||||
|
||||
sprintf(text,"odid_wifi_build_message_pack_nan_action_frame() = %d, transmit_wifi2() = %d\r\n",
|
||||
length,(int) wifi_status);
|
||||
Debug_Serial->print(text);
|
||||
|
||||
#if DIAGNOSTICS
|
||||
|
||||
} else {
|
||||
|
||||
sprintf(text,"ID_OpenDrone::%s ... ",__func__);
|
||||
Debug_Serial->print(text);
|
||||
|
||||
for (int i = 0; i < 32; ++i) {
|
||||
|
||||
sprintf(text,"%02x ",buffer[16 + i]);
|
||||
Debug_Serial->print(text);
|
||||
}
|
||||
|
||||
Debug_Serial->print(" ... \r\n");
|
||||
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
#endif // NAN
|
||||
|
||||
#if ID_OD_WIFI_BEACON
|
||||
|
||||
#if USE_BEACON_FUNC
|
||||
|
||||
if ((length = odid_wifi_build_message_pack_beacon_frame(&UAS_data,(char *) WiFi_mac_addr,
|
||||
ssid,ssid_length,
|
||||
beacon_interval,++beacon_counter,
|
||||
beacon_frame,BEACON_FRAME_SIZE)) > 0) {
|
||||
|
||||
wifi_status = transmit_wifi2(beacon_frame,length);
|
||||
}
|
||||
|
||||
#if DIAGNOSTICS && 1
|
||||
|
||||
if (Debug_Serial) {
|
||||
|
||||
sprintf(text,"ID_OpenDrone::%s * %02x ... ",__func__,beacon_frame[0]);
|
||||
Debug_Serial->print(text);
|
||||
|
||||
for (int i = 0; i < 20; ++i) {
|
||||
|
||||
sprintf(text,"%02x ",beacon_frame[22 + i]);
|
||||
Debug_Serial->print(text);
|
||||
}
|
||||
|
||||
Debug_Serial->print(" ... *\r\n");
|
||||
}
|
||||
|
||||
#endif // DIAG
|
||||
|
||||
#else
|
||||
|
||||
int i, len2 = 0;
|
||||
|
||||
++*beacon_counter;
|
||||
|
||||
for (i = 0; i < 8; ++i) {
|
||||
|
||||
beacon_timestamp[i] = (usecs >> (i * 8)) & 0xff;
|
||||
}
|
||||
|
||||
#if 1
|
||||
beacon_seq[0] = (uint8_t) (sequence << 4);
|
||||
beacon_seq[1] = (uint8_t) (sequence >> 4);
|
||||
#endif
|
||||
|
||||
length = (prepacked > 0) ? prepacked:
|
||||
odid_message_build_pack(&UAS_data,beacon_payload,beacon_max_packed);
|
||||
|
||||
if (length > 0) {
|
||||
|
||||
*beacon_length = length + 5;
|
||||
|
||||
wifi_status = transmit_wifi2(beacon_frame,len2 = beacon_offset + length);
|
||||
}
|
||||
|
||||
#if DIAGNOSTICS && 1
|
||||
|
||||
if (Debug_Serial) {
|
||||
|
||||
sprintf(text,"ID_OpenDrone::%s %d %d+%d=%d ",
|
||||
__func__,beacon_max_packed,beacon_offset,length,len2);
|
||||
Debug_Serial->print(text);
|
||||
|
||||
sprintf(text,"* %02x ... ",beacon_frame[0]);
|
||||
Debug_Serial->print(text);
|
||||
|
||||
for (int i = 0; i < 16; ++i) {
|
||||
|
||||
if ((i == 3)||(i == 10)) {
|
||||
|
||||
Debug_Serial->print("| ");
|
||||
}
|
||||
|
||||
sprintf(text,"%02x ",beacon_frame[beacon_offset - 10 + i]);
|
||||
Debug_Serial->print(text);
|
||||
}
|
||||
|
||||
sprintf(text,"... %02x (%2d,%4u,%4u)\r\n",beacon_frame[len2 - 1],
|
||||
wifi_status,wifi_interval,ble_interval);
|
||||
Debug_Serial->print(text);
|
||||
}
|
||||
|
||||
#endif // DIAG
|
||||
|
||||
#endif // FUNC
|
||||
|
||||
#endif // BEACON
|
||||
|
||||
#endif // WIFI
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
int ID_OpenDrone::transmit_ble(uint8_t *odid_msg,int length) {
|
||||
|
||||
uint32_t msecs;
|
||||
static uint32_t last_ble;
|
||||
|
||||
msecs = millis();
|
||||
ble_interval = msecs - last_ble;
|
||||
last_ble = msecs;
|
||||
|
||||
#if ID_OD_BT
|
||||
|
||||
int i, j, k, len, status;
|
||||
uint8_t *a;
|
||||
|
||||
i = j = k = len = 0;
|
||||
a = ble_message;
|
||||
|
||||
memset(ble_message,0,sizeof(ble_message));
|
||||
|
||||
//
|
||||
|
||||
ble_message[j++] = 0x1e;
|
||||
ble_message[j++] = 0x16;
|
||||
ble_message[j++] = 0xfa; // ASTM
|
||||
ble_message[j++] = 0xff; //
|
||||
ble_message[j++] = 0x0d;
|
||||
|
||||
#if 0
|
||||
ble_message[j++] = ++counter;
|
||||
#else
|
||||
ble_message[j++] = ++msg_counter[odid_msg[0] >> 4];
|
||||
#endif
|
||||
|
||||
for (i = 0; (i < length)&&(j < sizeof(ble_message)); ++i, ++j) {
|
||||
|
||||
ble_message[j] = odid_msg[i];
|
||||
}
|
||||
|
||||
status = transmit_ble2(ble_message,len = j);
|
||||
|
||||
#if DIAGNOSTICS && 0
|
||||
|
||||
char text[64], text2[34];
|
||||
static int first = 1;
|
||||
|
||||
if (Debug_Serial) {
|
||||
|
||||
if (first) {
|
||||
|
||||
first = 0;
|
||||
|
||||
Debug_Serial->print("0000000 00 ");
|
||||
|
||||
for (i = 0; (i < 32); ++i) {
|
||||
|
||||
sprintf(text,"%02d ",i);
|
||||
Debug_Serial->print(text);
|
||||
}
|
||||
|
||||
Debug_Serial->print("\r\n");
|
||||
}
|
||||
|
||||
sprintf(text,"%7lu %02x (%2d,%2d) .. ",
|
||||
millis(),len - 1,len - 1,length);
|
||||
Debug_Serial->print(text);
|
||||
|
||||
for (i = 0; (i < len)&&(i < 32); ++i) {
|
||||
|
||||
sprintf(text,"%02x ",a[i]);
|
||||
text2[i] = ((a[i] > 31)&&(a[i] < 127)) ? a[i]: '.';
|
||||
Debug_Serial->print(text);
|
||||
}
|
||||
|
||||
text2[i] = 0;
|
||||
|
||||
Debug_Serial->print(text2);
|
||||
Debug_Serial->print("\r\n");
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#endif // BT
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
184
id_open.h
Normal file
184
id_open.h
Normal file
@ -0,0 +1,184 @@
|
||||
/* -*- tab-width: 2; mode: c; -*-
|
||||
*
|
||||
* C++ class for Arduino to function as a wrapper around opendroneid.
|
||||
*
|
||||
* Copyright (c) 2020-2023, Steve Jack.
|
||||
*
|
||||
* MIT licence.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef ID_OPENDRONE_H
|
||||
#define ID_OPENDRONE_H
|
||||
|
||||
/*
|
||||
* Using an ESP32 and enabling both WiFi and Bluetooth will almost certainly
|
||||
* require a partition scheme with > 1.2M for the application.
|
||||
*/
|
||||
|
||||
#if defined(ARDUINO_ARCH_ESP32)
|
||||
|
||||
#define ID_OD_WIFI_NAN 0
|
||||
#define ID_OD_WIFI_BEACON 1
|
||||
#define ID_OD_BT 0 // ASTM F3411-19 / ASD-STAN 4709-002.
|
||||
|
||||
#define USE_BEACON_FUNC 0
|
||||
#define ESP32_WIFI_OPTION 0
|
||||
|
||||
#elif defined(ARDUINO_ARCH_ESP8266)
|
||||
|
||||
#define ID_OD_WIFI_NAN 0
|
||||
#define ID_OD_WIFI_BEACON 1
|
||||
#define ID_OD_BT 0
|
||||
|
||||
#define USE_BEACON_FUNC 0
|
||||
|
||||
#elif defined(ARDUINO_ARCH_RP2040)
|
||||
|
||||
// The Pico doesn't have BT and the NAN/OD beacon code needs work to get it to compile for the Pico.
|
||||
|
||||
#define ID_OD_WIFI_NAN 0
|
||||
#define ID_OD_WIFI_BEACON 1
|
||||
#define ID_OD_BT 0
|
||||
|
||||
#define USE_BEACON_FUNC 0
|
||||
|
||||
#elif defined(ARDUINO_ARCH_NRF52)
|
||||
|
||||
#define ID_OD_WIFI_NAN 0
|
||||
#define ID_OD_WIFI_BEACON 0
|
||||
#define ID_OD_BT 1
|
||||
|
||||
#define USE_BEACON_FUNC 0
|
||||
|
||||
#else
|
||||
|
||||
error "No configuration for this processor."
|
||||
|
||||
#endif
|
||||
|
||||
// National/regional specific RIDs.
|
||||
|
||||
#define ID_JAPAN 0 // Experimental
|
||||
|
||||
#if (ID_JAPAN) && (ID_OD_WIFI_NAN || USE_BEACON_FUNC || !ID_OD_WIFI_BEACON)
|
||||
#warning "National IDs will only work with WIFI_BEACON"
|
||||
#define ID_NATIONAL 0
|
||||
#else
|
||||
#define ID_NATIONAL ID_JAPAN
|
||||
#endif
|
||||
|
||||
#if ID_OD_WIFI_NAN || ID_OD_WIFI_BEACON
|
||||
#define ID_OD_WIFI 1
|
||||
#else
|
||||
#define ID_OD_WIFI 0
|
||||
#endif
|
||||
|
||||
#define WIFI_CHANNEL 6 // Be careful changing this.
|
||||
#define BEACON_FRAME_SIZE 512
|
||||
#define BEACON_INTERVAL 0 // ms, defaults to 500. Android apps would prefer 100ms.
|
||||
|
||||
// Used by the id_open_beacon and id_open_esp32.
|
||||
|
||||
#if ID_JAPAN
|
||||
#define WIFI_COUNTRY_CC "JP"
|
||||
#define WIFI_COUNTRY_NCHAN 13
|
||||
#else
|
||||
#define WIFI_COUNTRY_CC "ZZ"
|
||||
#define WIFI_COUNTRY_NCHAN 11
|
||||
#endif
|
||||
|
||||
#define ID_OD_AUTH_DATUM 1546300800LU
|
||||
|
||||
//
|
||||
|
||||
#include "utm.h"
|
||||
|
||||
#include "opendroneid.h"
|
||||
|
||||
//
|
||||
// Functions in a processor specific file.
|
||||
//
|
||||
|
||||
void construct2(void);
|
||||
void init2(char *,int,uint8_t *,uint8_t);
|
||||
uint8_t *capability(void);
|
||||
int tag_rates(uint8_t *,int);
|
||||
int tag_ext_rates(uint8_t *,int);
|
||||
int misc_tags(uint8_t *,int);
|
||||
int transmit_wifi2(uint8_t *,int);
|
||||
int transmit_ble2(uint8_t *,int);
|
||||
|
||||
//
|
||||
|
||||
class ID_OpenDrone {
|
||||
|
||||
public:
|
||||
ID_OpenDrone();
|
||||
void init(struct UTM_parameters *);
|
||||
void set_self_id(char *);
|
||||
void set_auth(char *);
|
||||
void set_auth(uint8_t *,short int,uint8_t);
|
||||
int transmit(struct UTM_data *);
|
||||
#if ID_NATIONAL
|
||||
void init_national(struct UTM_parameters *);
|
||||
void auth_key_national(uint8_t *,int,uint8_t *,int);
|
||||
#endif
|
||||
|
||||
private:
|
||||
|
||||
void init_beacon(void);
|
||||
#if ID_NATIONAL
|
||||
int pack_encrypt_national(uint8_t *);
|
||||
#endif
|
||||
int transmit_wifi(struct UTM_data *,int);
|
||||
int transmit_ble(uint8_t *,int);
|
||||
|
||||
int auth_page = 0, auth_page_count = 0, key_length = 0, iv_length = 0;
|
||||
char *UAS_operator;
|
||||
uint8_t msg_counter[16];
|
||||
uint16_t wifi_interval = 0, ble_interval = 0;
|
||||
Stream *Debug_Serial = NULL;
|
||||
|
||||
char ssid[32];
|
||||
size_t ssid_length = 0;
|
||||
uint8_t WiFi_mac_addr[6], wifi_channel = WIFI_CHANNEL,
|
||||
*auth_key = NULL, *auth_iv = NULL;
|
||||
#if ID_OD_WIFI
|
||||
uint16_t sequence = 1, beacon_interval = 0x200;
|
||||
#if ID_OD_WIFI_BEACON
|
||||
int beacon_offset = 0, beacon_max_packed = 30;
|
||||
uint8_t beacon_frame[BEACON_FRAME_SIZE],
|
||||
#if USE_BEACON_FUNC
|
||||
beacon_counter = 0;
|
||||
#else
|
||||
*beacon_payload, *beacon_timestamp, *beacon_counter, *beacon_length, *beacon_seq;
|
||||
#endif
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if ID_OD_BT
|
||||
uint8_t ble_message[36], counter = 0;
|
||||
#endif
|
||||
|
||||
ODID_UAS_Data UAS_data;
|
||||
ODID_BasicID_data *basicID_data;
|
||||
ODID_Location_data *location_data;
|
||||
ODID_Auth_data *auth_data[ODID_AUTH_MAX_PAGES];
|
||||
ODID_SelfID_data *selfID_data;
|
||||
ODID_System_data *system_data;
|
||||
ODID_OperatorID_data *operatorID_data;
|
||||
|
||||
ODID_BasicID_encoded basicID_enc[2];
|
||||
ODID_Location_encoded location_enc;
|
||||
ODID_Auth_encoded auth_enc;
|
||||
ODID_SelfID_encoded selfID_enc;
|
||||
ODID_System_encoded system_enc;
|
||||
ODID_OperatorID_encoded operatorID_enc;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
192
id_open_beacon.cpp
Normal file
192
id_open_beacon.cpp
Normal file
@ -0,0 +1,192 @@
|
||||
/* -*- tab-width: 2; mode: c; -*-
|
||||
*
|
||||
* C++ class for Arduino to function as a wrapper around opendroneid.
|
||||
* This file has the wifi beacon setup code.
|
||||
*
|
||||
* Copyright (c) 2023, Steve Jack.
|
||||
*
|
||||
* May '23: WiFi country code now defined in id_open.h.
|
||||
*
|
||||
* Nov. '22: Split out from id_open.cpp.
|
||||
*
|
||||
* MIT licence.
|
||||
*
|
||||
* NOTES
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma GCC diagnostic warning "-Wunused-variable"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "id_open.h"
|
||||
|
||||
#if ID_OD_WIFI_BEACON && (!USE_BEACON_FUNC)
|
||||
|
||||
/*
|
||||
* The variables setup by the following function are defined in id_open.h.
|
||||
* Some of the tags are copied from beacon frames sent by the Raspberry Pi
|
||||
* which is known to work with Android ID apps.
|
||||
*
|
||||
*/
|
||||
|
||||
void ID_OpenDrone::init_beacon() {
|
||||
|
||||
int i;
|
||||
const uint8_t wifi_channels = WIFI_COUNTRY_NCHAN;
|
||||
const char wifi_country[] = WIFI_COUNTRY_CC;
|
||||
|
||||
struct __attribute__((__packed__)) beacon_header {
|
||||
|
||||
uint8_t control[2]; // 0-1: frame control
|
||||
uint8_t duration[2]; // 2-3: duration
|
||||
uint8_t dest_addr[6]; // 4-9: destination
|
||||
uint8_t src_addr[6]; // 10-15: source
|
||||
uint8_t bssid[6]; // 16-21: base station
|
||||
uint8_t seq[2]; // 22-23: sequence
|
||||
uint8_t timestamp[8]; // 24-31:
|
||||
uint8_t interval[2]; //
|
||||
uint8_t capability[2];
|
||||
} *header;
|
||||
|
||||
header = (struct beacon_header *) beacon_frame;
|
||||
beacon_timestamp = header->timestamp;
|
||||
beacon_seq = header->seq;
|
||||
|
||||
header->control[0] = 0x80;
|
||||
header->interval[0] = (uint8_t) beacon_interval;
|
||||
header->interval[1] = (uint8_t) (beacon_interval >> 8);
|
||||
|
||||
memcpy(header->capability,capability(),2);
|
||||
|
||||
for (i = 0; i < 6; ++i) {
|
||||
|
||||
header->dest_addr[i] = 0xff;
|
||||
header->src_addr[i] =
|
||||
header->bssid[i] = WiFi_mac_addr[i];
|
||||
}
|
||||
|
||||
beacon_offset = sizeof(struct beacon_header);
|
||||
|
||||
beacon_frame[beacon_offset++] = 0;
|
||||
beacon_frame[beacon_offset++] = ssid_length;
|
||||
|
||||
for (i = 0; (i < 32)&&(ssid[i]); ++i) {
|
||||
|
||||
beacon_frame[beacon_offset++] = ssid[i];
|
||||
}
|
||||
|
||||
// Supported rates
|
||||
#if 0
|
||||
beacon_frame[beacon_offset++] = 0x01; // This is what ODID 1.0 does.
|
||||
beacon_frame[beacon_offset++] = 0x01;
|
||||
beacon_frame[beacon_offset++] = 0x8c; // 11b, 6(B) Mbit/sec
|
||||
#else
|
||||
beacon_offset = tag_rates(beacon_frame,beacon_offset);
|
||||
#endif
|
||||
|
||||
// DS
|
||||
beacon_frame[beacon_offset++] = 0x03;
|
||||
beacon_frame[beacon_offset++] = 0x01;
|
||||
beacon_frame[beacon_offset++] = wifi_channel;
|
||||
|
||||
#if 1
|
||||
// Traffic Indication Map
|
||||
beacon_frame[beacon_offset++] = 0x05;
|
||||
beacon_frame[beacon_offset++] = 0x04;
|
||||
beacon_frame[beacon_offset++] = 0x00;
|
||||
beacon_frame[beacon_offset++] = 0x02;
|
||||
beacon_frame[beacon_offset++] = 0x00;
|
||||
beacon_frame[beacon_offset++] = 0x00;
|
||||
#endif
|
||||
|
||||
#if 1
|
||||
// Country Information
|
||||
beacon_frame[beacon_offset++] = 0x07;
|
||||
beacon_frame[beacon_offset++] = 0x06;
|
||||
beacon_frame[beacon_offset++] = wifi_country[0];
|
||||
beacon_frame[beacon_offset++] = wifi_country[1];
|
||||
beacon_frame[beacon_offset++] = 0x20;
|
||||
beacon_frame[beacon_offset++] = 0x01;
|
||||
beacon_frame[beacon_offset++] = wifi_channels;
|
||||
beacon_frame[beacon_offset++] = 0x14;
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
// Power Constraint
|
||||
beacon_frame[beacon_offset++] = 0x20;
|
||||
beacon_frame[beacon_offset++] = 0x01;
|
||||
beacon_frame[beacon_offset++] = 0x00;
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
// TPC Report Transmit Power
|
||||
beacon_frame[beacon_offset++] = 0x23;
|
||||
beacon_frame[beacon_offset++] = 0x02;
|
||||
beacon_frame[beacon_offset++] = 0x11;
|
||||
beacon_frame[beacon_offset++] = 0x00;
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
// ERP Information
|
||||
beacon_frame[beacon_offset++] = 0x2a;
|
||||
beacon_frame[beacon_offset++] = 0x01;
|
||||
beacon_frame[beacon_offset++] = 0x00;
|
||||
#endif
|
||||
|
||||
#if 1
|
||||
// Extended Supported Rates
|
||||
beacon_offset = tag_ext_rates(beacon_frame,beacon_offset);
|
||||
#endif
|
||||
|
||||
#if 1
|
||||
// Other WiFi chip dependent tags, e.g. HT Capabilities, HT Information
|
||||
beacon_offset = misc_tags(beacon_frame,beacon_offset);
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
// WPA Information
|
||||
beacon_frame[beacon_offset++] = 0xdd;
|
||||
beacon_frame[beacon_offset++] = 0x1a;
|
||||
|
||||
beacon_frame[beacon_offset++] = 0x00; // Microsoft
|
||||
beacon_frame[beacon_offset++] = 0x50;
|
||||
beacon_frame[beacon_offset++] = 0xf2;
|
||||
beacon_frame[beacon_offset++] = 0x01;
|
||||
beacon_frame[beacon_offset++] = 0x01;
|
||||
beacon_frame[beacon_offset++] = 0x00;
|
||||
|
||||
beacon_frame[beacon_offset++] = 0x00;
|
||||
beacon_frame[beacon_offset++] = 0x50;
|
||||
beacon_frame[beacon_offset++] = 0xf2;
|
||||
beacon_frame[beacon_offset++] = 0x02;
|
||||
beacon_frame[beacon_offset++] = 0x02;
|
||||
beacon_frame[beacon_offset++] = 0x00;
|
||||
|
||||
beacon_frame[beacon_offset++] = 0x00;
|
||||
beacon_frame[beacon_offset++] = 0x50;
|
||||
beacon_frame[beacon_offset++] = 0xf2;
|
||||
beacon_frame[beacon_offset++] = 0x04;
|
||||
|
||||
beacon_frame[beacon_offset++] = 0x00;
|
||||
beacon_frame[beacon_offset++] = 0x50;
|
||||
beacon_frame[beacon_offset++] = 0xf2;
|
||||
beacon_frame[beacon_offset++] = 0x02;
|
||||
beacon_frame[beacon_offset++] = 0x01;
|
||||
beacon_frame[beacon_offset++] = 0x00;
|
||||
|
||||
beacon_frame[beacon_offset++] = 0x00;
|
||||
beacon_frame[beacon_offset++] = 0x50;
|
||||
beacon_frame[beacon_offset++] = 0xf2;
|
||||
beacon_frame[beacon_offset++] = 0x02;
|
||||
#endif
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
#endif
|
||||
347
id_open_esp32.cpp
Normal file
347
id_open_esp32.cpp
Normal file
@ -0,0 +1,347 @@
|
||||
/* -*- tab-width: 2; mode: c; -*-
|
||||
*
|
||||
* C++ class for Arduino to function as a wrapper around opendroneid.
|
||||
* This file has the ESP32 specific code.
|
||||
*
|
||||
* Copyright (c) 2020-2023, Steve Jack.
|
||||
*
|
||||
* Nov. '22: Split out from id_open.cpp.
|
||||
*
|
||||
* MIT licence.
|
||||
*
|
||||
* NOTES
|
||||
*
|
||||
* Bluetooth 4 works well with the opendroneid app on my G7.
|
||||
* WiFi beacon works with an ESP32 scanner, but with the G7 only the occasional frame gets through.
|
||||
*
|
||||
* Features
|
||||
*
|
||||
* esp_wifi_80211_tx() seems to zero the WiFi timestamp in addition to setting the sequence.
|
||||
* (The timestamp is set in ID_OpenDrone::transmit_wifi(), but WireShark says that it is zero.)
|
||||
*
|
||||
* BLE
|
||||
*
|
||||
* A case of fighting the API to get it to do what I want.
|
||||
* For certain things, it is easier to bypass the 'user friendly' Arduino API and
|
||||
* use the esp_ functions.
|
||||
*
|
||||
* Reference
|
||||
*
|
||||
* https://github.com/opendroneid/receiver-android/issues/7
|
||||
*
|
||||
* From the Android app -
|
||||
*
|
||||
* OpenDroneID Bluetooth beacons identify themselves by setting the GAP AD Type to
|
||||
* "Service Data - 16-bit UUID" and the value to 0xFFFA for ASTM International, ASTM Remote ID.
|
||||
* https://www.bluetooth.com/specifications/assigned-numbers/generic-access-profile/
|
||||
* https://www.bluetooth.com/specifications/assigned-numbers/16-bit-uuids-for-sdos/
|
||||
* Vol 3, Part B, Section 2.5.1 of the Bluetooth 5.1 Core Specification
|
||||
* The AD Application Code is set to 0x0D = Open Drone ID.
|
||||
*
|
||||
private static final UUID SERVICE_UUID = UUID.fromString("0000fffa-0000-1000-8000-00805f9b34fb");
|
||||
private static final byte[] OPEN_DRONE_ID_AD_CODE = new byte[]{(byte) 0x0D};
|
||||
*
|
||||
*/
|
||||
|
||||
#define DIAGNOSTICS 1
|
||||
|
||||
//
|
||||
|
||||
#if defined(ARDUINO_ARCH_ESP32)
|
||||
|
||||
#pragma GCC diagnostic warning "-Wunused-variable"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "id_open.h"
|
||||
|
||||
#if ID_OD_WIFI
|
||||
|
||||
#include <WiFi.h>
|
||||
|
||||
#include <esp_system.h>
|
||||
#include <esp_event.h>
|
||||
#include <esp_event_loop.h>
|
||||
#include <esp_wifi.h>
|
||||
#include <esp_wifi_types.h>
|
||||
#include <nvs_flash.h>
|
||||
|
||||
esp_err_t esp_wifi_80211_tx(wifi_interface_t ifx,const void *buffer,int len,bool en_sys_seq);
|
||||
|
||||
esp_err_t event_handler(void *,system_event_t *);
|
||||
|
||||
#if ESP32_WIFI_OPTION == 0
|
||||
static const char *password = "password";
|
||||
#endif
|
||||
|
||||
#endif // WIFI
|
||||
|
||||
#if ID_OD_BT
|
||||
|
||||
#include "BLEDevice.h"
|
||||
#include "BLEUtils.h"
|
||||
|
||||
static esp_ble_adv_data_t advData;
|
||||
static esp_ble_adv_params_t advParams;
|
||||
static BLEUUID service_uuid;
|
||||
|
||||
#endif // BT
|
||||
|
||||
static Stream *Debug_Serial = NULL;
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
void construct2() {
|
||||
|
||||
#if ID_OD_BT
|
||||
|
||||
memset(&advData,0,sizeof(advData));
|
||||
|
||||
advData.set_scan_rsp = false;
|
||||
advData.include_name = false;
|
||||
advData.include_txpower = false;
|
||||
advData.min_interval = 0x0006;
|
||||
advData.max_interval = 0x0050;
|
||||
advData.flag = (ESP_BLE_ADV_FLAG_GEN_DISC | ESP_BLE_ADV_FLAG_BREDR_NOT_SPT);
|
||||
|
||||
memset(&advParams,0,sizeof(advParams));
|
||||
|
||||
advParams.adv_int_min = 0x0020;
|
||||
advParams.adv_int_max = 0x0040;
|
||||
advParams.adv_type = ADV_TYPE_IND;
|
||||
advParams.own_addr_type = BLE_ADDR_TYPE_PUBLIC;
|
||||
advParams.channel_map = ADV_CHNL_ALL;
|
||||
advParams.adv_filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY;
|
||||
advParams.peer_addr_type = BLE_ADDR_TYPE_PUBLIC;
|
||||
|
||||
service_uuid = BLEUUID("0000fffa-0000-1000-8000-00805f9b34fb");
|
||||
|
||||
#endif // ID_OD_BT
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
void init2(char *ssid,int ssid_length,uint8_t *WiFi_mac_addr,uint8_t wifi_channel) {
|
||||
|
||||
int status;
|
||||
char text[128];
|
||||
|
||||
status = 0;
|
||||
text[0] = text[63] = 0;
|
||||
|
||||
#if DIAGNOSTICS
|
||||
Debug_Serial = &Serial;
|
||||
#endif
|
||||
|
||||
#if ID_OD_WIFI
|
||||
|
||||
int8_t wifi_power;
|
||||
wifi_config_t ap_config;
|
||||
static wifi_country_t country = {WIFI_COUNTRY_CC,1,WIFI_COUNTRY_NCHAN,20,WIFI_COUNTRY_POLICY_AUTO};
|
||||
|
||||
memset(&ap_config,0,sizeof(ap_config));
|
||||
|
||||
#if ESP32_WIFI_OPTION
|
||||
|
||||
WiFi.softAP(ssid,"password",wifi_channel);
|
||||
|
||||
esp_wifi_get_config(WIFI_IF_AP,&ap_config);
|
||||
|
||||
// ap_config.ap.ssid_hidden = 1;
|
||||
status = esp_wifi_set_config(WIFI_IF_AP,&ap_config);
|
||||
|
||||
#else
|
||||
|
||||
wifi_init_config_t init_cfg = WIFI_INIT_CONFIG_DEFAULT();
|
||||
|
||||
nvs_flash_init();
|
||||
tcpip_adapter_init();
|
||||
|
||||
esp_event_loop_init(event_handler,NULL);
|
||||
esp_wifi_init(&init_cfg);
|
||||
esp_wifi_set_storage(WIFI_STORAGE_RAM);
|
||||
esp_wifi_set_mode(WIFI_MODE_AP);
|
||||
|
||||
strcpy((char *) ap_config.ap.ssid,ssid);
|
||||
strcpy((char *) ap_config.ap.password,password);
|
||||
ap_config.ap.ssid_len = strlen(ssid);
|
||||
ap_config.ap.channel = (uint8_t) wifi_channel;
|
||||
ap_config.ap.authmode = WIFI_AUTH_WPA2_PSK;
|
||||
ap_config.ap.ssid_hidden = 0;
|
||||
ap_config.ap.max_connection = 4;
|
||||
ap_config.ap.beacon_interval = 1000; // Pass beacon_interval from id_open.cpp?
|
||||
|
||||
esp_wifi_set_config(WIFI_IF_AP,&ap_config);
|
||||
esp_wifi_start();
|
||||
esp_wifi_set_ps(WIFI_PS_NONE);
|
||||
|
||||
#endif
|
||||
|
||||
esp_wifi_set_country(&country);
|
||||
status = esp_wifi_set_bandwidth(WIFI_IF_AP,WIFI_BW_HT20);
|
||||
|
||||
// esp_wifi_set_max_tx_power(78);
|
||||
esp_wifi_get_max_tx_power(&wifi_power);
|
||||
|
||||
status = esp_read_mac(WiFi_mac_addr,ESP_MAC_WIFI_STA);
|
||||
|
||||
if (Debug_Serial) {
|
||||
|
||||
sprintf(text,"esp_read_mac(): %02x:%02x:%02x:%02x:%02x:%02x\r\n",
|
||||
WiFi_mac_addr[0],WiFi_mac_addr[1],WiFi_mac_addr[2],
|
||||
WiFi_mac_addr[3],WiFi_mac_addr[4],WiFi_mac_addr[5]);
|
||||
Debug_Serial->print(text);
|
||||
// power <= 72, dbm = power/4, but 78 = 20dbm.
|
||||
sprintf(text,"max_tx_power(): %d dBm\r\n",(int) ((wifi_power + 2) / 4));
|
||||
Debug_Serial->print(text);
|
||||
sprintf(text,"wifi country: %s\r\n",country.cc);
|
||||
Debug_Serial->print(text);
|
||||
}
|
||||
|
||||
#endif // WIFI
|
||||
|
||||
#if ID_OD_BT
|
||||
|
||||
int power_db;
|
||||
esp_power_level_t power;
|
||||
|
||||
BLEDevice::init(ssid);
|
||||
|
||||
// Using BLEDevice::setPower() seems to have no effect.
|
||||
// ESP_PWR_LVL_N12 ... ESP_PWR_LVL_N0 ... ESP_PWR_LVL_P9
|
||||
|
||||
esp_ble_tx_power_set(ESP_BLE_PWR_TYPE_DEFAULT,ESP_PWR_LVL_P9);
|
||||
esp_ble_tx_power_set(ESP_BLE_PWR_TYPE_ADV,ESP_PWR_LVL_P9);
|
||||
|
||||
power = esp_ble_tx_power_get(ESP_BLE_PWR_TYPE_DEFAULT);
|
||||
power_db = 3 * ((int) power - 4);
|
||||
|
||||
#endif
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* Processor dependent bits for the wifi frame header.
|
||||
*/
|
||||
|
||||
uint8_t *capability() {
|
||||
|
||||
// 0x21 = ESS | Short preamble
|
||||
// 0x04 = Short slot time
|
||||
|
||||
static uint8_t capa[2] = {0x21,0x04};
|
||||
|
||||
return capa;
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
int tag_rates(uint8_t *beacon_frame,int beacon_offset) {
|
||||
|
||||
beacon_frame[beacon_offset++] = 0x01;
|
||||
beacon_frame[beacon_offset++] = 0x08;
|
||||
beacon_frame[beacon_offset++] = 0x8b; // 5.5
|
||||
beacon_frame[beacon_offset++] = 0x96; // 11
|
||||
beacon_frame[beacon_offset++] = 0x82; // 1
|
||||
beacon_frame[beacon_offset++] = 0x84; // 2
|
||||
beacon_frame[beacon_offset++] = 0x0c; // 6
|
||||
beacon_frame[beacon_offset++] = 0x18; // 12
|
||||
beacon_frame[beacon_offset++] = 0x30; // 24
|
||||
beacon_frame[beacon_offset++] = 0x60; // 48
|
||||
|
||||
return beacon_offset;
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
int tag_ext_rates(uint8_t *beacon_frame,int beacon_offset) {
|
||||
|
||||
beacon_frame[beacon_offset++] = 0x32;
|
||||
beacon_frame[beacon_offset++] = 0x04;
|
||||
beacon_frame[beacon_offset++] = 0x6c; // 54
|
||||
beacon_frame[beacon_offset++] = 0x12; // 9
|
||||
beacon_frame[beacon_offset++] = 0x24; // 18
|
||||
beacon_frame[beacon_offset++] = 0x48; // 36
|
||||
|
||||
return beacon_offset;
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
int misc_tags(uint8_t *beacon_frame,int beacon_offset) {
|
||||
|
||||
return beacon_offset;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
int transmit_wifi2(uint8_t *buffer,int length) {
|
||||
|
||||
esp_err_t wifi_status = 0;
|
||||
|
||||
#if ID_OD_WIFI
|
||||
|
||||
if (length) {
|
||||
|
||||
wifi_status = esp_wifi_80211_tx(WIFI_IF_AP,buffer,length,true);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
return (int) wifi_status;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
int transmit_ble2(uint8_t *ble_message,int length) {
|
||||
|
||||
esp_err_t ble_status = 0;
|
||||
|
||||
#if ID_OD_BT
|
||||
|
||||
static int advertising = 0;
|
||||
|
||||
if (advertising) {
|
||||
|
||||
ble_status = esp_ble_gap_stop_advertising();
|
||||
}
|
||||
|
||||
ble_status = esp_ble_gap_config_adv_data_raw(ble_message,length);
|
||||
ble_status = esp_ble_gap_start_advertising(&advParams);
|
||||
|
||||
advertising = 1;
|
||||
|
||||
#endif // BT
|
||||
|
||||
return (int) ble_status;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
#if ID_OD_WIFI
|
||||
|
||||
esp_err_t event_handler(void *ctx, system_event_t *event) {
|
||||
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
#endif
|
||||
187
id_open_esp8266.cpp
Normal file
187
id_open_esp8266.cpp
Normal file
@ -0,0 +1,187 @@
|
||||
/* -*- tab-width: 2; mode: c; -*-
|
||||
*
|
||||
* C++ class for Arduino to function as a wrapper around opendroneid.
|
||||
* This file has the ESP8266 specific code.
|
||||
*
|
||||
* Copyright (c) 2022, Steve Jack.
|
||||
*
|
||||
* Nov. '22: Split out from id_open.cpp.
|
||||
*
|
||||
* MIT licence.
|
||||
*
|
||||
* NOTES
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
#define DIAGNOSTICS 0
|
||||
|
||||
//
|
||||
|
||||
#if defined(ARDUINO_ARCH_ESP8266)
|
||||
|
||||
#pragma GCC diagnostic warning "-Wunused-variable"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "id_open.h"
|
||||
|
||||
#if ID_OD_WIFI
|
||||
|
||||
#include <ESP8266WiFi.h>
|
||||
|
||||
extern "C" {
|
||||
int wifi_send_pkt_freedom(uint8 *,int,bool);
|
||||
}
|
||||
|
||||
#endif // WIFI
|
||||
|
||||
static Stream *Debug_Serial = NULL;
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
void construct2() {
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
void init2(char *ssid,int ssid_length,uint8_t *WiFi_mac_addr,uint8_t wifi_channel) {
|
||||
|
||||
char text[128];
|
||||
|
||||
text[0] = text[63] = 0;
|
||||
|
||||
#if DIAGNOSTICS
|
||||
Debug_Serial = &Serial;
|
||||
#endif
|
||||
|
||||
#if ID_OD_WIFI
|
||||
|
||||
softap_config wifi_config;
|
||||
|
||||
WiFi.mode(WIFI_OFF);
|
||||
|
||||
WiFi.macAddress(WiFi_mac_addr);
|
||||
|
||||
WiFi.softAP(ssid,NULL,wifi_channel,false,0);
|
||||
WiFi.setOutputPower(20.0);
|
||||
|
||||
wifi_softap_get_config(&wifi_config);
|
||||
// wifi_config.beacon_interval = 1000; // Pass beacon_interval from id_open.cpp?
|
||||
// wifi_softap_set_config(&wifi_config);
|
||||
|
||||
if (Debug_Serial) {
|
||||
|
||||
sprintf(text,"esp_read_mac(): %02x:%02x:%02x:%02x:%02x:%02x\r\n",
|
||||
WiFi_mac_addr[0],WiFi_mac_addr[1],WiFi_mac_addr[2],
|
||||
WiFi_mac_addr[3],WiFi_mac_addr[4],WiFi_mac_addr[5]);
|
||||
Debug_Serial->print(text);
|
||||
}
|
||||
|
||||
#endif // WIFI
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
uint8_t *capability() {
|
||||
|
||||
static uint8_t capa[2] = {0x11,0x00};
|
||||
|
||||
return capa;
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
int tag_rates(uint8_t *beacon_frame,int beacon_offset) {
|
||||
|
||||
beacon_frame[beacon_offset++] = 0x01;
|
||||
beacon_frame[beacon_offset++] = 0x08;
|
||||
beacon_frame[beacon_offset++] = 0x8b; // 5.5
|
||||
beacon_frame[beacon_offset++] = 0x96; // 11
|
||||
beacon_frame[beacon_offset++] = 0x82; // 1
|
||||
beacon_frame[beacon_offset++] = 0x84; // 2
|
||||
beacon_frame[beacon_offset++] = 0x0c; // 6
|
||||
beacon_frame[beacon_offset++] = 0x18; // 12
|
||||
beacon_frame[beacon_offset++] = 0x30; // 24
|
||||
beacon_frame[beacon_offset++] = 0x60; // 48
|
||||
|
||||
return beacon_offset;
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
int tag_ext_rates(uint8_t *beacon_frame,int beacon_offset) {
|
||||
|
||||
beacon_frame[beacon_offset++] = 0x32;
|
||||
beacon_frame[beacon_offset++] = 0x04;
|
||||
beacon_frame[beacon_offset++] = 0x6c; // 54
|
||||
beacon_frame[beacon_offset++] = 0x12; // 9
|
||||
beacon_frame[beacon_offset++] = 0x24; // 18
|
||||
beacon_frame[beacon_offset++] = 0x48; // 36
|
||||
|
||||
return beacon_offset;
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
int misc_tags(uint8_t *beacon_frame,int beacon_offset) {
|
||||
|
||||
// Espressif
|
||||
beacon_frame[beacon_offset++] = 0xdd;
|
||||
beacon_frame[beacon_offset++] = 0x09;
|
||||
|
||||
beacon_frame[beacon_offset++] = 0x18;
|
||||
beacon_frame[beacon_offset++] = 0xfe;
|
||||
beacon_frame[beacon_offset++] = 0x34;
|
||||
beacon_frame[beacon_offset++] = 0x03;
|
||||
beacon_frame[beacon_offset++] = 0x01;
|
||||
beacon_frame[beacon_offset++] = 0x00;
|
||||
beacon_frame[beacon_offset++] = 0x00;
|
||||
beacon_frame[beacon_offset++] = 0x00;
|
||||
beacon_frame[beacon_offset++] = 0x00;
|
||||
|
||||
return beacon_offset;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
int transmit_wifi2(uint8_t *buffer,int length) {
|
||||
|
||||
#if ID_OD_WIFI
|
||||
|
||||
if (length) {
|
||||
|
||||
wifi_send_pkt_freedom(buffer,length,1);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
int transmit_ble2(uint8_t *ble_message,int length) {
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
#endif
|
||||
348
id_open_nrf52.cpp
Normal file
348
id_open_nrf52.cpp
Normal file
@ -0,0 +1,348 @@
|
||||
/* -*- tab-width: 2; mode: c; -*-
|
||||
*
|
||||
* C++ class for Arduino to function as a wrapper around opendroneid.
|
||||
* This file has nRF52 specific code.
|
||||
*
|
||||
* Copyright (c) 2022, Steve Jack.
|
||||
*
|
||||
* December '22
|
||||
*
|
||||
* MIT licence.
|
||||
*
|
||||
* NOTES
|
||||
*
|
||||
* Experimental.
|
||||
*
|
||||
* BLE_OPTION == 1
|
||||
*
|
||||
* Uses Adafruit's BLE libraries. Works for BT4. It doesn't work for BT5 coded.
|
||||
* Adafruit use Nordic's S140 and support for coded in S410 is experimental.
|
||||
*
|
||||
* BLE_OPTION == 2
|
||||
*
|
||||
* To do. Uses the nRF functions directly.
|
||||
* Probably easier and better to use Zephyr rather than Arduino for this.
|
||||
*
|
||||
*/
|
||||
|
||||
#define DIAGNOSTICS 1
|
||||
#define BLE_OPTION 1
|
||||
|
||||
//
|
||||
|
||||
#if defined(ARDUINO_ARCH_NRF52)
|
||||
|
||||
#pragma GCC diagnostic warning "-Wunused-variable"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "id_open.h"
|
||||
#include "id_open_nrf52.h"
|
||||
|
||||
#if ID_OD_WIFI
|
||||
|
||||
#endif // WIFI
|
||||
|
||||
#if ID_OD_BT
|
||||
|
||||
#if BLE_OPTION == 1
|
||||
|
||||
// Use Adafruit's BLE libraries.
|
||||
// nRF SDK, Soft Device S140.
|
||||
|
||||
#define PATCHED_BLEADV 0
|
||||
|
||||
BLEService BLE_ODID_service;
|
||||
|
||||
#elif BLE_OPTION == 2
|
||||
|
||||
#endif
|
||||
|
||||
//
|
||||
|
||||
const uint8_t ODID_Uuid[] = {0x00, 0x00, 0xff, 0xfa, 0x00, 0x00, 0x10, 0x00,
|
||||
0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb};
|
||||
static char BT_type_toggle[32];
|
||||
|
||||
#endif
|
||||
|
||||
extern "C" {
|
||||
;
|
||||
}
|
||||
|
||||
static void check_error(char *);
|
||||
static void connect_callback(uint16_t);
|
||||
static void disconnect_callback(uint16_t,uint8_t);
|
||||
|
||||
static Stream *Debug_Serial = NULL;
|
||||
#if DIAGNOSTICS
|
||||
static char text[128];
|
||||
#endif
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
void construct2() {
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
void init2(char *ssid,int ssid_length,uint8_t *WiFi_mac_addr,uint8_t wifi_channel) {
|
||||
|
||||
memset(BT_type_toggle,4,sizeof(BT_type_toggle));
|
||||
|
||||
#if DIAGNOSTICS
|
||||
text[0] = text[63] = 0;
|
||||
|
||||
Debug_Serial = &Serial;
|
||||
#endif
|
||||
|
||||
#if ID_OD_WIFI
|
||||
|
||||
#endif // WIFI
|
||||
|
||||
#if ID_OD_BT
|
||||
|
||||
#if BLE_OPTION == 1
|
||||
|
||||
err_t error;
|
||||
|
||||
Bluefruit.begin();
|
||||
Bluefruit.setTxPower(8);
|
||||
Bluefruit.setName(ssid);
|
||||
Bluefruit.Periph.setConnectCallback(connect_callback);
|
||||
Bluefruit.Periph.setDisconnectCallback(disconnect_callback);
|
||||
|
||||
Bluefruit.Advertising.addFlags(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
|
||||
|
||||
BLE_ODID_service.setUuid(BLEUuid(ODID_Uuid));
|
||||
Bluefruit.Advertising.addService(BLE_ODID_service);
|
||||
error = BLE_ODID_service.begin();
|
||||
|
||||
Bluefruit.Advertising.addName();
|
||||
// Bluefruit.Advertising.setType(BLE_GAP_ADV_TYPE_EXTENDED_NONCONNECTABLE_NONSCANNABLE_UNDIRECTED);
|
||||
|
||||
Bluefruit.Advertising.restartOnDisconnect(true);
|
||||
Bluefruit.Advertising.setInterval(100,100); // in unit of 0.625 ms
|
||||
Bluefruit.Advertising.setFastTimeout(30); // number of seconds in fast mode
|
||||
|
||||
Bluefruit.Advertising.start(0); // 0 = Don't stop advertising after n seconds
|
||||
|
||||
#elif BLE_OPTION == 2
|
||||
|
||||
#endif
|
||||
|
||||
#endif // BT
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
uint8_t *capability() {
|
||||
|
||||
static uint8_t capa[2] = {0x11,0x05};
|
||||
|
||||
return capa;
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
int tag_rates(uint8_t *beacon_frame,int beacon_offset) {
|
||||
|
||||
return beacon_offset;
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
int tag_ext_rates(uint8_t *beacon_frame,int beacon_offset) {
|
||||
|
||||
return beacon_offset;
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
int misc_tags(uint8_t *beacon_frame,int beacon_offset) {
|
||||
|
||||
return beacon_offset;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
int transmit_wifi2(uint8_t *buffer,int length) {
|
||||
|
||||
#if ID_OD_WIFI
|
||||
|
||||
;
|
||||
|
||||
#endif
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
* To do:
|
||||
*
|
||||
* Alternate BT4 & BT5 ?
|
||||
*
|
||||
*/
|
||||
|
||||
int transmit_ble2(uint8_t *ble_message,int length) {
|
||||
|
||||
short int BT_type = 5, index = 0;
|
||||
ODID_BasicID_encoded *basic_encoded;
|
||||
ODID_Auth_encoded_page_zero *auth_encoded;
|
||||
|
||||
basic_encoded = (ODID_BasicID_encoded *) &ble_message[6];
|
||||
auth_encoded = (ODID_Auth_encoded_page_zero *) &ble_message[6];
|
||||
|
||||
#if ID_OD_BT
|
||||
|
||||
#if DIAGNOSTICS
|
||||
static bool diag = 0;
|
||||
|
||||
if ((!diag)&&(Debug_Serial)&&(millis() > 5000)) {
|
||||
|
||||
diag = 1;
|
||||
|
||||
// This should go in init2, but doing it this way gives us chance to
|
||||
// open the serial port.
|
||||
|
||||
ble_version_t version;
|
||||
|
||||
if (text[0]) {
|
||||
Debug_Serial->print(text);
|
||||
}
|
||||
|
||||
sd_ble_version_get(&version);
|
||||
|
||||
sprintf(text,"nRF sd_ble_version_get() = 0x%04x %d %d \n",
|
||||
version.company_id,version.version_number,version.subversion_number);
|
||||
Debug_Serial->print(text);
|
||||
}
|
||||
#endif
|
||||
|
||||
if (length < (ODID_MESSAGE_SIZE + 10)) {
|
||||
|
||||
// Toggle between BT4 and BT5.
|
||||
|
||||
switch (basic_encoded->MessageType) {
|
||||
|
||||
case ODID_MESSAGETYPE_BASIC_ID:
|
||||
|
||||
index = (basic_encoded->IDType < 6) ? basic_encoded->IDType: 0;
|
||||
break;
|
||||
|
||||
case ODID_MESSAGETYPE_LOCATION:
|
||||
case ODID_MESSAGETYPE_SELF_ID:
|
||||
case ODID_MESSAGETYPE_SYSTEM:
|
||||
case ODID_MESSAGETYPE_OPERATOR_ID:
|
||||
|
||||
index = basic_encoded->MessageType + 6;
|
||||
break;
|
||||
|
||||
case ODID_MESSAGETYPE_AUTH:
|
||||
index = auth_encoded->DataPage + 14;
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
index = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
BT_type = BT_type_toggle[index];
|
||||
BT_type_toggle[index] = (BT_type_toggle[index] == 4) ? 5: 4;
|
||||
}
|
||||
|
||||
#if BLE_OPTION == 1
|
||||
|
||||
Bluefruit.Advertising.stop();
|
||||
|
||||
if (BT_type == 4) {
|
||||
|
||||
Bluefruit.Advertising.setType(BLE_GAP_ADV_TYPE_NONCONNECTABLE_NONSCANNABLE_UNDIRECTED);
|
||||
#if PATCHED_BLEADV
|
||||
Bluefruit.Advertising.setPhy(BLE_GAP_PHY_AUTO);
|
||||
#endif
|
||||
} else {
|
||||
|
||||
Bluefruit.Advertising.setType(BLE_GAP_ADV_TYPE_EXTENDED_NONCONNECTABLE_SCANNABLE_UNDIRECTED);
|
||||
#if PATCHED_BLEADV
|
||||
Bluefruit.Advertising.setPhy(BLE_GAP_PHY_CODED);
|
||||
#endif
|
||||
}
|
||||
|
||||
Bluefruit.Advertising.setData(ble_message,length);
|
||||
Bluefruit.Advertising.start(0);
|
||||
|
||||
#if DIAGNOSTICS
|
||||
if (Debug_Serial) {
|
||||
|
||||
sprintf(text,"\r%s %08x %2d BT%d ",__func__,ble_message,length,BT_type);
|
||||
Debug_Serial->print(text);
|
||||
|
||||
for (int j = 0; j < 12; ++j) {
|
||||
|
||||
sprintf(text,"%02x ",ble_message[j]);
|
||||
Debug_Serial->print(text);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#elif BLE_OPTION == 2
|
||||
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
void connect_callback(uint16_t conn_handle) {
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
void disconnect_callback(uint16_t conn_handle, uint8_t reason) {
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* Misc. utility functions.
|
||||
*/
|
||||
|
||||
void check_error(char * name) {
|
||||
|
||||
char text[128];
|
||||
|
||||
if ((Debug_Serial)) {
|
||||
|
||||
sprintf(text,"%s\n",name);
|
||||
Debug_Serial->print(text);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
#endif // NRF52
|
||||
41
id_open_nrf52.h
Normal file
41
id_open_nrf52.h
Normal file
@ -0,0 +1,41 @@
|
||||
/* -*- tab-width: 2; mode: c; -*-
|
||||
*
|
||||
* C++ class for Arduino to function as a wrapper around opendroneid.
|
||||
* This file has nRF52 specific code.
|
||||
*
|
||||
* Copyright (c) 2022, Steve Jack.
|
||||
*
|
||||
* NOTES
|
||||
*
|
||||
*/
|
||||
|
||||
#if defined(ARDUINO_ARCH_NRF52)
|
||||
|
||||
#pragma GCC diagnostic warning "-Wunused-variable"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#if ID_OD_BT
|
||||
|
||||
#include <ble.h>
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
#if BLE_OPTION == 1
|
||||
|
||||
#include <bluefruit.h>
|
||||
#include <BLEAdvertising.h>
|
||||
|
||||
#elif BLE_OPTION == 2
|
||||
|
||||
#endif
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
#endif // ID_OD_BT
|
||||
|
||||
#endif // nRF52
|
||||
10
library.properties
Normal file
10
library.properties
Normal file
@ -0,0 +1,10 @@
|
||||
name=id_open
|
||||
version=1.2
|
||||
author=Steve Jack
|
||||
maintainer=Steve Jack
|
||||
sentence=Arduino/ESP32 opendroneid.
|
||||
paragraph=Arduino/ESP32 library to act as a wrapper around opendroneid.
|
||||
category=Uncategorized
|
||||
url=https://github.com/sxjack/uav_electronic_ids/tree/main/id_open
|
||||
architectures=esp32,esp8266,rp2040,nrf52
|
||||
includes=id_open.h
|
||||
106
odid_wifi.h
Normal file
106
odid_wifi.h
Normal file
@ -0,0 +1,106 @@
|
||||
/*
|
||||
Copyright (C) 2019 Intel Corporation
|
||||
|
||||
SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
Open Drone ID C Library
|
||||
|
||||
Maintainer:
|
||||
Gabriel Cox
|
||||
gabriel.c.cox@intel.com
|
||||
*/
|
||||
|
||||
#ifndef _ODID_WIFI_H_
|
||||
#define _ODID_WIFI_H_
|
||||
|
||||
/**
|
||||
* IEEE 802.11 structs to build management action frame
|
||||
*/
|
||||
struct __attribute__((__packed__)) ieee80211_mgmt {
|
||||
uint16_t frame_control;
|
||||
uint16_t duration;
|
||||
uint8_t da[6];
|
||||
uint8_t sa[6];
|
||||
uint8_t bssid[6];
|
||||
uint16_t seq_ctrl;
|
||||
};
|
||||
|
||||
struct __attribute__((__packed__)) ieee80211_beacon {
|
||||
uint64_t timestamp;
|
||||
uint16_t beacon_interval;
|
||||
uint16_t capability;
|
||||
};
|
||||
|
||||
struct __attribute__((__packed__)) ieee80211_ssid {
|
||||
uint8_t element_id;
|
||||
uint8_t length;
|
||||
uint8_t ssid[];
|
||||
};
|
||||
|
||||
struct __attribute__((__packed__)) ieee80211_supported_rates {
|
||||
uint8_t element_id;
|
||||
uint8_t length;
|
||||
uint8_t supported_rates;
|
||||
};
|
||||
|
||||
struct __attribute__((__packed__)) ieee80211_vendor_specific {
|
||||
uint8_t element_id;
|
||||
uint8_t length;
|
||||
uint8_t oui[3];
|
||||
uint8_t oui_type;
|
||||
};
|
||||
|
||||
struct __attribute__((__packed__)) nan_service_discovery {
|
||||
uint8_t category;
|
||||
uint8_t action_code;
|
||||
uint8_t oui[3];
|
||||
uint8_t oui_type;
|
||||
};
|
||||
|
||||
struct __attribute__((__packed__)) nan_attribute_header {
|
||||
uint8_t attribute_id;
|
||||
uint16_t length;
|
||||
};
|
||||
|
||||
struct __attribute__((__packed__)) nan_master_indication_attribute {
|
||||
struct nan_attribute_header header;
|
||||
uint8_t master_preference;
|
||||
uint8_t random_factor;
|
||||
};
|
||||
|
||||
struct __attribute__((__packed__)) nan_cluster_attribute {
|
||||
struct nan_attribute_header header;
|
||||
uint8_t device_mac[6];
|
||||
uint8_t random_factor;
|
||||
uint8_t master_preference;
|
||||
uint8_t hop_count_to_anchor_master;
|
||||
uint8_t anchor_master_beacon_transmission_time[4];
|
||||
};
|
||||
|
||||
struct __attribute__((__packed__)) nan_service_id_list_attribute {
|
||||
struct nan_attribute_header header;
|
||||
uint8_t service_id[6];
|
||||
};
|
||||
|
||||
struct __attribute__((__packed__)) nan_service_descriptor_attribute {
|
||||
struct nan_attribute_header header;
|
||||
uint8_t service_id[6];
|
||||
uint8_t instance_id;
|
||||
uint8_t requestor_instance_id;
|
||||
uint8_t service_control;
|
||||
uint8_t service_info_length;
|
||||
};
|
||||
|
||||
struct __attribute__((__packed__)) nan_service_descriptor_extension_attribute {
|
||||
struct nan_attribute_header header;
|
||||
uint8_t instance_id;
|
||||
uint16_t control;
|
||||
uint8_t service_update_indicator;
|
||||
};
|
||||
|
||||
struct __attribute__((__packed__)) ODID_service_info {
|
||||
uint8_t message_counter;
|
||||
ODID_MessagePack_encoded odid_message_pack[];
|
||||
};
|
||||
|
||||
#endif // _ODID_WIFI_H_
|
||||
1476
opendroneid.c
Normal file
1476
opendroneid.c
Normal file
File diff suppressed because it is too large
Load Diff
762
opendroneid.h
Normal file
762
opendroneid.h
Normal file
@ -0,0 +1,762 @@
|
||||
/*
|
||||
Copyright (C) 2019 Intel Corporation
|
||||
|
||||
SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
Open Drone ID C Library
|
||||
|
||||
Maintainer:
|
||||
Gabriel Cox
|
||||
gabriel.c.cox@intel.com
|
||||
*/
|
||||
|
||||
#ifndef _OPENDRONEID_H_
|
||||
#define _OPENDRONEID_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define ODID_MESSAGE_SIZE 25
|
||||
#define ODID_ID_SIZE 20
|
||||
#define ODID_STR_SIZE 23
|
||||
|
||||
/*
|
||||
* This implementation is compliant with the:
|
||||
* - ASTM F3411 Specification for Remote ID and Tracking
|
||||
* - ASD-STAN prEN 4709-002 Direct Remote Identification
|
||||
*
|
||||
* Since the strategy of the standardization for drone ID has been to not break
|
||||
* backwards compatibility when adding new functionality, no attempt in this
|
||||
* implementation is made to verify the version number when decoding messages.
|
||||
* It is assumed that newer versions can be decoded but some data elements
|
||||
* might be missing in the output.
|
||||
*
|
||||
* The following protocol versions have been in use:
|
||||
* 0: ASTM F3411-19. Published Feb 14, 2020. https://www.astm.org/f3411-19.html
|
||||
* 1: ASD-STAN prEN 4709-002 P1. Published 31-Oct-2021. http://asd-stan.org/downloads/asd-stan-pren-4709-002-p1/
|
||||
* ASTM F3411 v1.1 draft sent for first ballot round autumn 2021
|
||||
* 2: ASTM F3411-v1.1 draft sent for second ballot round Q1 2022. (ASTM F3411-22 ?)
|
||||
* The delta to protocol version 1 is small:
|
||||
* - New enum values ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE, ODID_DESC_TYPE_EMERGENCY and ODID_DESC_TYPE_EXTENDED_STATUS
|
||||
* - New Timestamp field in the System message
|
||||
*/
|
||||
#define ODID_PROTOCOL_VERSION 2
|
||||
|
||||
/*
|
||||
* To save memory on implementations that do not need support for 16 pages of
|
||||
* authentication data, define ODID_AUTH_MAX_PAGES to the desired value before
|
||||
* including opendroneid.h. E.g. "-DODID_AUTH_MAX_PAGES=5" when calling cmake.
|
||||
*/
|
||||
#ifndef ODID_AUTH_MAX_PAGES
|
||||
#define ODID_AUTH_MAX_PAGES 16
|
||||
#endif
|
||||
#if (ODID_AUTH_MAX_PAGES < 1) || (ODID_AUTH_MAX_PAGES > 16)
|
||||
#error "ODID_AUTH_MAX_PAGES must be between 1 and 16."
|
||||
#endif
|
||||
|
||||
#define ODID_AUTH_PAGE_ZERO_DATA_SIZE 17
|
||||
#define ODID_AUTH_PAGE_NONZERO_DATA_SIZE 23
|
||||
#define MAX_AUTH_LENGTH (ODID_AUTH_PAGE_ZERO_DATA_SIZE + \
|
||||
ODID_AUTH_PAGE_NONZERO_DATA_SIZE * (ODID_AUTH_MAX_PAGES - 1))
|
||||
|
||||
#ifndef ODID_BASIC_ID_MAX_MESSAGES
|
||||
#define ODID_BASIC_ID_MAX_MESSAGES 2
|
||||
#endif
|
||||
#if (ODID_BASIC_ID_MAX_MESSAGES < 1) || (ODID_BASIC_ID_MAX_MESSAGES > 5)
|
||||
#error "ODID_BASIC_ID_MAX_MESSAGES must be between 1 and 5."
|
||||
#endif
|
||||
|
||||
#define ODID_PACK_MAX_MESSAGES 9
|
||||
|
||||
#define ODID_SUCCESS 0
|
||||
#define ODID_FAIL 1
|
||||
|
||||
#define MIN_DIR 0 // Minimum direction
|
||||
#define MAX_DIR 360 // Maximum direction
|
||||
#define INV_DIR 361 // Invalid direction
|
||||
#define MIN_SPEED_H 0 // Minimum speed horizontal
|
||||
#define MAX_SPEED_H 254.25f // Maximum speed horizontal
|
||||
#define INV_SPEED_H 255 // Invalid speed horizontal
|
||||
#define MIN_SPEED_V (-62) // Minimum speed vertical
|
||||
#define MAX_SPEED_V 62 // Maximum speed vertical
|
||||
#define INV_SPEED_V 63 // Invalid speed vertical
|
||||
#define MIN_LAT (-90) // Minimum latitude
|
||||
#define MAX_LAT 90 // Maximum latitude
|
||||
#define MIN_LON (-180) // Minimum longitude
|
||||
#define MAX_LON 180 // Maximum longitude
|
||||
#define MIN_ALT (-1000) // Minimum altitude
|
||||
#define MAX_ALT 31767.5f// Maximum altitude
|
||||
#define INV_ALT MIN_ALT // Invalid altitude
|
||||
#define MAX_TIMESTAMP (60 * 60)
|
||||
#define INV_TIMESTAMP 0xFFFF // Invalid, No Value or Unknown Timestamp
|
||||
#define MAX_AREA_RADIUS 2550
|
||||
|
||||
typedef enum ODID_messagetype {
|
||||
ODID_MESSAGETYPE_BASIC_ID = 0,
|
||||
ODID_MESSAGETYPE_LOCATION = 1,
|
||||
ODID_MESSAGETYPE_AUTH = 2,
|
||||
ODID_MESSAGETYPE_SELF_ID = 3,
|
||||
ODID_MESSAGETYPE_SYSTEM = 4,
|
||||
ODID_MESSAGETYPE_OPERATOR_ID = 5,
|
||||
ODID_MESSAGETYPE_PACKED = 0xF,
|
||||
ODID_MESSAGETYPE_INVALID = 0xFF,
|
||||
} ODID_messagetype_t;
|
||||
|
||||
// Each message type must maintain it's own message uint8_t counter, which must
|
||||
// be incremented if the message content changes. For repeated transmission of
|
||||
// the same message content, incrementing the counter is optional.
|
||||
typedef enum ODID_msg_counter {
|
||||
ODID_MSG_COUNTER_BASIC_ID = 0,
|
||||
ODID_MSG_COUNTER_LOCATION = 1,
|
||||
ODID_MSG_COUNTER_AUTH = 2,
|
||||
ODID_MSG_COUNTER_SELF_ID = 3,
|
||||
ODID_MSG_COUNTER_SYSTEM = 4,
|
||||
ODID_MSG_COUNTER_OPERATOR_ID = 5,
|
||||
ODID_MSG_COUNTER_PACKED = 6,
|
||||
ODID_MSG_COUNTER_AMOUNT = 7,
|
||||
} ODID_msg_counter_t;
|
||||
|
||||
typedef enum ODID_idtype {
|
||||
ODID_IDTYPE_NONE = 0,
|
||||
ODID_IDTYPE_SERIAL_NUMBER = 1,
|
||||
ODID_IDTYPE_CAA_REGISTRATION_ID = 2, // Civil Aviation Authority
|
||||
ODID_IDTYPE_UTM_ASSIGNED_UUID = 3, // UAS (Unmanned Aircraft System) Traffic Management
|
||||
ODID_IDTYPE_SPECIFIC_SESSION_ID = 4, // The exact id type is specified by the first byte of UASID and these type
|
||||
// values are managed by ICAO. 0 is reserved. 1 - 224 are managed by ICAO.
|
||||
// 225 - 255 are available for private experimental usage only
|
||||
// 5 to 15 reserved
|
||||
} ODID_idtype_t;
|
||||
|
||||
typedef enum ODID_uatype {
|
||||
ODID_UATYPE_NONE = 0,
|
||||
ODID_UATYPE_AEROPLANE = 1, // Fixed wing
|
||||
ODID_UATYPE_HELICOPTER_OR_MULTIROTOR = 2,
|
||||
ODID_UATYPE_GYROPLANE = 3,
|
||||
ODID_UATYPE_HYBRID_LIFT = 4, // Fixed wing aircraft that can take off vertically
|
||||
ODID_UATYPE_ORNITHOPTER = 5,
|
||||
ODID_UATYPE_GLIDER = 6,
|
||||
ODID_UATYPE_KITE = 7,
|
||||
ODID_UATYPE_FREE_BALLOON = 8,
|
||||
ODID_UATYPE_CAPTIVE_BALLOON = 9,
|
||||
ODID_UATYPE_AIRSHIP = 10, // Such as a blimp
|
||||
ODID_UATYPE_FREE_FALL_PARACHUTE = 11, // Unpowered
|
||||
ODID_UATYPE_ROCKET = 12,
|
||||
ODID_UATYPE_TETHERED_POWERED_AIRCRAFT = 13,
|
||||
ODID_UATYPE_GROUND_OBSTACLE = 14,
|
||||
ODID_UATYPE_OTHER = 15,
|
||||
} ODID_uatype_t;
|
||||
|
||||
typedef enum ODID_status {
|
||||
ODID_STATUS_UNDECLARED = 0,
|
||||
ODID_STATUS_GROUND = 1,
|
||||
ODID_STATUS_AIRBORNE = 2,
|
||||
ODID_STATUS_EMERGENCY = 3,
|
||||
ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE = 4,
|
||||
// 3 to 15 reserved
|
||||
} ODID_status_t;
|
||||
|
||||
typedef enum ODID_Height_reference {
|
||||
ODID_HEIGHT_REF_OVER_TAKEOFF = 0,
|
||||
ODID_HEIGHT_REF_OVER_GROUND = 1,
|
||||
} ODID_Height_reference_t;
|
||||
|
||||
typedef enum ODID_Horizontal_accuracy {
|
||||
ODID_HOR_ACC_UNKNOWN = 0,
|
||||
ODID_HOR_ACC_10NM = 1, // Nautical Miles. 18.52 km
|
||||
ODID_HOR_ACC_4NM = 2, // 7.408 km
|
||||
ODID_HOR_ACC_2NM = 3, // 3.704 km
|
||||
ODID_HOR_ACC_1NM = 4, // 1.852 km
|
||||
ODID_HOR_ACC_0_5NM = 5, // 926 m
|
||||
ODID_HOR_ACC_0_3NM = 6, // 555.6 m
|
||||
ODID_HOR_ACC_0_1NM = 7, // 185.2 m
|
||||
ODID_HOR_ACC_0_05NM = 8, // 92.6 m
|
||||
ODID_HOR_ACC_30_METER = 9,
|
||||
ODID_HOR_ACC_10_METER = 10,
|
||||
ODID_HOR_ACC_3_METER = 11,
|
||||
ODID_HOR_ACC_1_METER = 12,
|
||||
// 13 to 15 reserved
|
||||
} ODID_Horizontal_accuracy_t;
|
||||
|
||||
typedef enum ODID_Vertical_accuracy {
|
||||
ODID_VER_ACC_UNKNOWN = 0,
|
||||
ODID_VER_ACC_150_METER = 1,
|
||||
ODID_VER_ACC_45_METER = 2,
|
||||
ODID_VER_ACC_25_METER = 3,
|
||||
ODID_VER_ACC_10_METER = 4,
|
||||
ODID_VER_ACC_3_METER = 5,
|
||||
ODID_VER_ACC_1_METER = 6,
|
||||
// 7 to 15 reserved
|
||||
} ODID_Vertical_accuracy_t;
|
||||
|
||||
typedef enum ODID_Speed_accuracy {
|
||||
ODID_SPEED_ACC_UNKNOWN = 0,
|
||||
ODID_SPEED_ACC_10_METERS_PER_SECOND = 1,
|
||||
ODID_SPEED_ACC_3_METERS_PER_SECOND = 2,
|
||||
ODID_SPEED_ACC_1_METERS_PER_SECOND = 3,
|
||||
ODID_SPEED_ACC_0_3_METERS_PER_SECOND = 4,
|
||||
// 5 to 15 reserved
|
||||
} ODID_Speed_accuracy_t;
|
||||
|
||||
typedef enum ODID_Timestamp_accuracy {
|
||||
ODID_TIME_ACC_UNKNOWN = 0,
|
||||
ODID_TIME_ACC_0_1_SECOND = 1,
|
||||
ODID_TIME_ACC_0_2_SECOND = 2,
|
||||
ODID_TIME_ACC_0_3_SECOND = 3,
|
||||
ODID_TIME_ACC_0_4_SECOND = 4,
|
||||
ODID_TIME_ACC_0_5_SECOND = 5,
|
||||
ODID_TIME_ACC_0_6_SECOND = 6,
|
||||
ODID_TIME_ACC_0_7_SECOND = 7,
|
||||
ODID_TIME_ACC_0_8_SECOND = 8,
|
||||
ODID_TIME_ACC_0_9_SECOND = 9,
|
||||
ODID_TIME_ACC_1_0_SECOND = 10,
|
||||
ODID_TIME_ACC_1_1_SECOND = 11,
|
||||
ODID_TIME_ACC_1_2_SECOND = 12,
|
||||
ODID_TIME_ACC_1_3_SECOND = 13,
|
||||
ODID_TIME_ACC_1_4_SECOND = 14,
|
||||
ODID_TIME_ACC_1_5_SECOND = 15,
|
||||
} ODID_Timestamp_accuracy_t;
|
||||
|
||||
typedef enum ODID_authtype {
|
||||
ODID_AUTH_NONE = 0,
|
||||
ODID_AUTH_UAS_ID_SIGNATURE = 1, // Unmanned Aircraft System
|
||||
ODID_AUTH_OPERATOR_ID_SIGNATURE = 2,
|
||||
ODID_AUTH_MESSAGE_SET_SIGNATURE = 3,
|
||||
ODID_AUTH_NETWORK_REMOTE_ID = 4, // Authentication provided by Network Remote ID
|
||||
ODID_AUTH_SPECIFIC_AUTHENTICATION = 5, // Specific auth method. The exact authentication type is indicated by the
|
||||
// first byte of AuthData and these type values are managed by ICAO.
|
||||
// 0 is reserved. 1 - 224 are managed by ICAO. 225 - 255 are available for
|
||||
// private experimental usage only
|
||||
// 6 to 9 reserved for the specification. 0xA to 0xF reserved for private use
|
||||
} ODID_authtype_t;
|
||||
|
||||
typedef enum ODID_desctype {
|
||||
ODID_DESC_TYPE_TEXT = 0, // General free-form information text
|
||||
ODID_DESC_TYPE_EMERGENCY = 1, // Additional clarification when ODID_status == EMERGENCY
|
||||
ODID_DESC_TYPE_EXTENDED_STATUS = 2, // Additional clarification when ODID_status != EMERGENCY
|
||||
// 3 to 200 reserved
|
||||
// 201 to 255 available for private use
|
||||
} ODID_desctype_t;
|
||||
|
||||
typedef enum ODID_operatorIdType {
|
||||
ODID_OPERATOR_ID = 0,
|
||||
// 1 to 200 reserved
|
||||
// 201 to 255 available for private use
|
||||
} ODID_operatorIdType_t;
|
||||
|
||||
typedef enum ODID_operator_location_type {
|
||||
ODID_OPERATOR_LOCATION_TYPE_TAKEOFF = 0, // Takeoff location and altitude
|
||||
ODID_OPERATOR_LOCATION_TYPE_LIVE_GNSS = 1, // Dynamic/Live location and altitude
|
||||
ODID_OPERATOR_LOCATION_TYPE_FIXED = 2, // Fixed location and altitude
|
||||
// 3 to 255 reserved
|
||||
} ODID_operator_location_type_t;
|
||||
|
||||
typedef enum ODID_classification_type {
|
||||
ODID_CLASSIFICATION_TYPE_UNDECLARED = 0,
|
||||
ODID_CLASSIFICATION_TYPE_EU = 1, // European Union
|
||||
// 2 to 7 reserved
|
||||
} ODID_classification_type_t;
|
||||
|
||||
typedef enum ODID_category_EU {
|
||||
ODID_CATEGORY_EU_UNDECLARED = 0,
|
||||
ODID_CATEGORY_EU_OPEN = 1,
|
||||
ODID_CATEGORY_EU_SPECIFIC = 2,
|
||||
ODID_CATEGORY_EU_CERTIFIED = 3,
|
||||
// 4 to 15 reserved
|
||||
} ODID_category_EU_t;
|
||||
|
||||
typedef enum ODID_class_EU {
|
||||
ODID_CLASS_EU_UNDECLARED = 0,
|
||||
ODID_CLASS_EU_CLASS_0 = 1,
|
||||
ODID_CLASS_EU_CLASS_1 = 2,
|
||||
ODID_CLASS_EU_CLASS_2 = 3,
|
||||
ODID_CLASS_EU_CLASS_3 = 4,
|
||||
ODID_CLASS_EU_CLASS_4 = 5,
|
||||
ODID_CLASS_EU_CLASS_5 = 6,
|
||||
ODID_CLASS_EU_CLASS_6 = 7,
|
||||
// 8 to 15 reserved
|
||||
} ODID_class_EU_t;
|
||||
|
||||
/*
|
||||
* @name ODID_DataStructs
|
||||
* ODID Data Structures in their normative (non-packed) form.
|
||||
* This is the structure that any input adapters should form to
|
||||
* let the encoders put the data into encoded form.
|
||||
*/
|
||||
typedef struct ODID_BasicID_data {
|
||||
ODID_uatype_t UAType;
|
||||
ODID_idtype_t IDType;
|
||||
char UASID[ODID_ID_SIZE+1]; // Additional byte to allow for null term in normative form. Fill unused space with NULL
|
||||
} ODID_BasicID_data;
|
||||
|
||||
typedef struct ODID_Location_data {
|
||||
ODID_status_t Status;
|
||||
float Direction; // Degrees. 0 <= x < 360. Route course based on true North. Invalid, No Value, or Unknown: 361deg
|
||||
float SpeedHorizontal; // m/s. Positive only. Invalid, No Value, or Unknown: 255m/s. If speed is >= 254.25 m/s: 254.25m/s
|
||||
float SpeedVertical; // m/s. Invalid, No Value, or Unknown: 63m/s. If speed is >= 62m/s: 62m/s
|
||||
double Latitude; // Invalid, No Value, or Unknown: 0 deg (both Lat/Lon)
|
||||
double Longitude; // Invalid, No Value, or Unknown: 0 deg (both Lat/Lon)
|
||||
float AltitudeBaro; // meter (Ref 29.92 inHg, 1013.24 mb). Invalid, No Value, or Unknown: -1000m
|
||||
float AltitudeGeo; // meter (WGS84-HAE). Invalid, No Value, or Unknown: -1000m
|
||||
ODID_Height_reference_t HeightType;
|
||||
float Height; // meter. Invalid, No Value, or Unknown: -1000m
|
||||
ODID_Horizontal_accuracy_t HorizAccuracy;
|
||||
ODID_Vertical_accuracy_t VertAccuracy;
|
||||
ODID_Vertical_accuracy_t BaroAccuracy;
|
||||
ODID_Speed_accuracy_t SpeedAccuracy;
|
||||
ODID_Timestamp_accuracy_t TSAccuracy;
|
||||
float TimeStamp; // seconds after the full hour relative to UTC. Invalid, No Value, or Unknown: 0xFFFF
|
||||
} ODID_Location_data;
|
||||
|
||||
/*
|
||||
* The Authentication message can have two different formats:
|
||||
* - For data page 0, the fields LastPageIndex, Length and TimeStamp are present.
|
||||
* The size of AuthData is maximum 17 bytes (ODID_AUTH_PAGE_ZERO_DATA_SIZE).
|
||||
* - For data page 1 through (ODID_AUTH_MAX_PAGES - 1), LastPageIndex, Length and
|
||||
* TimeStamp are not present.
|
||||
* For pages 1 to LastPageIndex, the size of AuthData is maximum
|
||||
* 23 bytes (ODID_AUTH_PAGE_NONZERO_DATA_SIZE).
|
||||
*
|
||||
* Unused bytes in the AuthData field must be filled with NULLs (i.e. 0x00).
|
||||
*
|
||||
* Since the Length field is uint8_t, the precise amount of data bytes
|
||||
* transmitted over multiple pages of AuthData can only be specified up to 255.
|
||||
* I.e. the maximum is one page 0, then 10 full pages and finally a page with
|
||||
* 255 - (10*23 + 17) = 8 bytes of data.
|
||||
*
|
||||
* The payload data consisting of actual authentication data can never be
|
||||
* larger than 255 bytes. However, it is possible to transmit additional
|
||||
* support data, such as Forward Error Correction (FEC) data beyond that.
|
||||
* This is e.g. useful when transmitting on Bluetooth 4, which does not have
|
||||
* built-in FEC in the transmission protocol. The presence of this additional
|
||||
* data is indicated by a combination of LastPageIndex and the value of the
|
||||
* AuthData byte right after the last data byte indicated by Length. If this
|
||||
* additional byte is non-zero/non-NULL, the value of the byte indicates the
|
||||
* amount of additional (e.g. FEC) data bytes. The value of LastPageIndex must
|
||||
* always be large enough to include all pages containing normal authentication
|
||||
* and additional data. Some examples are given below. The value in the
|
||||
* "FEC data" column must be stored in the "(Length - 1) + 1" position in the
|
||||
* transmitted AuthData[] array. The total size of valid data in the AuthData
|
||||
* array will be = Length + 1 + "FEC data".
|
||||
* Unused bytes
|
||||
* Authentication data FEC data LastPageIndex Length on last page
|
||||
* 17 + 1*23 + 23 = 63 bytes 0 bytes 2 63 0
|
||||
* 17 + 1*23 + 23 = 63 bytes 22 bytes 3 63 0
|
||||
* 17 + 2*23 + 1 = 64 bytes 0 bytes 3 64 22
|
||||
* 17 + 2*23 + 1 = 64 bytes 21 bytes 3 64 0
|
||||
* 17 + 2*23 + 1 = 64 bytes 22 bytes 4 64 22
|
||||
* ...
|
||||
* 17 + 4*23 + 19 = 128 bytes 0 bytes 5 128 4
|
||||
* 17 + 4*23 + 19 = 128 bytes 3 bytes 5 128 0
|
||||
* 17 + 4*23 + 20 = 128 bytes 16 bytes 6 128 10
|
||||
* 17 + 4*23 + 20 = 128 bytes 26 bytes 6 128 0
|
||||
* ...
|
||||
* 17 + 9*23 + 23 = 247 bytes 0 bytes 10 247 0
|
||||
* 17 + 9*23 + 23 = 247 bytes 22 bytes 11 247 0
|
||||
* 17 + 10*23 + 1 = 248 bytes 0 bytes 11 248 22
|
||||
* 17 + 10*23 + 1 = 248 bytes 44 bytes 12 248 0
|
||||
* ...
|
||||
* 17 + 10*23 + 8 = 255 bytes 0 bytes 11 255 15
|
||||
* 17 + 10*23 + 8 = 255 bytes 14 bytes 11 255 0
|
||||
* 17 + 10*23 + 8 = 255 bytes 37 bytes 12 255 0
|
||||
* 17 + 10*23 + 8 = 255 bytes 60 bytes 13 255 0
|
||||
*/
|
||||
typedef struct ODID_Auth_data {
|
||||
uint8_t DataPage; // 0 - (ODID_AUTH_MAX_PAGES - 1)
|
||||
ODID_authtype_t AuthType;
|
||||
uint8_t LastPageIndex; // Page 0 only. Maximum (ODID_AUTH_MAX_PAGES - 1)
|
||||
uint8_t Length; // Page 0 only. Bytes. See description above.
|
||||
uint32_t Timestamp; // Page 0 only. Relative to 00:00:00 01/01/2019 UTC/Unix Time
|
||||
uint8_t AuthData[ODID_AUTH_PAGE_NONZERO_DATA_SIZE+1]; // Additional byte to allow for null term in normative form
|
||||
} ODID_Auth_data;
|
||||
|
||||
typedef struct ODID_SelfID_data {
|
||||
ODID_desctype_t DescType;
|
||||
char Desc[ODID_STR_SIZE+1]; // Additional byte to allow for null term in normative form. Fill unused space with NULL
|
||||
} ODID_SelfID_data;
|
||||
|
||||
typedef struct ODID_System_data {
|
||||
ODID_operator_location_type_t OperatorLocationType;
|
||||
ODID_classification_type_t ClassificationType;
|
||||
double OperatorLatitude; // Invalid, No Value, or Unknown: 0 deg (both Lat/Lon)
|
||||
double OperatorLongitude; // Invalid, No Value, or Unknown: 0 deg (both Lat/Lon)
|
||||
uint16_t AreaCount; // Default 1
|
||||
uint16_t AreaRadius; // meter. Default 0
|
||||
float AreaCeiling; // meter. Invalid, No Value, or Unknown: -1000m
|
||||
float AreaFloor; // meter. Invalid, No Value, or Unknown: -1000m
|
||||
ODID_category_EU_t CategoryEU; // Only filled if ClassificationType = ODID_CLASSIFICATION_TYPE_EU
|
||||
ODID_class_EU_t ClassEU; // Only filled if ClassificationType = ODID_CLASSIFICATION_TYPE_EU
|
||||
float OperatorAltitudeGeo;// meter (WGS84-HAE). Invalid, No Value, or Unknown: -1000m
|
||||
uint32_t Timestamp; // Relative to 00:00:00 01/01/2019 UTC/Unix Time
|
||||
} ODID_System_data;
|
||||
|
||||
typedef struct ODID_OperatorID_data {
|
||||
ODID_operatorIdType_t OperatorIdType;
|
||||
char OperatorId[ODID_ID_SIZE+1]; // Additional byte to allow for null term in normative form. Fill unused space with NULL
|
||||
} ODID_OperatorID_data;
|
||||
|
||||
typedef struct ODID_UAS_Data {
|
||||
ODID_BasicID_data BasicID[ODID_BASIC_ID_MAX_MESSAGES];
|
||||
ODID_Location_data Location;
|
||||
ODID_Auth_data Auth[ODID_AUTH_MAX_PAGES];
|
||||
ODID_SelfID_data SelfID;
|
||||
ODID_System_data System;
|
||||
ODID_OperatorID_data OperatorID;
|
||||
|
||||
uint8_t BasicIDValid[ODID_BASIC_ID_MAX_MESSAGES];
|
||||
uint8_t LocationValid;
|
||||
uint8_t AuthValid[ODID_AUTH_MAX_PAGES];
|
||||
uint8_t SelfIDValid;
|
||||
uint8_t SystemValid;
|
||||
uint8_t OperatorIDValid;
|
||||
} ODID_UAS_Data;
|
||||
|
||||
/**
|
||||
* @Name ODID_PackedStructs
|
||||
* Packed Data Structures prepared for broadcast
|
||||
* It's best not directly access these. Use the encoders/decoders.
|
||||
*/
|
||||
|
||||
typedef struct __attribute__((__packed__)) ODID_BasicID_encoded {
|
||||
// Byte 0 [MessageType][ProtoVersion] -- must define LSb first
|
||||
uint8_t ProtoVersion: 4;
|
||||
uint8_t MessageType : 4;
|
||||
|
||||
// Byte 1 [IDType][UAType] -- must define LSb first
|
||||
uint8_t UAType: 4;
|
||||
uint8_t IDType: 4;
|
||||
|
||||
// Bytes 2-21
|
||||
char UASID[ODID_ID_SIZE];
|
||||
|
||||
// 22-24
|
||||
char Reserved[3];
|
||||
} ODID_BasicID_encoded;
|
||||
|
||||
typedef struct __attribute__((__packed__)) ODID_Location_encoded {
|
||||
// Byte 0 [MessageType][ProtoVersion] -- must define LSb first
|
||||
uint8_t ProtoVersion: 4;
|
||||
uint8_t MessageType : 4;
|
||||
|
||||
// Byte 1 [Status][Reserved][NSMult][EWMult] -- must define LSb first
|
||||
uint8_t SpeedMult: 1;
|
||||
uint8_t EWDirection: 1;
|
||||
uint8_t HeightType: 1;
|
||||
uint8_t Reserved: 1;
|
||||
uint8_t Status: 4;
|
||||
|
||||
// Bytes 2-18
|
||||
uint8_t Direction;
|
||||
uint8_t SpeedHorizontal;
|
||||
int8_t SpeedVertical;
|
||||
int32_t Latitude;
|
||||
int32_t Longitude;
|
||||
uint16_t AltitudeBaro;
|
||||
uint16_t AltitudeGeo;
|
||||
uint16_t Height;
|
||||
|
||||
// Byte 19 [VertAccuracy][HorizAccuracy] -- must define LSb first
|
||||
uint8_t HorizAccuracy:4;
|
||||
uint8_t VertAccuracy:4;
|
||||
|
||||
// Byte 20 [BaroAccuracy][SpeedAccuracy] -- must define LSb first
|
||||
uint8_t SpeedAccuracy:4;
|
||||
uint8_t BaroAccuracy:4;
|
||||
|
||||
// Byte 21-22
|
||||
uint16_t TimeStamp;
|
||||
|
||||
// Byte 23 [Reserved2][TSAccuracy] -- must define LSb first
|
||||
uint8_t TSAccuracy:4;
|
||||
uint8_t Reserved2:4;
|
||||
|
||||
// Byte 24
|
||||
char Reserved3;
|
||||
} ODID_Location_encoded;
|
||||
|
||||
typedef struct __attribute__((__packed__)) ODID_Auth_encoded_page_zero {
|
||||
// Byte 0 [MessageType][ProtoVersion] -- must define LSb first
|
||||
uint8_t ProtoVersion: 4;
|
||||
uint8_t MessageType : 4;
|
||||
|
||||
// Byte 1 [AuthType][DataPage]
|
||||
uint8_t DataPage: 4;
|
||||
uint8_t AuthType: 4;
|
||||
|
||||
// Bytes 2-7
|
||||
uint8_t LastPageIndex;
|
||||
uint8_t Length;
|
||||
uint32_t Timestamp;
|
||||
|
||||
// Byte 8-24
|
||||
uint8_t AuthData[ODID_AUTH_PAGE_ZERO_DATA_SIZE];
|
||||
} ODID_Auth_encoded_page_zero;
|
||||
|
||||
typedef struct __attribute__((__packed__)) ODID_Auth_encoded_page_non_zero {
|
||||
// Byte 0 [MessageType][ProtoVersion] -- must define LSb first
|
||||
uint8_t ProtoVersion: 4;
|
||||
uint8_t MessageType : 4;
|
||||
|
||||
// Byte 1 [AuthType][DataPage]
|
||||
uint8_t DataPage: 4;
|
||||
uint8_t AuthType: 4;
|
||||
|
||||
// Byte 2-24
|
||||
uint8_t AuthData[ODID_AUTH_PAGE_NONZERO_DATA_SIZE];
|
||||
} ODID_Auth_encoded_page_non_zero;
|
||||
|
||||
/*
|
||||
* It is safe to access the first four fields (i.e. ProtoVersion, MessageType,
|
||||
* DataPage and AuthType) from either of the union members, since the declarations
|
||||
* for these fields are identical and the first parts of the structures.
|
||||
* The ISO/IEC 9899:1999 Chapter 6.5.2.3 part 5 and related Example 3 documents this.
|
||||
* https://www.iso.org/standard/29237.html
|
||||
*/
|
||||
typedef union ODID_Auth_encoded{
|
||||
ODID_Auth_encoded_page_zero page_zero;
|
||||
ODID_Auth_encoded_page_non_zero page_non_zero;
|
||||
} ODID_Auth_encoded;
|
||||
|
||||
typedef struct __attribute__((__packed__)) ODID_SelfID_encoded {
|
||||
// Byte 0 [MessageType][ProtoVersion] -- must define LSb first
|
||||
uint8_t ProtoVersion: 4;
|
||||
uint8_t MessageType : 4;
|
||||
|
||||
// Byte 1
|
||||
uint8_t DescType;
|
||||
|
||||
// Byte 2-24
|
||||
char Desc[ODID_STR_SIZE];
|
||||
} ODID_SelfID_encoded;
|
||||
|
||||
typedef struct __attribute__((__packed__)) ODID_System_encoded {
|
||||
// Byte 0 [MessageType][ProtoVersion] -- must define LSb first
|
||||
uint8_t ProtoVersion: 4;
|
||||
uint8_t MessageType : 4;
|
||||
|
||||
// Byte 1 [Reserved][ClassificationType][OperatorLocationType] -- must define LSb first
|
||||
uint8_t OperatorLocationType: 2;
|
||||
uint8_t ClassificationType: 3;
|
||||
uint8_t Reserved: 3;
|
||||
|
||||
// Byte 2-9
|
||||
int32_t OperatorLatitude;
|
||||
int32_t OperatorLongitude;
|
||||
|
||||
// Byte 10-16
|
||||
uint16_t AreaCount;
|
||||
uint8_t AreaRadius;
|
||||
uint16_t AreaCeiling;
|
||||
uint16_t AreaFloor;
|
||||
|
||||
// Byte 17 [CategoryEU][ClassEU] -- must define LSb first
|
||||
uint8_t ClassEU: 4;
|
||||
uint8_t CategoryEU: 4;
|
||||
|
||||
// Byte 18-23
|
||||
uint16_t OperatorAltitudeGeo;
|
||||
uint32_t Timestamp;
|
||||
|
||||
// Byte 24
|
||||
char Reserved2;
|
||||
} ODID_System_encoded;
|
||||
|
||||
typedef struct __attribute__((__packed__)) ODID_OperatorID_encoded {
|
||||
// Byte 0 [MessageType][ProtoVersion] -- must define LSb first
|
||||
uint8_t ProtoVersion: 4;
|
||||
uint8_t MessageType : 4;
|
||||
|
||||
// Byte 1
|
||||
uint8_t OperatorIdType;
|
||||
|
||||
// Bytes 2-21
|
||||
char OperatorId[ODID_ID_SIZE];
|
||||
|
||||
// 22-24
|
||||
char Reserved[3];
|
||||
} ODID_OperatorID_encoded;
|
||||
|
||||
/*
|
||||
* It is safe to access the first two fields (i.e. ProtoVersion and MessageType)
|
||||
* from any of the structure parts of the union members, since the declarations
|
||||
* for these fields are identical and the first parts of the structures.
|
||||
* The ISO/IEC 9899:1999 Chapter 6.5.2.3 part 5 and related Example 3 documents this.
|
||||
* https://www.iso.org/standard/29237.html
|
||||
*/
|
||||
typedef union ODID_Message_encoded {
|
||||
uint8_t rawData[ODID_MESSAGE_SIZE];
|
||||
ODID_BasicID_encoded basicId;
|
||||
ODID_Location_encoded location;
|
||||
ODID_Auth_encoded auth;
|
||||
ODID_SelfID_encoded selfId;
|
||||
ODID_System_encoded system;
|
||||
ODID_OperatorID_encoded operatorId;
|
||||
} ODID_Message_encoded;
|
||||
|
||||
typedef struct __attribute__((__packed__)) ODID_MessagePack_encoded {
|
||||
// Byte 0 [MessageType][ProtoVersion] -- must define LSb first
|
||||
uint8_t ProtoVersion: 4;
|
||||
uint8_t MessageType : 4;
|
||||
|
||||
// Byte 1 - 2
|
||||
uint8_t SingleMessageSize;
|
||||
uint8_t MsgPackSize;
|
||||
|
||||
// Byte 3 - 227
|
||||
ODID_Message_encoded Messages[ODID_PACK_MAX_MESSAGES];
|
||||
} ODID_MessagePack_encoded;
|
||||
|
||||
typedef struct ODID_MessagePack_data {
|
||||
uint8_t SingleMessageSize; // Must always be ODID_MESSAGE_SIZE
|
||||
uint8_t MsgPackSize; // Number of messages in pack (NOT number of bytes)
|
||||
|
||||
ODID_Message_encoded Messages[ODID_PACK_MAX_MESSAGES];
|
||||
} ODID_MessagePack_data;
|
||||
|
||||
// API Calls
|
||||
void odid_initBasicIDData(ODID_BasicID_data *data);
|
||||
void odid_initLocationData(ODID_Location_data *data);
|
||||
void odid_initAuthData(ODID_Auth_data *data);
|
||||
void odid_initSelfIDData(ODID_SelfID_data *data);
|
||||
void odid_initSystemData(ODID_System_data *data);
|
||||
void odid_initOperatorIDData(ODID_OperatorID_data *data);
|
||||
void odid_initMessagePackData(ODID_MessagePack_data *data);
|
||||
void odid_initUasData(ODID_UAS_Data *data);
|
||||
|
||||
int encodeBasicIDMessage(ODID_BasicID_encoded *outEncoded, ODID_BasicID_data *inData);
|
||||
int encodeLocationMessage(ODID_Location_encoded *outEncoded, ODID_Location_data *inData);
|
||||
int encodeAuthMessage(ODID_Auth_encoded *outEncoded, ODID_Auth_data *inData);
|
||||
int encodeSelfIDMessage(ODID_SelfID_encoded *outEncoded, ODID_SelfID_data *inData);
|
||||
int encodeSystemMessage(ODID_System_encoded *outEncoded, ODID_System_data *inData);
|
||||
int encodeOperatorIDMessage(ODID_OperatorID_encoded *outEncoded, ODID_OperatorID_data *inData);
|
||||
int encodeMessagePack(ODID_MessagePack_encoded *outEncoded, ODID_MessagePack_data *inData);
|
||||
|
||||
int decodeBasicIDMessage(ODID_BasicID_data *outData, ODID_BasicID_encoded *inEncoded);
|
||||
int decodeLocationMessage(ODID_Location_data *outData, ODID_Location_encoded *inEncoded);
|
||||
int decodeAuthMessage(ODID_Auth_data *outData, ODID_Auth_encoded *inEncoded);
|
||||
int decodeSelfIDMessage(ODID_SelfID_data *outData, ODID_SelfID_encoded *inEncoded);
|
||||
int decodeSystemMessage(ODID_System_data *outData, ODID_System_encoded *inEncoded);
|
||||
int decodeOperatorIDMessage(ODID_OperatorID_data *outData, ODID_OperatorID_encoded *inEncoded);
|
||||
int decodeMessagePack(ODID_UAS_Data *uasData, ODID_MessagePack_encoded *pack);
|
||||
|
||||
int getBasicIDType(ODID_BasicID_encoded *inEncoded, enum ODID_idtype *idType);
|
||||
int getAuthPageNum(ODID_Auth_encoded *inEncoded, int *pageNum);
|
||||
ODID_messagetype_t decodeMessageType(uint8_t byte);
|
||||
ODID_messagetype_t decodeOpenDroneID(ODID_UAS_Data *uas_data, uint8_t *msg_data);
|
||||
|
||||
// Helper Functions
|
||||
ODID_Horizontal_accuracy_t createEnumHorizontalAccuracy(float Accuracy);
|
||||
ODID_Vertical_accuracy_t createEnumVerticalAccuracy(float Accuracy);
|
||||
ODID_Speed_accuracy_t createEnumSpeedAccuracy(float Accuracy);
|
||||
ODID_Timestamp_accuracy_t createEnumTimestampAccuracy(float Accuracy);
|
||||
|
||||
float decodeHorizontalAccuracy(ODID_Horizontal_accuracy_t Accuracy);
|
||||
float decodeVerticalAccuracy(ODID_Vertical_accuracy_t Accuracy);
|
||||
float decodeSpeedAccuracy(ODID_Speed_accuracy_t Accuracy);
|
||||
float decodeTimestampAccuracy(ODID_Timestamp_accuracy_t Accuracy);
|
||||
|
||||
// OpenDroneID WiFi functions
|
||||
|
||||
/**
|
||||
* drone_export_gps_data - prints drone information to json style string,
|
||||
* according to odid message specification
|
||||
* @UAS_Data: general drone status information
|
||||
* @buf: buffer for the json style string
|
||||
* @buf_size: size of the string buffer
|
||||
*
|
||||
* Returns pointer to gps_data string on success, otherwise returns NULL
|
||||
*/
|
||||
void drone_export_gps_data(ODID_UAS_Data *UAS_Data, char *buf, size_t buf_size);
|
||||
|
||||
/**
|
||||
* odid_message_build_pack - combines the messages and encodes the odid pack
|
||||
* @UAS_Data: general drone status information
|
||||
* @pack: buffer space to write to
|
||||
* @buflen: maximum length of buffer space
|
||||
*
|
||||
* Returns length on success, < 0 on failure. @buf only contains a valid message
|
||||
* if the return code is >0
|
||||
*/
|
||||
int odid_message_build_pack(ODID_UAS_Data *UAS_Data, void *pack, size_t buflen);
|
||||
|
||||
/* odid_wifi_build_nan_sync_beacon_frame - creates a NAN sync beacon frame
|
||||
* that shall be send just before the NAN action frame.
|
||||
* @mac: mac address of the wifi adapter where the NAN frame will be sent
|
||||
* @buf: pointer to buffer space where the NAN will be written to
|
||||
* @buf_size: maximum size of the buffer
|
||||
*
|
||||
* Returns the packet length on success, or < 0 on error.
|
||||
*/
|
||||
int odid_wifi_build_nan_sync_beacon_frame(char *mac, uint8_t *buf, size_t buf_size);
|
||||
|
||||
/* odid_wifi_build_message_pack_nan_action_frame - creates a message pack
|
||||
* with each type of message from the drone information into an NAN action frame.
|
||||
* @UAS_Data: general drone status information
|
||||
* @mac: mac address of the wifi adapter where the NAN frame will be sent
|
||||
* @send_counter: sequence number, to be increased for each call of this function
|
||||
* @buf: pointer to buffer space where the NAN will be written to
|
||||
* @buf_size: maximum size of the buffer
|
||||
*
|
||||
* Returns the packet length on success, or < 0 on error.
|
||||
*/
|
||||
int odid_wifi_build_message_pack_nan_action_frame(ODID_UAS_Data *UAS_Data, char *mac,
|
||||
uint8_t send_counter,
|
||||
uint8_t *buf, size_t buf_size);
|
||||
|
||||
/* odid_wifi_build_message_pack_beacon_frame - creates a message pack
|
||||
* with each type of message from the drone information into an Beacon frame.
|
||||
* @UAS_Data: general drone status information
|
||||
* @mac: mac address of the wifi adapter where the Beacon frame will be sent
|
||||
* @SSID: SSID of the wifi network to be sent
|
||||
* @SSID_len: length in bytes of the SSID string
|
||||
* @interval_tu: beacon interval in wifi Time Units
|
||||
* @send_counter: sequence number, to be increased for each call of this function
|
||||
* @buf: pointer to buffer space where the Beacon will be written to
|
||||
* @buf_size: maximum size of the buffer
|
||||
*
|
||||
* Returns the packet length on success, or < 0 on error.
|
||||
*/
|
||||
int odid_wifi_build_message_pack_beacon_frame(ODID_UAS_Data *UAS_Data, char *mac,
|
||||
const char *SSID, size_t SSID_len,
|
||||
uint16_t interval_tu, uint8_t send_counter,
|
||||
uint8_t *buf, size_t buf_size);
|
||||
|
||||
/* odid_message_process_pack - decodes the messages from the odid message pack
|
||||
* @UAS_Data: general drone status information
|
||||
* @pack: buffer space to read from
|
||||
* @buflen: length of buffer space
|
||||
*
|
||||
* Returns 0 on success
|
||||
*/
|
||||
int odid_message_process_pack(ODID_UAS_Data *UAS_Data, uint8_t *pack, size_t buflen);
|
||||
|
||||
/* odid_wifi_receive_message_pack_nan_action_frame - processes a received message pack
|
||||
* with each type of message from the drone information into an NAN action frame
|
||||
* @UAS_Data: general drone status information
|
||||
* @mac: mac address of the wifi adapter where the NAN frame will be sent
|
||||
* @buf: pointer to buffer space where the NAN is stored
|
||||
* @buf_size: maximum size of the buffer
|
||||
*
|
||||
* Returns 0 on success, or < 0 on error. Will fill 6 bytes into @mac.
|
||||
*/
|
||||
int odid_wifi_receive_message_pack_nan_action_frame(ODID_UAS_Data *UAS_Data,
|
||||
char *mac, uint8_t *buf, size_t buf_size);
|
||||
|
||||
#ifndef ODID_DISABLE_PRINTF
|
||||
void printByteArray(uint8_t *byteArray, uint16_t asize, int spaced);
|
||||
void printBasicID_data(ODID_BasicID_data *BasicID);
|
||||
void printLocation_data(ODID_Location_data *Location);
|
||||
void printAuth_data(ODID_Auth_data *Auth);
|
||||
void printSelfID_data(ODID_SelfID_data *SelfID);
|
||||
void printOperatorID_data(ODID_OperatorID_data *OperatorID);
|
||||
void printSystem_data(ODID_System_data *System_data);
|
||||
#endif // ODID_DISABLE_PRINTF
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // _OPENDRONEID_H_
|
||||
182
utm.cpp
Normal file
182
utm.cpp
Normal file
@ -0,0 +1,182 @@
|
||||
/* -*- tab-width: 2; mode: c; -*-
|
||||
*
|
||||
* UTM/eID utility functions.
|
||||
*
|
||||
* Copyright (c) 2020, Steve Jack.
|
||||
*
|
||||
* Notes
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma GCC diagnostic warning "-Wunused-variable"
|
||||
|
||||
#define DIAGNOSTICS 1
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "utm.h"
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
UTM_Utilities::UTM_Utilities() {
|
||||
|
||||
memset(s,0,sizeof(s));
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
void UTM_Utilities::calc_m_per_deg(double lat_d,double long_d,double *m_deg_lat,double *m_deg_long) {
|
||||
|
||||
calc_m_per_deg(lat_d,m_deg_lat,m_deg_long);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
void UTM_Utilities::calc_m_per_deg(double lat_d,double *m_deg_lat,double *m_deg_long) {
|
||||
|
||||
double pi, deg2rad, sin_lat, cos_lat;
|
||||
|
||||
pi = 4.0 * atan(1.0);
|
||||
deg2rad = pi / 180.0;
|
||||
|
||||
lat_d *= deg2rad;
|
||||
|
||||
sin_lat = sin(lat_d);
|
||||
cos_lat = cos(lat_d);
|
||||
|
||||
#if 1 // Wikipedia
|
||||
|
||||
double a = 0.08181922, b, radius;
|
||||
|
||||
b = a * sin_lat;
|
||||
radius = 6378137.0 * cos_lat / sqrt(1.0 - (b * b));
|
||||
*m_deg_long = deg2rad * radius;
|
||||
*m_deg_lat = 111132.954 - (559.822 * cos(2.0 * lat_d)) -
|
||||
(1.175 * cos(4.0 * lat_d));
|
||||
|
||||
#else // Astronomical Algorithms
|
||||
|
||||
double a = 6378140.0, c, d, e = 0.08181922, rho, Rp = 0.0, Rm = 0.0;
|
||||
|
||||
rho = 0.9983271 + (0.0016764 * cos(2.0 * lat_d)) - (0.0000035 * cos(4.0 * lat_d));
|
||||
c = e * sin_lat;
|
||||
d = sqrt(1.0 - (c * c));
|
||||
Rp = a * cos_lat / d;
|
||||
*m_deg_long = deg2rad * Rp;
|
||||
Rm = a * (1.0 - (e * e)) / pow(d,3);
|
||||
*m_deg_lat = deg2rad * Rm;
|
||||
|
||||
#endif
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
int UTM_Utilities::check_EU_op_id(const char *id,const char *secret) {
|
||||
|
||||
int i, j;
|
||||
char check;
|
||||
|
||||
if ((strlen(id) != 16)&&(strlen(secret) != 3)) {
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
for (i = 0, j = 0; i < 12; ++i) {
|
||||
|
||||
s[j++] = id[i + 3];
|
||||
}
|
||||
|
||||
for (i = 0; i < 3; ++i) {
|
||||
|
||||
s[j++] = secret[i];
|
||||
}
|
||||
|
||||
s[j] = 0;
|
||||
|
||||
check = luhn36_check(s);
|
||||
|
||||
return ((id[15] == check) ? 1: 0);
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
char UTM_Utilities::luhn36_check(const char *s) {
|
||||
|
||||
int sum = 0, factor = 2, l, i, add, rem;
|
||||
const int base = 36;
|
||||
|
||||
l = strlen(s);
|
||||
|
||||
for (i = l - 1; i >= 0; --i) {
|
||||
|
||||
add = luhn36_c2i(s[i]) * factor;
|
||||
sum += (add / base) + (add % base);
|
||||
|
||||
factor = (factor == 2) ? 1: 2;
|
||||
}
|
||||
|
||||
rem = sum % base;
|
||||
|
||||
return luhn36_i2c(base - rem);
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
int UTM_Utilities::luhn36_c2i(char c) {
|
||||
|
||||
if ((c >= '0')&&(c <= '9')) {
|
||||
|
||||
return (c - '0');
|
||||
|
||||
} else if ((c >= 'a')&&(c <= 'z')) {
|
||||
|
||||
return (10 + (c - 'a'));
|
||||
|
||||
} else if ((c >= 'A')&&(c <= 'Z')) {
|
||||
|
||||
return (10 + (c - 'A'));
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
char UTM_Utilities::luhn36_i2c(int i) {
|
||||
|
||||
if ((i >= 0)&&(i <= 9)) {
|
||||
|
||||
return ('0' + i);
|
||||
|
||||
} else if ((i >= 10)&&(i < 36)) {
|
||||
|
||||
return ('a' + i - 10);
|
||||
}
|
||||
|
||||
return '0';
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
93
utm.h
Normal file
93
utm.h
Normal file
@ -0,0 +1,93 @@
|
||||
/* -*- tab-width: 2; mode: c; -*-
|
||||
*
|
||||
* UTM/eID interface structure definition, some defines and an object for
|
||||
* utility functions.
|
||||
*
|
||||
* Copyright (c) 2020, Steve Jack.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef UTM_H
|
||||
#define UTM_H
|
||||
|
||||
#if not defined(SATS_LEVEL_1)
|
||||
#define SATS_LEVEL_1 4
|
||||
#endif
|
||||
|
||||
#if not defined(SATS_LEVEL_2)
|
||||
#define SATS_LEVEL_2 7
|
||||
#endif
|
||||
|
||||
#if not defined(SATS_LEVEL_3)
|
||||
#define SATS_LEVEL_3 10
|
||||
#endif
|
||||
|
||||
#define ID_SIZE 24
|
||||
|
||||
//
|
||||
|
||||
struct UTM_parameters {
|
||||
|
||||
char UAS_operator[ID_SIZE];
|
||||
char UAV_id[ID_SIZE];
|
||||
char flight_desc[ID_SIZE];
|
||||
uint8_t UA_type, ID_type, region, spare1,
|
||||
EU_category, EU_class, ID_type2, spare3;
|
||||
char UTM_id[ID_SIZE * 2];
|
||||
char secret[4];
|
||||
uint8_t spare[28];
|
||||
};
|
||||
|
||||
//
|
||||
|
||||
struct UTM_data {
|
||||
|
||||
int years;
|
||||
int months;
|
||||
int days;
|
||||
int hours;
|
||||
int minutes;
|
||||
int seconds;
|
||||
int csecs;
|
||||
double latitude_d;
|
||||
double longitude_d;
|
||||
float alt_msl_m;
|
||||
float alt_agl_m;
|
||||
int speed_kn;
|
||||
int heading;
|
||||
char *hdop_s;
|
||||
char *vdop_s;
|
||||
int satellites;
|
||||
double base_latitude;
|
||||
double base_longitude;
|
||||
float base_alt_m;
|
||||
int base_valid;
|
||||
int vel_N_cm;
|
||||
int vel_E_cm;
|
||||
int vel_D_cm;
|
||||
};
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
|
||||
class UTM_Utilities {
|
||||
|
||||
public:
|
||||
|
||||
UTM_Utilities(void);
|
||||
|
||||
void calc_m_per_deg(double,double,double *,double *);
|
||||
void calc_m_per_deg(double,double *,double *);
|
||||
|
||||
int check_EU_op_id(const char *,const char *);
|
||||
char luhn36_check(const char *);
|
||||
int luhn36_c2i(char);
|
||||
char luhn36_i2c(int);
|
||||
|
||||
private:
|
||||
|
||||
char s[20];
|
||||
};
|
||||
|
||||
#endif
|
||||
618
wifi.c
Normal file
618
wifi.c
Normal file
@ -0,0 +1,618 @@
|
||||
/*
|
||||
Copyright (C) 2020 Simon Wunderlich, Marek Sobe
|
||||
Copyright (C) 2020 Doodle Labs
|
||||
|
||||
SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
Open Drone ID C Library
|
||||
|
||||
Maintainer:
|
||||
Simon Wunderlich
|
||||
sw@simonwunderlich.de
|
||||
*/
|
||||
|
||||
#if defined(ARDUINO_ARCH_ESP32)
|
||||
#include <Arduino.h>
|
||||
int clock_gettime(clockid_t, struct timespec *);
|
||||
#else
|
||||
#include <string.h>
|
||||
#include <stddef.h>
|
||||
#include <stdio.h>
|
||||
#endif
|
||||
|
||||
#include <errno.h>
|
||||
#include <time.h>
|
||||
|
||||
#include "opendroneid.h"
|
||||
#include "odid_wifi.h"
|
||||
|
||||
#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
|
||||
#define cpu_to_le16(x) (x)
|
||||
#define cpu_to_le64(x) (x)
|
||||
#else
|
||||
#define cpu_to_le16(x) (bswap_16(x))
|
||||
#define cpu_to_le64(x) (bswap_64(x))
|
||||
#endif
|
||||
|
||||
#define IEEE80211_FCTL_FTYPE 0x000c
|
||||
#define IEEE80211_FCTL_STYPE 0x00f0
|
||||
|
||||
#define IEEE80211_FTYPE_MGMT 0x0000
|
||||
#define IEEE80211_STYPE_ACTION 0x00D0
|
||||
#define IEEE80211_STYPE_BEACON 0x0080
|
||||
|
||||
/* IEEE 802.11-2016 capability info */
|
||||
#define IEEE80211_CAPINFO_ESS 0x0001
|
||||
#define IEEE80211_CAPINFO_IBSS 0x0002
|
||||
#define IEEE80211_CAPINFO_CF_POLLABLE 0x0004
|
||||
#define IEEE80211_CAPINFO_CF_POLLREQ 0x0008
|
||||
#define IEEE80211_CAPINFO_PRIVACY 0x0010
|
||||
#define IEEE80211_CAPINFO_SHORT_PREAMBLE 0x0020
|
||||
/* bits 6-7 reserved */
|
||||
#define IEEE80211_CAPINFO_SPECTRUM_MGMT 0x0100
|
||||
#define IEEE80211_CAPINFO_QOS 0x0200
|
||||
#define IEEE80211_CAPINFO_SHORT_SLOTTIME 0x0400
|
||||
#define IEEE80211_CAPINFO_APSD 0x0800
|
||||
#define IEEE80211_CAPINFO_RADIOMEAS 0x1000
|
||||
/* bit 13 reserved */
|
||||
#define IEEE80211_CAPINFO_DEL_BLOCK_ACK 0x4000
|
||||
#define IEEE80211_CAPINFO_IMM_BLOCK_ACK 0x8000
|
||||
|
||||
/* IEEE 802.11 Element IDs */
|
||||
#define IEEE80211_ELEMID_SSID 0x00
|
||||
#define IEEE80211_ELEMID_RATES 0x01
|
||||
#define IEEE80211_ELEMID_VENDOR 0xDD
|
||||
|
||||
/* Neighbor Awareness Networking Specification v3.1 in section 2.8.2
|
||||
* The NAN Cluster ID is a MAC address that takes a value from
|
||||
* 50-6F-9A-01-00-00 to 50-6F-9A-01-FF-FF and is carried in the A3 field of
|
||||
* some of the NAN frames. The NAN Cluster ID is randomly chosen by the device
|
||||
* that initiates the NAN Cluster.
|
||||
* However, the ASTM Remote ID specification v1.1 specifies that the NAN
|
||||
* cluster ID must be fixed to the value 50-6F-9A-01-00-FF.
|
||||
*/
|
||||
static const uint8_t *get_nan_cluster_id(void)
|
||||
{
|
||||
static const uint8_t cluster_id[6] = { 0x50, 0x6F, 0x9A, 0x01, 0x00, 0xFF };
|
||||
return cluster_id;
|
||||
}
|
||||
|
||||
static int buf_fill_ieee80211_mgmt(uint8_t *buf, size_t *len, size_t buf_size,
|
||||
const uint16_t subtype,
|
||||
const uint8_t *dst_addr,
|
||||
const uint8_t *src_addr,
|
||||
const uint8_t *bssid)
|
||||
{
|
||||
if (*len + sizeof(struct ieee80211_mgmt) > buf_size)
|
||||
return -ENOMEM;
|
||||
|
||||
struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)(buf + *len);
|
||||
mgmt->frame_control = (uint16_t) cpu_to_le16(IEEE80211_FTYPE_MGMT | subtype);
|
||||
mgmt->duration = cpu_to_le16(0x0000);
|
||||
memcpy(mgmt->da, dst_addr, sizeof(mgmt->da));
|
||||
memcpy(mgmt->sa, src_addr, sizeof(mgmt->sa));
|
||||
memcpy(mgmt->bssid, bssid, sizeof(mgmt->bssid));
|
||||
mgmt->seq_ctrl = cpu_to_le16(0x0000);
|
||||
*len += sizeof(*mgmt);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int buf_fill_ieee80211_beacon(uint8_t *buf, size_t *len, size_t buf_size, uint16_t interval_tu)
|
||||
{
|
||||
if (*len + sizeof(struct ieee80211_beacon) > buf_size)
|
||||
return -ENOMEM;
|
||||
|
||||
struct ieee80211_beacon *beacon = (struct ieee80211_beacon *)(buf + *len);
|
||||
struct timespec ts;
|
||||
uint64_t mono_us = 0;
|
||||
|
||||
#if defined(CLOCK_MONOTONIC)
|
||||
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||
mono_us = (uint64_t)((double) ts.tv_sec * 1e6 + (double) ts.tv_nsec * 1e-3);
|
||||
#elif defined(CLOCK_REALTIME)
|
||||
clock_gettime(CLOCK_REALTIME, &ts);
|
||||
mono_us = (uint64_t)((double) ts.tv_sec * 1e6 + (double) ts.tv_nsec * 1e-3);
|
||||
#elif defined(ARDUINO)
|
||||
#warning "No REALTIME or MONOTONIC clock, using micros()."
|
||||
mono_us = micros();
|
||||
#else
|
||||
#warning "Unable to set wifi timestamp."
|
||||
#endif
|
||||
beacon->timestamp = cpu_to_le64(mono_us);
|
||||
beacon->beacon_interval = cpu_to_le16(interval_tu);
|
||||
beacon->capability = cpu_to_le16(IEEE80211_CAPINFO_SHORT_SLOTTIME | IEEE80211_CAPINFO_SHORT_PREAMBLE);
|
||||
*len += sizeof(*beacon);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void drone_export_gps_data(ODID_UAS_Data *UAS_Data, char *buf, size_t buf_size)
|
||||
{
|
||||
ptrdiff_t len = 0;
|
||||
|
||||
#define mprintf(...) {\
|
||||
len += snprintf(buf + len, buf_size - (size_t)len, __VA_ARGS__); \
|
||||
if ((len < 0) || ((size_t)len >= buf_size)) \
|
||||
return; \
|
||||
}
|
||||
|
||||
mprintf("{\n\t\"Version\": \"1.1\",\n\t\"Response\": {\n");
|
||||
|
||||
mprintf("\t\t\"BasicID\": {\n");
|
||||
for (int i = 0; i < ODID_BASIC_ID_MAX_MESSAGES; i++) {
|
||||
if (!UAS_Data->BasicIDValid[i])
|
||||
continue;
|
||||
mprintf("\t\t\t\"UAType%d\": %d,\n", i, UAS_Data->BasicID[i].UAType);
|
||||
mprintf("\t\t\t\"IDType%d\": %d,\n", i, UAS_Data->BasicID[i].IDType);
|
||||
mprintf("\t\t\t\"UASID%d\": %s,\n", i, UAS_Data->BasicID[i].UASID);
|
||||
}
|
||||
mprintf("\t\t},\n");
|
||||
|
||||
mprintf("\t\t\"Location\": {\n");
|
||||
mprintf("\t\t\t\"Status\": %d,\n", (int)UAS_Data->Location.Status);
|
||||
mprintf("\t\t\t\"Direction\": %f,\n", (double) UAS_Data->Location.Direction);
|
||||
mprintf("\t\t\t\"SpeedHorizontal\": %f,\n", (double) UAS_Data->Location.SpeedHorizontal);
|
||||
mprintf("\t\t\t\"SpeedVertical\": %f,\n", (double) UAS_Data->Location.SpeedVertical);
|
||||
mprintf("\t\t\t\"Latitude\": %f,\n", UAS_Data->Location.Latitude);
|
||||
mprintf("\t\t\t\"Longitude\": %f,\n", UAS_Data->Location.Longitude);
|
||||
mprintf("\t\t\t\"AltitudeBaro\": %f,\n", (double) UAS_Data->Location.AltitudeBaro);
|
||||
mprintf("\t\t\t\"AltitudeGeo\": %f,\n", (double) UAS_Data->Location.AltitudeGeo);
|
||||
mprintf("\t\t\t\"HeightType\": %d,\n", UAS_Data->Location.HeightType);
|
||||
mprintf("\t\t\t\"Height\": %f,\n", (double) UAS_Data->Location.Height);
|
||||
mprintf("\t\t\t\"HorizAccuracy\": %d,\n", UAS_Data->Location.HorizAccuracy);
|
||||
mprintf("\t\t\t\"VertAccuracy\": %d,\n", UAS_Data->Location.VertAccuracy);
|
||||
mprintf("\t\t\t\"BaroAccuracy\": %d,\n", UAS_Data->Location.BaroAccuracy);
|
||||
mprintf("\t\t\t\"SpeedAccuracy\": %d,\n", UAS_Data->Location.SpeedAccuracy);
|
||||
mprintf("\t\t\t\"TSAccuracy\": %d,\n", UAS_Data->Location.TSAccuracy);
|
||||
mprintf("\t\t\t\"TimeStamp\": %f,\n", (double) UAS_Data->Location.TimeStamp);
|
||||
mprintf("\t\t},\n");
|
||||
|
||||
mprintf("\t\t\"Authentication\": {\n");
|
||||
mprintf("\t\t\t\"AuthType\": %d,\n", UAS_Data->Auth[0].AuthType);
|
||||
mprintf("\t\t\t\"LastPageIndex\": %d,\n", UAS_Data->Auth[0].LastPageIndex);
|
||||
mprintf("\t\t\t\"Length\": %d,\n", UAS_Data->Auth[0].Length);
|
||||
mprintf("\t\t\t\"Timestamp\": %u,\n", UAS_Data->Auth[0].Timestamp);
|
||||
for (int i = 0; i <= UAS_Data->Auth[0].LastPageIndex; i++) {
|
||||
mprintf("\t\t\t\"AuthData Page %d,\": %s\n", i, UAS_Data->Auth[i].AuthData);
|
||||
}
|
||||
mprintf("\t\t},\n");
|
||||
|
||||
mprintf("\t\t\"SelfID\": {\n");
|
||||
mprintf("\t\t\t\"Description Type\": %d,\n", UAS_Data->SelfID.DescType);
|
||||
mprintf("\t\t\t\"Description\": %s,\n", UAS_Data->SelfID.Desc);
|
||||
mprintf("\t\t},\n");
|
||||
|
||||
mprintf("\t\t\"Operator\": {\n");
|
||||
mprintf("\t\t\t\"OperatorLocationType\": %d,\n", UAS_Data->System.OperatorLocationType);
|
||||
mprintf("\t\t\t\"ClassificationType\": %d,\n", UAS_Data->System.ClassificationType);
|
||||
mprintf("\t\t\t\"OperatorLatitude\": %f,\n", UAS_Data->System.OperatorLatitude);
|
||||
mprintf("\t\t\t\"OperatorLongitude\": %f,\n", UAS_Data->System.OperatorLongitude);
|
||||
mprintf("\t\t\t\"AreaCount\": %d,\n", UAS_Data->System.AreaCount);
|
||||
mprintf("\t\t\t\"AreaRadius\": %d,\n", UAS_Data->System.AreaRadius);
|
||||
mprintf("\t\t\t\"AreaCeiling\": %f,\n", (double) UAS_Data->System.AreaCeiling);
|
||||
mprintf("\t\t\t\"AreaFloor\": %f,\n", (double) UAS_Data->System.AreaFloor);
|
||||
mprintf("\t\t\t\"CategoryEU\": %d,\n", UAS_Data->System.CategoryEU);
|
||||
mprintf("\t\t\t\"ClassEU\": %d,\n", UAS_Data->System.ClassEU);
|
||||
mprintf("\t\t\t\"OperatorAltitudeGeo\": %f,\n", (double) UAS_Data->System.OperatorAltitudeGeo);
|
||||
mprintf("\t\t\t\"Timestamp\": %u,\n", UAS_Data->System.Timestamp);
|
||||
mprintf("\t\t}\n");
|
||||
|
||||
mprintf("\t\t\"OperatorID\": {\n");
|
||||
mprintf("\t\t\t\"OperatorIdType\": %d,\n", UAS_Data->OperatorID.OperatorIdType);
|
||||
mprintf("\t\t\t\"OperatorId\": \"%s\",\n", UAS_Data->OperatorID.OperatorId);
|
||||
mprintf("\t\t},\n");
|
||||
|
||||
mprintf("\t}\n}");
|
||||
}
|
||||
|
||||
int odid_message_build_pack(ODID_UAS_Data *UAS_Data, void *pack, size_t buflen)
|
||||
{
|
||||
ODID_MessagePack_data msg_pack;
|
||||
ODID_MessagePack_encoded *msg_pack_enc;
|
||||
size_t len;
|
||||
|
||||
/* create a complete message pack */
|
||||
msg_pack.SingleMessageSize = ODID_MESSAGE_SIZE;
|
||||
msg_pack.MsgPackSize = 0;
|
||||
for (int i = 0; i < ODID_BASIC_ID_MAX_MESSAGES; i++) {
|
||||
if (UAS_Data->BasicIDValid[i]) {
|
||||
if (msg_pack.MsgPackSize >= ODID_PACK_MAX_MESSAGES)
|
||||
return -EINVAL;
|
||||
if (encodeBasicIDMessage((void *)&msg_pack.Messages[msg_pack.MsgPackSize], &UAS_Data->BasicID[i]) == ODID_SUCCESS)
|
||||
msg_pack.MsgPackSize++;
|
||||
}
|
||||
}
|
||||
if (UAS_Data->LocationValid) {
|
||||
if (msg_pack.MsgPackSize >= ODID_PACK_MAX_MESSAGES)
|
||||
return -EINVAL;
|
||||
if (encodeLocationMessage((void *)&msg_pack.Messages[msg_pack.MsgPackSize], &UAS_Data->Location) == ODID_SUCCESS)
|
||||
msg_pack.MsgPackSize++;
|
||||
}
|
||||
for (int i = 0; i < ODID_AUTH_MAX_PAGES; i++)
|
||||
{
|
||||
if (UAS_Data->AuthValid[i]) {
|
||||
if (msg_pack.MsgPackSize >= ODID_PACK_MAX_MESSAGES)
|
||||
return -EINVAL;
|
||||
if (encodeAuthMessage((void *)&msg_pack.Messages[msg_pack.MsgPackSize], &UAS_Data->Auth[i]) == ODID_SUCCESS)
|
||||
msg_pack.MsgPackSize++;
|
||||
}
|
||||
}
|
||||
if (UAS_Data->SelfIDValid) {
|
||||
if (msg_pack.MsgPackSize >= ODID_PACK_MAX_MESSAGES)
|
||||
return -EINVAL;
|
||||
if (encodeSelfIDMessage((void *)&msg_pack.Messages[msg_pack.MsgPackSize], &UAS_Data->SelfID) == ODID_SUCCESS)
|
||||
msg_pack.MsgPackSize++;
|
||||
}
|
||||
if (UAS_Data->SystemValid) {
|
||||
if (msg_pack.MsgPackSize >= ODID_PACK_MAX_MESSAGES)
|
||||
return -EINVAL;
|
||||
if (encodeSystemMessage((void *)&msg_pack.Messages[msg_pack.MsgPackSize], &UAS_Data->System) == ODID_SUCCESS)
|
||||
msg_pack.MsgPackSize++;
|
||||
}
|
||||
if (UAS_Data->OperatorIDValid) {
|
||||
if (msg_pack.MsgPackSize >= ODID_PACK_MAX_MESSAGES)
|
||||
return -EINVAL;
|
||||
if (encodeOperatorIDMessage((void *)&msg_pack.Messages[msg_pack.MsgPackSize], &UAS_Data->OperatorID) == ODID_SUCCESS)
|
||||
msg_pack.MsgPackSize++;
|
||||
}
|
||||
|
||||
/* check that there is at least one message to send. */
|
||||
if (msg_pack.MsgPackSize == 0)
|
||||
return -EINVAL;
|
||||
|
||||
/* calculate the exact encoded message pack size. */
|
||||
len = sizeof(*msg_pack_enc) - (ODID_PACK_MAX_MESSAGES - msg_pack.MsgPackSize) * ODID_MESSAGE_SIZE;
|
||||
|
||||
/* check if there is enough space for the message pack. */
|
||||
if (len > buflen)
|
||||
return -ENOMEM;
|
||||
|
||||
msg_pack_enc = (ODID_MessagePack_encoded *) pack;
|
||||
if (encodeMessagePack(msg_pack_enc, &msg_pack) != ODID_SUCCESS)
|
||||
return -1;
|
||||
|
||||
return (int) len;
|
||||
}
|
||||
|
||||
int odid_wifi_build_nan_sync_beacon_frame(char *mac, uint8_t *buf, size_t buf_size)
|
||||
{
|
||||
/* Broadcast address */
|
||||
uint8_t target_addr[6] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
|
||||
uint8_t wifi_alliance_oui[3] = { 0x50, 0x6F, 0x9A };
|
||||
/* "org.opendroneid.remoteid" hash */
|
||||
uint8_t service_id[6] = { 0x88, 0x69, 0x19, 0x9D, 0x92, 0x09 };
|
||||
const uint8_t *cluster_id = get_nan_cluster_id();
|
||||
struct ieee80211_vendor_specific *vendor;
|
||||
struct nan_master_indication_attribute *master_indication_attr;
|
||||
struct nan_cluster_attribute *cluster_attr;
|
||||
struct nan_service_id_list_attribute *nsila;
|
||||
int ret;
|
||||
size_t len = 0;
|
||||
|
||||
/* IEEE 802.11 Management Header */
|
||||
ret = buf_fill_ieee80211_mgmt(buf, &len, buf_size, IEEE80211_STYPE_BEACON, target_addr, (uint8_t *)mac, cluster_id);
|
||||
if (ret <0)
|
||||
return ret;
|
||||
|
||||
/* Beacon */
|
||||
ret = buf_fill_ieee80211_beacon(buf, &len, buf_size, 0x0200);
|
||||
if (ret <0)
|
||||
return ret;
|
||||
|
||||
/* Vendor Specific */
|
||||
if (len + sizeof(*vendor) > buf_size)
|
||||
return -ENOMEM;
|
||||
|
||||
vendor = (struct ieee80211_vendor_specific *)(buf + len);
|
||||
memset(vendor, 0, sizeof(*vendor));
|
||||
vendor->element_id = IEEE80211_ELEMID_VENDOR;
|
||||
vendor->length = 0x22;
|
||||
memcpy(vendor->oui, wifi_alliance_oui, sizeof(vendor->oui));
|
||||
vendor->oui_type = 0x13;
|
||||
len += sizeof(*vendor);
|
||||
|
||||
/* NAN Master Indication attribute */
|
||||
if (len + sizeof(*master_indication_attr) > buf_size)
|
||||
return -ENOMEM;
|
||||
|
||||
master_indication_attr = (struct nan_master_indication_attribute *)(buf + len);
|
||||
memset(master_indication_attr, 0, sizeof(*master_indication_attr));
|
||||
master_indication_attr->header.attribute_id = 0x00;
|
||||
master_indication_attr->header.length = cpu_to_le16(0x0002);
|
||||
/* Information that is used to indicate a NAN Device’s preference to serve
|
||||
* as the role of Master, with a larger value indicating a higher
|
||||
* preference. Values 1 and 255 are used for testing purposes only.
|
||||
*/
|
||||
master_indication_attr->master_preference = 0xFE;
|
||||
/* Random factor value 0xEA is recommended by the European Standard */
|
||||
master_indication_attr->random_factor = 0xEA;
|
||||
len += sizeof(*master_indication_attr);
|
||||
|
||||
/* NAN Cluster attribute */
|
||||
if (len + sizeof(*cluster_attr) > buf_size)
|
||||
return -ENOMEM;
|
||||
|
||||
cluster_attr = (struct nan_cluster_attribute *)(buf + len);
|
||||
memset(cluster_attr, 0, sizeof(*cluster_attr));
|
||||
cluster_attr->header.attribute_id = 0x1;
|
||||
cluster_attr->header.length = cpu_to_le16(0x000D);
|
||||
memcpy(cluster_attr->device_mac, mac, sizeof(cluster_attr->device_mac));
|
||||
cluster_attr->random_factor = 0xEA;
|
||||
cluster_attr->master_preference = 0xFE;
|
||||
cluster_attr->hop_count_to_anchor_master = 0x00;
|
||||
memset(cluster_attr->anchor_master_beacon_transmission_time, 0, sizeof(cluster_attr->anchor_master_beacon_transmission_time));
|
||||
len += sizeof(*cluster_attr);
|
||||
|
||||
/* NAN attributes */
|
||||
if (len + sizeof(*nsila) > buf_size)
|
||||
return -ENOMEM;
|
||||
|
||||
nsila = (struct nan_service_id_list_attribute *)(buf + len);
|
||||
memset(nsila, 0, sizeof(*nsila));
|
||||
nsila->header.attribute_id = 0x02;
|
||||
nsila->header.length = cpu_to_le16(0x0006);
|
||||
memcpy(nsila->service_id, service_id, sizeof(service_id));
|
||||
len += sizeof(*nsila);
|
||||
|
||||
return (int) len;
|
||||
}
|
||||
|
||||
int odid_wifi_build_message_pack_nan_action_frame(ODID_UAS_Data *UAS_Data, char *mac,
|
||||
uint8_t send_counter,
|
||||
uint8_t *buf, size_t buf_size)
|
||||
{
|
||||
/* Neighbor Awareness Networking Specification v3.0 in section 2.8.1
|
||||
* NAN Network ID calls for the destination mac to be 51-6F-9A-01-00-00 */
|
||||
uint8_t target_addr[6] = { 0x51, 0x6F, 0x9A, 0x01, 0x00, 0x00 };
|
||||
/* "org.opendroneid.remoteid" hash */
|
||||
uint8_t service_id[6] = { 0x88, 0x69, 0x19, 0x9D, 0x92, 0x09 };
|
||||
uint8_t wifi_alliance_oui[3] = { 0x50, 0x6F, 0x9A };
|
||||
const uint8_t *cluster_id = get_nan_cluster_id();
|
||||
struct nan_service_discovery *nsd;
|
||||
struct nan_service_descriptor_attribute *nsda;
|
||||
struct nan_service_descriptor_extension_attribute *nsdea;
|
||||
struct ODID_service_info *si;
|
||||
int ret;
|
||||
size_t len = 0;
|
||||
|
||||
/* IEEE 802.11 Management Header */
|
||||
ret = buf_fill_ieee80211_mgmt(buf, &len, buf_size, IEEE80211_STYPE_ACTION, target_addr, (uint8_t *)mac, cluster_id);
|
||||
if (ret <0)
|
||||
return ret;
|
||||
|
||||
/* NAN Service Discovery header */
|
||||
if (len + sizeof(*nsd) > buf_size)
|
||||
return -ENOMEM;
|
||||
|
||||
nsd = (struct nan_service_discovery *)(buf + len);
|
||||
memset(nsd, 0, sizeof(*nsd));
|
||||
nsd->category = 0x04; /* IEEE 802.11 Public Action frame */
|
||||
nsd->action_code = 0x09; /* IEEE 802.11 Public Action frame Vendor Specific*/
|
||||
memcpy(nsd->oui, wifi_alliance_oui, sizeof(nsd->oui));
|
||||
nsd->oui_type = 0x13; /* Identify Type and version of the NAN */
|
||||
len += sizeof(*nsd);
|
||||
|
||||
/* NAN Attribute for Service Descriptor header */
|
||||
if (len + sizeof(*nsda) > buf_size)
|
||||
return -ENOMEM;
|
||||
|
||||
nsda = (struct nan_service_descriptor_attribute *)(buf + len);
|
||||
nsda->header.attribute_id = 0x3; /* Service Descriptor Attribute type */
|
||||
memcpy(nsda->service_id, service_id, sizeof(service_id));
|
||||
/* always 1 */
|
||||
nsda->instance_id = 0x01; /* always 1 */
|
||||
nsda->requestor_instance_id = 0x00; /* from triggering frame */
|
||||
nsda->service_control = 0x10; /* follow up */
|
||||
len += sizeof(*nsda);
|
||||
|
||||
/* ODID Service Info Attribute header */
|
||||
if (len + sizeof(*si) > buf_size)
|
||||
return -ENOMEM;
|
||||
|
||||
si = (struct ODID_service_info *)(buf + len);
|
||||
memset(si, 0, sizeof(*si));
|
||||
si->message_counter = send_counter;
|
||||
len += sizeof(*si);
|
||||
|
||||
ret = odid_message_build_pack(UAS_Data, buf + len, buf_size - len);
|
||||
if (ret < 0)
|
||||
return ret;
|
||||
len += ret;
|
||||
|
||||
/* set the lengths according to the message pack lengths */
|
||||
nsda->service_info_length = sizeof(*si) + ret;
|
||||
nsda->header.length = cpu_to_le16(sizeof(*nsda) - sizeof(struct nan_attribute_header) + nsda->service_info_length);
|
||||
|
||||
/* NAN Attribute for Service Descriptor extension header */
|
||||
if (len + sizeof(*nsdea) > buf_size)
|
||||
return -ENOMEM;
|
||||
|
||||
nsdea = (struct nan_service_descriptor_extension_attribute *)(buf + len);
|
||||
nsdea->header.attribute_id = 0xE;
|
||||
nsdea->header.length = cpu_to_le16(0x0004);
|
||||
nsdea->instance_id = 0x01;
|
||||
nsdea->control = cpu_to_le16(0x0200);
|
||||
nsdea->service_update_indicator = send_counter;
|
||||
len += sizeof(*nsdea);
|
||||
|
||||
return (int) len;
|
||||
}
|
||||
|
||||
int odid_wifi_build_message_pack_beacon_frame(ODID_UAS_Data *UAS_Data, char *mac,
|
||||
const char *SSID, size_t SSID_len,
|
||||
uint16_t interval_tu, uint8_t send_counter,
|
||||
uint8_t *buf, size_t buf_size)
|
||||
{
|
||||
/* Broadcast address */
|
||||
uint8_t target_addr[6] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
|
||||
uint8_t asd_stan_oui[3] = { 0xFA, 0x0B, 0xBC };
|
||||
/* Mgmt Beacon frame mandatory fields + IE 221 */
|
||||
struct ieee80211_ssid *ssid_s;
|
||||
struct ieee80211_supported_rates *rates;
|
||||
struct ieee80211_vendor_specific *vendor;
|
||||
|
||||
/* Message Pack */
|
||||
struct ODID_service_info *si;
|
||||
|
||||
int ret;
|
||||
size_t len = 0;
|
||||
|
||||
/* IEEE 802.11 Management Header */
|
||||
ret = buf_fill_ieee80211_mgmt(buf, &len, buf_size, IEEE80211_STYPE_BEACON, target_addr, (uint8_t *)mac, (uint8_t *)mac);
|
||||
if (ret <0)
|
||||
return ret;
|
||||
|
||||
/* Mandatory Beacon as of 802.11-2016 Part 11 */
|
||||
ret = buf_fill_ieee80211_beacon(buf, &len, buf_size, interval_tu);
|
||||
if (ret <0)
|
||||
return ret;
|
||||
|
||||
/* SSID: 1-32 bytes */
|
||||
if (len + sizeof(*ssid_s) > buf_size)
|
||||
return -ENOMEM;
|
||||
|
||||
ssid_s = (struct ieee80211_ssid *)(buf + len);
|
||||
if(!SSID || (SSID_len ==0) || (SSID_len > 32))
|
||||
return -EINVAL;
|
||||
ssid_s->element_id = IEEE80211_ELEMID_SSID;
|
||||
ssid_s->length = (uint8_t) SSID_len;
|
||||
memcpy(ssid_s->ssid, SSID, ssid_s->length);
|
||||
len += sizeof(*ssid_s) + SSID_len;
|
||||
|
||||
/* Supported Rates: 1 record at minimum */
|
||||
if (len + sizeof(*rates) > buf_size)
|
||||
return -ENOMEM;
|
||||
|
||||
rates = (struct ieee80211_supported_rates *)(buf + len);
|
||||
rates->element_id = IEEE80211_ELEMID_RATES;
|
||||
rates->length = 1; // One rate only
|
||||
rates->supported_rates = 0x8C; // 6 Mbps
|
||||
len += sizeof(*rates);
|
||||
|
||||
/* Vendor Specific Information Element (IE 221) */
|
||||
if (len + sizeof(*vendor) > buf_size)
|
||||
return -ENOMEM;
|
||||
|
||||
vendor = (struct ieee80211_vendor_specific *)(buf + len);
|
||||
vendor->element_id = IEEE80211_ELEMID_VENDOR;
|
||||
vendor->length = 0x00; // Length updated at end of function
|
||||
memcpy(vendor->oui, asd_stan_oui, sizeof(vendor->oui));
|
||||
vendor->oui_type = 0x0D;
|
||||
len += sizeof(*vendor);
|
||||
|
||||
/* ODID Service Info Attribute header */
|
||||
if (len + sizeof(*si) > buf_size)
|
||||
return -ENOMEM;
|
||||
|
||||
si = (struct ODID_service_info *)(buf + len);
|
||||
memset(si, 0, sizeof(*si));
|
||||
si->message_counter = send_counter;
|
||||
len += sizeof(*si);
|
||||
|
||||
ret = odid_message_build_pack(UAS_Data, buf + len, buf_size - len);
|
||||
if (ret < 0)
|
||||
return ret;
|
||||
len += ret;
|
||||
|
||||
/* set the lengths according to the message pack lengths */
|
||||
vendor->length = sizeof(vendor->oui) + sizeof(vendor->oui_type) + sizeof(*si) + ret;
|
||||
|
||||
return (int) len;
|
||||
}
|
||||
|
||||
int odid_message_process_pack(ODID_UAS_Data *UAS_Data, uint8_t *pack, size_t buflen)
|
||||
{
|
||||
ODID_MessagePack_encoded *msg_pack_enc = (ODID_MessagePack_encoded *) pack;
|
||||
size_t size = sizeof(*msg_pack_enc) - ODID_MESSAGE_SIZE * (ODID_PACK_MAX_MESSAGES - msg_pack_enc->MsgPackSize);
|
||||
if (size > buflen)
|
||||
return -ENOMEM;
|
||||
|
||||
odid_initUasData(UAS_Data);
|
||||
|
||||
if (decodeMessagePack(UAS_Data, msg_pack_enc) != ODID_SUCCESS)
|
||||
return -1;
|
||||
|
||||
return (int) size;
|
||||
}
|
||||
|
||||
int odid_wifi_receive_message_pack_nan_action_frame(ODID_UAS_Data *UAS_Data,
|
||||
char *mac, uint8_t *buf, size_t buf_size)
|
||||
{
|
||||
struct ieee80211_mgmt *mgmt;
|
||||
struct nan_service_discovery *nsd;
|
||||
struct nan_service_descriptor_attribute *nsda;
|
||||
struct nan_service_descriptor_extension_attribute *nsdea;
|
||||
struct ODID_service_info *si;
|
||||
uint8_t target_addr[6] = { 0x51, 0x6F, 0x9A, 0x01, 0x00, 0x00 };
|
||||
uint8_t wifi_alliance_oui[3] = { 0x50, 0x6F, 0x9A };
|
||||
uint8_t service_id[6] = { 0x88, 0x69, 0x19, 0x9D, 0x92, 0x09 };
|
||||
int ret;
|
||||
size_t len = 0;
|
||||
|
||||
/* IEEE 802.11 Management Header */
|
||||
if (len + sizeof(*mgmt) > buf_size)
|
||||
return -EINVAL;
|
||||
mgmt = (struct ieee80211_mgmt *)(buf + len);
|
||||
if ((mgmt->frame_control & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) !=
|
||||
cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_ACTION))
|
||||
return -EINVAL;
|
||||
if (memcmp(mgmt->da, target_addr, sizeof(mgmt->da)) != 0)
|
||||
return -EINVAL;
|
||||
memcpy(mac, mgmt->sa, sizeof(mgmt->sa));
|
||||
|
||||
len += sizeof(*mgmt);
|
||||
|
||||
/* NAN Service Discovery header */
|
||||
if (len + sizeof(*nsd) > buf_size)
|
||||
return -EINVAL;
|
||||
nsd = (struct nan_service_discovery *)(buf + len);
|
||||
if (nsd->category != 0x04)
|
||||
return -EINVAL;
|
||||
if (nsd->action_code != 0x09)
|
||||
return -EINVAL;
|
||||
if (memcmp(nsd->oui, wifi_alliance_oui, sizeof(wifi_alliance_oui)) != 0)
|
||||
return -EINVAL;
|
||||
if (nsd->oui_type != 0x13)
|
||||
return -EINVAL;
|
||||
len += sizeof(*nsd);
|
||||
|
||||
/* NAN Attribute for Service Descriptor header */
|
||||
if (len + sizeof(*nsda) > buf_size)
|
||||
return -EINVAL;
|
||||
nsda = (struct nan_service_descriptor_attribute *)(buf + len);
|
||||
if (nsda->header.attribute_id != 0x3)
|
||||
return -EINVAL;
|
||||
if (memcmp(nsda->service_id, service_id, sizeof(service_id)) != 0)
|
||||
return -EINVAL;
|
||||
if (nsda->instance_id != 0x01)
|
||||
return -EINVAL;
|
||||
if (nsda->service_control != 0x10)
|
||||
return -EINVAL;
|
||||
len += sizeof(*nsda);
|
||||
|
||||
si = (struct ODID_service_info *)(buf + len);
|
||||
ret = odid_message_process_pack(UAS_Data, buf + len + sizeof(*si), buf_size - len - sizeof(*nsdea));
|
||||
if (ret < 0)
|
||||
return -EINVAL;
|
||||
if (nsda->service_info_length != (sizeof(*si) + ret))
|
||||
return -EINVAL;
|
||||
if (nsda->header.length != (cpu_to_le16(sizeof(*nsda) - sizeof(struct nan_attribute_header) + nsda->service_info_length)))
|
||||
return -EINVAL;
|
||||
len += sizeof(*si) + ret;
|
||||
|
||||
/* NAN Attribute for Service Descriptor extension header */
|
||||
if (len + sizeof(*nsdea) > buf_size)
|
||||
return -ENOMEM;
|
||||
nsdea = (struct nan_service_descriptor_extension_attribute *)(buf + len);
|
||||
if (nsdea->header.attribute_id != 0xE)
|
||||
return -EINVAL;
|
||||
if (nsdea->header.length != cpu_to_le16(0x0004))
|
||||
return -EINVAL;
|
||||
if (nsdea->instance_id != 0x01)
|
||||
return -EINVAL;
|
||||
if (nsdea->control != cpu_to_le16(0x0200))
|
||||
return -EINVAL;
|
||||
|
||||
return 0;
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user