/* 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 #include #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;xUASID, 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