#include "user_rid.h" #include "dronecan.h" #include #include #include #include #include #include #include #include "string.h" Connect_check remoteid_link; _armStatusID_msg StatusId; _location_msg locationId; _operatorId_msg operatorId; _basicId_msg basicId; _selfId_msg selfId; _sys_operator_msg systemId; void send_location_msg( void ) { static uint8_t transfer_id = 0; struct dronecan_remoteid_Location msg; uint8_t buffer[DRONECAN_REMOTEID_LOCATION_MAX_SIZE] = {0}; msg.id_or_mac.len = 20; memcpy(&msg.id_or_mac.data, &locationId.id_or_mac, msg.id_or_mac.len); msg.status = locationId.status; msg.direction = locationId.direction; msg.speed_horizontal = locationId.speed_horizontal; msg.speed_vertical = locationId.speed_vertical; msg.latitude = locationId.latitude; msg.longitude = locationId.longitude; msg.altitude_barometric = locationId.altitude_barometric; msg.altitude_geodetic = locationId.altitude_geodetic; msg.height_reference = locationId.height_reference; msg.height = locationId.height; msg.horizontal_accuracy = locationId.horizontal_accuracy; msg.vertical_accuracy = locationId.vertical_accuracy; msg.barometer_accuracy = locationId.barometer_accuracy; msg.speed_accuracy = locationId.speed_accuracy; msg.timestamp = locationId.timestamp; msg.timestamp_accuracy = locationId.timestamp_accuracy; uint32_t len = dronecan_remoteid_Location_encode(&msg, buffer); CanardTxTransfer tx_transfer = { .data_type_signature = DRONECAN_REMOTEID_LOCATION_SIGNATURE, .data_type_id = DRONECAN_REMOTEID_LOCATION_ID, .inout_transfer_id = &transfer_id, .priority = CANARD_TRANSFER_PRIORITY_MEDIUM, .transfer_type = CanardTransferTypeBroadcast, .payload = buffer, .payload_len = len, }; dronecan_broadcast(&tx_transfer); } void send_basicID(void) { static uint8_t transfer_id = 0; struct dronecan_remoteid_BasicID msg; uint8_t buffer[DRONECAN_REMOTEID_BASICID_MAX_SIZE] = {0}; msg.id_or_mac.len = 20; memcpy(&msg.id_or_mac.data, &basicId.id_or_mac, msg.id_or_mac.len); msg.id_type = basicId.id_type; msg.ua_type = basicId.ua_type; msg.uas_id.len = 20; memcpy(&msg.uas_id.data, &basicId.uas_id, msg.uas_id.len); uint32_t len = dronecan_remoteid_BasicID_encode(&msg, buffer); CanardTxTransfer tx_transfer = { .data_type_signature = DRONECAN_REMOTEID_BASICID_SIGNATURE, .data_type_id = DRONECAN_REMOTEID_BASICID_ID, .inout_transfer_id = &transfer_id, .priority = CANARD_TRANSFER_PRIORITY_MEDIUM, .transfer_type = CanardTransferTypeBroadcast, .payload = buffer, .payload_len = len, }; dronecan_broadcast(&tx_transfer); } void send_operatorID(void) { static uint8_t transfer_id = 0; struct dronecan_remoteid_OperatorID msg; uint8_t buffer[DRONECAN_REMOTEID_OPERATORID_MAX_SIZE] = {0}; msg.id_or_mac.len = 20; memcpy(&msg.id_or_mac.data, &operatorId.id_or_mac, msg.id_or_mac.len); msg.operator_id_type = operatorId.operator_id_type; msg.operator_id.len = 20; memcpy(&msg.operator_id.data, &operatorId.operator_id, msg.operator_id.len); uint32_t len = dronecan_remoteid_OperatorID_encode(&msg, buffer); CanardTxTransfer tx_transfer = { .data_type_signature = DRONECAN_REMOTEID_OPERATORID_SIGNATURE, .data_type_id = DRONECAN_REMOTEID_OPERATORID_ID, .inout_transfer_id = &transfer_id, .priority = CANARD_TRANSFER_PRIORITY_MEDIUM, .transfer_type = CanardTransferTypeBroadcast, .payload = buffer, .payload_len = len, }; dronecan_broadcast(&tx_transfer); } void send_sysOperatorID(void) { static uint8_t transfer_id = 0; struct dronecan_remoteid_System msg; uint8_t buffer[DRONECAN_REMOTEID_SYSTEM_MAX_SIZE] = {0}; msg.id_or_mac.len = 20; memcpy(&msg.id_or_mac.data, &systemId.id_or_mac, msg.id_or_mac.len); msg.operator_location_type = systemId.operator_location_type; msg.classification_type = systemId.classification_type; msg.operator_latitude = systemId.operator_latitude; msg.operator_longitude = systemId.operator_longitude; msg.area_count = systemId.area_count; msg.area_radius = systemId.area_radius; msg.area_ceiling = systemId.area_ceiling; msg.area_floor = systemId.area_floor; msg.category_eu = systemId.category_eu; msg.class_eu = systemId.class_eu; msg.operator_altitude_geo = systemId.operator_altitude_geo; msg.timestamp = systemId.timestamp; uint32_t len = dronecan_remoteid_System_encode(&msg, buffer); CanardTxTransfer tx_transfer = { .data_type_signature = DRONECAN_REMOTEID_SYSTEM_SIGNATURE, .data_type_id = DRONECAN_REMOTEID_SYSTEM_ID, .inout_transfer_id = &transfer_id, .priority = CANARD_TRANSFER_PRIORITY_MEDIUM, .transfer_type = CanardTransferTypeBroadcast, .payload = buffer, .payload_len = len, }; dronecan_broadcast(&tx_transfer); } void send_selfID(void) { static uint8_t transfer_id = 0; struct dronecan_remoteid_SelfID msg; uint8_t buffer[DRONECAN_REMOTEID_SELFID_MAX_SIZE] = {0}; msg.id_or_mac.len = 20; memcpy(&msg.id_or_mac.data, &selfId.id_or_mac, msg.id_or_mac.len); msg.description_type = selfId.description_type; msg.description.len = 23; memcpy(&msg.description.data, &selfId.description, msg.description.len); uint32_t len = dronecan_remoteid_SelfID_encode(&msg, buffer); CanardTxTransfer tx_transfer = { .data_type_signature = DRONECAN_REMOTEID_SELFID_SIGNATURE, .data_type_id = DRONECAN_REMOTEID_SELFID_ID, .inout_transfer_id = &transfer_id, .priority = CANARD_TRANSFER_PRIORITY_MEDIUM, .transfer_type = CanardTransferTypeBroadcast, .payload = buffer, .payload_len = len, }; dronecan_broadcast(&tx_transfer); } int remoteid_OnRecieved(CanardInstance *canardIns, CanardRxTransfer *transfer) { int ret = 0; struct dronecan_remoteid_ArmStatus info; static int TimeDelay = 0; if (transfer->transfer_type == CanardTransferTypeBroadcast) { if( transfer->data_type_id == DRONECAN_REMOTEID_ARMSTATUS_ID) { if (!dronecan_remoteid_ArmStatus_decode(transfer, &info)) { StatusId.status = info.status; if( StatusId.status != 0 && HAL_GetTick() - TimeDelay > 2500) { memcpy(&StatusId.status, &info.status, sizeof(struct dronecan_remoteid_ArmStatus)); } else { StatusId.status = 0; TimeDelay = HAL_GetTick(); memset(&StatusId.len,0,51); } StatusId.len = 50; remoteid_link.connect_status = COMP_NORMAL; } } } return ret; } bool remoteid_shouldAcceptTransfer(const CanardInstance *ins, uint64_t *out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, uint8_t source_node_id) { bool ret = false; if (data_type_id == DRONECAN_REMOTEID_ARMSTATUS_ID && transfer_type == CanardTransferTypeBroadcast) { *out_data_type_signature = DRONECAN_REMOTEID_ARMSTATUS_SIGNATURE; ret = true; } if (data_type_id == UAVCAN_PROTOCOL_NODESTATUS_ID && transfer_type == CanardTransferTypeBroadcast) { *out_data_type_signature = UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE; ret = true; } return ret; } void send_msg_to_remoteid(void) { static uint32_t SendTime_N2HZ = 0; static uint32_t SendTime_N3HZ = 0; static uint32_t SendTime_N4HZ = 0; static uint32_t SendTime_N5HZ = 0; static uint32_t SendTime_N6HZ = 0; if ( HAL_GetTick() - SendTime_N2HZ > 555 ) { //send_node_status(); send_location_msg(); SendTime_N2HZ = HAL_GetTick(); } if ( HAL_GetTick() - SendTime_N3HZ > 666 ) { send_sysOperatorID(); SendTime_N3HZ = HAL_GetTick(); } if ( HAL_GetTick() - SendTime_N4HZ > 333 ) { send_basicID(); SendTime_N4HZ = HAL_GetTick(); } if ( HAL_GetTick() - SendTime_N6HZ > 1777 ) { send_operatorID(); SendTime_N6HZ = HAL_GetTick(); } if ( HAL_GetTick() - SendTime_N5HZ > 1888 ) { send_selfID(); SendTime_N5HZ = HAL_GetTick(); } dronecan_tx_processing(); }