commit 2a749eade07be9a15038f1f54bbf05cece897012 Author: Jet Date: Tue May 23 22:20:43 2023 +0100 first commit diff --git a/README.md b/README.md new file mode 100644 index 0000000..c859467 --- /dev/null +++ b/README.md @@ -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. diff --git a/RemoteIDSpoofer.ino b/RemoteIDSpoofer.ino new file mode 100644 index 0000000..995ae0e --- /dev/null +++ b/RemoteIDSpoofer.ino @@ -0,0 +1,131 @@ +// ESP8266 RemoteID spoofer +// Heavily adapted from https://github.com/sxjack/uav_electronic_ids + +#pragma GCC diagnostic warning "-Wunused-variable" + +#include +#include +#include +#include + +#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(); +} \ No newline at end of file diff --git a/alt_unix_time.c b/alt_unix_time.c new file mode 100644 index 0000000..b97885f --- /dev/null +++ b/alt_unix_time.c @@ -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 + +#else + +#include +#include +#include +#include + +#endif + +#include + +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; +} + +/* + * + */ + diff --git a/id_open.cpp b/id_open.cpp new file mode 100644 index 0000000..826d6d7 --- /dev/null +++ b/id_open.cpp @@ -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 + +#include +#include + +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; +} + +/* + * + */ diff --git a/id_open.h b/id_open.h new file mode 100644 index 0000000..0f7abaa --- /dev/null +++ b/id_open.h @@ -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 + +/* + * + */ diff --git a/id_open_beacon.cpp b/id_open_beacon.cpp new file mode 100644 index 0000000..03654af --- /dev/null +++ b/id_open_beacon.cpp @@ -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 + +#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 diff --git a/id_open_esp32.cpp b/id_open_esp32.cpp new file mode 100644 index 0000000..748581e --- /dev/null +++ b/id_open_esp32.cpp @@ -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 + +#include "id_open.h" + +#if ID_OD_WIFI + +#include + +#include +#include +#include +#include +#include +#include + +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 diff --git a/id_open_esp8266.cpp b/id_open_esp8266.cpp new file mode 100644 index 0000000..5dc332a --- /dev/null +++ b/id_open_esp8266.cpp @@ -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 + +#include "id_open.h" + +#if ID_OD_WIFI + +#include + +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 diff --git a/id_open_nrf52.cpp b/id_open_nrf52.cpp new file mode 100644 index 0000000..09d2ff8 --- /dev/null +++ b/id_open_nrf52.cpp @@ -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 + +#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 diff --git a/id_open_nrf52.h b/id_open_nrf52.h new file mode 100644 index 0000000..0f466c3 --- /dev/null +++ b/id_open_nrf52.h @@ -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 + +#if ID_OD_BT + +#include + +/* + * + */ + +#if BLE_OPTION == 1 + +#include +#include + +#elif BLE_OPTION == 2 + +#endif + +/* + * + */ + +#endif // ID_OD_BT + +#endif // nRF52 diff --git a/library.properties b/library.properties new file mode 100644 index 0000000..c80b353 --- /dev/null +++ b/library.properties @@ -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 diff --git a/odid_wifi.h b/odid_wifi.h new file mode 100644 index 0000000..c517658 --- /dev/null +++ b/odid_wifi.h @@ -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_ diff --git a/opendroneid.c b/opendroneid.c new file mode 100644 index 0000000..a53c877 --- /dev/null +++ b/opendroneid.c @@ -0,0 +1,1476 @@ +/* +Copyright (C) 2019 Intel Corporation + +SPDX-License-Identifier: Apache-2.0 + +Open Drone ID C Library + +Maintainer: +Gabriel Cox +gabriel.c.cox@intel.com +*/ + +#include "opendroneid.h" +#include +#include +#define ENABLE_DEBUG 1 + +const float SPEED_DIV[2] = {0.25f, 0.75f}; +const float VSPEED_DIV = 0.5f; +const int32_t LATLON_MULT = 10000000; +const float ALT_DIV = 0.5f; +const int ALT_ADDER = 1000; + +static char *safe_dec_copyfill(char *dstStr, const char *srcStr, int dstSize); +static int intRangeMax(int64_t inValue, int startRange, int endRange); +static int intInRange(int inValue, int startRange, int endRange); + +/** +* Initialize basic ID data fields to their default values +* +* @param data (non encoded/packed) structure +*/ +void odid_initBasicIDData(ODID_BasicID_data *data) +{ + if (!data) + return; + memset(data, 0, sizeof(ODID_BasicID_data)); +} + +/** +* Initialize location data fields to their default values +* +* @param data (non encoded/packed) structure +*/ +void odid_initLocationData(ODID_Location_data *data) +{ + if (!data) + return; + memset(data, 0, sizeof(ODID_Location_data)); + data->Direction = INV_DIR; + data->SpeedHorizontal = INV_SPEED_H; + data->SpeedVertical = INV_SPEED_V; + data->AltitudeBaro = INV_ALT; + data->AltitudeGeo = INV_ALT; + data->Height = INV_ALT; +} + +/** +* Initialize authorization data fields to their default values +* +* @param data (non encoded/packed) structure +*/ +void odid_initAuthData(ODID_Auth_data *data) +{ + if (!data) + return; + memset(data, 0, sizeof(ODID_Auth_data)); +} + +/** +* Initialize self ID data fields to their default values +* +* @param data (non encoded/packed) structure +*/ +void odid_initSelfIDData(ODID_SelfID_data *data) +{ + if (!data) + return; + memset(data, 0, sizeof(ODID_SelfID_data)); +} + +/** +* Initialize system data fields to their default values +* +* @param data (non encoded/packed) structure +*/ + +void odid_initSystemData(ODID_System_data *data) +{ + if (!data) + return; + memset(data, 0, sizeof(ODID_System_data)); + data->AreaCount = 1; + data->AreaCeiling = INV_ALT; + data->AreaFloor = INV_ALT; + data->OperatorAltitudeGeo = INV_ALT; +} + +/** +* Initialize operator ID data fields to their default values +* +* @param data (non encoded/packed) structure +*/ + +void odid_initOperatorIDData(ODID_OperatorID_data *data) +{ + if (!data) + return; + memset(data, 0, sizeof(ODID_OperatorID_data)); +} + +/** +* Initialize message pack data fields to their default values +* +* @param data (non encoded/packed) structure +*/ + +void odid_initMessagePackData(ODID_MessagePack_data *data) +{ + if (!data) + return; + memset(data, 0, sizeof(ODID_MessagePack_data)); + data->SingleMessageSize = ODID_MESSAGE_SIZE; +} + +/** +* Initialize UAS data fields to their default values +* +* @param data (non encoded/packed) structure +*/ + +void odid_initUasData(ODID_UAS_Data *data) +{ + if (!data) + return; + for (int i = 0; i < ODID_BASIC_ID_MAX_MESSAGES; i++) { + data->BasicIDValid[i] = 0; + odid_initBasicIDData(&data->BasicID[i]); + } + data->LocationValid = 0; + odid_initLocationData(&data->Location); + for (int i = 0; i < ODID_AUTH_MAX_PAGES; i++) { + data->AuthValid[i] = 0; + odid_initAuthData(&data->Auth[i]); + } + data->SelfIDValid = 0; + odid_initSelfIDData(&data->SelfID); + data->SystemValid = 0; + odid_initSystemData(&data->System); + data->OperatorIDValid = 0; + odid_initOperatorIDData(&data->OperatorID); +} + +/** +* Encode direction as defined by Open Drone ID +* +* The encoding method uses 8 bits for the direction in degrees and +* one extra bit for indicating the East/West direction. +* +* @param Direcction in degrees. 0 <= x < 360. Route course based on true North +* @param EWDirection Bit flag indicating whether the direction is towards + East (0 - 179 degrees) or West (180 - 359) +* @return Encoded Direction in a single byte +*/ +static uint8_t encodeDirection(float Direction, uint8_t *EWDirection) +{ + unsigned int direction_int = (unsigned int) roundf(Direction); + if (direction_int < 180) { + *EWDirection = 0; + } else { + *EWDirection = 1; + direction_int -= 180; + } + return (uint8_t) intRangeMax(direction_int, 0, UINT8_MAX); +} + +/** +* Encode speed into units defined by Open Drone ID +* +* The quantization algorithm allows for speed to be stored in units of 0.25 m/s +* on the low end of the scale and 0.75 m/s on the high end of the scale. +* This allows for more precise speeds to be represented in a single Uint8 byte +* rather than using a large float value. +* +* @param Speed_data Speed (and decimal) in m/s +* @param mult a (write only) value that sets the multiplier flag +* @return Encoded Speed in a single byte or max speed if over max encoded speed. +*/ +static uint8_t encodeSpeedHorizontal(float Speed_data, uint8_t *mult) +{ + if (Speed_data <= UINT8_MAX * SPEED_DIV[0]) { + *mult = 0; + return (uint8_t) (Speed_data / SPEED_DIV[0]); + } else { + *mult = 1; + int big_value = (int) ((Speed_data - (UINT8_MAX * SPEED_DIV[0])) / SPEED_DIV[1]); + return (uint8_t) intRangeMax(big_value, 0, UINT8_MAX); + } +} + +/** +* Encode Vertical Speed into a signed Integer ODID format +* +* @param SpeedVertical_data vertical speed (in m/s) +* @return Encoded vertical speed +*/ +static int8_t encodeSpeedVertical(float SpeedVertical_data) +{ + int encValue = (int) (SpeedVertical_data / VSPEED_DIV); + return (int8_t) intRangeMax(encValue, INT8_MIN, INT8_MAX); +} + +/** +* Encode Latitude or Longitude value into a signed Integer ODID format +* +* This encodes a 64bit double into a 32 bit integer yet still maintains +* 10^7 of a degree of accuracy (about 1cm) +* +* @param LatLon_data Either Lat or Lon double float value +* @return Encoded Lat or Lon +*/ +static int32_t encodeLatLon(double LatLon_data) +{ + return (int32_t) intRangeMax((int64_t) (LatLon_data * LATLON_MULT), -180 * LATLON_MULT, 180 * LATLON_MULT); +} + +/** +* Encode Altitude value into an int16 ODID format +* +* This encodes a 32bit floating point altitude into an uint16 compressed +* scale that starts at -1000m. +* +* @param Alt_data Altitude to encode (in meters) +* @return Encoded Altitude +*/ +static uint16_t encodeAltitude(float Alt_data) +{ + return (uint16_t) intRangeMax( (int) ((Alt_data + (float) ALT_ADDER) / ALT_DIV), 0, UINT16_MAX); +} + +/** +* Encode timestamp data in ODID format +* +* This encodes a fractional seconds value into a 2 byte int16 +* on a scale of tenths of seconds since after the hour. +* +* @param Seconds_data Seconds (to at least 1 decimal place) since the hour +* @return Encoded timestamp (Tenths of seconds since the hour) +*/ +static uint16_t encodeTimeStamp(float Seconds_data) +{ + if (Seconds_data == INV_TIMESTAMP) + return INV_TIMESTAMP; + else + return (uint16_t) intRangeMax((int64_t) roundf(Seconds_data*10), 0, MAX_TIMESTAMP * 10); +} + +/** +* Encode area radius data in ODID format +* +* This encodes the area radius in meters into a 1 byte value +* +* @param Radius The radius of the drone area/swarm +* @return Encoded area radius +*/ +static uint8_t encodeAreaRadius(uint16_t Radius) +{ + return (uint8_t) intRangeMax(Radius / 10, 0, 255); +} + +/** +* Encode Basic ID message (packed, ready for broadcast) +* +* @param outEncoded Output (encoded/packed) structure +* @param inData Input data (non encoded/packed) structure +* @return ODID_SUCCESS or ODID_FAIL; +*/ +int encodeBasicIDMessage(ODID_BasicID_encoded *outEncoded, ODID_BasicID_data *inData) +{ + if (!outEncoded || !inData || + !intInRange(inData->IDType, 0, 15) || + !intInRange(inData->UAType, 0, 15)) + return ODID_FAIL; + + outEncoded->MessageType = ODID_MESSAGETYPE_BASIC_ID; + outEncoded->ProtoVersion = ODID_PROTOCOL_VERSION; + outEncoded->IDType = inData->IDType; + outEncoded->UAType = inData->UAType; + strncpy(outEncoded->UASID, inData->UASID, sizeof(outEncoded->UASID)); + memset(outEncoded->Reserved, 0, sizeof(outEncoded->Reserved)); + return ODID_SUCCESS; +} + +/** +* Encode Location message (packed, ready for broadcast) +* +* @param outEncoded Output (encoded/packed) structure +* @param inData Input data (non encoded/packed) structure +* @return ODID_SUCCESS or ODID_FAIL; +*/ +int encodeLocationMessage(ODID_Location_encoded *outEncoded, ODID_Location_data *inData) +{ + uint8_t bitflag; + if (!outEncoded || !inData || + !intInRange(inData->Status, 0, 15) || + !intInRange(inData->HeightType, 0, 1) || + !intInRange(inData->HorizAccuracy, 0, 15) || + !intInRange(inData->VertAccuracy, 0, 15) || + !intInRange(inData->BaroAccuracy, 0, 15) || + !intInRange(inData->SpeedAccuracy, 0, 15) || + !intInRange(inData->TSAccuracy, 0, 15)) + return ODID_FAIL; + + if (inData->Direction < MIN_DIR || inData->Direction > INV_DIR || + (inData->Direction > MAX_DIR && inData->Direction < INV_DIR)) + return ODID_FAIL; + + if (inData->SpeedHorizontal < MIN_SPEED_H || inData->SpeedHorizontal > INV_SPEED_H || + (inData->SpeedHorizontal > MAX_SPEED_H && inData->SpeedHorizontal < INV_SPEED_H)) + return ODID_FAIL; + + if (inData->SpeedVertical < MIN_SPEED_V || inData->SpeedVertical > INV_SPEED_V || + (inData->SpeedVertical > MAX_SPEED_V && inData->SpeedVertical < INV_SPEED_V)) + return ODID_FAIL; + + if (inData->Latitude < MIN_LAT || inData->Latitude > MAX_LAT || + inData->Longitude < MIN_LON || inData->Longitude > MAX_LON) + return ODID_FAIL; + + if (inData->AltitudeBaro < MIN_ALT || inData->AltitudeBaro > MAX_ALT || + inData->AltitudeGeo < MIN_ALT || inData->AltitudeGeo > MAX_ALT || + inData->Height < MIN_ALT || inData->Height > MAX_ALT) + return ODID_FAIL; + + if (inData->TimeStamp < 0 || + (inData->TimeStamp > MAX_TIMESTAMP && inData->TimeStamp != INV_TIMESTAMP)) + return ODID_FAIL; + + outEncoded->MessageType = ODID_MESSAGETYPE_LOCATION; + outEncoded->ProtoVersion = ODID_PROTOCOL_VERSION; + outEncoded->Status = inData->Status; + outEncoded->Reserved = 0; + outEncoded->Direction = encodeDirection(inData->Direction, &bitflag); + outEncoded->EWDirection = bitflag; + outEncoded->SpeedHorizontal = encodeSpeedHorizontal(inData->SpeedHorizontal, &bitflag); + outEncoded->SpeedMult = bitflag; + outEncoded->SpeedVertical = encodeSpeedVertical(inData->SpeedVertical); + outEncoded->Latitude = encodeLatLon(inData->Latitude); + outEncoded->Longitude = encodeLatLon(inData->Longitude); + outEncoded->AltitudeBaro = encodeAltitude(inData->AltitudeBaro); + outEncoded->AltitudeGeo = encodeAltitude(inData->AltitudeGeo); + outEncoded->HeightType = inData->HeightType; + outEncoded->Height = encodeAltitude(inData->Height); + outEncoded->HorizAccuracy = inData->HorizAccuracy; + outEncoded->VertAccuracy = inData->VertAccuracy; + outEncoded->BaroAccuracy = inData->BaroAccuracy; + outEncoded->SpeedAccuracy = inData->SpeedAccuracy; + outEncoded->TSAccuracy = inData->TSAccuracy; + outEncoded->Reserved2 = 0; + outEncoded->TimeStamp = encodeTimeStamp(inData->TimeStamp); + outEncoded->Reserved3 = 0; + return ODID_SUCCESS; +} + +/** +* Encode Auth message (packed, ready for broadcast) +* +* @param outEncoded Output (encoded/packed) structure +* @param inData Input data (non encoded/packed) structure +* @return ODID_SUCCESS or ODID_FAIL; +*/ +int encodeAuthMessage(ODID_Auth_encoded *outEncoded, ODID_Auth_data *inData) +{ + if (!outEncoded || !inData || !intInRange(inData->AuthType, 0, 15)) + return ODID_FAIL; + + if (inData->DataPage >= ODID_AUTH_MAX_PAGES) + return ODID_FAIL; + + if (inData->DataPage == 0) { + if (inData->LastPageIndex >= ODID_AUTH_MAX_PAGES) + return ODID_FAIL; + +#if (MAX_AUTH_LENGTH < UINT8_MAX) + if (inData->Length > MAX_AUTH_LENGTH) + return ODID_FAIL; +#endif + + int len = ODID_AUTH_PAGE_ZERO_DATA_SIZE + + inData->LastPageIndex * ODID_AUTH_PAGE_NONZERO_DATA_SIZE; + if (len < inData->Length) + return ODID_FAIL; + } + + outEncoded->page_zero.MessageType = ODID_MESSAGETYPE_AUTH; + outEncoded->page_zero.ProtoVersion = ODID_PROTOCOL_VERSION; + outEncoded->page_zero.AuthType = inData->AuthType; + outEncoded->page_zero.DataPage = inData->DataPage; + + if (inData->DataPage == 0) { + outEncoded->page_zero.LastPageIndex = inData->LastPageIndex; + outEncoded->page_zero.Length = inData->Length; + outEncoded->page_zero.Timestamp = inData->Timestamp; + memcpy(outEncoded->page_zero.AuthData, inData->AuthData, + sizeof(outEncoded->page_zero.AuthData)); + } else { + memcpy(outEncoded->page_non_zero.AuthData, inData->AuthData, + sizeof(outEncoded->page_non_zero.AuthData)); + } + return ODID_SUCCESS; +} + +/** +* Encode Self ID message (packed, ready for broadcast) +* +* @param outEncoded Output (encoded/packed) structure +* @param inData Input data (non encoded/packed) structure +* @return ODID_SUCCESS or ODID_FAIL; +*/ +int encodeSelfIDMessage(ODID_SelfID_encoded *outEncoded, ODID_SelfID_data *inData) +{ + if (!outEncoded || !inData || !intInRange(inData->DescType, 0, 255)) + return ODID_FAIL; + + outEncoded->MessageType = ODID_MESSAGETYPE_SELF_ID; + outEncoded->ProtoVersion = ODID_PROTOCOL_VERSION; + outEncoded->DescType = inData->DescType; + strncpy(outEncoded->Desc, inData->Desc, sizeof(outEncoded->Desc)); + return ODID_SUCCESS; +} + +/** +* Encode System message (packed, ready for broadcast) +* +* @param outEncoded Output (encoded/packed) structure +* @param inData Input data (non encoded/packed) structure +* @return ODID_SUCCESS or ODID_FAIL; +*/ +int encodeSystemMessage(ODID_System_encoded *outEncoded, ODID_System_data *inData) +{ + if (!outEncoded || !inData || + !intInRange(inData->OperatorLocationType, 0, 3) || + !intInRange(inData->ClassificationType, 0, 7) || + !intInRange(inData->CategoryEU, 0, 15) || + !intInRange(inData->ClassEU, 0, 15)) + return ODID_FAIL; + + if (inData->OperatorLatitude < MIN_LAT || inData->OperatorLatitude > MAX_LAT || + inData->OperatorLongitude < MIN_LON || inData->OperatorLongitude > MAX_LON) + return ODID_FAIL; + + if (inData->AreaRadius > MAX_AREA_RADIUS) + return ODID_FAIL; + + if (inData->AreaCeiling < MIN_ALT || inData->AreaCeiling > MAX_ALT || + inData->AreaFloor < MIN_ALT || inData->AreaFloor > MAX_ALT || + inData->OperatorAltitudeGeo < MIN_ALT || inData->OperatorAltitudeGeo > MAX_ALT) + return ODID_FAIL; + + outEncoded->MessageType = ODID_MESSAGETYPE_SYSTEM; + outEncoded->ProtoVersion = ODID_PROTOCOL_VERSION; + outEncoded->Reserved = 0; + outEncoded->OperatorLocationType = inData->OperatorLocationType; + outEncoded->ClassificationType = inData->ClassificationType; + outEncoded->OperatorLatitude = encodeLatLon(inData->OperatorLatitude); + outEncoded->OperatorLongitude = encodeLatLon(inData->OperatorLongitude); + outEncoded->AreaCount = inData->AreaCount; + outEncoded->AreaRadius = encodeAreaRadius(inData->AreaRadius); + outEncoded->AreaCeiling = encodeAltitude(inData->AreaCeiling); + outEncoded->AreaFloor = encodeAltitude(inData->AreaFloor); + outEncoded->CategoryEU = inData->CategoryEU; + outEncoded->ClassEU = inData->ClassEU; + outEncoded->OperatorAltitudeGeo = encodeAltitude(inData->OperatorAltitudeGeo); + outEncoded->Timestamp = inData->Timestamp; + outEncoded->Reserved2 = 0; + return ODID_SUCCESS; +} + +/** +* Encode Operator ID message (packed, ready for broadcast) +* +* @param outEncoded Output (encoded/packed) structure +* @param inData Input data (non encoded/packed) structure +* @return ODID_SUCCESS or ODID_FAIL; +*/ +int encodeOperatorIDMessage(ODID_OperatorID_encoded *outEncoded, ODID_OperatorID_data *inData) +{ + if (!outEncoded || !inData || !intInRange(inData->OperatorIdType, 0, 255)) + return ODID_FAIL; + + outEncoded->MessageType = ODID_MESSAGETYPE_OPERATOR_ID; + outEncoded->ProtoVersion = ODID_PROTOCOL_VERSION; + outEncoded->OperatorIdType = inData->OperatorIdType; + strncpy(outEncoded->OperatorId, inData->OperatorId, sizeof(outEncoded->OperatorId)); + memset(outEncoded->Reserved, 0, sizeof(outEncoded->Reserved)); + return ODID_SUCCESS; +} + +/** +* Check whether the data fields of a pack structure are valid +* +* @param msgs Pointer to the buffer containing the messages +* @param amount The amount of messages in the pack +* @return ODID_SUCCESS or ODID_FAIL; +*/ +static int checkPackContent(ODID_Message_encoded *msgs, int amount) +{ + if (amount <= 0 || amount > ODID_PACK_MAX_MESSAGES) + return ODID_FAIL; + + int numMessages[6] = { 0 }; // Counters for relevant parts of ODID_messagetype_t + for (int i = 0; i < amount; i++) { + uint8_t MessageType = decodeMessageType(msgs[i].rawData[0]); + + // Check for illegal content. This also avoids recursive calls between + // decodeOpenDroneID() and decodeMessagePack()/checkPackContent() + if (MessageType <= ODID_MESSAGETYPE_OPERATOR_ID) + numMessages[MessageType]++; + else + return ODID_FAIL; + } + + // Allow max one of each message except Basic ID and Authorization. + if (numMessages[ODID_MESSAGETYPE_BASIC_ID] > ODID_BASIC_ID_MAX_MESSAGES || + numMessages[ODID_MESSAGETYPE_LOCATION] > 1 || + numMessages[ODID_MESSAGETYPE_AUTH] > ODID_AUTH_MAX_PAGES || + numMessages[ODID_MESSAGETYPE_SELF_ID] > 1 || + numMessages[ODID_MESSAGETYPE_SYSTEM] > 1 || + numMessages[ODID_MESSAGETYPE_OPERATOR_ID] > 1) + return ODID_FAIL; + + return ODID_SUCCESS; +} + +/** +* Encode message pack. I.e. a collection of multiple encoded messages +* +* @param outEncoded Output (encoded/packed) structure +* @param inData Input data (non encoded/packed) structure +* @return ODID_SUCCESS or ODID_FAIL; +*/ +int encodeMessagePack(ODID_MessagePack_encoded *outEncoded, ODID_MessagePack_data *inData) +{ + if (!outEncoded || !inData || inData->SingleMessageSize != ODID_MESSAGE_SIZE) + return ODID_FAIL; + + if (checkPackContent(inData->Messages, inData->MsgPackSize) != ODID_SUCCESS) + return ODID_FAIL; + + outEncoded->MessageType = ODID_MESSAGETYPE_PACKED; + outEncoded->ProtoVersion = ODID_PROTOCOL_VERSION; + + outEncoded->SingleMessageSize = inData->SingleMessageSize; + outEncoded->MsgPackSize = inData->MsgPackSize; + + for (int i = 0; i < inData->MsgPackSize; i++) + memcpy(&outEncoded->Messages[i], &inData->Messages[i], ODID_MESSAGE_SIZE); + + return ODID_SUCCESS; +} + +/** +* Dencode direction from Open Drone ID packed message +* +* @param Direction_enc encoded direction +* @param EWDirection East/West direction flag +* @return direction in degrees (0 - 359) +*/ +static float decodeDirection(uint8_t Direction_enc, uint8_t EWDirection) +{ + if (EWDirection) + return (float) Direction_enc + 180; + else + return (float) Direction_enc; +} + +/** +* Dencode speed from Open Drone ID packed message +* +* @param Speed_enc encoded speed +* @param mult multiplier flag +* @return decoded speed in m/s +*/ +static float decodeSpeedHorizontal(uint8_t Speed_enc, uint8_t mult) +{ + if (mult) + return ((float) Speed_enc * SPEED_DIV[1]) + (UINT8_MAX * SPEED_DIV[0]); + else + return (float) Speed_enc * SPEED_DIV[0]; +} + +/** +* Decode Vertical Speed from Open Drone ID Packed Message +* +* @param SpeedVertical_enc Encoded Vertical Speed +* @return decoded Vertical Speed in m/s +*/ +static float decodeSpeedVertical(int8_t SpeedVertical_enc) +{ + return (float) SpeedVertical_enc * VSPEED_DIV; +} + +/** +* Decode Latitude or Longitude value into a signed Integer ODID format +* +* @param LatLon_enc Either Lat or Lon ecoded int value +* @return decoded (double) Lat or Lon +*/ +static double decodeLatLon(int32_t LatLon_enc) +{ + return (double) LatLon_enc / LATLON_MULT; +} + +/** +* Decode Altitude from ODID packed format +* +* @param Alt_enc Encoded Altitude to decode +* @return decoded Altitude (in meters) +*/ +static float decodeAltitude(uint16_t Alt_enc) +{ + return (float) Alt_enc * ALT_DIV - (float) ALT_ADDER; +} + +/** +* Decode timestamp data from ODID packed format +* +* @param Seconds_enc Encoded Timestamp +* @return Decoded timestamp (seconds since the hour) +*/ +static float decodeTimeStamp(uint16_t Seconds_enc) +{ + if (Seconds_enc == INV_TIMESTAMP) + return INV_TIMESTAMP; + else + return (float) Seconds_enc / 10; +} + +/** +* Decode area radius data from ODID format +* +* This decodes a 1 byte value to the area radius in meters +* +* @param Radius_enc Encoded area radius +* @return The radius of the drone area/swarm in meters +*/ +static uint16_t decodeAreaRadius(uint8_t Radius_enc) +{ + return (uint16_t) ((int) Radius_enc * 10); +} + +/** +* Get the ID type of the basic ID message +* +* @param inEncoded Input message (encoded/packed) structure +* @param idType Output: The ID type of this basic ID message +* @return ODID_SUCCESS or ODID_FAIL; +*/ +int getBasicIDType(ODID_BasicID_encoded *inEncoded, enum ODID_idtype *idType) +{ + if (!inEncoded || !idType || inEncoded->MessageType != ODID_MESSAGETYPE_BASIC_ID) + return ODID_FAIL; + + *idType = (enum ODID_idtype) inEncoded->IDType; + return ODID_SUCCESS; +} + +/** +* Decode Basic ID data from packed message +* +* @param outData Output: decoded message +* @param inEncoded Input message (encoded/packed) structure +* @return ODID_SUCCESS or ODID_FAIL; +*/ +int decodeBasicIDMessage(ODID_BasicID_data *outData, ODID_BasicID_encoded *inEncoded) +{ + if (!outData || !inEncoded || + inEncoded->MessageType != ODID_MESSAGETYPE_BASIC_ID || + !intInRange(inEncoded->IDType, 0, 15) || + !intInRange(inEncoded->UAType, 0, 15)) + return ODID_FAIL; + + outData->IDType = (ODID_idtype_t) inEncoded->IDType; + outData->UAType = (ODID_uatype_t) inEncoded->UAType; + safe_dec_copyfill(outData->UASID, inEncoded->UASID, sizeof(outData->UASID)); + return ODID_SUCCESS; +} + +/** +* Decode Location data from packed message +* +* @param outData Output: decoded message +* @param inEncoded Input message (encoded/packed) structure +* @return ODID_SUCCESS or ODID_FAIL; +*/ +int decodeLocationMessage(ODID_Location_data *outData, ODID_Location_encoded *inEncoded) +{ + if (!outData || !inEncoded || + inEncoded->MessageType != ODID_MESSAGETYPE_LOCATION || + !intInRange(inEncoded->Status, 0, 15)) + return ODID_FAIL; + + outData->Status = (ODID_status_t) inEncoded->Status; + outData->Direction = decodeDirection(inEncoded->Direction, inEncoded-> EWDirection); + outData->SpeedHorizontal = decodeSpeedHorizontal(inEncoded->SpeedHorizontal, inEncoded->SpeedMult); + outData->SpeedVertical = decodeSpeedVertical(inEncoded->SpeedVertical); + outData->Latitude = decodeLatLon(inEncoded->Latitude); + outData->Longitude = decodeLatLon(inEncoded->Longitude); + outData->AltitudeBaro = decodeAltitude(inEncoded->AltitudeBaro); + outData->AltitudeGeo = decodeAltitude(inEncoded->AltitudeGeo); + outData->HeightType = (ODID_Height_reference_t) inEncoded->HeightType; + outData->Height = decodeAltitude(inEncoded->Height); + outData->HorizAccuracy = (ODID_Horizontal_accuracy_t) inEncoded->HorizAccuracy; + outData->VertAccuracy = (ODID_Vertical_accuracy_t) inEncoded->VertAccuracy; + outData->BaroAccuracy = (ODID_Vertical_accuracy_t) inEncoded->BaroAccuracy; + outData->SpeedAccuracy = (ODID_Speed_accuracy_t) inEncoded->SpeedAccuracy; + outData->TSAccuracy = (ODID_Timestamp_accuracy_t) inEncoded->TSAccuracy; + outData->TimeStamp = decodeTimeStamp(inEncoded->TimeStamp); + return ODID_SUCCESS; +} + +/** +* Get the page number of the authorization message +* +* @param inEncoded Input message (encoded/packed) structure +* @param pageNum Output: The page number of this authorization message +* @return ODID_SUCCESS or ODID_FAIL; +*/ +int getAuthPageNum(ODID_Auth_encoded *inEncoded, int *pageNum) +{ + if (!inEncoded || !pageNum || + inEncoded->page_zero.MessageType != ODID_MESSAGETYPE_AUTH || + !intInRange(inEncoded->page_zero.AuthType, 0, 15) || + !intInRange(inEncoded->page_zero.DataPage, 0, ODID_AUTH_MAX_PAGES - 1)) + return ODID_FAIL; + + *pageNum = inEncoded->page_zero.DataPage; + return ODID_SUCCESS; +} + +/** +* Decode Auth data from packed message +* +* @param outData Output: decoded message +* @param inEncoded Input message (encoded/packed) structure +* @return ODID_SUCCESS or ODID_FAIL; +*/ +int decodeAuthMessage(ODID_Auth_data *outData, ODID_Auth_encoded *inEncoded) +{ + if (!outData || !inEncoded || + inEncoded->page_zero.MessageType != ODID_MESSAGETYPE_AUTH || + !intInRange(inEncoded->page_zero.AuthType, 0, 15) || + !intInRange(inEncoded->page_zero.DataPage, 0, ODID_AUTH_MAX_PAGES - 1)) + return ODID_FAIL; + + if (inEncoded->page_zero.DataPage == 0) { + if (inEncoded->page_zero.LastPageIndex >= ODID_AUTH_MAX_PAGES) + return ODID_FAIL; + +#if (MAX_AUTH_LENGTH < UINT8_MAX) + if (inEncoded->page_zero.Length > MAX_AUTH_LENGTH) + return ODID_FAIL; +#endif + + int len = ODID_AUTH_PAGE_ZERO_DATA_SIZE + + inEncoded->page_zero.LastPageIndex * ODID_AUTH_PAGE_NONZERO_DATA_SIZE; + if (len < inEncoded->page_zero.Length) + return ODID_FAIL; + } + + outData->AuthType = (ODID_authtype_t) inEncoded->page_zero.AuthType; + outData->DataPage = inEncoded->page_zero.DataPage; + if (inEncoded->page_zero.DataPage == 0) { + outData->LastPageIndex = inEncoded->page_zero.LastPageIndex; + outData->Length = inEncoded->page_zero.Length; + outData->Timestamp = inEncoded->page_zero.Timestamp; + memset(outData->AuthData, 0, sizeof(outData->AuthData)); + memcpy(outData->AuthData, inEncoded->page_zero.AuthData, + ODID_AUTH_PAGE_ZERO_DATA_SIZE); + } else { + memset(outData->AuthData, 0, sizeof(outData->AuthData)); + memcpy(outData->AuthData, inEncoded->page_non_zero.AuthData, + ODID_AUTH_PAGE_NONZERO_DATA_SIZE); + } + + return ODID_SUCCESS; +} + +/** +* Decode Self ID data from packed message +* +* @param outData Output: decoded message +* @param inEncoded Input message (encoded/packed) structure +* @return ODID_SUCCESS or ODID_FAIL; +*/ +int decodeSelfIDMessage(ODID_SelfID_data *outData, ODID_SelfID_encoded *inEncoded) +{ + if (!outData || !inEncoded || + inEncoded->MessageType != ODID_MESSAGETYPE_SELF_ID) + return ODID_FAIL; + + outData->DescType = (ODID_desctype_t) inEncoded->DescType; + safe_dec_copyfill(outData->Desc, inEncoded->Desc, sizeof(outData->Desc)); + return ODID_SUCCESS; +} + +/** +* Decode System data from packed message +* +* @param outData Output: decoded message +* @param inEncoded Input message (encoded/packed) structure +* @return ODID_SUCCESS or ODID_FAIL; +*/ +int decodeSystemMessage(ODID_System_data *outData, ODID_System_encoded *inEncoded) +{ + if (!outData || !inEncoded || + inEncoded->MessageType != ODID_MESSAGETYPE_SYSTEM) + return ODID_FAIL; + + outData->OperatorLocationType = + (ODID_operator_location_type_t) inEncoded->OperatorLocationType; + outData->ClassificationType = + (ODID_classification_type_t) inEncoded->ClassificationType; + outData->OperatorLatitude = decodeLatLon(inEncoded->OperatorLatitude); + outData->OperatorLongitude = decodeLatLon(inEncoded->OperatorLongitude); + outData->AreaCount = inEncoded->AreaCount; + outData->AreaRadius = decodeAreaRadius(inEncoded->AreaRadius); + outData->AreaCeiling = decodeAltitude(inEncoded->AreaCeiling); + outData->AreaFloor = decodeAltitude(inEncoded->AreaFloor); + outData->CategoryEU = (ODID_category_EU_t) inEncoded->CategoryEU; + outData->ClassEU = (ODID_class_EU_t) inEncoded->ClassEU; + outData->OperatorAltitudeGeo = decodeAltitude(inEncoded->OperatorAltitudeGeo); + outData->Timestamp = inEncoded->Timestamp; + return ODID_SUCCESS; +} + +/** +* Decode Operator ID data from packed message +* +* @param outData Output: decoded message +* @param inEncoded Input message (encoded/packed) structure +* @return ODID_SUCCESS or ODID_FAIL; +*/ +int decodeOperatorIDMessage(ODID_OperatorID_data *outData, ODID_OperatorID_encoded *inEncoded) +{ + if (!outData || !inEncoded || + inEncoded->MessageType != ODID_MESSAGETYPE_OPERATOR_ID) + return ODID_FAIL; + + outData->OperatorIdType = (ODID_operatorIdType_t) inEncoded->OperatorIdType; + safe_dec_copyfill(outData->OperatorId, inEncoded->OperatorId, sizeof(outData->OperatorId)); + return ODID_SUCCESS; +} + +/** +* Decode Message Pack from packed message +* +* The various Valid flags in uasData are set true whenever a message has been +* decoded and the corresponding data structure has been filled. The caller must +* clear these flags before calling decodeMessagePack(). +* +* @param uasData Output: Structure containing buffers for all message data +* @param pack Pointer to an encoded packed message +* @return ODID_SUCCESS or ODID_FAIL; +*/ +int decodeMessagePack(ODID_UAS_Data *uasData, ODID_MessagePack_encoded *pack) +{ + if (!uasData || !pack || pack->MessageType != ODID_MESSAGETYPE_PACKED) + return ODID_FAIL; + + if (pack->SingleMessageSize != ODID_MESSAGE_SIZE) + return ODID_FAIL; + + if (checkPackContent(pack->Messages, pack->MsgPackSize) != ODID_SUCCESS) + return ODID_FAIL; + + for (int i = 0; i < pack->MsgPackSize; i++) { + decodeOpenDroneID(uasData, pack->Messages[i].rawData); + } + return ODID_SUCCESS; +} + +/** +* Decodes the message type of a packed Open Drone ID message +* +* @param byte The first byte of the message +* @return The message type: ODID_messagetype_t +*/ +ODID_messagetype_t decodeMessageType(uint8_t byte) +{ + switch (byte >> 4) + { + case ODID_MESSAGETYPE_BASIC_ID: + return ODID_MESSAGETYPE_BASIC_ID; + case ODID_MESSAGETYPE_LOCATION: + return ODID_MESSAGETYPE_LOCATION; + case ODID_MESSAGETYPE_AUTH: + return ODID_MESSAGETYPE_AUTH; + case ODID_MESSAGETYPE_SELF_ID: + return ODID_MESSAGETYPE_SELF_ID; + case ODID_MESSAGETYPE_SYSTEM: + return ODID_MESSAGETYPE_SYSTEM; + case ODID_MESSAGETYPE_OPERATOR_ID: + return ODID_MESSAGETYPE_OPERATOR_ID; + case ODID_MESSAGETYPE_PACKED: + return ODID_MESSAGETYPE_PACKED; + default: + return ODID_MESSAGETYPE_INVALID; + } +} + +/** +* Parse encoded Open Drone ID data to identify the message type. Then decode +* from Open Drone ID packed format into the appropriate Open Drone ID structure +* +* This function assumes that msgData points to a buffer containing all +* ODID_MESSAGE_SIZE bytes of an Open Drone ID message. +* +* The various Valid flags in uasData are set true whenever a message has been +* decoded and the corresponding data structure has been filled. The caller must +* clear these flags before calling decodeOpenDroneID(). +* +* @param uasData Structure containing buffers for all message data +* @param msgData Pointer to a buffer containing a full encoded Open Drone ID +* message +* @return The message type: ODID_messagetype_t +*/ +ODID_messagetype_t decodeOpenDroneID(ODID_UAS_Data *uasData, uint8_t *msgData) +{ + if (!uasData || !msgData) + return ODID_MESSAGETYPE_INVALID; + + switch (decodeMessageType(msgData[0])) + { + case ODID_MESSAGETYPE_BASIC_ID: { + ODID_BasicID_encoded *basicId = (ODID_BasicID_encoded *) msgData; + enum ODID_idtype idType; + if (getBasicIDType(basicId, &idType) == ODID_SUCCESS) { + // Find a free slot to store the current message in or overwrite old data of the same type. + for (int i = 0; i < ODID_BASIC_ID_MAX_MESSAGES; i++) { + enum ODID_idtype storedType = uasData->BasicID[i].IDType; + if (storedType == ODID_IDTYPE_NONE || storedType == idType) { + if (decodeBasicIDMessage(&uasData->BasicID[i], basicId) == ODID_SUCCESS) { + uasData->BasicIDValid[i] = 1; + return ODID_MESSAGETYPE_BASIC_ID; + } + } + } + } + break; + } + case ODID_MESSAGETYPE_LOCATION: { + ODID_Location_encoded *location = (ODID_Location_encoded *) msgData; + if (decodeLocationMessage(&uasData->Location, location) == ODID_SUCCESS) { + uasData->LocationValid = 1; + return ODID_MESSAGETYPE_LOCATION; + } + break; + } + case ODID_MESSAGETYPE_AUTH: { + ODID_Auth_encoded *auth = (ODID_Auth_encoded *) msgData; + int pageNum; + if (getAuthPageNum(auth, &pageNum) == ODID_SUCCESS) { + ODID_Auth_data *authData = &uasData->Auth[pageNum]; + if (decodeAuthMessage(authData, auth) == ODID_SUCCESS) { + uasData->AuthValid[pageNum] = 1; + return ODID_MESSAGETYPE_AUTH; + } + } + break; + } + case ODID_MESSAGETYPE_SELF_ID: { + ODID_SelfID_encoded *selfId = (ODID_SelfID_encoded *) msgData; + if (decodeSelfIDMessage(&uasData->SelfID, selfId) == ODID_SUCCESS) { + uasData->SelfIDValid = 1; + return ODID_MESSAGETYPE_SELF_ID; + } + break; + } + case ODID_MESSAGETYPE_SYSTEM: { + ODID_System_encoded *system = (ODID_System_encoded *) msgData; + if (decodeSystemMessage(&uasData->System, system) == ODID_SUCCESS) { + uasData->SystemValid = 1; + return ODID_MESSAGETYPE_SYSTEM; + } + break; + } + case ODID_MESSAGETYPE_OPERATOR_ID: { + ODID_OperatorID_encoded *operatorId = (ODID_OperatorID_encoded *) msgData; + if (decodeOperatorIDMessage(&uasData->OperatorID, operatorId) == ODID_SUCCESS) { + uasData->OperatorIDValid = 1; + return ODID_MESSAGETYPE_OPERATOR_ID; + } + break; + } + case ODID_MESSAGETYPE_PACKED: { + ODID_MessagePack_encoded *pack = (ODID_MessagePack_encoded *) msgData; + if (decodeMessagePack(uasData, pack) == ODID_SUCCESS) + return ODID_MESSAGETYPE_PACKED; + break; + } + default: + break; + } + + return ODID_MESSAGETYPE_INVALID; +} + +/** +* Safely fill then copy string to destination (when decoding) +* +* This prevents overrun and guarantees copy behavior (fully null padded) +* This function was specially made because the encoded data may not be null +* terminated (if full size). +* Therefore, the destination must use the last byte for a null (and is +1 in size) +* +* @param dstStr Destination string +* @param srcStr Source string +* @param dstSize Destination size +*/ +static char *safe_dec_copyfill(char *dstStr, const char *srcStr, int dstSize) +{ + memset(dstStr, 0, dstSize); // fills destination with nulls + strncpy(dstStr, srcStr, dstSize-1); // copy only up to dst size-1 (no overruns) + return dstStr; +} + +/** +* Safely range check a value and return the minimum or max within the range if exceeded +* +* @param inValue Value to range-check +* @param startRange Start of range to compare +* @param endRange End of range to compare +* @return same value if it fits, otherwise, min or max of range as appropriate. +*/ +static int intRangeMax(int64_t inValue, int startRange, int endRange) { + if ( inValue < startRange ) { + return startRange; + } else if (inValue > endRange) { + return endRange; + } else { + return (int) inValue; + } +} + +/** + * Determine if an Int is in range + * + * @param inValue Value to range-check + * @param startRange Start of range to compare + * @param endRange End of range to compare + * @return 1 = yes, 0 = no + */ +static int intInRange(int inValue, int startRange, int endRange) +{ + if (inValue < startRange || inValue > endRange) { + return 0; + } else { + return 1; + } +} + +/** +* This converts a horizontal accuracy float value to the corresponding enum +* +* @param Accuracy The horizontal accuracy in meters +* @return Enum value representing the accuracy +*/ +ODID_Horizontal_accuracy_t createEnumHorizontalAccuracy(float Accuracy) +{ + if (Accuracy >= 18520) + return ODID_HOR_ACC_UNKNOWN; + else if (Accuracy >= 7408) + return ODID_HOR_ACC_10NM; + else if (Accuracy >= 3704) + return ODID_HOR_ACC_4NM; + else if (Accuracy >= 1852) + return ODID_HOR_ACC_2NM; + else if (Accuracy >= 926) + return ODID_HOR_ACC_1NM; + else if (Accuracy >= 555.6f) + return ODID_HOR_ACC_0_5NM; + else if (Accuracy >= 185.2f) + return ODID_HOR_ACC_0_3NM; + else if (Accuracy >= 92.6f) + return ODID_HOR_ACC_0_1NM; + else if (Accuracy >= 30) + return ODID_HOR_ACC_0_05NM; + else if (Accuracy >= 10) + return ODID_HOR_ACC_30_METER; + else if (Accuracy >= 3) + return ODID_HOR_ACC_10_METER; + else if (Accuracy >= 1) + return ODID_HOR_ACC_3_METER; + else if (Accuracy > 0) + return ODID_HOR_ACC_1_METER; + else + return ODID_HOR_ACC_UNKNOWN; +} + +/** +* This converts a vertical accuracy float value to the corresponding enum +* +* @param Accuracy The vertical accuracy in meters +* @return Enum value representing the accuracy +*/ +ODID_Vertical_accuracy_t createEnumVerticalAccuracy(float Accuracy) +{ + if (Accuracy >= 150) + return ODID_VER_ACC_UNKNOWN; + else if (Accuracy >= 45) + return ODID_VER_ACC_150_METER; + else if (Accuracy >= 25) + return ODID_VER_ACC_45_METER; + else if (Accuracy >= 10) + return ODID_VER_ACC_25_METER; + else if (Accuracy >= 3) + return ODID_VER_ACC_10_METER; + else if (Accuracy >= 1) + return ODID_VER_ACC_3_METER; + else if (Accuracy > 0) + return ODID_VER_ACC_1_METER; + else + return ODID_VER_ACC_UNKNOWN; +} + +/** +* This converts a speed accuracy float value to the corresponding enum +* +* @param Accuracy The speed accuracy in m/s +* @return Enum value representing the accuracy +*/ +ODID_Speed_accuracy_t createEnumSpeedAccuracy(float Accuracy) +{ + if (Accuracy >= 10) + return ODID_SPEED_ACC_UNKNOWN; + else if (Accuracy >= 3) + return ODID_SPEED_ACC_10_METERS_PER_SECOND; + else if (Accuracy >= 1) + return ODID_SPEED_ACC_3_METERS_PER_SECOND; + else if (Accuracy >= 0.3f) + return ODID_SPEED_ACC_1_METERS_PER_SECOND; + else if (Accuracy > 0) + return ODID_SPEED_ACC_0_3_METERS_PER_SECOND; + else + return ODID_SPEED_ACC_UNKNOWN; +} + +/** +* This converts a timestamp accuracy float value to the corresponding enum +* +* @param Accuracy The timestamp accuracy in seconds +* @return Enum value representing the accuracy +*/ +ODID_Timestamp_accuracy_t createEnumTimestampAccuracy(float Accuracy) +{ + if (Accuracy > 1.5f) + return ODID_TIME_ACC_UNKNOWN; + else if (Accuracy > 1.4f) + return ODID_TIME_ACC_1_5_SECOND; + else if (Accuracy > 1.3f) + return ODID_TIME_ACC_1_4_SECOND; + else if (Accuracy > 1.2f) + return ODID_TIME_ACC_1_3_SECOND; + else if (Accuracy > 1.1f) + return ODID_TIME_ACC_1_2_SECOND; + else if (Accuracy > 1.0f) + return ODID_TIME_ACC_1_1_SECOND; + else if (Accuracy > 0.9f) + return ODID_TIME_ACC_1_0_SECOND; + else if (Accuracy > 0.8f) + return ODID_TIME_ACC_0_9_SECOND; + else if (Accuracy > 0.7f) + return ODID_TIME_ACC_0_8_SECOND; + else if (Accuracy > 0.6f) + return ODID_TIME_ACC_0_7_SECOND; + else if (Accuracy > 0.5f) + return ODID_TIME_ACC_0_6_SECOND; + else if (Accuracy > 0.4f) + return ODID_TIME_ACC_0_5_SECOND; + else if (Accuracy > 0.3f) + return ODID_TIME_ACC_0_4_SECOND; + else if (Accuracy > 0.2f) + return ODID_TIME_ACC_0_3_SECOND; + else if (Accuracy > 0.1f) + return ODID_TIME_ACC_0_2_SECOND; + else if (Accuracy > 0.0f) + return ODID_TIME_ACC_0_1_SECOND; + else + return ODID_TIME_ACC_UNKNOWN; +} + +/** +* This decodes a horizontal accuracy enum to the corresponding float value +* +* @param Accuracy Enum value representing the accuracy +* @return The maximum horizontal accuracy in meters +*/ +float decodeHorizontalAccuracy(ODID_Horizontal_accuracy_t Accuracy) +{ + switch (Accuracy) + { + case ODID_HOR_ACC_UNKNOWN: + return 18520; + case ODID_HOR_ACC_10NM: + return 18520; + case ODID_HOR_ACC_4NM: + return 7808; + case ODID_HOR_ACC_2NM: + return 3704; + case ODID_HOR_ACC_1NM: + return 1852; + case ODID_HOR_ACC_0_5NM: + return 926; + case ODID_HOR_ACC_0_3NM: + return 555.6f; + case ODID_HOR_ACC_0_1NM: + return 185.2f; + case ODID_HOR_ACC_0_05NM: + return 92.6f; + case ODID_HOR_ACC_30_METER: + return 30; + case ODID_HOR_ACC_10_METER: + return 10; + case ODID_HOR_ACC_3_METER: + return 3; + case ODID_HOR_ACC_1_METER: + return 1; + default: + return 18520; + } +} + +/** +* This decodes a vertical accuracy enum to the corresponding float value +* +* @param Accuracy Enum value representing the accuracy +* @return The maximum vertical accuracy in meters +*/ +float decodeVerticalAccuracy(ODID_Vertical_accuracy_t Accuracy) +{ + switch (Accuracy) + { + case ODID_VER_ACC_UNKNOWN: + return 150; + case ODID_VER_ACC_150_METER: + return 150; + case ODID_VER_ACC_45_METER: + return 45; + case ODID_VER_ACC_25_METER: + return 25; + case ODID_VER_ACC_10_METER: + return 10; + case ODID_VER_ACC_3_METER: + return 3; + case ODID_VER_ACC_1_METER: + return 1; + default: + return 150; + } +} + +/** +* This decodes a speed accuracy enum to the corresponding float value +* +* @param Accuracy Enum value representing the accuracy +* @return The maximum speed accuracy in m/s +*/ +float decodeSpeedAccuracy(ODID_Speed_accuracy_t Accuracy) +{ + switch (Accuracy) + { + case ODID_SPEED_ACC_UNKNOWN: + return 10; + case ODID_SPEED_ACC_10_METERS_PER_SECOND: + return 10; + case ODID_SPEED_ACC_3_METERS_PER_SECOND: + return 3; + case ODID_SPEED_ACC_1_METERS_PER_SECOND: + return 1; + case ODID_SPEED_ACC_0_3_METERS_PER_SECOND: + return 0.3f; + default: + return 10; + } +} + +/** +* This decodes a timestamp accuracy enum to the corresponding float value +* +* @param Accuracy Enum value representing the accuracy +* @return The maximum timestamp accuracy in seconds +*/ +float decodeTimestampAccuracy(ODID_Timestamp_accuracy_t Accuracy) +{ + switch (Accuracy) + { + case ODID_TIME_ACC_UNKNOWN: + return 0.0f; + case ODID_TIME_ACC_0_1_SECOND: + return 0.1f; + case ODID_TIME_ACC_0_2_SECOND: + return 0.2f; + case ODID_TIME_ACC_0_3_SECOND: + return 0.3f; + case ODID_TIME_ACC_0_4_SECOND: + return 0.4f; + case ODID_TIME_ACC_0_5_SECOND: + return 0.5f; + case ODID_TIME_ACC_0_6_SECOND: + return 0.6f; + case ODID_TIME_ACC_0_7_SECOND: + return 0.7f; + case ODID_TIME_ACC_0_8_SECOND: + return 0.8f; + case ODID_TIME_ACC_0_9_SECOND: + return 0.9f; + case ODID_TIME_ACC_1_0_SECOND: + return 1.0f; + case ODID_TIME_ACC_1_1_SECOND: + return 1.1f; + case ODID_TIME_ACC_1_2_SECOND: + return 1.2f; + case ODID_TIME_ACC_1_3_SECOND: + return 1.3f; + case ODID_TIME_ACC_1_4_SECOND: + return 1.4f; + case ODID_TIME_ACC_1_5_SECOND: + return 1.5f; + default: + return 0.0f; + } +} + +#ifndef ODID_DISABLE_PRINTF + +/** +* Print array of bytes as a hex string +* +* @param byteArray Array of bytes to be printed +* @param asize Size of array of bytes to be printed +*/ + +void printByteArray(uint8_t *byteArray, uint16_t asize, int spaced) +{ + if (ENABLE_DEBUG) { + int x; + for (x=0;xUASID, ODID_ID_SIZE); + + const char ODID_BasicID_data_format[] = + "UAType: %d\nIDType: %d\nUASID: %s\n"; + printf(ODID_BasicID_data_format, BasicID->IDType, BasicID->UAType, buf); +} + +/** +* Print formatted Location Data +* +* @param Location structure to be printed +*/ +void printLocation_data(ODID_Location_data *Location) +{ + const char ODID_Location_data_format[] = + "Status: %d\nDirection: %.1f\nSpeedHori: %.2f\nSpeedVert: "\ + "%.2f\nLat/Lon: %.7f, %.7f\nAlt: Baro, Geo, Height above %s: %.2f, "\ + "%.2f, %.2f\nHoriz, Vert, Baro, Speed, TS Accuracy: %.1f, %.1f, %.1f, "\ + "%.1f, %.1f\nTimeStamp: %.2f\n"; + printf(ODID_Location_data_format, Location->Status, + (double) Location->Direction, (double) Location->SpeedHorizontal, + (double) Location->SpeedVertical, Location->Latitude, + Location->Longitude, Location->HeightType ? "Ground" : "TakeOff", + (double) Location->AltitudeBaro, (double) Location->AltitudeGeo, + (double) Location->Height, + (double) decodeHorizontalAccuracy(Location->HorizAccuracy), + (double) decodeVerticalAccuracy(Location->VertAccuracy), + (double) decodeVerticalAccuracy(Location->BaroAccuracy), + (double) decodeSpeedAccuracy(Location->SpeedAccuracy), + (double) decodeTimestampAccuracy(Location->TSAccuracy), + (double) Location->TimeStamp); +} + +/** +* Print formatted Auth Data +* +* @param Auth structure to be printed +*/ +void printAuth_data(ODID_Auth_data *Auth) +{ + if (Auth->DataPage == 0) { + const char ODID_Auth_data_format[] = + "AuthType: %d\nDataPage: %d\nLastPageIndex: %d\nLength: %d\n"\ + "Timestamp: %u\nAuthData: "; + printf(ODID_Auth_data_format, Auth->AuthType, Auth->DataPage, + Auth->LastPageIndex, Auth->Length, Auth->Timestamp); + for (int i = 0; i < ODID_AUTH_PAGE_ZERO_DATA_SIZE; i++) + printf("0x%02X ", Auth->AuthData[i]); + } else { + const char ODID_Auth_data_format[] = + "AuthType: %d\nDataPage: %d\nAuthData: "; + printf(ODID_Auth_data_format, Auth->AuthType, Auth->DataPage); + for (int i = 0; i < ODID_AUTH_PAGE_NONZERO_DATA_SIZE; i++) + printf("0x%02X ", Auth->AuthData[i]); + } + printf("\n"); +} + +/** +* Print formatted SelfID Data +* +* @param SelfID structure to be printed +*/ +void printSelfID_data(ODID_SelfID_data *SelfID) +{ + // Ensure the description is null-terminated + char buf[ODID_STR_SIZE + 1] = { 0 }; + memcpy(buf, SelfID->Desc, ODID_STR_SIZE); + + const char ODID_SelfID_data_format[] = "DescType: %d\nDesc: %s\n"; + printf(ODID_SelfID_data_format, SelfID->DescType, buf); +} + +/** +* Print formatted System Data +* +* @param System_data structure to be printed +*/ +void printSystem_data(ODID_System_data *System_data) +{ + const char ODID_System_data_format[] = "Operator Location Type: %d\n" + "Classification Type: %d\nLat/Lon: %.7f, %.7f\n" + "Area Count, Radius, Ceiling, Floor: %d, %d, %.2f, %.2f\n" + "Category EU: %d, Class EU: %d, Altitude: %.2f, Timestamp: %u\n"; + printf(ODID_System_data_format, System_data->OperatorLocationType, + System_data->ClassificationType, + System_data->OperatorLatitude, System_data->OperatorLongitude, + System_data->AreaCount, System_data->AreaRadius, + (double) System_data->AreaCeiling, (double) System_data->AreaFloor, + System_data->CategoryEU, System_data->ClassEU, + (double) System_data->OperatorAltitudeGeo, System_data->Timestamp); +} + +/** +* Print formatted OperatorID Data +* +* @param OperatorID structure to be printed +*/ +void printOperatorID_data(ODID_OperatorID_data *operatorID) +{ + // Ensure the ID is null-terminated + char buf[ODID_ID_SIZE + 1] = { 0 }; + memcpy(buf, operatorID->OperatorId, ODID_ID_SIZE); + + const char ODID_OperatorID_data_format[] = + "OperatorIdType: %d\nOperatorId: %s\n"; + printf(ODID_OperatorID_data_format, operatorID->OperatorIdType, buf); +} + +#endif // ODID_DISABLE_PRINTF diff --git a/opendroneid.h b/opendroneid.h new file mode 100644 index 0000000..00f32ae --- /dev/null +++ b/opendroneid.h @@ -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 +#include + +#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_ diff --git a/utm.cpp b/utm.cpp new file mode 100644 index 0000000..407bfe5 --- /dev/null +++ b/utm.cpp @@ -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 + +#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'; +} + +/* + * + */ + + \ No newline at end of file diff --git a/utm.h b/utm.h new file mode 100644 index 0000000..1802bca --- /dev/null +++ b/utm.h @@ -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 \ No newline at end of file diff --git a/wifi.c b/wifi.c new file mode 100644 index 0000000..59c8f3a --- /dev/null +++ b/wifi.c @@ -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 +int clock_gettime(clockid_t, struct timespec *); +#else +#include +#include +#include +#endif + +#include +#include + +#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; +}