/* DroneCAN class for handling OpenDroneID messages with thanks to David Buzz for ArduPilot ESP32 HAL */ #include #include "board_config.h" #include "version.h" #include #include "DroneCAN.h" #include "parameters.h" #include #include "util.h" #include "monocypher.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #ifndef CAN_BOARD_ID #define CAN_BOARD_ID 10001 #endif #ifndef CAN_APP_NODE_NAME #define CAN_APP_NODE_NAME "ArduPilot RemoteIDModule" #endif #define UNUSED(x) (void)(x) static void onTransferReceived_trampoline(CanardInstance* ins, CanardRxTransfer* transfer); static bool shouldAcceptTransfer_trampoline(const CanardInstance* ins, uint64_t* out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, uint8_t source_node_id); // decoded messages void DroneCAN::init(void) { can_driver.init(1000000); canardInit(&canard, (uint8_t *)canard_memory_pool, sizeof(canard_memory_pool), onTransferReceived_trampoline, shouldAcceptTransfer_trampoline, NULL); if (g.can_node > 0 && g.can_node < 128) { canardSetLocalNodeID(&canard, g.can_node); } canard.user_reference = (void*)this; } void DroneCAN::update(void) { if (do_DNA()) { const uint32_t now_ms = millis(); if (now_ms - last_node_status_ms >= 1000) { last_node_status_ms = now_ms; node_status_send(); arm_status_send(); } } processTx(); processRx(); } void DroneCAN::node_status_send(void) { uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE]; node_status.uptime_sec = millis() / 1000U; node_status.vendor_specific_status_code = 0; const uint16_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); static uint8_t tx_id; canardBroadcast(&canard, UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, UAVCAN_PROTOCOL_NODESTATUS_ID, &tx_id, CANARD_TRANSFER_PRIORITY_LOW, (void*)buffer, len); } void DroneCAN::arm_status_send(void) { uint8_t buffer[DRONECAN_REMOTEID_ARMSTATUS_MAX_SIZE]; dronecan_remoteid_ArmStatus arm_status {}; const uint8_t status = parse_fail==nullptr?MAV_ODID_GOOD_TO_ARM:MAV_ODID_PRE_ARM_FAIL_GENERIC; const char *reason = parse_fail==nullptr?"":parse_fail; arm_status.status = status; arm_status.error.len = strlen(reason); strncpy((char*)arm_status.error.data, reason, sizeof(arm_status.error.data)); const uint16_t len = dronecan_remoteid_ArmStatus_encode(&arm_status, buffer); static uint8_t tx_id; canardBroadcast(&canard, DRONECAN_REMOTEID_ARMSTATUS_SIGNATURE, DRONECAN_REMOTEID_ARMSTATUS_ID, &tx_id, CANARD_TRANSFER_PRIORITY_LOW, (void*)buffer, len); } void DroneCAN::onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer) { if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) { if (transfer->transfer_type == CanardTransferTypeBroadcast && transfer->data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID) { handle_allocation_response(ins, transfer); } return; } const uint32_t now_ms = millis(); switch (transfer->data_type_id) { case UAVCAN_PROTOCOL_GETNODEINFO_ID: handle_get_node_info(ins, transfer); break; case UAVCAN_PROTOCOL_RESTARTNODE_ID: Serial.printf("RestartNode\n"); delay(20); esp_restart(); break; case DRONECAN_REMOTEID_BASICID_ID: Serial.printf("Got BasicID\n"); handle_BasicID(transfer); break; case DRONECAN_REMOTEID_LOCATION_ID: Serial.printf("Got Location\n"); handle_Location(transfer); break; case DRONECAN_REMOTEID_SELFID_ID: Serial.printf("Got SelfID\n"); handle_SelfID(transfer); break; case DRONECAN_REMOTEID_SYSTEM_ID: Serial.printf("Got System\n"); handle_System(transfer); break; case DRONECAN_REMOTEID_OPERATORID_ID: Serial.printf("Got OperatorID\n"); handle_OperatorID(transfer); break; case UAVCAN_PROTOCOL_PARAM_GETSET_ID: handle_param_getset(ins, transfer); break; case DRONECAN_REMOTEID_SECURECOMMAND_ID: handle_SecureCommand(ins, transfer); break; default: //Serial.printf("reject %u\n", transfer->data_type_id); break; } } bool DroneCAN::shouldAcceptTransfer(const CanardInstance* ins, uint64_t* out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, uint8_t source_node_id) { if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID && data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID) { *out_data_type_signature = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE; return true; } #define ACCEPT_ID(name) case name ## _ID: *out_data_type_signature = name ## _SIGNATURE; return true switch (data_type_id) { ACCEPT_ID(UAVCAN_PROTOCOL_GETNODEINFO); ACCEPT_ID(UAVCAN_PROTOCOL_RESTARTNODE); ACCEPT_ID(DRONECAN_REMOTEID_BASICID); ACCEPT_ID(DRONECAN_REMOTEID_LOCATION); ACCEPT_ID(DRONECAN_REMOTEID_SELFID); ACCEPT_ID(DRONECAN_REMOTEID_OPERATORID); ACCEPT_ID(DRONECAN_REMOTEID_SYSTEM); ACCEPT_ID(DRONECAN_REMOTEID_SECURECOMMAND); ACCEPT_ID(UAVCAN_PROTOCOL_PARAM_GETSET); return true; } //Serial.printf("%u: reject ID 0x%x\n", millis(), data_type_id); return false; } static void onTransferReceived_trampoline(CanardInstance* ins, CanardRxTransfer* transfer) { DroneCAN *dc = (DroneCAN *)ins->user_reference; dc->onTransferReceived(ins, transfer); } /* see if we want to process this packet */ static bool shouldAcceptTransfer_trampoline(const CanardInstance* ins, uint64_t* out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, uint8_t source_node_id) { DroneCAN *dc = (DroneCAN *)ins->user_reference; return dc->shouldAcceptTransfer(ins, out_data_type_signature, data_type_id, transfer_type, source_node_id); } void DroneCAN::processTx(void) { for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) { CANFrame txmsg {}; txmsg.dlc = CANFrame::dataLengthToDlc(txf->data_len); memcpy(txmsg.data, txf->data, txf->data_len); txmsg.id = (txf->id | CANFrame::FlagEFF); // push message with 1s timeout if (can_driver.send(txmsg)) { canardPopTxQueue(&canard); tx_fail_count = 0; } else { if (tx_fail_count < 8) { tx_fail_count++; } else { canardPopTxQueue(&canard); } break; } } } void DroneCAN::processRx(void) { CANFrame rxmsg; while (can_driver.receive(rxmsg)) { CanardCANFrame rx_frame {}; uint64_t timestamp = micros64(); rx_frame.data_len = CANFrame::dlcToDataLength(rxmsg.dlc); memcpy(rx_frame.data, rxmsg.data, rx_frame.data_len); rx_frame.id = rxmsg.id; int err = canardHandleRxFrame(&canard, &rx_frame, timestamp); #if 0 Serial.printf("%u: FX %08x %02x %02x %02x %02x %02x %02x %02x %02x (%u) -> %d\n", millis(), rx_frame.id, rxmsg.data[0], rxmsg.data[1], rxmsg.data[2], rxmsg.data[3], rxmsg.data[4], rxmsg.data[5], rxmsg.data[6], rxmsg.data[7], rx_frame.data_len, err); #else UNUSED(err); #endif } } CANFrame::CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame) : id(can_id) { if ((can_data == nullptr) || (data_len == 0) || (data_len > MaxDataLen)) { return; } memcpy(this->data, can_data, data_len); if (data_len <= 8) { dlc = data_len; } else { dlc = 8; } } uint8_t CANFrame::dataLengthToDlc(uint8_t data_length) { if (data_length <= 8) { return data_length; } else if (data_length <= 12) { return 9; } else if (data_length <= 16) { return 10; } else if (data_length <= 20) { return 11; } else if (data_length <= 24) { return 12; } else if (data_length <= 32) { return 13; } else if (data_length <= 48) { return 14; } return 15; } uint8_t CANFrame::dlcToDataLength(uint8_t dlc) { /* Data Length Code 9 10 11 12 13 14 15 Number of data bytes 12 16 20 24 32 48 64 */ if (dlc <= 8) { return dlc; } else if (dlc == 9) { return 12; } else if (dlc == 10) { return 16; } else if (dlc == 11) { return 20; } else if (dlc == 12) { return 24; } else if (dlc == 13) { return 32; } else if (dlc == 14) { return 48; } return 64; } uint64_t DroneCAN::micros64(void) { uint32_t us = micros(); if (us < last_micros32) { base_micros64 += 0x100000000ULL; } last_micros32 = us; return us + base_micros64; } /* handle a GET_NODE_INFO request */ void DroneCAN::handle_get_node_info(CanardInstance* ins, CanardRxTransfer* transfer) { uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE] {}; uavcan_protocol_GetNodeInfoResponse pkt {}; node_status.uptime_sec = millis() / 1000U; pkt.status = node_status; pkt.software_version.major = FW_VERSION_MAJOR; pkt.software_version.minor = FW_VERSION_MINOR; pkt.software_version.optional_field_flags = UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT | UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_IMAGE_CRC; pkt.software_version.vcs_commit = GIT_VERSION; readUniqueID(pkt.hardware_version.unique_id); pkt.hardware_version.major = CAN_BOARD_ID >> 8; pkt.hardware_version.minor = CAN_BOARD_ID & 0xFF; snprintf((char*)pkt.name.data, sizeof(pkt.name.data), "%s", CAN_APP_NODE_NAME); pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data)); uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); canardRequestOrRespond(ins, transfer->source_node_id, UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, UAVCAN_PROTOCOL_GETNODEINFO_ID, &transfer->transfer_id, transfer->priority, CanardResponse, &buffer[0], total_size); } void DroneCAN::handle_allocation_response(CanardInstance* ins, CanardRxTransfer* transfer) { // Rule C - updating the randomized time interval send_next_node_id_allocation_request_at_ms = millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + random(1, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) { node_id_allocation_unique_id_offset = 0; return; } // Copying the unique ID from the message uavcan_protocol_dynamic_node_id_Allocation msg; uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &msg); // Obtaining the local unique ID uint8_t my_unique_id[sizeof(msg.unique_id.data)] {}; readUniqueID(my_unique_id); // Matching the received UID against the local one if (memcmp(msg.unique_id.data, my_unique_id, msg.unique_id.len) != 0) { node_id_allocation_unique_id_offset = 0; return; } if (msg.unique_id.len < sizeof(msg.unique_id.data)) { // The allocator has confirmed part of unique ID, switching to the next stage and updating the timeout. node_id_allocation_unique_id_offset = msg.unique_id.len; send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS; } else { // Allocation complete - copying the allocated node ID from the message canardSetLocalNodeID(ins, msg.node_id); Serial.printf("Node ID allocated: %u\n", unsigned(msg.node_id)); } } bool DroneCAN::do_DNA(void) { if (canardGetLocalNodeID(&canard) != CANARD_BROADCAST_NODE_ID) { return true; } const uint32_t now = millis(); if (now - last_DNA_start_ms < 1000 && node_id_allocation_unique_id_offset == 0) { return false; } last_DNA_start_ms = now; uint8_t node_id_allocation_transfer_id = 0; UNUSED(node_id_allocation_transfer_id); send_next_node_id_allocation_request_at_ms = now + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + random(1, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1] {}; allocation_request[0] = 0; if (node_id_allocation_unique_id_offset == 0) { allocation_request[0] |= 1; } uint8_t my_unique_id[sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data)] {}; readUniqueID(my_unique_id); static const uint8_t MaxLenOfUniqueIDInRequest = 6; uint8_t uid_size = (uint8_t)(sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data) - node_id_allocation_unique_id_offset); if (uid_size > MaxLenOfUniqueIDInRequest) { uid_size = MaxLenOfUniqueIDInRequest; } memmove(&allocation_request[1], &my_unique_id[node_id_allocation_unique_id_offset], uid_size); // Broadcasting the request static uint8_t tx_id; canardBroadcast(&canard, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID, &tx_id, CANARD_TRANSFER_PRIORITY_LOW, &allocation_request[0], (uint16_t) (uid_size + 1)); node_id_allocation_unique_id_offset = 0; return false; } void DroneCAN::readUniqueID(uint8_t id[6]) { esp_efuse_mac_get_default(id); } #define IMIN(a,b) ((a)<(b)?(a):(b)) #define COPY_FIELD(fname) mpkt.fname = pkt.fname #define COPY_STR(fname) memcpy(mpkt.fname, pkt.fname.data, IMIN(pkt.fname.len, sizeof(mpkt.fname))) void DroneCAN::handle_BasicID(CanardRxTransfer* transfer) { dronecan_remoteid_BasicID pkt {}; auto &mpkt = basic_id; dronecan_remoteid_BasicID_decode(transfer, &pkt); last_basic_id_ms = millis(); memset(&mpkt, 0, sizeof(mpkt)); COPY_STR(id_or_mac); COPY_FIELD(id_type); COPY_FIELD(ua_type); COPY_STR(uas_id); } void DroneCAN::handle_SelfID(CanardRxTransfer* transfer) { dronecan_remoteid_SelfID pkt {}; auto &mpkt = self_id; dronecan_remoteid_SelfID_decode(transfer, &pkt); last_self_id_ms = millis(); memset(&mpkt, 0, sizeof(mpkt)); COPY_STR(id_or_mac); COPY_FIELD(description_type); COPY_STR(description); } void DroneCAN::handle_System(CanardRxTransfer* transfer) { dronecan_remoteid_System pkt {}; auto &mpkt = system; dronecan_remoteid_System_decode(transfer, &pkt); last_system_ms = millis(); memset(&mpkt, 0, sizeof(mpkt)); COPY_STR(id_or_mac); COPY_FIELD(operator_location_type); COPY_FIELD(classification_type); COPY_FIELD(operator_latitude); COPY_FIELD(operator_longitude); COPY_FIELD(area_count); COPY_FIELD(area_radius); COPY_FIELD(area_ceiling); COPY_FIELD(area_floor); COPY_FIELD(category_eu); COPY_FIELD(class_eu); COPY_FIELD(operator_altitude_geo); COPY_FIELD(timestamp); } void DroneCAN::handle_OperatorID(CanardRxTransfer* transfer) { dronecan_remoteid_OperatorID pkt {}; auto &mpkt = operator_id; dronecan_remoteid_OperatorID_decode(transfer, &pkt); last_operator_id_ms = millis(); memset(&mpkt, 0, sizeof(mpkt)); COPY_STR(id_or_mac); COPY_FIELD(operator_id_type); COPY_STR(operator_id); } void DroneCAN::handle_Location(CanardRxTransfer* transfer) { dronecan_remoteid_Location pkt {}; auto &mpkt = location; dronecan_remoteid_Location_decode(transfer, &pkt); last_location_ms = millis(); memset(&mpkt, 0, sizeof(mpkt)); COPY_STR(id_or_mac); COPY_FIELD(status); COPY_FIELD(direction); COPY_FIELD(speed_horizontal); COPY_FIELD(speed_vertical); COPY_FIELD(latitude); COPY_FIELD(longitude); COPY_FIELD(altitude_barometric); COPY_FIELD(altitude_geodetic); COPY_FIELD(height_reference); COPY_FIELD(height); COPY_FIELD(horizontal_accuracy); COPY_FIELD(vertical_accuracy); COPY_FIELD(barometer_accuracy); COPY_FIELD(speed_accuracy); COPY_FIELD(timestamp); COPY_FIELD(timestamp_accuracy); } /* handle parameter GetSet request */ void DroneCAN::handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) { uavcan_protocol_param_GetSetRequest req; if (uavcan_protocol_param_GetSetRequest_decode(transfer, &req)) { return; } uavcan_protocol_param_GetSetResponse pkt {}; const Parameters::Param *vp = nullptr; if (req.name.len != 0 && req.name.len > PARAM_NAME_MAX_LEN) { vp = nullptr; } else if (req.name.len != 0 && req.name.len <= PARAM_NAME_MAX_LEN) { memcpy((char *)pkt.name.data, (char *)req.name.data, req.name.len); vp = Parameters::find((char *)pkt.name.data); } else { vp = Parameters::find_by_index(req.index); } if (vp->flags & PARAM_FLAG_HIDDEN) { vp = nullptr; } if (vp != nullptr && req.name.len != 0 && req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY) { if (g.lock_level != 0) { can_printf("Parameters locked"); } else { // param set switch (vp->ptype) { case Parameters::ParamType::UINT8: if (req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE) { return; } vp->set_uint8(uint8_t(req.value.integer_value)); break; case Parameters::ParamType::UINT32: if (req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE) { return; } vp->set_uint32(uint32_t(req.value.integer_value)); break; case Parameters::ParamType::FLOAT: if (req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE) { return; } vp->set_float(req.value.real_value); break; case Parameters::ParamType::CHAR20: { if (req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE) { return; } char v[21] {}; strncpy(v, (const char *)&req.value.string_value.data[0], req.value.string_value.len); if (vp->min_len > 0 && strlen(v) < vp->min_len) { can_printf("%s too short - min %u", vp->name, vp->min_len); } else { vp->set_char20(v); } break; } case Parameters::ParamType::CHAR64: { if (req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE) { return; } char v[65] {}; strncpy(v, (const char *)&req.value.string_value.data[0], req.value.string_value.len); vp->set_char64(v); break; } default: return; } } } if (vp != nullptr) { switch (vp->ptype) { case Parameters::ParamType::UINT8: pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; pkt.value.integer_value = vp->get_uint8(); pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; pkt.default_value.integer_value = uint8_t(vp->default_value); pkt.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE; pkt.min_value.integer_value = uint8_t(vp->min_value); pkt.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE; pkt.max_value.integer_value = uint8_t(vp->max_value); break; case Parameters::ParamType::UINT32: pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; pkt.value.integer_value = vp->get_uint32(); pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; pkt.default_value.integer_value = uint32_t(vp->default_value); pkt.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE; pkt.min_value.integer_value = uint32_t(vp->min_value); pkt.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE; pkt.max_value.integer_value = uint32_t(vp->max_value); break; case Parameters::ParamType::FLOAT: pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE; pkt.value.real_value = vp->get_float(); pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE; pkt.default_value.real_value = vp->default_value; pkt.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE; pkt.min_value.real_value = vp->min_value; pkt.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE; pkt.max_value.real_value = vp->max_value; break; case Parameters::ParamType::CHAR20: { pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE; const char *s = vp->get_char20(); if (vp->flags & PARAM_FLAG_PASSWORD) { s = "********"; } strncpy((char*)pkt.value.string_value.data, s, sizeof(pkt.value.string_value.data)); pkt.value.string_value.len = strlen(s); break; } case Parameters::ParamType::CHAR64: { pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE; const char *s = vp->get_char64(); strncpy((char*)pkt.value.string_value.data, s, sizeof(pkt.value.string_value.data)); pkt.value.string_value.len = strlen(s); break; } default: return; } pkt.name.len = strlen(vp->name); strncpy((char *)pkt.name.data, vp->name, sizeof(pkt.name.data)); } uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE] {}; uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer); canardRequestOrRespond(ins, transfer->source_node_id, UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE, UAVCAN_PROTOCOL_PARAM_GETSET_ID, &transfer->transfer_id, transfer->priority, CanardResponse, &buffer[0], total_size); } /* handle SecureCommand */ void DroneCAN::handle_SecureCommand(CanardInstance* ins, CanardRxTransfer* transfer) { dronecan_remoteid_SecureCommandRequest req; if (dronecan_remoteid_SecureCommandRequest_decode(transfer, &req)) { return; } dronecan_remoteid_SecureCommandResponse reply {}; reply.result = DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_RESULT_UNSUPPORTED; reply.sequence = req.sequence; reply.operation = req.operation; if (!check_signature(req.sig_length, req.data.len-req.sig_length, req.sequence, req.operation, req.data.data)) { reply.result = DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_RESULT_DENIED; goto send_reply; } switch (req.operation) { case DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_SECURE_COMMAND_GET_REMOTEID_SESSION_KEY: { make_session_key(session_key); memcpy(reply.data.data, session_key, sizeof(session_key)); reply.data.len = sizeof(session_key); reply.result = DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_RESULT_ACCEPTED; break; } case DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_SECURE_COMMAND_SET_REMOTEID_CONFIG: { Serial.printf("SECURE_COMMAND_SET_REMOTEID_CONFIG\n"); int16_t data_len = req.data.len - req.sig_length; req.data.data[data_len] = 0; /* command buffer is nul separated set of NAME=VALUE pairs */ reply.result = DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_RESULT_ACCEPTED; char *command = (char *)req.data.data; while (data_len > 0) { uint8_t cmdlen = strlen(command); Serial.printf("set_config %s", command); char *eq = strchr(command, '='); if (eq != nullptr) { *eq = 0; if (!g.set_by_name_string(command, eq+1)) { reply.result = DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_RESULT_FAILED; } } command += cmdlen+1; data_len -= cmdlen+1; } break; } } send_reply: uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE] {}; uint16_t total_size = dronecan_remoteid_SecureCommandResponse_encode(&reply, buffer); canardRequestOrRespond(ins, transfer->source_node_id, DRONECAN_REMOTEID_SECURECOMMAND_SIGNATURE, DRONECAN_REMOTEID_SECURECOMMAND_ID, &transfer->transfer_id, transfer->priority, CanardResponse, &buffer[0], total_size); } // printf to CAN LogMessage for debugging void DroneCAN::can_printf(const char *fmt, ...) { uavcan_protocol_debug_LogMessage pkt {}; uint8_t buffer[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE] {}; va_list ap; va_start(ap, fmt); uint32_t n = vsnprintf((char*)pkt.text.data, sizeof(pkt.text.data), fmt, ap); va_end(ap); pkt.text.len = n; if (sizeof(pkt.text.data) < n) { pkt.text.len = sizeof(pkt.text.data); } uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer); static uint8_t tx_id; canardBroadcast(&canard, UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE, UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID, &tx_id, CANARD_TRANSFER_PRIORITY_LOW, buffer, len); } #if 0 // xprintf is useful when debugging in C code such as libcanard extern "C" { void xprintf(const char *fmt, ...); } void xprintf(const char *fmt, ...) { char buffer[200] {}; va_list ap; va_start(ap, fmt); uint32_t n = vsnprintf(buffer, sizeof(buffer), fmt, ap); va_end(ap); Serial.printf("%s", buffer); } #endif