From 9689ae8ab054f2f5137b63cfbd708e07add48606 Mon Sep 17 00:00:00 2001 From: Sumit Batra Date: Tue, 26 Nov 2024 15:01:08 +0530 Subject: [PATCH] Bluetooth: OpenDroneID Transmitter Application This application is a standard way to Identify UAVs Co-authored-by: Mayank Mahajan Signed-off-by: Sumit Batra --- samples/bluetooth/opendroneid/CMakeLists.txt | 8 + samples/bluetooth/opendroneid/README.rst | 30 + samples/bluetooth/opendroneid/prj.conf | 3 + samples/bluetooth/opendroneid/sample.yaml | 9 + samples/bluetooth/opendroneid/src/main.c | 281 +++ .../bluetooth/opendroneid/src/opendroneid.c | 1555 +++++++++++++++++ .../bluetooth/opendroneid/src/opendroneid.h | 804 +++++++++ 7 files changed, 2690 insertions(+) create mode 100644 samples/bluetooth/opendroneid/CMakeLists.txt create mode 100644 samples/bluetooth/opendroneid/README.rst create mode 100644 samples/bluetooth/opendroneid/prj.conf create mode 100644 samples/bluetooth/opendroneid/sample.yaml create mode 100644 samples/bluetooth/opendroneid/src/main.c create mode 100644 samples/bluetooth/opendroneid/src/opendroneid.c create mode 100644 samples/bluetooth/opendroneid/src/opendroneid.h diff --git a/samples/bluetooth/opendroneid/CMakeLists.txt b/samples/bluetooth/opendroneid/CMakeLists.txt new file mode 100644 index 000000000000000..b76c24d684dcb76 --- /dev/null +++ b/samples/bluetooth/opendroneid/CMakeLists.txt @@ -0,0 +1,8 @@ +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20.0) +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(opendroneid) + +target_sources(app PRIVATE src/main.c) +target_sources(app PRIVATE src/opendroneid.c) diff --git a/samples/bluetooth/opendroneid/README.rst b/samples/bluetooth/opendroneid/README.rst new file mode 100644 index 000000000000000..6284b6a0dcd8ab4 --- /dev/null +++ b/samples/bluetooth/opendroneid/README.rst @@ -0,0 +1,30 @@ +.. zephyr:code-sample:: bluetooth_opendroneid + :name: OpenDroneID + :relevant-api: bluetooth + + Advertise ODID Messages using GAP Broadcaster role. + +Overview +******** + +Open Drone ID messages let's you distinguish between the Unmanned Aerial Vehicles. +It also has UAV's real-time location/altitude, UA serial number, operator ID/location etc. +The message format is defined in the ASTM F3411 Remote ID and the +ASD-STAN prEN 4709-002 Direct Remote ID specifications. + +This application demonstrates ODID Message transmission over Bluetooth in GAP Broadcaster role. +Currently this program supports transmitting static drone data via Bluetooth Beacon. +But this could easily be extended to simulate a bit more dynamic flight example. + +Requirements +************ + +* BlueZ running on the host, or +* A board with Bluetooth LE support + +Building and Running +******************** + +This sample can be found under :zephyr_file:`samples/bluetooth/opendroneid` in the Zephyr tree. + +See :zephyr:code-sample-category:`bluetooth` samples for details. diff --git a/samples/bluetooth/opendroneid/prj.conf b/samples/bluetooth/opendroneid/prj.conf new file mode 100644 index 000000000000000..3514aeae115704b --- /dev/null +++ b/samples/bluetooth/opendroneid/prj.conf @@ -0,0 +1,3 @@ +CONFIG_BT=y +CONFIG_LOG=y +CONFIG_BT_DEVICE_NAME="Test opendroneid" diff --git a/samples/bluetooth/opendroneid/sample.yaml b/samples/bluetooth/opendroneid/sample.yaml new file mode 100644 index 000000000000000..199ff46e4e5db90 --- /dev/null +++ b/samples/bluetooth/opendroneid/sample.yaml @@ -0,0 +1,9 @@ +sample: + name: Bluetooth ODID +tests: + sample.bluetooth.opendroneid: + harness: bluetooth + platform_allow: + - qemu_x86 + - kw45b41z_evk + tags: bluetooth diff --git a/samples/bluetooth/opendroneid/src/main.c b/samples/bluetooth/opendroneid/src/main.c new file mode 100644 index 000000000000000..bed5da4571b2317 --- /dev/null +++ b/samples/bluetooth/opendroneid/src/main.c @@ -0,0 +1,281 @@ +/* SPDX-License-Identifier: Apache-2.0 + * + * Copyright (C) 2021, Soren Friis + * Copyright 2024 NXP + * + * Referred from https://github.com/opendroneid/transmitter-linux + */ + +#include +#include +#include + +#include +#include + +#include "opendroneid.h" + +#define DEVICE_NAME CONFIG_BT_DEVICE_NAME +#define DEVICE_NAME_LEN (sizeof(DEVICE_NAME) - 1) + +#define BT_LE_ADV_PARAM_NCONN BT_LE_ADV_PARAM(BT_LE_ADV_OPT_USE_IDENTITY, 0x20, 0x20, NULL) + +static uint8_t payload[29] = { + 0xFA, 0xFF, /* 0xFFFA = ASTM International, ASTM Remote ID. */ + 0x0D, /* AD Application Code within the ASTM address space = ODID. */ + 0x00, /* message counter starting at 0x00 and wrapping around at 0xFF. */ + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 /* 25-bytes data */ +}; + +static const struct bt_data ad[] = { + { + .type = BT_DATA_SVC_DATA16, + .data_len = ARRAY_SIZE(payload), + .data = payload, + }, +}; + +#define MINIMUM(a, b) (((a) < (b)) ? (a) : (b)) + +#define BASIC_ID_POS_ZERO 0 +#define BASIC_ID_POS_ONE 1 + +static void fill_example_data(struct ODID_UAS_Data *uasData) +{ + uasData->BasicID[BASIC_ID_POS_ZERO].UAType = ODID_UATYPE_HELICOPTER_OR_MULTIROTOR; + uasData->BasicID[BASIC_ID_POS_ZERO].IDType = ODID_IDTYPE_SERIAL_NUMBER; + char uas_id[] = "112624150A90E3AE1EC0"; + + strncpy(uasData->BasicID[BASIC_ID_POS_ZERO].UASID, uas_id, + MINIMUM(sizeof(uas_id), sizeof(uasData->BasicID[BASIC_ID_POS_ZERO].UASID))); + + uasData->BasicID[BASIC_ID_POS_ONE].UAType = ODID_UATYPE_HELICOPTER_OR_MULTIROTOR; + uasData->BasicID[BASIC_ID_POS_ONE].IDType = ODID_IDTYPE_SPECIFIC_SESSION_ID; + char uas_caa_id[] = "FD3454B778E565C24B70"; + + strncpy(uasData->BasicID[BASIC_ID_POS_ONE].UASID, uas_caa_id, + MINIMUM(sizeof(uas_caa_id), sizeof(uasData->BasicID[BASIC_ID_POS_ONE].UASID))); + + uasData->Auth[0].AuthType = ODID_AUTH_UAS_ID_SIGNATURE; + uasData->Auth[0].DataPage = 0; + uasData->Auth[0].LastPageIndex = 2; + uasData->Auth[0].Length = 63; + uasData->Auth[0].Timestamp = 28000000; + char auth0_data[] = "12345678901234567"; + + memcpy(uasData->Auth[0].AuthData, auth0_data, + MINIMUM(sizeof(auth0_data), sizeof(uasData->Auth[0].AuthData))); + + uasData->Auth[1].AuthType = ODID_AUTH_UAS_ID_SIGNATURE; + uasData->Auth[1].DataPage = 1; + char auth1_data[] = "12345678901234567890123"; + + memcpy(uasData->Auth[1].AuthData, auth1_data, + MINIMUM(sizeof(auth1_data), sizeof(uasData->Auth[1].AuthData))); + + uasData->Auth[2].AuthType = ODID_AUTH_UAS_ID_SIGNATURE; + uasData->Auth[2].DataPage = 2; + char auth2_data[] = "12345678901234567890123"; + + memcpy(uasData->Auth[2].AuthData, auth2_data, + MINIMUM(sizeof(auth2_data), sizeof(uasData->Auth[2].AuthData))); + + uasData->SelfID.DescType = ODID_DESC_TYPE_TEXT; + char description[] = "Drone ID test flight---"; + + strncpy(uasData->SelfID.Desc, description, + MINIMUM(sizeof(description), sizeof(uasData->SelfID.Desc))); + + uasData->System.OperatorLocationType = ODID_OPERATOR_LOCATION_TYPE_TAKEOFF; + uasData->System.ClassificationType = ODID_CLASSIFICATION_TYPE_EU; + uasData->System.OperatorLatitude = uasData->Location.Latitude + 0.001; + uasData->System.OperatorLongitude = uasData->Location.Longitude - 0.001; + uasData->System.AreaCount = 1; + uasData->System.AreaRadius = 0; + uasData->System.AreaCeiling = 0; + uasData->System.AreaFloor = 0; + uasData->System.CategoryEU = ODID_CATEGORY_EU_OPEN; + uasData->System.ClassEU = ODID_CLASS_EU_CLASS_1; + uasData->System.OperatorAltitudeGeo = 20.5f; + uasData->System.Timestamp = 28056789; + + uasData->OperatorID.OperatorIdType = ODID_OPERATOR_ID; + char operatorId[] = "FIN87astrdge12k8"; + + strncpy(uasData->OperatorID.OperatorId, operatorId, + MINIMUM(sizeof(operatorId), sizeof(uasData->OperatorID.OperatorId))); +} + +static void fill_example_gps_data(struct ODID_UAS_Data *uasData) +{ + uasData->Location.Status = ODID_STATUS_AIRBORNE; + uasData->Location.Direction = 361.f; + uasData->Location.SpeedHorizontal = 0.0f; + uasData->Location.SpeedVertical = 0.35f; + uasData->Location.Latitude = 51.4791; + uasData->Location.Longitude = -0.0013; + uasData->Location.AltitudeBaro = 100; + uasData->Location.AltitudeGeo = 110; + uasData->Location.HeightType = ODID_HEIGHT_REF_OVER_GROUND; + uasData->Location.Height = 80; + uasData->Location.HorizAccuracy = createEnumHorizontalAccuracy(5.5f); + uasData->Location.VertAccuracy = createEnumVerticalAccuracy(9.5f); + uasData->Location.BaroAccuracy = createEnumVerticalAccuracy(0.5f); + uasData->Location.SpeedAccuracy = createEnumSpeedAccuracy(0.5f); + uasData->Location.TSAccuracy = createEnumTimestampAccuracy(0.1f); + uasData->Location.TimeStamp = 360.52f; +} + +static struct ODID_UAS_Data uasData; +static union ODID_Message_encoded encoded; +static uint8_t msg_counters[ODID_MSG_COUNTER_AMOUNT]; + +static void bt_ready(int err) +{ + char addr_s[BT_ADDR_LE_STR_LEN]; + bt_addr_le_t addr = {0}; + size_t count = 1; + + if (err) { + printf("Bluetooth init failed (err %d)\n", err); + return; + } + + printf("Bluetooth initialized\n"); + + /* Start advertising */ + err = bt_le_adv_start(BT_LE_ADV_PARAM_NCONN, ad, ARRAY_SIZE(ad), NULL, 0); + if (err) { + printf("Advertising failed to start (err %d)\n", err); + return; + } + + bt_id_get(&addr, &count); + bt_addr_le_to_str(&addr, addr_s, sizeof(addr_s)); + + printf("ODID started, advertising as %s\n", addr_s); +} + +static void update_payload(uint8_t turn) +{ + int err = 0; + + switch (turn) { + case 0: /* BasicID */ + err = encodeBasicIDMessage((ODID_BasicID_encoded *)&encoded, &uasData.BasicID[0]); + if (err == ODID_SUCCESS) { + memcpy(&payload[4], &encoded, sizeof(ODID_BasicID_encoded)); + memcpy(&payload[3], &msg_counters[ODID_MSG_COUNTER_BASIC_ID], 1); + ++msg_counters[ODID_MSG_COUNTER_BASIC_ID]; + } + break; + case 1: /* Location */ + /* Updating location for checking whether messages are dropped or not. */ + uasData.Location.Latitude = uasData.Location.Latitude - 0.01; + uasData.Location.Longitude = uasData.Location.Longitude + 0.01; + err = encodeLocationMessage((ODID_Location_encoded *)&encoded, &uasData.Location); + if (err == ODID_SUCCESS) { + memcpy(&payload[4], &encoded, sizeof(ODID_Location_encoded)); + memcpy(&payload[3], &msg_counters[ODID_MSG_COUNTER_LOCATION], 1); + ++msg_counters[ODID_MSG_COUNTER_LOCATION]; + } + break; + case 2: /* Auth */ + err = encodeAuthMessage((ODID_Auth_encoded *)&encoded, &uasData.Auth[0]); + if (err == ODID_SUCCESS) { + memcpy(&payload[4], &encoded, sizeof(ODID_Auth_encoded)); + memcpy(&payload[3], &msg_counters[ODID_MSG_COUNTER_AUTH], 1); + ++msg_counters[ODID_MSG_COUNTER_AUTH]; + } + break; + case 3: /* SelfID */ + err = encodeSelfIDMessage((ODID_SelfID_encoded *)&encoded, &uasData.SelfID); + if (err == ODID_SUCCESS) { + memcpy(&payload[4], &encoded, sizeof(ODID_SelfID_encoded)); + memcpy(&payload[3], &msg_counters[ODID_MSG_COUNTER_SELF_ID], 1); + ++msg_counters[ODID_MSG_COUNTER_SELF_ID]; + } + break; + case 4: /* System */ + err = encodeSystemMessage((ODID_System_encoded *)&encoded, &uasData.System); + if (err == ODID_SUCCESS) { + memcpy(&payload[4], &encoded, sizeof(ODID_System_encoded)); + memcpy(&payload[3], &msg_counters[ODID_MSG_COUNTER_SYSTEM], 1); + ++msg_counters[ODID_MSG_COUNTER_SYSTEM]; + } + break; + case 5: /* OperatorID */ + err = encodeOperatorIDMessage((ODID_OperatorID_encoded *)&encoded, + &uasData.OperatorID); + if (err == ODID_SUCCESS) { + memcpy(&payload[4], &encoded, sizeof(ODID_OperatorID_encoded)); + memcpy(&payload[3], &msg_counters[ODID_MSG_COUNTER_OPERATOR_ID], 1); + ++msg_counters[ODID_MSG_COUNTER_OPERATOR_ID]; + } + break; + default: + break; + } +} + +int main(void) +{ + int err; + + printf("Starting ODID Demo\n"); + + /* Initialize UAS data. */ + odid_initUasData(&uasData); + fill_example_data(&uasData); + fill_example_gps_data(&uasData); + memset(&encoded, 0, sizeof(union ODID_Message_encoded)); + for (int i = 0; i < ODID_MSG_COUNTER_AMOUNT; ++i) { + msg_counters[i] = 0; + } + + /* Initialize the Bluetooth Subsystem */ + err = bt_enable(bt_ready); + if (err) { + printf("Bluetooth init failed (err %d)\n", err); + return -1; + } + + while (true) { + if (bt_is_ready()) { + break; + } + + printf("Bluetooth not ready. Checking again in 100 ms\n"); + k_sleep(K_MSEC(100)); + } + + /* Modify ODID data and update adv data. */ + uint8_t tx_counter = 0; + + while (true) { + uint8_t turn = tx_counter % 6; + + update_payload(turn); + + static const struct bt_data ad_new[] = { + { + .type = BT_DATA_SVC_DATA16, + .data_len = ARRAY_SIZE(payload), + .data = payload, + }, + }; + + err = bt_le_adv_update_data(ad_new, ARRAY_SIZE(ad_new), NULL, 0); + if (err) { + printf("Bluetooth update adv data failed (err %d)\n", err); + continue; + } + + k_sleep(K_MSEC(100)); + + ++tx_counter; + } + + return 0; +} diff --git a/samples/bluetooth/opendroneid/src/opendroneid.c b/samples/bluetooth/opendroneid/src/opendroneid.c new file mode 100644 index 000000000000000..332140274ec77f8 --- /dev/null +++ b/samples/bluetooth/opendroneid/src/opendroneid.c @@ -0,0 +1,1555 @@ +/* SPDX-License-Identifier: Apache-2.0 + * + * Copyright (C) 2019 Intel Corporation + * Copyright 2024 NXP + * + * Referred from - https://github.com/opendroneid/opendroneid-core-c + */ + +#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 == 360) { + direction_int = 0; + } + 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]); + } + + *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; + switch (inData->IDType) { + case ODID_IDTYPE_SERIAL_NUMBER: + case ODID_IDTYPE_CAA_REGISTRATION_ID: + memset(outEncoded->UASID, 0, sizeof(outEncoded->UASID)); + strncpy(outEncoded->UASID, inData->UASID, sizeof(outEncoded->UASID)); + break; + default: + memcpy(outEncoded->UASID, inData->UASID, sizeof(outEncoded->UASID)); + break; + } + 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; + switch (inEncoded->IDType) { + case ODID_IDTYPE_SERIAL_NUMBER: + case ODID_IDTYPE_CAA_REGISTRATION_ID: + safe_dec_copyfill(outData->UASID, inEncoded->UASID, sizeof(outData->UASID)); + break; + default: + memcpy(outData->UASID, inEncoded->UASID, sizeof(outData->UASID)); + break; + } + 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 *odid_system = (ODID_System_encoded *)msgData; + + if (decodeSystemMessage(&uasData->System, odid_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; x < asize; x++) { + printf("%02x", (unsigned int)byteArray[x]); + if (spaced) { + printf(" "); + } + } + printf("\n"); + } +} + +/* + * Print formatted BasicID Data + * + * @param BasicID structure to be printed + */ +void printBasicID_data(ODID_BasicID_data *BasicID) +{ + /* Ensure the ID is null-terminated */ + char buf[ODID_ID_SIZE + 1] = {0}; + + memcpy(buf, BasicID->UASID, ODID_ID_SIZE); + + static const char ODID_BasicID_data_format[] = "UAType: %d\nIDType: %d\nUASID: %s\n"; + + printf(ODID_BasicID_data_format, BasicID->UAType, BasicID->IDType, 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) { + static const char ODID_Auth_data_format[] = "AuthType: %d\nDataPage: %d\n" + "LastPageIndex: %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 { + static 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); + + static 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) +{ + static 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); + + static 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/samples/bluetooth/opendroneid/src/opendroneid.h b/samples/bluetooth/opendroneid/src/opendroneid.h new file mode 100644 index 000000000000000..3f1d73001e84757 --- /dev/null +++ b/samples/bluetooth/opendroneid/src/opendroneid.h @@ -0,0 +1,804 @@ +/* SPDX-License-Identifier: Apache-2.0 + * + * Copyright (C) 2019 Intel Corporation + * Copyright 2024 NXP + * + * Referred from - https://github.com/opendroneid/opendroneid-core-c + */ + +#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) /* 1 hour in seconds */ +#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 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 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 __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 __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 __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 __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 __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 __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 __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 __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; /* no of messages in pack */ + + /* 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 message pack length on success, or < 0 on error. + */ +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 */ + +typedef struct FRDID_UAS_Data { + const char *Identifier; + const char *ANSICTA2063Identifier; + double Latitude; + double Longitude; + int Altitude; + int Height; + double TakeoffLatitude; + double TakeoffLongitude; + int HorizontalSpeed; + int TrueCourse; +} FRDID_UAS_Data; + +int frdid_wifi_build_beacon_frame(const FRDID_UAS_Data *UAS_Data, const char *mac, const char *SSID, + size_t SSID_len, uint16_t interval_tu, uint8_t *buf, + size_t buf_size); + +int frdid_build(const FRDID_UAS_Data *UAS_Data, uint8_t *buf, size_t buf_size); + +#ifdef __cplusplus +} +#endif + +#endif /* _OPENDRONEID_H_ */