github.com/jjshoots.RemoteIDSpoofer
1
0
Fork 0
mirror of https://github.com/jjshoots/RemoteIDSpoofer.git synced 2026-02-17 13:18:50 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
a80d820e7d97699b7db1accc620d2a6f21504b97
jjshoots.RemoteIDSpoofer/RemoteIDSpoofer/opendroneid.c
jjshoots d86ab7cf7f files organization, add linux scripts
2023-06-22 19:40:05 +01:00

1477 lines
47 KiB
C
Raw Blame History

/*
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 <math.h>
#include <stdio.h>
#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;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);
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
Reference in New Issue View Git Blame Copy Permalink