feature-remoteid/RemoteIDModule/cndroneid.cpp

/*
Copyright (C) 2019 Intel Corporation

SPDX-License-Identifier: Apache-2.0

Open Drone ID C Library

Maintainer:
Gabriel Cox
gabriel.c.cox@intel.com
*/
/*看到最后发现，国标跟国际的区别：去掉认证 认证ID 系统里对保留的3bit用了2bit作为坐标系 其他完全一样*/
#include "cndroneid.h"
#include <math.h>
#include <stdio.h>
#define ENABLE_DEBUG 1

const float CN_SPEED_DIV[2] = {0.25f, 0.75f};
const float CN_VSPEED_DIV = 0.5f;
const int32_t CN_LATLON_MULT = 10000000;
const float CN_ALT_DIV = 0.5f;
const int CN_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 cndid_initBasicIDData(CNDID_BasicID_data *data)
{
    if (!data)
        return;
    memset(data, 0, sizeof(CNDID_BasicID_data));
}

/**
* Initialize location data fields to their default values
*
* @param data (non encoded/packed) structure
*/
void cndid_initLocationData(CNDID_Location_data *data)
{
    if (!data)
        return;
    memset(data, 0, sizeof(CNDID_Location_data));
    data->Direction = CN_INV_DIR;
    data->SpeedHorizontal = CN_INV_SPEED_H;
    data->SpeedVertical = CN_INV_SPEED_V;
    data->AltitudeBaro = CN_INV_ALT;
    data->AltitudeGeo = CN_INV_ALT;
    data->Height = CN_INV_ALT;
}

/**
* Initialize self ID data fields to their default values
*
* @param data (non encoded/packed) structure
*/
void cndid_initSelfIDData(CNDID_SelfID_data *data)
{
    if (!data)
        return;
    memset(data, 0, sizeof(CNDID_SelfID_data));
}

/**
* Initialize system data fields to their default values
*
* @param data (non encoded/packed) structure
*/

void cndid_initSystemData(CNDID_System_data *data)
{
    if (!data)
        return;
    memset(data, 0, sizeof(CNDID_System_data));
    data->AreaCount = 1;
    data->AreaCeiling = CN_INV_ALT;
    data->AreaFloor = CN_INV_ALT;
    data->OperatorAltitudeGeo = CN_INV_ALT;
}


/**
* Initialize message pack data fields to their default values
*
* @param data (non encoded/packed) structure
*/

void cndid_initMessagePackData(CNDID_MessagePack_data *data)
{
    if (!data)
        return;
    memset(data, 0, sizeof(CNDID_MessagePack_data));
    data->SingleMessageSize = CNDID_MESSAGE_SIZE;
}

/**
* Initialize UAS data fields to their default values
*
* @param data (non encoded/packed) structure
*/

void cndid_initUasData(CNDID_UAS_Data *data)
{
    if (!data)
        return;
    for (int i = 0; i < CNDID_BASIC_ID_MAX_MESSAGES; i++) {
        data->BasicIDValid[i] = 0;
        cndid_initBasicIDData(&data->BasicID[i]);
    }
    data->LocationValid = 0;
    cndid_initLocationData(&data->Location);
    data->SelfIDValid = 0;
    cndid_initSelfIDData(&data->SelfID);
    data->SystemValid = 0;
    cndid_initSystemData(&data->System);

}

/**
* 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 * CN_SPEED_DIV[0]) {
        *mult = 0;
        return (uint8_t) (Speed_data / CN_SPEED_DIV[0]);
    } else {
        *mult = 1;
        int big_value = (int) ((Speed_data - (UINT8_MAX * CN_SPEED_DIV[0])) / CN_SPEED_DIV[1]);
        return (uint8_t) intRangeMax(big_value, 0, UINT8_MAX);
    }
}

/**
* Encode Vertical Speed into a signed Integer CNDID 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 / CN_VSPEED_DIV);
    return (int8_t) intRangeMax(encValue, INT8_MIN, INT8_MAX);
}

/**
* Encode Latitude or Longitude value into a signed Integer CNDID 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 * CN_LATLON_MULT), -180 * CN_LATLON_MULT, 180 * CN_LATLON_MULT);
}

/**
* Encode Altitude value into an int16 CNDID 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) CN_ALT_ADDER) / CN_ALT_DIV), 0, UINT16_MAX);
}

/**
* Encode timestamp data in CNDID 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 == CN_INV_TIMESTAMP)
        return CN_INV_TIMESTAMP;
    else
        return (uint16_t) intRangeMax((int64_t) roundf(Seconds_data*10), 0, CN_MAX_TIMESTAMP * 10);
}

/**
* Encode area radius data in CNDID 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           CNDID_SUCCESS or CNDID_FAIL;
*/
int encodeCNBasicIDMessage(CNDID_BasicID_encoded *outEncoded, CNDID_BasicID_data *inData)
{
    if (!outEncoded || !inData ||
        !intInRange(inData->IDType, 0, 15) ||
        !intInRange(inData->UAType, 0, 15))
        return CNDID_FAIL;

    outEncoded->MessageType = CNDID_MESSAGETYPE_BASIC_ID;
    outEncoded->ProtoVersion = CNDID_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 CNDID_SUCCESS;
}

/**
* Encode Location message (packed, ready for broadcast)
*
* @param outEncoded Output (encoded/packed) structure
* @param inData     Input data (non encoded/packed) structure
* @return           CNDID_SUCCESS or CNDID_FAIL;
*/
int encodeCNLocationMessage(CNDID_Location_encoded *outEncoded, CNDID_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 CNDID_FAIL;

    if (inData->Direction < CN_MIN_DIR || inData->Direction > CN_INV_DIR ||
        (inData->Direction > CN_MAX_DIR && inData->Direction < CN_INV_DIR))
        return CNDID_FAIL;

    if (inData->SpeedHorizontal < CN_MIN_SPEED_H || inData->SpeedHorizontal > CN_INV_SPEED_H ||
        (inData->SpeedHorizontal > CN_MAX_SPEED_H && inData->SpeedHorizontal < CN_INV_SPEED_H))
        return CNDID_FAIL;

    if (inData->SpeedVertical < CN_MIN_SPEED_V || inData->SpeedVertical > CN_INV_SPEED_V ||
        (inData->SpeedVertical > CN_MAX_SPEED_V && inData->SpeedVertical < CN_INV_SPEED_V))
        return CNDID_FAIL;

    if (inData->Latitude < CN_MIN_LAT || inData->Latitude > CN_MAX_LAT ||
        inData->Longitude < CN_MIN_LON || inData->Longitude > CN_MAX_LON)
        return CNDID_FAIL;

    if (inData->AltitudeBaro < CN_MIN_ALT || inData->AltitudeBaro > CN_MAX_ALT ||
        inData->AltitudeGeo < CN_MIN_ALT || inData->AltitudeGeo > CN_MAX_ALT ||
        inData->Height < CN_MIN_ALT || inData->Height > CN_MAX_ALT)
        return CNDID_FAIL;

    if (inData->TimeStamp < 0 ||
        (inData->TimeStamp > CN_MAX_TIMESTAMP && inData->TimeStamp != CN_INV_TIMESTAMP))
        return CNDID_FAIL;

    outEncoded->MessageType = CNDID_MESSAGETYPE_LOCATION;
    outEncoded->ProtoVersion = CNDID_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 CNDID_SUCCESS;
}


/**
* Encode Self ID message (packed, ready for broadcast)
*
* @param outEncoded Output (encoded/packed) structure
* @param inData     Input data (non encoded/packed) structure
* @return           CNDID_SUCCESS or CNDID_FAIL;
*/
int encodeCNSelfIDMessage(CNDID_SelfID_encoded *outEncoded, CNDID_SelfID_data *inData)
{
    if (!outEncoded || !inData || !intInRange(inData->DescType, 0, 255))
        return CNDID_FAIL;

    outEncoded->MessageType = CNDID_MESSAGETYPE_SELF_ID;
    outEncoded->ProtoVersion = CNDID_PROTOCOL_VERSION;
    outEncoded->DescType = inData->DescType;
    strncpy(outEncoded->Desc, inData->Desc, sizeof(outEncoded->Desc));
    return CNDID_SUCCESS;
}

/**
* Encode System message (packed, ready for broadcast)
*
* @param outEncoded Output (encoded/packed) structure
* @param inData     Input data (non encoded/packed) structure
* @return           CNDID_SUCCESS or CNDID_FAIL;
*/
int encodeCNSystemMessage(CNDID_System_encoded *outEncoded, CNDID_System_data *inData)
{
    if (!outEncoded || !inData ||
        !intInRange(inData->OperatorLocationType, 0, 3) ||
        !intInRange(inData->Classification_Type, 0, 7) ||
        !intInRange(inData->CategoryCN, 0, 15) ||
        !intInRange(inData->ClassCN, 0, 15)||
        !intInRange(inData->Coord_Type,0, 3)) // 坐标系类型
        return CNDID_FAIL;

    if (inData->OperatorLatitude < CN_MIN_LAT || inData->OperatorLatitude > CN_MAX_LAT ||
        inData->OperatorLongitude < CN_MIN_LON || inData->OperatorLongitude > CN_MAX_LON)
        return CNDID_FAIL;

    if (inData->AreaRadius > CN_MAX_AREA_RADIUS)
        return CNDID_FAIL;

    if (inData->AreaCeiling < CN_MIN_ALT || inData->AreaCeiling > CN_MAX_ALT ||
        inData->AreaFloor < CN_MIN_ALT || inData->AreaFloor > CN_MAX_ALT ||
        inData->OperatorAltitudeGeo < CN_MIN_ALT || inData->OperatorAltitudeGeo > CN_MAX_ALT)
        return CNDID_FAIL;

    outEncoded->MessageType = CNDID_MESSAGETYPE_SYSTEM;
    outEncoded->ProtoVersion = CNDID_PROTOCOL_VERSION;
    outEncoded->Reserved = 0;
    outEncoded->CoordType = inData->Coord_Type;
    outEncoded->OperatorLocationType = inData->OperatorLocationType;
    outEncoded->ClassificationType = inData->Classification_Type;
    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->CategoryCN = inData->CategoryCN;
    outEncoded->ClassCN = inData->ClassCN;
    outEncoded->OperatorAltitudeGeo = encodeAltitude(inData->OperatorAltitudeGeo);
    outEncoded->Timestamp = inData->Timestamp;
    outEncoded->Reserved2 = 0;
    return CNDID_SUCCESS;
}


/**
* 检查数据包结构的数据字段是否有效
*
* @param msgs   指向包含消息的缓冲区的指针
* @param amount 数据包中的消息数量
* @return       CNDID_SUCCESS 或 CNDID_FAIL；
*/
static int checkPackContent(CNDID_Message_encoded *msgs, int amount)
{
    if (amount <= 0 || amount > CNDID_PACK_MAX_MESSAGES)
        return CNDID_FAIL;
    int numMessages[4] = { 0 }; // CNDID_messagetype_t相关部分的计数器
    for (int i = 0; i < amount; i++) {
        uint8_t MessageType = decodeCNMessageType(msgs[i].rawData[0]);

        // 检查非法内容。这也避免了之间的递归调用
        // decodeOpenDroneID（）以及decodeMessagePack（）/checkPackContent（）
        if (MessageType <= CNDID_MESSAGETYPE_SYSTEM)  // 对待发送组包消息进行格式检查 最后一条消息就是系统报文
            numMessages[MessageType]++;
        else
            return CNDID_FAIL;
    }

    // 除基本ID和授权外，每条消息最多允许出现一次。
    if (numMessages[CNDID_MESSAGETYPE_BASIC_ID] > CNDID_BASIC_ID_MAX_MESSAGES ||
        numMessages[CNDID_MESSAGETYPE_LOCATION] > 1 ||
        numMessages[CNDID_MESSAGETYPE_SELF_ID] > 1 ||
        numMessages[CNDID_MESSAGETYPE_SYSTEM] > 1 )
        return CNDID_FAIL;

    return CNDID_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           CNDID_SUCCESS or CNDID_FAIL;
*/
int encodeCNMessagePack(CNDID_MessagePack_encoded *outEncoded, CNDID_MessagePack_data *inData)
{
    if (!outEncoded || !inData || inData->SingleMessageSize != CNDID_MESSAGE_SIZE)
        return CNDID_FAIL;

    if (checkPackContent(inData->Messages, inData->MsgPackSize) != CNDID_SUCCESS)
        return CNDID_FAIL;

    outEncoded->MessageType = CNDID_MESSAGETYPE_PACKED;
    outEncoded->ProtoVersion = CNDID_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], CNDID_MESSAGE_SIZE);

    return CNDID_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 * CN_SPEED_DIV[1]) + (UINT8_MAX * CN_SPEED_DIV[0]);
    else
        return (float) Speed_enc * CN_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 * CN_VSPEED_DIV;
}

/**
* Decode Latitude or Longitude value into a signed Integer CNDID 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 / CN_LATLON_MULT;
}

/**
* Decode Altitude from CNDID 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 * CN_ALT_DIV - (float) CN_ALT_ADDER;
}

/**
* Decode timestamp data from CNDID packed format
*
* @param Seconds_enc Encoded Timestamp
* @return Decoded timestamp (seconds since the hour)
*/
static float decodeTimeStamp(uint16_t Seconds_enc)
{
    if (Seconds_enc == CN_INV_TIMESTAMP)
        return CN_INV_TIMESTAMP;
    else
        return (float) Seconds_enc / 10;
}

/**
* Decode area radius data from CNDID 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           CNDID_SUCCESS or CNDID_FAIL;
*/
int getCNBasicIDType(CNDID_BasicID_encoded *inEncoded, enum CNDID_idtype *idType)
{
    if (!inEncoded || !idType || inEncoded->MessageType != CNDID_MESSAGETYPE_BASIC_ID)
        return CNDID_FAIL;

    *idType = (enum CNDID_idtype) inEncoded->IDType;
    return CNDID_SUCCESS;
}

/**
* 从打包消息中解码基本ID数据
*
* @param outData   输出：解码后的消息
* @param inEncoded 输入消息（已编码/打包）结构
* @return          CNDID_SUCCESS 或 CNDID_FAIL；
*/
int decodeCNBasicIDMessage(CNDID_BasicID_data *outData, CNDID_BasicID_encoded *inEncoded)
{
    if (!outData || !inEncoded ||
        inEncoded->MessageType != CNDID_MESSAGETYPE_BASIC_ID ||
        !intInRange(inEncoded->IDType, 0, 15) ||
        !intInRange(inEncoded->UAType, 0, 15))
        return CNDID_FAIL;

    outData->IDType = (CNDID_IDType_t) inEncoded->IDType; 
    outData->UAType = (CNDID_UAType_t) inEncoded->UAType;
    safe_dec_copyfill(outData->UASID, inEncoded->UASID, sizeof(outData->UASID));
    return CNDID_SUCCESS;
}


/**
* Decode Location data from packed message
*
* @param outData   Output: decoded message
* @param inEncoded Input message (encoded/packed) structure
* @return          CNDID_SUCCESS or CNDID_FAIL;
*/
int decodeCNLocationMessage(CNDID_Location_data *outData, CNDID_Location_encoded *inEncoded)
{
    if (!outData || !inEncoded ||
        inEncoded->MessageType != CNDID_MESSAGETYPE_LOCATION ||
        !intInRange(inEncoded->Status, 0, 15))
        return CNDID_FAIL;

    outData->Status = (CNDID_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 = (CNDID_HeightReference_t) inEncoded->HeightType;
    outData->Height = decodeAltitude(inEncoded->Height);
    outData->HorizAccuracy = (CNDID_HorizontalAccuracy_t) inEncoded->HorizAccuracy;
    outData->VertAccuracy = (CNDID_VerticalAccuracy_t) inEncoded->VertAccuracy;
    outData->BaroAccuracy = (CNDID_VerticalAccuracy_t) inEncoded->BaroAccuracy;
    outData->SpeedAccuracy = (CNDID_SpeedAccuracy_t) inEncoded->SpeedAccuracy;
    outData->TSAccuracy = (CNDID_TimeStampAccuracy_t) inEncoded->TSAccuracy;
    outData->TimeStamp = decodeTimeStamp(inEncoded->TimeStamp);
    return CNDID_SUCCESS;
}



/**
* Decode Self ID data from packed message
*
* @param outData   Output: decoded message
* @param inEncoded Input message (encoded/packed) structure
* @return          CNDID_SUCCESS or CNDID_FAIL;
*/
int decodeCNSelfIDMessage(CNDID_SelfID_data *outData, CNDID_SelfID_encoded *inEncoded)
{
    if (!outData || !inEncoded ||
        inEncoded->MessageType != CNDID_MESSAGETYPE_SELF_ID)
        return CNDID_FAIL;

    outData->DescType = (CNDID_DescType_t) inEncoded->DescType;
    safe_dec_copyfill(outData->Desc, inEncoded->Desc, sizeof(outData->Desc));
    return CNDID_SUCCESS;
}

/**
* Decode System data from packed message
*
* @param outData   Output: decoded message
* @param inEncoded Input message (encoded/packed) structure
* @return          CNDID_SUCCESS or CNDID_FAIL;
*/
int decodeCNSystemMessage(CNDID_System_data *outData, CNDID_System_encoded *inEncoded)
{
    if (!outData || !inEncoded ||
        inEncoded->MessageType != CNDID_MESSAGETYPE_SYSTEM)
        return CNDID_FAIL;

    outData->OperatorLocationType =
        (CNDID_operator_location_type_t) inEncoded->OperatorLocationType;
    outData->Classification_Type =
        (CNDID_classification_type_t) inEncoded->ClassificationType;
    outData->Coord_Type =
        (CNDID_CoordType_t) inEncoded->CoordType;
    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->CategoryCN = (CNDID_category_CN_t) inEncoded->CategoryCN;
    outData->ClassCN = (CNDID_class_CN_t) inEncoded->ClassCN;
    outData->OperatorAltitudeGeo = decodeAltitude(inEncoded->OperatorAltitudeGeo);
    outData->Timestamp = inEncoded->Timestamp;
    return CNDID_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        CNDID_SUCCESS or CNDID_FAIL;
*/
int decodeCNMessagePack(CNDID_UAS_Data *uasData, CNDID_MessagePack_encoded *pack)
{
    if (!uasData || !pack || pack->MessageType != CNDID_MESSAGETYPE_PACKED)
        return CNDID_FAIL;

    if (pack->SingleMessageSize != CNDID_MESSAGE_SIZE)
        return CNDID_FAIL;

    if (checkPackContent(pack->Messages, pack->MsgPackSize) != CNDID_SUCCESS)
        return CNDID_FAIL;

    for (int i = 0; i < pack->MsgPackSize; i++) {
        decodeCNDroneID(uasData, pack->Messages[i].rawData);
    }
    return CNDID_SUCCESS;
}

/**
* Decodes the message type of a packed Open Drone ID message
*
* @param byte   The first byte of the message
* @return       The message type: CNDID_messagetype_t
*/
CNDID_MessageType_t decodeCNMessageType(uint8_t byte)
{
    switch (byte >> 4)
    {
    case CNDID_MESSAGETYPE_BASIC_ID:
        return CNDID_MESSAGETYPE_BASIC_ID;
    case CNDID_MESSAGETYPE_LOCATION:
        return CNDID_MESSAGETYPE_LOCATION;
    case CNDID_MESSAGETYPE_SELF_ID:
        return CNDID_MESSAGETYPE_SELF_ID;
    case CNDID_MESSAGETYPE_SYSTEM:
        return CNDID_MESSAGETYPE_SYSTEM;
    case CNDID_MESSAGETYPE_PACKED:
        return CNDID_MESSAGETYPE_PACKED;
    default:
        return CNDID_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
* CNDID_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: CNDID_messagetype_t
*/
CNDID_MessageType_t decodeCNDroneID(CNDID_UAS_Data *uasData, uint8_t *msgData)
{
    if (!uasData || !msgData)
        return CNDID_MESSAGETYPE_INVALID;

    switch (decodeCNMessageType(msgData[0]))
    {
    case CNDID_MESSAGETYPE_BASIC_ID: {
        CNDID_BasicID_encoded *basicId = (CNDID_BasicID_encoded *) msgData;
        enum CNDID_idtype idType;
        if (getCNBasicIDType(basicId, &idType) == CNDID_SUCCESS) {
            // 找到一个空闲位置来存储当前消息，或者覆盖相同类型的旧数据。
            for (int i = 0; i < CNDID_BASIC_ID_MAX_MESSAGES; i++) {
                enum CNDID_idtype storedType = uasData->BasicID[i].IDType;
                if (storedType == CNDID_IDTYPE_NONE || storedType == idType) {
                    if (decodeCNBasicIDMessage(&uasData->BasicID[i], basicId) == CNDID_SUCCESS) {
                        uasData->BasicIDValid[i] = 1; // 重复ID ID非法
                        return CNDID_MESSAGETYPE_BASIC_ID;
                    }
                }
            }
        }
        break;
    }
    case CNDID_MESSAGETYPE_LOCATION: {
        CNDID_Location_encoded *location = (CNDID_Location_encoded *) msgData;
        if (decodeCNLocationMessage(&uasData->Location, location) == CNDID_SUCCESS) {
            uasData->LocationValid = 1;
            return CNDID_MESSAGETYPE_LOCATION;
        }
        break;
    }
    case CNDID_MESSAGETYPE_SELF_ID: {
        CNDID_SelfID_encoded *selfId = (CNDID_SelfID_encoded *) msgData;
        if (decodeCNSelfIDMessage(&uasData->SelfID, selfId) == CNDID_SUCCESS) {
            uasData->SelfIDValid = 1;
            return CNDID_MESSAGETYPE_SELF_ID;
        }
        break;
    }
    case CNDID_MESSAGETYPE_SYSTEM: {
        CNDID_System_encoded *system = (CNDID_System_encoded *) msgData;
        if (decodeCNSystemMessage(&uasData->System, system) == CNDID_SUCCESS) {
            uasData->SystemValid = 1;
            return CNDID_MESSAGETYPE_SYSTEM;
        }
        break;
    }
   
    case CNDID_MESSAGETYPE_PACKED: {
        CNDID_MessagePack_encoded *pack = (CNDID_MessagePack_encoded *) msgData;
        if (decodeCNMessagePack(uasData, pack) == CNDID_SUCCESS)
            return CNDID_MESSAGETYPE_PACKED;
        break;
    }
    default:
        break;
    }

    return CNDID_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
*/
CNDID_HorizontalAccuracy_t createCNEnumHorizontalAccuracy(float Accuracy)
{
    if (Accuracy >= 18520)
        return CNDID_HOR_ACC_UNKNOWN;
    else if (Accuracy >= 7408)
        return CNDID_HOR_ACC_10NM;
    else if (Accuracy >= 3704)
        return CNDID_HOR_ACC_4NM;
    else if (Accuracy >= 1852)
        return CNDID_HOR_ACC_2NM;
    else if (Accuracy >= 926)
        return CNDID_HOR_ACC_1NM;
    else if (Accuracy >= 555.6f)
        return CNDID_HOR_ACC_0_5NM;
    else if (Accuracy >= 185.2f)
        return CNDID_HOR_ACC_0_3NM;
    else if (Accuracy >= 92.6f)
        return CNDID_HOR_ACC_0_1NM;
    else if (Accuracy >= 30)
        return CNDID_HOR_ACC_0_05NM;
    else if (Accuracy >= 10)
        return CNDID_HOR_ACC_30_METER;
    else if (Accuracy >= 3)
        return CNDID_HOR_ACC_10_METER;
    else if (Accuracy >= 1)
        return CNDID_HOR_ACC_3_METER;
    else if (Accuracy > 0)
        return CNDID_HOR_ACC_1_METER;
    else
        return CNDID_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
*/
CNDID_VerticalAccuracy_t createCNEnumVerticalAccuracy(float Accuracy)
{
    if (Accuracy >= 150)
        return CNDID_VER_ACC_UNKNOWN;
    else if (Accuracy >= 45)
        return CNDID_VER_ACC_150_METER;
    else if (Accuracy >= 25)
        return CNDID_VER_ACC_45_METER;
    else if (Accuracy >= 10)
        return CNDID_VER_ACC_25_METER;
    else if (Accuracy >= 3)
        return CNDID_VER_ACC_10_METER;
    else if (Accuracy >= 1)
        return CNDID_VER_ACC_3_METER;
    else if (Accuracy > 0)
        return CNDID_VER_ACC_1_METER;
    else
        return CNDID_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
*/
CNDID_SpeedAccuracy_t createCNEnumSpeedAccuracy(float Accuracy)
{
    if (Accuracy >= 10)
        return CNDID_SPEED_ACC_UNKNOWN;
    else if (Accuracy >= 3)
        return CNDID_SPEED_ACC_10_METERS_PER_SECOND;
    else if (Accuracy >= 1)
        return CNDID_SPEED_ACC_3_METERS_PER_SECOND;
    else if (Accuracy >= 0.3f)
        return CNDID_SPEED_ACC_1_METERS_PER_SECOND;
    else if (Accuracy > 0)
        return CNDID_SPEED_ACC_0_3_METERS_PER_SECOND;
    else
        return CNDID_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
*/
CNDID_TimeStampAccuracy_t createCNEnumTimestampAccuracy(float Accuracy)
{
    if (Accuracy > 1.5f)
        return CNDID_TIME_ACC_UNKNOWN;
    else if (Accuracy > 1.4f)
        return CNDID_TIME_ACC_1_5_SECOND;
    else if (Accuracy > 1.3f)
        return CNDID_TIME_ACC_1_4_SECOND;
    else if (Accuracy > 1.2f)
        return CNDID_TIME_ACC_1_3_SECOND;
    else if (Accuracy > 1.1f)
        return CNDID_TIME_ACC_1_2_SECOND;
    else if (Accuracy > 1.0f)
        return CNDID_TIME_ACC_1_1_SECOND;
    else if (Accuracy > 0.9f)
        return CNDID_TIME_ACC_1_0_SECOND;
    else if (Accuracy > 0.8f)
        return CNDID_TIME_ACC_0_9_SECOND;
    else if (Accuracy > 0.7f)
        return CNDID_TIME_ACC_0_8_SECOND;
    else if (Accuracy > 0.6f)
        return CNDID_TIME_ACC_0_7_SECOND;
    else if (Accuracy > 0.5f)
        return CNDID_TIME_ACC_0_6_SECOND;
    else if (Accuracy > 0.4f)
        return CNDID_TIME_ACC_0_5_SECOND;
    else if (Accuracy > 0.3f)
        return CNDID_TIME_ACC_0_4_SECOND;
    else if (Accuracy > 0.2f)
        return CNDID_TIME_ACC_0_3_SECOND;
    else if (Accuracy > 0.1f)
        return CNDID_TIME_ACC_0_2_SECOND;
    else if (Accuracy > 0.0f)
        return CNDID_TIME_ACC_0_1_SECOND;
    else
        return CNDID_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 decodeCNHorizontalAccuracy(CNDID_HorizontalAccuracy_t Accuracy)
{
    switch (Accuracy)
    {
    case CNDID_HOR_ACC_UNKNOWN:
        return 18520;
    case CNDID_HOR_ACC_10NM:
        return 18520;
    case CNDID_HOR_ACC_4NM:
        return 7808;
    case CNDID_HOR_ACC_2NM:
        return 3704;
    case CNDID_HOR_ACC_1NM:
        return 1852;
    case CNDID_HOR_ACC_0_5NM:
        return 926;
    case CNDID_HOR_ACC_0_3NM:
        return 555.6f;
    case CNDID_HOR_ACC_0_1NM:
        return 185.2f;
    case CNDID_HOR_ACC_0_05NM:
        return 92.6f;
    case CNDID_HOR_ACC_30_METER:
        return 30;
    case CNDID_HOR_ACC_10_METER:
        return 10;
    case CNDID_HOR_ACC_3_METER:
        return 3;
    case CNDID_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 decodeCNVerticalAccuracy(CNDID_VerticalAccuracy_t Accuracy)
{
    switch (Accuracy)
    {
    case CNDID_VER_ACC_UNKNOWN:
        return 150;
    case CNDID_VER_ACC_150_METER:
        return 150;
    case CNDID_VER_ACC_45_METER:
        return 45;
    case CNDID_VER_ACC_25_METER:
        return 25;
    case CNDID_VER_ACC_10_METER:
        return 10;
    case CNDID_VER_ACC_3_METER:
        return 3;
    case CNDID_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 decodeCNSpeedAccuracy(CNDID_SpeedAccuracy_t Accuracy)
{
    switch (Accuracy)
    {
    case CNDID_SPEED_ACC_UNKNOWN:
        return 10;
    case CNDID_SPEED_ACC_10_METERS_PER_SECOND:
        return 10;
    case CNDID_SPEED_ACC_3_METERS_PER_SECOND:
        return 3;
    case CNDID_SPEED_ACC_1_METERS_PER_SECOND:
        return 1;
    case CNDID_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 decodeCNTimestampAccuracy(CNDID_TimeStampAccuracy_t Accuracy)
{
    switch (Accuracy)
    {
    case CNDID_TIME_ACC_UNKNOWN:
        return 0.0f;
    case CNDID_TIME_ACC_0_1_SECOND:
        return 0.1f;
    case CNDID_TIME_ACC_0_2_SECOND:
        return 0.2f;
    case CNDID_TIME_ACC_0_3_SECOND:
        return 0.3f;
    case CNDID_TIME_ACC_0_4_SECOND:
        return 0.4f;
    case CNDID_TIME_ACC_0_5_SECOND:
        return 0.5f;
    case CNDID_TIME_ACC_0_6_SECOND:
        return 0.6f;
    case CNDID_TIME_ACC_0_7_SECOND:
        return 0.7f;
    case CNDID_TIME_ACC_0_8_SECOND:
        return 0.8f;
    case CNDID_TIME_ACC_0_9_SECOND:
        return 0.9f;
    case CNDID_TIME_ACC_1_0_SECOND:
        return 1.0f;
    case CNDID_TIME_ACC_1_1_SECOND:
        return 1.1f;
    case CNDID_TIME_ACC_1_2_SECOND:
        return 1.2f;
    case CNDID_TIME_ACC_1_3_SECOND:
        return 1.3f;
    case CNDID_TIME_ACC_1_4_SECOND:
        return 1.4f;
    case CNDID_TIME_ACC_1_5_SECOND:
        return 1.5f;
    default:
        return 0.0f;
    }
}

#ifndef CNDID_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(CNDID_BasicID_data *BasicID)
{
    // Ensure the ID is null-terminated
    char buf[CNDID_ID_SIZE + 1] = { 0 };
    memcpy(buf, BasicID->UASID, CNDID_ID_SIZE);

    const char CNDID_BasicID_data_format[] =
        "UAType: %d\nIDType: %d\nUASID: %s\n";
    printf(CNDID_BasicID_data_format, BasicID->IDType, BasicID->UAType, buf);
}

/**
* Print formatted Location Data
*
* @param Location structure to be printed
*/
void PrintLocation_data(CNDID_Location_data *Location)
{
    const char CNDID_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(CNDID_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) decodeCNHorizontalAccuracy(Location->HorizAccuracy),
        (double) decodeCNVerticalAccuracy(Location->VertAccuracy),
        (double) decodeCNVerticalAccuracy(Location->BaroAccuracy),
        (double) decodeCNSpeedAccuracy(Location->SpeedAccuracy),
        (double) decodeCNTimestampAccuracy(Location->TSAccuracy),
        (double) Location->TimeStamp);
}


/**
* Print formatted SelfID Data
*
* @param SelfID structure to be printed
*/
void PrintSelfID_data(CNDID_SelfID_data *SelfID)
{
    // Ensure the description is null-terminated
    char buf[CNDID_STR_SIZE + 1] = { 0 };
    memcpy(buf, SelfID->Desc, CNDID_STR_SIZE);

    const char CNDID_SelfID_data_format[] = "DescType: %d\nDesc: %s\n";
    printf(CNDID_SelfID_data_format, SelfID->DescType, buf);
}

/**
* Print formatted System Data
*
* @param System_data structure to be printed
*/
void PrintSystem_data(CNDID_System_data *System_data)
{
    const char CNDID_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(CNDID_System_data_format, System_data->OperatorLocationType,
        System_data->Classification_Type,
        System_data->OperatorLatitude, System_data->OperatorLongitude,
        System_data->AreaCount, System_data->AreaRadius,
        (double) System_data->AreaCeiling, (double) System_data->AreaFloor,
        System_data->CategoryCN, System_data->ClassCN,
        (double) System_data->OperatorAltitudeGeo, System_data->Timestamp);
}

#endif // CNDID_DISABLE_PRINTF