diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index f78f53a0e30..7b4f39c2edd 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -25,10 +25,10 @@ #include #include -/* Private variables ---------------------------------------------------------*/ +/* --- Private state --- */ -CanardInstance canard; -uint8_t memory_pool[1024]; +static CanardInstance canard; +static uint8_t memory_pool[1024]; static struct uavcan_protocol_NodeStatus node_status; enum dronecanState_e { STATE_DRONECAN_INIT, @@ -46,175 +46,189 @@ PG_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig, // NOTE: All canard handlers and senders are based on this reference: https://dronecan.github.io/Specification/7._List_of_standard_data_types/ // Alternatively, you can look at the corresponding generated header file in the dsdlc_generated folder -// Canard Handlers ( Many have code copied from libcanard esc_node example: https://github.com/dronecan/libcanard/blob/master/examples/ESCNode/esc_node.c ) - -void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_protocol_NodeStatus nodeStatus; +/* --- Forward declarations --- */ + +static void process1HzTasks(timeUs_t timestamp_usec); +static void processCanardTxQueue(void); +static void send_NodeStatus(void); +static bool shouldAcceptTransfer(const CanardInstance *ins, uint64_t *out_data_type_signature, + uint16_t data_type_id, CanardTransferType transfer_type, + uint8_t source_node_id); +static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer); +static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer); +static void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer); +static void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer); +static void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer); +static void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer); +static void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer); +static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer); + +/* --- Public API --- */ - if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) { - LOG_DEBUG(CAN, "NodeStatus decode failed"); - return; - } - - // LOG_DEBUG(CAN, "Node Health "); - - switch (nodeStatus.health) { - case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK: - // LOG_DEBUG(CAN, "OK"); - break; - case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING: - // LOG_DEBUG(CAN, "WARNING"); - break; - case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR: - // LOG_DEBUG(CAN, "ERROR"); - break; - case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL: - // LOG_DEBUG(CAN, "CRITICAL"); - break; - default: - // LOG_DEBUG(CAN, "UNKNOWN?"); - break; - } - - // LOG_DEBUG(CAN, "Node Mode "); - - switch(nodeStatus.mode) { - case UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL: - // LOG_DEBUG(CAN, "OPERATIONAL"); - break; - case UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION: - // LOG_DEBUG(CAN, "INITIALIZATION"); - break; - case UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE: - // LOG_DEBUG(CAN, "MAINTENANCE"); - break; - case UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE: - // LOG_DEBUG(CAN, "SOFTWARE UPDATE"); - break; - case UAVCAN_PROTOCOL_NODESTATUS_MODE_OFFLINE: - // LOG_DEBUG(CAN, "OFFLINE"); - break; - default: - // LOG_DEBUG(CAN, "UNKNOWN?"); - break; - } -} +void dronecanInit(void) +{ + LOG_DEBUG(CAN, "dronecan Init"); + uint32_t bitrate = 500000; // At least define 500000 -void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_equipment_gnss_Auxiliary gnssAuxiliary; + switch (dronecanConfig()->bitRateKbps){ + case DRONECAN_BITRATE_125KBPS: + bitrate = 125000; + break; - if (uavcan_equipment_gnss_Auxiliary_decode(transfer, &gnssAuxiliary)) { - LOG_DEBUG(CAN, "GNSSAuxiliary decode failed"); - return; - } - dronecanGPSReceiveGNSSAuxiliary(&gnssAuxiliary); - LOG_DEBUG(CAN, "GNSS Auxiliary: Sats=%d HDOP=%.1f", gnssAuxiliary.sats_used, (double)gnssAuxiliary.hdop); -} + case DRONECAN_BITRATE_250KBPS: + bitrate = 250000; + break; -void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_equipment_gnss_Fix gnssFix; + case DRONECAN_BITRATE_500KBPS: + bitrate = 500000; + break; - if (uavcan_equipment_gnss_Fix_decode(transfer, &gnssFix)) { - LOG_DEBUG(CAN, "GNSSFix decode failed"); - return; - } - dronecanGPSReceiveGNSSFix(&gnssFix); - LOG_DEBUG(CAN, "GNSS Fix received"); -} + case DRONECAN_BITRATE_1000KBPS: + bitrate = 1000000; + break; -void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_equipment_gnss_Fix2 gnssFix2; + case DRONECAN_BITRATE_COUNT: + LOG_ERROR(SYSTEM, "Undefined bitrate set in configuration. 500kbps selected"); + bitrate = 500000; + break; + } + if(canardSTM32CAN1_Init(bitrate) != CANARD_OK) + { + LOG_ERROR(CAN, "Unable to initialize the CAN peripheral"); + // TODO: Notify the user that CAN does not work and disable the peripheral + return; + } + /* + Initializing the Libcanard instance. + */ + canardInit(&canard, + memory_pool, + sizeof(memory_pool), + onTransferReceived, + shouldAcceptTransfer, + NULL); - if (uavcan_equipment_gnss_Fix2_decode(transfer, &gnssFix2)) { - LOG_DEBUG(CAN, "GNSSFix2 decode failed"); - return; - } - dronecanGPSReceiveGNSSFix2(&gnssFix2); - LOG_DEBUG(CAN, "GNSS Fix2 received"); + // Could use DNA (Dynamic Node Allocation) by following example in esc_node.c but that requires a lot of setup and I'm not too sure of what advantage it brings + // Instead, set a different NODE_ID for each device on the CAN bus by configuring node_settings + if (dronecanConfig()->nodeID > 0) { + canardSetLocalNodeID(&canard, dronecanConfig()->nodeID); + } else { + LOG_DEBUG(CAN, "Node ID is 0, this node is anonymous and can't transmit most messages. Please update this in config"); + } } -void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_equipment_gnss_RTCMStream gnssRTCMStream; - - if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &gnssRTCMStream)) { - LOG_DEBUG(CAN, "RTCMStream decode failed"); - return; - } - LOG_DEBUG(CAN, "GNSS RTCM"); -} +void dronecanUpdate(timeUs_t currentTimeUs) +{ + static timeUs_t next_1hz_service_at = 0; + static timeUs_t busoffTimeUs = 0; + CanardCANFrame rx_frame; + int numMessagesToProcess = 0; + static enum dronecanState_e dronecanState = STATE_DRONECAN_INIT; + canardProtocolStatus_t protocolStatus = {}; + uint64_t timestamp; + int16_t rx_res; -void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_equipment_power_BatteryInfo batteryInfo; + switch(dronecanState) { + case STATE_DRONECAN_INIT: + next_1hz_service_at = currentTimeUs + 1000000ULL; // First 1Hz tick in 1 second + dronecanState = STATE_DRONECAN_NORMAL; + break; - if (uavcan_equipment_power_BatteryInfo_decode(transfer, &batteryInfo)) { - LOG_DEBUG(CAN, "BatteryInfo decode failed"); - return; - } - dronecanBatterySensorReceiveInfo(&batteryInfo); - LOG_DEBUG(CAN, "Battery Info"); -} + case STATE_DRONECAN_NORMAL: + processCanardTxQueue(); -/* - handle a GetNodeInfo request -*/ + for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--) + { + timestamp = millis() * 1000ULL; + rx_res = canardSTM32Receive(&rx_frame); + + if (rx_res < 0) { + LOG_DEBUG(CAN, "Receive error %d", rx_res); + } + else if (rx_res > 0) // Success - process the frame + { + canardHandleRxFrame(&canard, &rx_frame, timestamp); + } + } + // Drain any TX frames queued by RX handlers (e.g. GetNodeInfo responses) + // in the same task cycle so multi-frame transfers complete before timeout. + processCanardTxQueue(); -// TODO: All the data in here is temporary for testing. If actually need to send valid data, edit accordingly. -void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) { - printf("GetNodeInfo request from %d", transfer->source_node_id); + if (currentTimeUs >= next_1hz_service_at) + { + next_1hz_service_at += 1000000ULL; + process1HzTasks(currentTimeUs); + processCanardTxQueue(); + } - uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; - struct uavcan_protocol_GetNodeInfoResponse pkt; + canardSTM32GetProtocolStatus(&protocolStatus); + if(protocolStatus.BusOff != 0) { + dronecanState = STATE_DRONECAN_BUS_OFF; + busoffTimeUs = currentTimeUs; + } + break; - memset(&pkt, 0, sizeof(pkt)); + case STATE_DRONECAN_BUS_OFF: + if(currentTimeUs > (busoffTimeUs + 100000)) { // Wait 100 mS + canardSTM32RecoverFromBusOff(); + busoffTimeUs = currentTimeUs; + } + canardSTM32GetProtocolStatus(&protocolStatus); + if(protocolStatus.BusOff == 0) { + dronecanState = STATE_DRONECAN_NORMAL; + } + break; - node_status.uptime_sec = millis() / 1000ULL; - pkt.status = node_status; + } - // fill in your major and minor firmware version - pkt.software_version.major = FC_VERSION_MAJOR; - pkt.software_version.minor = FC_VERSION_MINOR; - pkt.software_version.optional_field_flags = FC_VERSION_PATCH_LEVEL; - pkt.software_version.vcs_commit = strtoul(shortGitRevision, NULL, 16); // need to convert string to integer put git hash in here +} - // should fill in hardware version - pkt.hardware_version.major = 1; - pkt.hardware_version.minor = 0; +/* --- TX and periodic task processing --- */ - // just setting all 16 bytes to 1 for testing - canardSTM32GetUniqueID(pkt.hardware_version.unique_id); +static void processCanardTxQueue(void) { + for (const CanardCANFrame *tx_frame ; (tx_frame = canardPeekTxQueue(&canard)) != NULL;) + { + const int16_t tx_res = canardSTM32Transmit(tx_frame); - strncpy((char*)pkt.name.data, FC_FIRMWARE_NAME, sizeof(pkt.name.data)); - pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data)); + if (tx_res < 0) { + LOG_DEBUG(CAN, "Transmit error %d", tx_res); + canardPopTxQueue(&canard); // Error - discard frame + } else if (tx_res > 0) { + canardPopTxQueue(&canard); // Success - remove from queue + } else { + // tx_res == 0: SW TX queue full, retry on next dronecanUpdate cycle + break; + } + } +} - uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); +/* + This function is called at 1 Hz rate from the main loop. +*/ +static void process1HzTasks(timeUs_t timestamp_usec) +{ + /* + Purge transfers that are no longer transmitted. This can free up some memory + */ + canardCleanupStaleTransfers(&canard, timestamp_usec); - canardRequestOrRespond(ins, - transfer->source_node_id, - UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, - UAVCAN_PROTOCOL_GETNODEINFO_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &buffer[0], - total_size); + /* + Transmit the node status message + */ + send_NodeStatus(); } +/* --- Message senders (outgoing) --- */ + // Canard Senders /* send the 1Hz NodeStatus message. This is what allows a node to show up in the DroneCAN GUI tool and in the flight controller logs */ -void send_NodeStatus(void) { +static void send_NodeStatus(void) { uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE]; - // LOG_DEBUG(CAN, "Sending Node Status"); node_status.uptime_sec = millis() / 1000UL; if(isHardwareHealthy()){ node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; @@ -222,7 +236,7 @@ void send_NodeStatus(void) { else { node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL; } - + node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; // Indicates that node is able to communicate over CAN, not that it is in flight. node_status.sub_mode = 0; // Not currently used in dronecan @@ -232,7 +246,7 @@ void send_NodeStatus(void) { uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); // we need a static variable for the transfer ID. This is - // incremeneted on each transfer, allowing for detection of packet + // incremented on each transfer, allowing for detection of packet // loss static uint8_t transfer_id; @@ -246,6 +260,8 @@ void send_NodeStatus(void) { // PrintCanStatus(); } +/* --- Libcanard protocol callbacks --- */ + // Canard Util /* This callback is invoked by the library when it detects beginning of a new transfer on the bus that can be received @@ -256,37 +272,36 @@ void send_NodeStatus(void) { This function must fill in the out_data_type_signature to be the signature of the message. */ - -bool shouldAcceptTransfer(const CanardInstance *ins, +static bool shouldAcceptTransfer(const CanardInstance *ins, uint64_t *out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, uint8_t source_node_id) { - UNUSED(ins); + UNUSED(ins); UNUSED(source_node_id); if (transfer_type == CanardTransferTypeRequest) { - // check if we want to handle a specific service request - switch (data_type_id) { - case UAVCAN_PROTOCOL_GETNODEINFO_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE; - return true; - } - } - } - if (transfer_type == CanardTransferTypeResponse) { - // check if we want to handle a specific service request - switch (data_type_id) { - } - } - if (transfer_type == CanardTransferTypeBroadcast) { - // see if we want to handle a specific broadcast packet - switch (data_type_id) { - - case UAVCAN_PROTOCOL_NODESTATUS_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE; - return true; - } + // check if we want to handle a specific service request + switch (data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: { + *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE; + return true; + } + } + } + if (transfer_type == CanardTransferTypeResponse) { + // check if we want to handle a specific service request + switch (data_type_id) { + } + } + if (transfer_type == CanardTransferTypeBroadcast) { + // see if we want to handle a specific broadcast packet + switch (data_type_id) { + + case UAVCAN_PROTOCOL_NODESTATUS_ID: { + *out_data_type_signature = UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE; + return true; + } case UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID: { *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE; return true; @@ -307,224 +322,201 @@ bool shouldAcceptTransfer(const CanardInstance *ins, *out_data_type_signature = UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE; return true; } - } - } - // we don't want any other messages - return false; + } + } + // we don't want any other messages + return false; } /* This callback is invoked by the library when a new message or request or response is received. */ -void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { - // switch on data type ID to pass to the right handler function - LOG_DEBUG(CAN, "Transfer type: %u, Transfer ID: %u ", transfer->transfer_type, transfer->data_type_id); - //LOG_DEBUG(CAN, "0x"); - //LOG_BUFFER_ERROR(SYSTEM, transfer->payload_head, transfer->payload_len); - // for (int i = 0; i < transfer->payload_len; i++) { - // LOG_DEBUG(CAN,"%02x", transfer->payload_head[i]); - // } - - if (transfer->transfer_type == CanardTransferTypeRequest) { - // check if we want to handle a specific service request - switch (transfer->data_type_id) { - case UAVCAN_PROTOCOL_GETNODEINFO_ID: { - handle_GetNodeInfo(ins, transfer); - break; - } - } - } - if (transfer->transfer_type == CanardTransferTypeResponse) { - switch (transfer->data_type_id) { - } - } - if (transfer->transfer_type == CanardTransferTypeBroadcast) { - // check if we want to handle a specific broadcast message - switch (transfer->data_type_id) { - - case UAVCAN_PROTOCOL_NODESTATUS_ID: +static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { + // switch on data type ID to pass to the right handler function + if (transfer->transfer_type == CanardTransferTypeRequest) { + // check if we want to handle a specific service request + switch (transfer->data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: { + handle_GetNodeInfo(ins, transfer); + break; + } + } + } + if (transfer->transfer_type == CanardTransferTypeResponse) { + switch (transfer->data_type_id) { + } + } + if (transfer->transfer_type == CanardTransferTypeBroadcast) { + // check if we want to handle a specific broadcast message + switch (transfer->data_type_id) { + + case UAVCAN_PROTOCOL_NODESTATUS_ID: handle_NodeStatus(ins, transfer); break; - - case UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID: + case UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID: handle_GNSSAuxiliary(ins, transfer); break; - - case UAVCAN_EQUIPMENT_GNSS_FIX_ID: + + case UAVCAN_EQUIPMENT_GNSS_FIX_ID: handle_GNSSFix(ins, transfer); break; - - case UAVCAN_EQUIPMENT_GNSS_FIX2_ID: + + case UAVCAN_EQUIPMENT_GNSS_FIX2_ID: handle_GNSSFix2(ins, transfer); break; - - case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID: + + case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID: handle_GNSSRCTMStream(ins, transfer); break; - + case UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID: - LOG_DEBUG(CAN, "Battery Info"); handle_BatteryInfo(ins, transfer); break; } - } + } } +/* --- Message handlers (incoming) --- */ -void processCanardTxQueue(void) { - // Transmitting - for (const CanardCANFrame *tx_frame ; (tx_frame = canardPeekTxQueue(&canard)) != NULL;) - { - const int16_t tx_res = canardSTM32Transmit(tx_frame); +// Canard Handlers ( Many have code copied from libcanard esc_node example: https://github.com/dronecan/libcanard/blob/master/examples/ESCNode/esc_node.c ) - if (tx_res < 0) { - LOG_DEBUG(CAN, "Transmit error %d", tx_res); - canardPopTxQueue(&canard); // Error - discard frame - } else if (tx_res > 0) { - // LOG_DEBUG(CAN, "Successfully transmitted message"); - canardPopTxQueue(&canard); // Success - remove from queue - } else { - // tx_res == 0: TX FIFO full, retry later - break; - } - } +static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_protocol_NodeStatus nodeStatus; -} + if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) { + LOG_DEBUG(CAN, "NodeStatus decode failed"); + return; + } -/* - This function is called at 1 Hz rate from the main loop. -*/ -void process1HzTasks(timeUs_t timestamp_usec) -{ - /* - Purge transfers that are no longer transmitted. This can free up some memory - */ - canardCleanupStaleTransfers(&canard, timestamp_usec); - /* - Transmit the node status message - */ - send_NodeStatus(); + switch (nodeStatus.health) { + case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK: + break; + case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING: + break; + case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR: + break; + case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL: + break; + default: + break; + } + + + switch(nodeStatus.mode) { + case UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL: + break; + case UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION: + break; + case UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE: + break; + case UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE: + break; + case UAVCAN_PROTOCOL_NODESTATUS_MODE_OFFLINE: + break; + default: + break; + } } -void dronecanInit(void) -{ - LOG_DEBUG(CAN, "dronecan Init"); - uint32_t bitrate = 500000; // At least define 500000 +static void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_equipment_gnss_Auxiliary gnssAuxiliary; - switch (dronecanConfig()->bitRateKbps){ - case DRONECAN_BITRATE_125KBPS: - bitrate = 125000; - break; + if (uavcan_equipment_gnss_Auxiliary_decode(transfer, &gnssAuxiliary)) { + LOG_DEBUG(CAN, "GNSSAuxiliary decode failed"); + return; + } + dronecanGPSReceiveGNSSAuxiliary(&gnssAuxiliary); +} - case DRONECAN_BITRATE_250KBPS: - bitrate = 250000; - break; - - case DRONECAN_BITRATE_500KBPS: - bitrate = 500000; - break; +static void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_equipment_gnss_Fix gnssFix; - case DRONECAN_BITRATE_1000KBPS: - bitrate = 1000000; - break; + if (uavcan_equipment_gnss_Fix_decode(transfer, &gnssFix)) { + LOG_DEBUG(CAN, "GNSSFix decode failed"); + return; + } + dronecanGPSReceiveGNSSFix(&gnssFix); +} - case DRONECAN_BITRATE_COUNT: - LOG_ERROR(SYSTEM, "Undefined bitrate set in configuration. 500kbps selected"); - bitrate = 500000; - break; +static void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_equipment_gnss_Fix2 gnssFix2; + + if (uavcan_equipment_gnss_Fix2_decode(transfer, &gnssFix2)) { + LOG_DEBUG(CAN, "GNSSFix2 decode failed"); + return; } - if(canardSTM32CAN1_Init(bitrate) != CANARD_OK) - { - LOG_ERROR(CAN, "Unable to initialize the CAN peripheral"); - // TODO: Notify the user that CAN does not work and disable the peripheral + dronecanGPSReceiveGNSSFix2(&gnssFix2); +} + +static void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_equipment_gnss_RTCMStream gnssRTCMStream; + + if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &gnssRTCMStream)) { + LOG_DEBUG(CAN, "RTCMStream decode failed"); return; - } - /* - Initializing the Libcanard instance. - */ - canardInit(&canard, - memory_pool, - sizeof(memory_pool), - onTransferReceived, - shouldAcceptTransfer, - NULL); + } +} - // Could use DNA (Dynamic Node Allocation) by following example in esc_node.c but that requires a lot of setup and I'm not too sure of what advantage it brings - // Instead, set a different NODE_ID for each device on the CAN bus by configuring node_settings - if (dronecanConfig()->nodeID > 0) { - canardSetLocalNodeID(&canard, dronecanConfig()->nodeID); - } else { - LOG_DEBUG(CAN, "Node ID is 0, this node is anonymous and can't transmit most messages. Please update this in config"); +static void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_equipment_power_BatteryInfo batteryInfo; + + if (uavcan_equipment_power_BatteryInfo_decode(transfer, &batteryInfo)) { + LOG_DEBUG(CAN, "BatteryInfo decode failed"); + return; } + dronecanBatterySensorReceiveInfo(&batteryInfo); } -void dronecanUpdate(timeUs_t currentTimeUs) -{ - static timeUs_t next_1hz_service_at = 0; - static timeUs_t busoffTimeUs = 0; - CanardCANFrame rx_frame; - int numMessagesToProcess = 0; - static enum dronecanState_e dronecanState = STATE_DRONECAN_INIT; - canardProtocolStatus_t protocolStatus = {}; - uint64_t timestamp; - int16_t rx_res; +/* + handle a GetNodeInfo request +*/ - switch(dronecanState) { - case STATE_DRONECAN_INIT: - next_1hz_service_at = currentTimeUs + 1000000ULL; // First 1Hz tick in 1 second - dronecanState = STATE_DRONECAN_NORMAL; - break; +static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) { + LOG_DEBUG(CAN, "GetNodeInfo request from %d", transfer->source_node_id); - case STATE_DRONECAN_NORMAL: - processCanardTxQueue(); + uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; + struct uavcan_protocol_GetNodeInfoResponse pkt; - for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--) - { - //LOG_DEBUG(CAN, "Received a message"); - //LOG_DEBUG(CAN, "Rx FIFO Fill Level: %lu", canardSTM32GetRxFifoFillLevel()); - timestamp = millis() * 1000ULL; - rx_res = canardSTM32Recieve(&rx_frame); - - if (rx_res < 0) { - LOG_DEBUG(CAN, "Receive error %d", rx_res); - } - else if (rx_res > 0) // Success - process the frame - { - canardHandleRxFrame(&canard, &rx_frame, timestamp); - } - } - // Drain any TX frames queued by RX handlers (e.g. GetNodeInfo responses) - // in the same task cycle so multi-frame transfers complete before timeout. - processCanardTxQueue(); + memset(&pkt, 0, sizeof(pkt)); - if (currentTimeUs >= next_1hz_service_at) - { - next_1hz_service_at += 1000000ULL; - process1HzTasks(currentTimeUs); - processCanardTxQueue(); - } + node_status.uptime_sec = millis() / 1000ULL; + pkt.status = node_status; - canardSTM32GetProtocolStatus(&protocolStatus); - if(protocolStatus.BusOff != 0) { - dronecanState = STATE_DRONECAN_BUS_OFF; - busoffTimeUs = currentTimeUs; - } - break; + // fill in your major and minor firmware version + pkt.software_version.major = FC_VERSION_MAJOR; + pkt.software_version.minor = FC_VERSION_MINOR; + pkt.software_version.optional_field_flags = UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT; + pkt.software_version.vcs_commit = strtoul(shortGitRevision, NULL, 16); // need to convert string to integer put git hash in here - case STATE_DRONECAN_BUS_OFF: - if(currentTimeUs > (busoffTimeUs + 100000)) { // Wait 100 mS - canardSTM32RecoverFromBusOff(); - busoffTimeUs = currentTimeUs; - } - canardSTM32GetProtocolStatus(&protocolStatus); - if(protocolStatus.BusOff == 0) { - dronecanState = STATE_DRONECAN_NORMAL; - } - break; - - } - + // should fill in hardware version + pkt.hardware_version.major = 1; + pkt.hardware_version.minor = 0; + + // just setting all 16 bytes to 1 for testing + canardSTM32GetUniqueID(pkt.hardware_version.unique_id); + + strncpy((char*)pkt.name.data, FC_FIRMWARE_NAME, sizeof(pkt.name.data)); + 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); } + #endif diff --git a/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c b/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c index 8743e779dea..d2bcad307ad 100644 --- a/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c @@ -106,8 +106,7 @@ static int16_t sitlCANTransmitStub(const CanardCANFrame* const tx_frame) { } static void sitlCANGetStatsStub(canardProtocolStatus_t *pProtocolStat) { - pProtocolStat->BusOff = 0; - pProtocolStat->ErrorPassive = 0; + memset(pProtocolStat, 0, sizeof(*pProtocolStat)); } #ifdef __linux__ @@ -267,11 +266,9 @@ static int16_t sitlCANTransmitSocketCAN(const CanardCANFrame* const tx_frame) { * @param pProtocolStat Pointer to status structure to fill */ static void sitlCANGetStatsSocketCAN(canardProtocolStatus_t *pProtocolStat) { - // SocketCAN doesn't expose bus-off/error-passive directly - // We could check interface flags via netlink, but for SITL testing - // we assume the virtual CAN is always healthy - pProtocolStat->BusOff = 0; - pProtocolStat->ErrorPassive = 0; + // SocketCAN doesn't expose bus-off/error-passive directly. + // Assume the virtual CAN is always healthy for SITL testing. + memset(pProtocolStat, 0, sizeof(*pProtocolStat)); } #endif // __linux__ @@ -280,7 +277,7 @@ static void sitlCANGetStatsSocketCAN(canardProtocolStatus_t *pProtocolStat) { * @param rx_frame Pointer to frame structure to fill * @retval 0 if no frame available, 1 if frame received, negative on error */ -int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { +int16_t canardSTM32Receive(CanardCANFrame *const rx_frame) { if (rx_frame == NULL) { return -CANARD_ERROR_INVALID_ARGUMENT; } @@ -357,6 +354,14 @@ int32_t canardSTM32GetRxFifoFillLevel(void) { return 0; } +/** + * @brief Get SW TX queue fill level + * @retval 0 — SITL has no software TX queue + */ +int32_t canardSTM32GetTxQueueFillLevel(void) { + return 0; +} + /** * @brief Recover from bus-off condition */ diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h b/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h index fe70cddf1d9..7ea72881456 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h +++ b/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h @@ -12,16 +12,23 @@ typedef struct { uint32_t BusOff; uint32_t ErrorPassive; + uint8_t tec; // Transmit Error Counter (ESR[23:16]) + uint8_t rec; // Receive Error Counter (ESR[31:24]) + uint8_t lec; // Last Error Code (ESR[6:4]) + uint16_t tx_dropped; // Frames dropped due to SW TX queue full + uint8_t tx_queue_hwm; // TX SW queue high water mark + uint8_t rx_buffer_hwm; // RX buffer high water mark } canardProtocolStatus_t; #ifdef USE_DRONECAN int16_t canardSTM32CAN1_Init(uint32_t bitrate); -int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame); +int16_t canardSTM32Receive(CanardCANFrame *const rx_frame); int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame); void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat); int32_t canardSTM32GetRxFifoFillLevel(void); +int32_t canardSTM32GetTxQueueFillLevel(void); void canardSTM32RecoverFromBusOff(void); void canardSTM32GetUniqueID(uint8_t id[16]); diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index dd7d3e1b357..ed588d87e7b 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -1,5 +1,5 @@ /* - * canard_stm32_driver.c + * canard_stm32f7xx_driver.c * * Created on: Jul 8, 2024 * Author: Roni Kant @@ -7,7 +7,9 @@ #include "common/log.h" #include "common/time.h" +#include "build/atomic.h" #include "drivers/io.h" +#include "drivers/nvic.h" #include "canard.h" #include "canard_stm32_driver.h" @@ -18,7 +20,10 @@ #include #include +/* --- Internal types and state --- */ + #define RX_BUFFER_SIZE 32 +#define TX_QUEUE_SIZE 32 struct Timings { uint16_t prescaler; @@ -33,239 +38,27 @@ typedef struct { } RxFrame_t; static struct RxBuffer_t { - uint8_t writeIndex; - uint8_t readIndex; + volatile uint8_t writeIndex; // written in ISR, read in main loop + volatile uint8_t readIndex; RxFrame_t rxMsg[RX_BUFFER_SIZE]; } RxBuffer; -static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings*out_timings); -static void canardSTM32GPIO_Init(void); +static struct CanTxQueue_t { + CanardCANFrame frames[TX_QUEUE_SIZE]; + volatile uint8_t head; + volatile uint8_t tail; +} canTxQueue; static CAN_HandleTypeDef hcan1; -RxFrame_t rxMsg; - -uint8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { - uint8_t next; - RxFrame_t *pCurrentRxMsg; - - next = rxBuf->writeIndex + 1; - if(next >= RX_BUFFER_SIZE){ - next = 0; - } - - if(next == rxBuf->readIndex) { - return -1; // rxBuf is full - } - pCurrentRxMsg = &rxBuf->rxMsg[rxBuf->writeIndex]; - memcpy(pCurrentRxMsg, rxMsg, sizeof(RxFrame_t)); - rxBuf->writeIndex = next; - return 0; -} - -uint8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { - uint8_t next; - RxFrame_t *pCurrentRxMsg; - - if(rxBuf->writeIndex == rxBuf->readIndex){ - return -1; // Nothing to read - } - - next = rxBuf->readIndex + 1; - if (next >= RX_BUFFER_SIZE){ - next = 0; - } - pCurrentRxMsg = &rxBuf->rxMsg[rxBuf->readIndex]; - memcpy(rxMsg, pCurrentRxMsg, sizeof(RxFrame_t)); - rxBuf->readIndex = next; - return 0; -} - -uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf) { - if(rxBuf->writeIndex < rxBuf->readIndex) - return((rxBuf->writeIndex + RX_BUFFER_SIZE) - rxBuf->readIndex); - - return (rxBuf->writeIndex - rxBuf->readIndex); -} - -/** - * @brief Process CAN message from RxLocation FIFO into rx_frame - * @param rx_frame pointer to a CanardCANFrame structure where the received CAN message will be - * stored. - * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode - */ -int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { - RxFrame_t canRxFrame; - - if (rx_frame == NULL) { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - - if (rxBufferPopFrame(&RxBuffer, &canRxFrame) == 0) { // Wheres the data? - rx_frame->id = canRxFrame.header.ExtId; - - // Process ID to canard format - if (canRxFrame.header.IDE == CAN_ID_EXT) { // canard will only process the message if it is extended ID - rx_frame->id |= CANARD_CAN_FRAME_EFF; - } - - if (canRxFrame.header.RTR == CAN_RTR_REMOTE) { // canard won't process the message if it is a remote frame - rx_frame->id |= CANARD_CAN_FRAME_RTR; - } - - rx_frame->data_len = canRxFrame.header.DLC; - memcpy(rx_frame->data, canRxFrame.data, canRxFrame.header.DLC); - - // assume a single interface - rx_frame->iface_id = 0; - return 1; - } - // Either no CAN msg to be read, or an error that can be read from hfdcan->ErrorCode - return 0; -} - -/** - * @brief Process tx_frame CAN message into Tx FIFO/Queue and transmit it - * @param tx_frame pointer to a CanardCANFrame structure that contains the CAN message to - * transmit. - * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode - */ -int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { - CAN_TxHeaderTypeDef txHeader = {}; - uint8_t txData[8]; - uint32_t txMailbox; - uint32_t returnCode; - - if (tx_frame == NULL) { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - - if (tx_frame->id & CANARD_CAN_FRAME_ERR) { - return -CANARD_ERROR_INVALID_ARGUMENT; // unsupported frame format - } - - // Process canard id to STM FDCAN header format - if (tx_frame->id & CANARD_CAN_FRAME_EFF) { - txHeader.IDE = CAN_ID_EXT; - txHeader.ExtId = tx_frame->id & CANARD_CAN_EXT_ID_MASK; - } else { - txHeader.IDE = CAN_ID_STD; - txHeader.StdId = tx_frame->id & CANARD_CAN_STD_ID_MASK; - } - - txHeader.DLC = tx_frame->data_len; - - if (tx_frame->id & CANARD_CAN_FRAME_RTR) { - txHeader.RTR = CAN_RTR_REMOTE; - } else { - txHeader.RTR = CAN_RTR_DATA; - } - - memcpy(txData, tx_frame->data, tx_frame->data_len); - - returnCode = HAL_CAN_AddTxMessage(&hcan1, &txHeader, txData, &txMailbox); - if( returnCode == HAL_OK) { - // LOG_DEBUG(CAN, "Successfully sent message with id: %lu", tx_frame->id); - return 1; - } - - LOG_DEBUG(CAN, "Failed at adding message with id: %lu to Tx Queue. Error: %lu", tx_frame->id, returnCode); - - // TX failed (FIFO full or other error) - return 0 to signal retry needed - return 0; -} - -/** - * @brief FDCAN1 Initialization Function - * @param hfdcan pointer to an FDCAN_HandleTypeDef structure that contains - * the configuration information for the specified FDCAN. - * @param bitrate desired bitrate to run the CAN network at. - * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode - */ -int16_t canardSTM32CAN1_Init(uint32_t bitrate) -{ -// RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0}; - struct Timings out_timings; - - /* CAN1 clock enable */ - __HAL_RCC_CAN1_CLK_ENABLE(); - - // /* USER CODE BEGIN CAN1_MspInit 1 */ - - CAN_FilterTypeDef sFilterConfig; - sFilterConfig.FilterIdHigh = 0; - sFilterConfig.FilterIdLow = 0; - sFilterConfig.FilterMaskIdHigh = 0; - sFilterConfig.FilterMaskIdLow = 0; - sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0; - sFilterConfig.FilterBank = 0; - sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; - sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; - sFilterConfig.FilterActivation = ENABLE; - - hcan1.Instance = CAN1; - hcan1.Init.Mode = CAN_MODE_NORMAL; - hcan1.Init.TimeTriggeredMode = DISABLE; - hcan1.Init.AutoBusOff = ENABLE; - hcan1.Init.AutoWakeUp = DISABLE; - hcan1.Init.AutoRetransmission = ENABLE; - hcan1.Init.ReceiveFifoLocked = DISABLE; - hcan1.Init.TransmitFifoPriority = DISABLE; - - canardSTM32ComputeTimings(bitrate, &out_timings); - hcan1.Init.Prescaler = out_timings.prescaler; - hcan1.Init.SyncJumpWidth = (uint32_t)out_timings.sjw << CAN_BTR_SJW_Pos; - hcan1.Init.TimeSeg1 = (uint32_t)out_timings.bs1 << CAN_BTR_TS1_Pos; - hcan1.Init.TimeSeg2 = (uint32_t)out_timings.bs2 << CAN_BTR_TS2_Pos; - LOG_DEBUG(CAN, "Prescaler: %d, SJW: %d, BS1: %d, BS2: %d", out_timings.prescaler, out_timings.sjw, out_timings.bs1, out_timings.bs2); +// canTxDropped and canTxQueueHWM are written by main loop (canTxQueuePush), read by main loop only. +// canRxBufferHWM is written by ISR (rxBufferPushFrame), read by main loop only. +// All are uint8/uint16 so reads are single-instruction atomic on Cortex-M. +static volatile uint16_t canTxDropped = 0; +static volatile uint8_t canTxQueueHWM = 0; +static volatile uint8_t canRxBufferHWM = 0; - // hcan1.Init.StdFiltersNbr = 0; - // hcan1.Init.ExtFiltersNbr = 1; - // hcan1.Init.TxFifoQueueElmtsNbr = 32; - // LOG_DEBUG(CAN, "In CAN Init"); - - /** Initializes the peripherals clock - */ - // PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_FDCAN; - // PeriphClkInitStruct.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL; - // if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) - // { - // LOG_DEBUG(CAN, "Unable to configure peripheral clock"); - // } - - canardSTM32GPIO_Init(); // Set up the pins for CAN and optional listen only mode - - // LOG_DEBUG(CAN, "System Clock Speed: %lu", HAL_RCC_GetSysClockFreq()); - // LOG_DEBUG(CAN, "PClk1 Clock Speed: %lu", HAL_RCC_GetPCLK1Freq()); - if (HAL_CAN_Init(&hcan1) != HAL_OK) - { - LOG_ERROR(CAN, "Failed CAN Init"); - return -CANARD_ERROR_INTERNAL; - } - - /* USER CODE BEGIN FDCAN1_Init 2 */ - if (HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig) != HAL_OK) { - LOG_ERROR(CAN, "Failed Config Filter"); - return -CANARD_ERROR_INTERNAL; - } - - if (HAL_CAN_Start(&hcan1) != HAL_OK) { - LOG_ERROR(CAN, "Failed to Start"); - return -CANARD_ERROR_INTERNAL; - } - - if (HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK) { // persistent Rx notification - LOG_ERROR(CAN, "Failed to activate interrupt"); - return -CANARD_ERROR_INTERNAL; - } - - // Enable interrupt only after all initialization succeeds - // (if any previous step failed, we return early without enabling IRQ) - HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); - - return CANARD_OK; -} +/* --- Initialization --- */ /** * @brief GPIO Initialization Function @@ -274,17 +67,16 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) */ static void canardSTM32GPIO_Init(void) { - // Set up the Rx and Tx pins for CAN1 and if present, the standby or listen only pin. #if defined(CAN1_TX) && defined(CAN1_RX) IOInit(IOGetByTag(IO_TAG(CAN1_TX)), OWNER_DRONECAN, RESOURCE_CAN_TX, 0); - IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_TX)), IOCFG_AF_PP, GPIO_AF9_CAN1); // How do I make the alternate function crossplatform? + IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_TX)), IOCFG_AF_PP, GPIO_AF9_CAN1); IOInit(IOGetByTag(IO_TAG(CAN1_RX)), OWNER_DRONECAN, RESOURCE_CAN_RX, 0); - IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_RX)), IOCFG_AF_PP, GPIO_AF9_CAN1); // How do I make the alternate function crossplatform? + IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_RX)), IOCFG_AF_PP, GPIO_AF9_CAN1); #endif - #ifdef CAN1_STANDBY +#ifdef CAN1_STANDBY // Initialize the standby or listen only pin. Set default state to enable CAN. // TODO: Tie the pin state to a configuration option so we can turn CAN on and off. @@ -293,9 +85,17 @@ static void canardSTM32GPIO_Init(void) IOLo(IOGetByTag(IO_TAG(CAN1_STANDBY))); #endif } + +/** + * @brief Compute bxCAN bit-timing parameters (prescaler, SJW, BS1, BS2) for a + * given target bitrate, targeting an ~87.5% sample point location. + * @param target_bitrate Desired CAN bus bitrate in bits per second. + * @param out_timings Pointer to a Timings struct to receive the computed values. + * @retval true on success, false if no valid timing solution exists for the bitrate. + */ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings *out_timings) { - + if (target_bitrate < 1) { return false; } @@ -319,10 +119,7 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi * 250 kbps 16 17 * 125 kbps 16 17 */ - const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 18; - LOG_DEBUG(CAN, "Baudrate: %lu", target_bitrate); - LOG_DEBUG(CAN, "Max Quanta per bit: %i", max_quanta_per_bit); - LOG_DEBUG(CAN, "Pclk1: %lu", pclk); + const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 18; // TODO: reference paper max is 17; H7 uses 17 — revisit alongside SJW tuning static const int MaxSamplePointLocation = 900; /* @@ -336,7 +133,6 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi * PRESCALER_BS = PCLK / BITRATE */ const uint32_t prescaler_bs = pclk / target_bitrate; - LOG_DEBUG(CAN, "Prescaler BS product: %lu", prescaler_bs); /* * Searching for such prescaler value so that the number of quanta per bit is highest. */ @@ -353,8 +149,6 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi if ((prescaler < 1U) || (prescaler > 1024U)) { return false; // No solution } - LOG_DEBUG(CAN, "Prescaler: %lu", prescaler); - /* * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum. * We need to find the values so that the sample point is as close as possible to the optimal value. @@ -408,33 +202,420 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi (int)(1 + solution.bs1 + solution.bs2), (double)(solution.sample_point_permill) / (double)(10.0)); out_timings->prescaler = (uint16_t)(prescaler); - out_timings->sjw = 3; // Not happy with this value, but 1MBPs with unshielded cable? + out_timings->sjw = 3; // TODO: review SJW value — stored raw (no N-1 offset unlike BS1/BS2), so register encodes 4TQ not 3TQ; verify this is intentional out_timings->bs1 = (uint8_t)(solution.bs1)-1; // The HAL does not take care of the 1 bs offset in the register so remove it here like AP does. out_timings->bs2 = (uint8_t)(solution.bs2)-1; // The HAL does not take care of the 1 bs offset in the register so remove it here like AP does. return true; } -void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){ +/** + * @brief Initialize the bxCAN1 peripheral at the requested bitrate. + * Configures GPIO pins, bit timings, a pass-all acceptance filter, + * starts the peripheral, and enables RX and TX interrupts. + * @param bitrate Desired CAN bus bitrate in bits per second (e.g. 1000000). + * @retval CANARD_OK (0) on success, negative CANARD_ERROR code on failure. + */ +int16_t canardSTM32CAN1_Init(uint32_t bitrate) +{ + struct Timings out_timings; + + /* CAN1 clock enable */ + __HAL_RCC_CAN1_CLK_ENABLE(); + + CAN_FilterTypeDef sFilterConfig; + sFilterConfig.FilterIdHigh = 0; + sFilterConfig.FilterIdLow = 0; + sFilterConfig.FilterMaskIdHigh = 0; + sFilterConfig.FilterMaskIdLow = 0; + sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0; + sFilterConfig.FilterBank = 0; + sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; + sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; + sFilterConfig.FilterActivation = ENABLE; + + hcan1.Instance = CAN1; + hcan1.Init.Mode = CAN_MODE_NORMAL; + hcan1.Init.TimeTriggeredMode = DISABLE; + hcan1.Init.AutoBusOff = ENABLE; + hcan1.Init.AutoWakeUp = DISABLE; + hcan1.Init.AutoRetransmission = ENABLE; + hcan1.Init.ReceiveFifoLocked = DISABLE; + hcan1.Init.TransmitFifoPriority = ENABLE; // transmit in request order, not mailbox-number order + + if (!canardSTM32ComputeTimings(bitrate, &out_timings)) { + LOG_ERROR(CAN, "Failed to compute CAN timings for bitrate %lu", (unsigned long)bitrate); + return -CANARD_ERROR_INTERNAL; + } + + hcan1.Init.Prescaler = out_timings.prescaler; + hcan1.Init.SyncJumpWidth = (uint32_t)out_timings.sjw << CAN_BTR_SJW_Pos; + hcan1.Init.TimeSeg1 = (uint32_t)out_timings.bs1 << CAN_BTR_TS1_Pos; + hcan1.Init.TimeSeg2 = (uint32_t)out_timings.bs2 << CAN_BTR_TS2_Pos; + LOG_DEBUG(CAN, "Prescaler: %d, SJW: %d, BS1: %d, BS2: %d", out_timings.prescaler, out_timings.sjw, out_timings.bs1, out_timings.bs2); + + canardSTM32GPIO_Init(); // Set up the pins for CAN and optional standby control + + if (HAL_CAN_Init(&hcan1) != HAL_OK) + { + LOG_ERROR(CAN, "Failed CAN Init"); + return -CANARD_ERROR_INTERNAL; + } + + if (HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig) != HAL_OK) { + LOG_ERROR(CAN, "Failed Config Filter"); + return -CANARD_ERROR_INTERNAL; + } + + if (HAL_CAN_Start(&hcan1) != HAL_OK) { + LOG_ERROR(CAN, "Failed to Start"); + return -CANARD_ERROR_INTERNAL; + } + + if (HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK) { // persistent Rx notification + LOG_ERROR(CAN, "Failed to activate interrupt"); + return -CANARD_ERROR_INTERNAL; + } + + // Enable interrupt only after all initialization succeeds + // (if any previous step failed, we return early without enabling IRQ) + HAL_NVIC_SetPriority(CAN1_RX0_IRQn, NVIC_PRIO_CAN, 0); + HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); + HAL_NVIC_SetPriority(CAN1_TX_IRQn, NVIC_PRIO_CAN, 0); + HAL_NVIC_EnableIRQ(CAN1_TX_IRQn); + + return CANARD_OK; +} + +/* --- RX subsystem --- */ + +/** + * @brief Push a received CAN frame into the ring buffer (called from ISR). + * @param rxBuf Pointer to the RxBuffer_t ring buffer. + * @param rxMsg Pointer to the RxFrame_t to copy into the buffer. + * @retval 0 on success, -1 if the buffer is full (frame dropped). + */ +static int8_t rxBufferPushFrame(volatile struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { + uint8_t next; + RxFrame_t *pCurrentRxMsg; + + next = rxBuf->writeIndex + 1; + if(next >= RX_BUFFER_SIZE){ + next = 0; + } + + if(next == rxBuf->readIndex) { + return -1; // rxBuf is full + } + pCurrentRxMsg = (RxFrame_t *)&rxBuf->rxMsg[rxBuf->writeIndex]; + memcpy(pCurrentRxMsg, rxMsg, sizeof(RxFrame_t)); + __DMB(); // ensure frame data is visible to main loop before writeIndex advances + rxBuf->writeIndex = next; + uint8_t rxFill = (next >= rxBuf->readIndex) ? (next - rxBuf->readIndex) : (next + RX_BUFFER_SIZE - rxBuf->readIndex); + if (rxFill > canRxBufferHWM) canRxBufferHWM = rxFill; + return 0; +} + +/** + * @brief Pop the oldest CAN frame from the ring buffer (called from main loop). + * @param rxBuf Pointer to the RxBuffer_t ring buffer. + * @param rxMsg Pointer to an RxFrame_t where the frame will be copied. + * @retval 0 on success, -1 if the buffer is empty. + */ +static int8_t rxBufferPopFrame(volatile struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { + uint8_t next; + RxFrame_t *pCurrentRxMsg; + + if(rxBuf->writeIndex == rxBuf->readIndex){ + return -1; // Nothing to read + } + + next = rxBuf->readIndex + 1; + if (next >= RX_BUFFER_SIZE){ + next = 0; + } + uint8_t ridx = rxBuf->readIndex; // snapshot index before barrier + __DMB(); // ensure ISR's frame data writes are visible before we read them + pCurrentRxMsg = (RxFrame_t *)&rxBuf->rxMsg[ridx]; + memcpy(rxMsg, pCurrentRxMsg, sizeof(RxFrame_t)); + rxBuf->readIndex = next; + return 0; +} + +/** + * @brief Return the number of frames currently held in the ring buffer. + * @param rxBuf Pointer to the RxBuffer_t ring buffer. + * @retval Number of frames available to read (0 to RX_BUFFER_SIZE-1). + */ +static uint8_t rxBufferNumMessages(volatile struct RxBuffer_t *rxBuf) { + if(rxBuf->writeIndex < rxBuf->readIndex) + return((rxBuf->writeIndex + RX_BUFFER_SIZE) - rxBuf->readIndex); + + return (rxBuf->writeIndex - rxBuf->readIndex); +} + +/** + * @brief CAN1 RX FIFO0 interrupt handler — delegates to the HAL IRQ handler. + */ +void CAN1_RX0_IRQHandler(void) { + HAL_CAN_IRQHandler(&hcan1); +} + +/** + * @brief HAL callback fired when a frame arrives in RX FIFO0 (runs in ISR context). + * Reads the frame from hardware and pushes it into the software RX ring buffer. + * @param hcan Pointer to the CAN_HandleTypeDef that triggered the callback. + */ +void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { + RxFrame_t frame; + if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &frame.header, frame.data) == HAL_OK) { + rxBufferPushFrame(&RxBuffer, &frame); + } +} + +/** + * @brief Process CAN message from RxLocation FIFO into rx_frame + * @param rx_frame pointer to a CanardCANFrame structure where the received CAN message will be + * stored. + * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode + */ +int16_t canardSTM32Receive(CanardCANFrame *const rx_frame) { + RxFrame_t canRxFrame; + + if (rx_frame == NULL) { + return -CANARD_ERROR_INVALID_ARGUMENT; + } + + if (rxBufferPopFrame(&RxBuffer, &canRxFrame) == 0) { + rx_frame->id = canRxFrame.header.ExtId; + + // Process ID to canard format + if (canRxFrame.header.IDE == CAN_ID_EXT) { // canard will only process the message if it is extended ID + rx_frame->id |= CANARD_CAN_FRAME_EFF; + } - pProtocolStat->BusOff = __HAL_CAN_GET_FLAG(&hcan1, CAN_FLAG_BOF); + if (canRxFrame.header.RTR == CAN_RTR_REMOTE) { // canard won't process the message if it is a remote frame + rx_frame->id |= CANARD_CAN_FRAME_RTR; + } + + rx_frame->data_len = canRxFrame.header.DLC; + memcpy(rx_frame->data, canRxFrame.data, canRxFrame.header.DLC); + + // assume a single interface + rx_frame->iface_id = 0; + return 1; + } + // Either no CAN msg to be read, or an error that can be read from hfdcan->ErrorCode + return 0; +} + +/* --- TX subsystem --- */ + +/** + * @brief Push a frame onto the software TX queue (called from main loop). + * Updates the high-water mark and increments the drop counter if full. + * @param frame Pointer to the CanardCANFrame to enqueue. + * @retval true if the frame was enqueued, false if the queue was full (frame dropped). + */ +static bool canTxQueuePush(const CanardCANFrame *frame) { + uint8_t next = (canTxQueue.head + 1) % TX_QUEUE_SIZE; + // Read tail fresh: ISR only advances tail, so a fresh read can only show more space, + // never less — eliminates false-full when ISR drains a slot concurrently. + if (next == canTxQueue.tail) { + canTxDropped++; + return false; + } + canTxQueue.frames[canTxQueue.head] = *frame; + __DMB(); // ensure frame data is visible before head advances + canTxQueue.head = next; + uint8_t tail_snapshot = canTxQueue.tail; // snapshot after push; may undercount fill if ISR drains concurrently, acceptable for HWM + uint8_t fill = (next - tail_snapshot + TX_QUEUE_SIZE) % TX_QUEUE_SIZE; + if (fill > canTxQueueHWM) canTxQueueHWM = fill; + return true; +} + +/** + * @brief Check whether the software TX queue is empty. + * @retval true if the queue contains no frames, false otherwise. + */ +static bool canTxQueueIsEmpty(void) { + return canTxQueue.head == canTxQueue.tail; +} + +/** + * @brief Return a pointer to the next frame to transmit without consuming it. + * Issues a DMB to ensure all producer stores are visible before the data is read. + * Must be paired with canTxQueueConsume() after the frame is accepted by hardware. + * @retval Pointer to the tail CanardCANFrame, or NULL if the queue is empty. + */ +static const CanardCANFrame *canTxQueuePeek(void) { + if (canTxQueue.head == canTxQueue.tail) { + return NULL; + } + __DMB(); + return &canTxQueue.frames[canTxQueue.tail]; +} + +/** + * @brief Advance the tail to consume the frame returned by canTxQueuePeek(). + * Must only be called after canTxQueuePeek() returned non-NULL and the + * frame has been successfully accepted by the hardware (HAL_OK). + */ +static void canTxQueueConsume(void) { + canTxQueue.tail = (canTxQueue.tail + 1) % TX_QUEUE_SIZE; +} + +/** + * @brief Populate a HAL CAN TX header and data buffer from a CanardCANFrame. + * @param frame Pointer to the source CanardCANFrame. + * @param header Pointer to the CAN_TxHeaderTypeDef to fill. + * @param data Buffer of at least 8 bytes to copy frame payload into. + */ +static void buildTxHeader(const CanardCANFrame *frame, CAN_TxHeaderTypeDef *header, uint8_t *data) { + if (frame->id & CANARD_CAN_FRAME_EFF) { + header->IDE = CAN_ID_EXT; + header->ExtId = frame->id & CANARD_CAN_EXT_ID_MASK; + } else { + header->IDE = CAN_ID_STD; + header->StdId = frame->id & CANARD_CAN_STD_ID_MASK; + } + header->DLC = frame->data_len; + header->RTR = (frame->id & CANARD_CAN_FRAME_RTR) ? CAN_RTR_REMOTE : CAN_RTR_DATA; + header->TransmitGlobalTime = DISABLE; + memcpy(data, frame->data, frame->data_len); +} + +/** + * @brief Drain the software TX queue into available hardware mailboxes. + * Called both from the TX-complete ISR callbacks and from canardSTM32Transmit() + * (with IRQs disabled) to seed the first transmission. With TXFP=ENABLE, + * hardware transmits mailboxes in request order, preserving frame sequence. + * Deactivates the TX empty notification when the queue is fully drained. + * @param hcan Pointer to the CAN_HandleTypeDef to load frames into. + */ +static void canTxDrainQueue(CAN_HandleTypeDef *hcan) { + // With TXFP=ENABLE the hardware transmits mailboxes in request order + // (chronological), not mailbox-number order, so filling multiple mailboxes + // per callback is safe and preserves frame sequence. + const CanardCANFrame *frame; + while ((frame = canTxQueuePeek()) != NULL && HAL_CAN_GetTxMailboxesFreeLevel(hcan) > 0) { + CAN_TxHeaderTypeDef txHeader = {}; + uint8_t txData[8]; + uint32_t txMailbox; + buildTxHeader(frame, &txHeader, txData); + if (HAL_CAN_AddTxMessage(hcan, &txHeader, txData, &txMailbox) != HAL_OK) { + break; // frame stays in queue and will retry on next ISR + } + canTxQueueConsume(); + } + if (canTxQueueIsEmpty()) { + HAL_CAN_DeactivateNotification(hcan, CAN_IT_TX_MAILBOX_EMPTY); + } +} + +/** + * @brief CAN1 TX interrupt handler — delegates to the HAL IRQ handler. + */ +void CAN1_TX_IRQHandler(void) { + HAL_CAN_IRQHandler(&hcan1); +} + +/** + * @brief HAL callbacks fired when a TX mailbox completes transmission (runs in ISR context). + * Each refills available mailboxes from the software TX queue via canTxDrainQueue(). + * AbortCallback is intentionally not wired: with AutoRetransmission=ENABLE the hardware + * retransmits automatically on arbitration loss (ALST) and the software abort path is + * never triggered. Bus-off is handled separately by the state machine in dronecanUpdate(). + */ +void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } +void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } +void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } + +/** + * @brief Process tx_frame CAN message into Tx FIFO/Queue and transmit it + * @param tx_frame pointer to a CanardCANFrame structure that contains the CAN message to + * transmit. + * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode + */ +int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { + if (tx_frame == NULL) { + return -CANARD_ERROR_INVALID_ARGUMENT; + } + if (tx_frame->id & CANARD_CAN_FRAME_ERR) { + return -CANARD_ERROR_INVALID_ARGUMENT; + } + + if (!canTxQueuePush(tx_frame)) { + return 0; // SW queue full - caller retries next cycle + } + + // If all mailboxes are idle, RQCP will never fire to start the ISR chain. + // Seed the HW via canTxDrainQueue while blocking only the CAN TX ISR (not + // higher-priority IRQs like the gyro timer) to preserve the SPSC contract — + // ISR is the sole consumer; we become the consumer only during this window. + ATOMIC_BLOCK(NVIC_PRIO_CAN) { + if (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) > 0) { + canTxDrainQueue(&hcan1); + } + if (!canTxQueueIsEmpty()) { + // If ActivateNotification fails (e.g. peripheral in error state) the frame is + // already in the SW queue. The next canardSTM32Transmit call will re-seed the HW. + HAL_CAN_ActivateNotification(&hcan1, CAN_IT_TX_MAILBOX_EMPTY); + } + } + return 1; +} + +/* --- Diagnostics --- */ + +/** + * @brief Read CAN bus error counters and queue statistics from hardware and + * software state into a canardProtocolStatus_t struct. + * @param pProtocolStat Pointer to the struct to populate with current status. + */ +void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){ + uint32_t esr = hcan1.Instance->ESR; + pProtocolStat->BusOff = __HAL_CAN_GET_FLAG(&hcan1, CAN_FLAG_BOF); pProtocolStat->ErrorPassive = __HAL_CAN_GET_FLAG(&hcan1, CAN_FLAG_EPV); - // LOG_DEBUG(CAN, "BusOff: %lu", pProtocolStat->BusOff); - // LOG_DEBUG(CAN, "ErrorPassive: %lu", pProtocolStat->ErrorPassive); + pProtocolStat->tec = (uint8_t)((esr >> 16) & 0xFF); + pProtocolStat->rec = (uint8_t)((esr >> 24) & 0xFF); + pProtocolStat->lec = (uint8_t)((esr >> 4) & 0x07); + pProtocolStat->tx_dropped = canTxDropped; + pProtocolStat->tx_queue_hwm = canTxQueueHWM; + pProtocolStat->rx_buffer_hwm = canRxBufferHWM; +} + +/** + * @brief Return the current number of frames pending in the software TX queue. + * @retval Number of frames queued (0 to TX_QUEUE_SIZE-1). + */ +int32_t canardSTM32GetTxQueueFillLevel(void) { + return (canTxQueue.head - canTxQueue.tail + TX_QUEUE_SIZE) % TX_QUEUE_SIZE; } +/** + * @brief Return the current number of frames waiting in the software RX ring buffer. + * @retval Number of frames available to read (0 to RX_BUFFER_SIZE-1). + */ int32_t canardSTM32GetRxFifoFillLevel(void){ return rxBufferNumMessages(&RxBuffer); } +/** + * @brief Attempt to recover the CAN peripheral from a bus-off condition. + * AutoBusOff=ENABLE in hcan1.Init means the hardware recovers automatically + * after 128 * 11 recessive bits; this function is a no-op placeholder. + */ void canardSTM32RecoverFromBusOff(void){ // Auto recover from bus off is enabled // CLEAR_BIT(hcan1.Instance->CCCR, FDCAN_CCCR_INIT); // Clear INIT bit to recover from Bus-Off } -/* - get a 16 byte unique ID for this node, this should be based on the CPU unique ID or other unique ID - */ +/** + * @brief Populate a 16-byte unique node ID from the STM32 96-bit factory UID. + * The 12 UID bytes are copied into the first 12 bytes; the remainder is zero-padded. + * @param id Output buffer of exactly 16 bytes to receive the unique ID. + */ void canardSTM32GetUniqueID(uint8_t id[16]) { uint32_t HALUniqueIDs[3]; // Make Unique ID out of the 96-bit STM32 UID and fill the rest with 0s @@ -444,14 +625,3 @@ void canardSTM32GetUniqueID(uint8_t id[16]) { HALUniqueIDs[2] = *(uint32_t *)(UID_BASE + 8); memcpy(id, HALUniqueIDs, 12); } - -void CAN1_RX0_IRQHandler(void) { - HAL_CAN_IRQHandler(&hcan1); -} - -void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { - RxFrame_t frame; - if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &frame.header, frame.data) == HAL_OK) { - rxBufferPushFrame(&RxBuffer, &frame); - } -} \ No newline at end of file diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c index 77c1f94921f..db8dc6ba237 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c @@ -1,5 +1,5 @@ /* - * canard_stm32_driver.c + * canard_stm32h7xx_driver.c * * Created on: Jul 8, 2024 * Author: Roni Kant @@ -18,6 +18,8 @@ #include #include +/* --- Internal types and state --- */ + struct Timings { uint16_t prescaler; uint8_t sjw; @@ -25,210 +27,9 @@ struct Timings { uint8_t bs2; }; -static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings*out_timings); -static void canardSTM32GPIO_Init(void); - static FDCAN_HandleTypeDef hfdcan1; -/** - * @brief Process CAN message from RxLocation FIFO into rx_frame - * @param hfdcan pointer to an FDCAN_HandleTypeDef structure that contains - * the configuration information for the specified FDCAN. - * @param RxLocation Location of the received message to be read. - * This parameter can be a value of @arg FDCAN_Rx_location. - * @param rx_frame pointer to a CanardCANFrame structure where the received CAN message will be - * stored. - * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode - */ -int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { - if (rx_frame == NULL) { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - - FDCAN_RxHeaderTypeDef RxHeader; - uint8_t RxData[8]; - - if (HAL_FDCAN_GetRxMessage(&hfdcan1, FDCAN_RX_FIFO0, &RxHeader, RxData) == HAL_OK) { - - // Process ID to canard format - rx_frame->id = RxHeader.Identifier; - - if (RxHeader.IdType == FDCAN_EXTENDED_ID) { // canard will only process the message if it is extended ID - rx_frame->id |= CANARD_CAN_FRAME_EFF; - } - - if (RxHeader.RxFrameType == FDCAN_REMOTE_FRAME) { // canard won't process the message if it is a remote frame - rx_frame->id |= CANARD_CAN_FRAME_RTR; - } - - rx_frame->data_len = RxHeader.DataLength; - memcpy(rx_frame->data, RxData, RxHeader.DataLength); - - // assume a single interface - rx_frame->iface_id = 0; - - return 1; - } - - // Either no CAN msg to be read, or an error that can be read from hfdcan->ErrorCode - return 0; -} - -/** - * @brief Process tx_frame CAN message into Tx FIFO/Queue and transmit it - * @param tx_frame pointer to a CanardCANFrame structure that contains the CAN message to - * transmit. - * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode - */ -int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { - if (tx_frame == NULL) { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - - if (tx_frame->id & CANARD_CAN_FRAME_ERR) { - return -CANARD_ERROR_INVALID_ARGUMENT; // unsupported frame format - } - - FDCAN_TxHeaderTypeDef TxHeader; - uint8_t TxData[8]; - - // Process canard id to STM FDCAN header format - if (tx_frame->id & CANARD_CAN_FRAME_EFF) { - TxHeader.IdType = FDCAN_EXTENDED_ID; - TxHeader.Identifier = tx_frame->id & CANARD_CAN_EXT_ID_MASK; - } else { - TxHeader.IdType = FDCAN_STANDARD_ID; - TxHeader.Identifier = tx_frame->id & CANARD_CAN_STD_ID_MASK; - } - - TxHeader.DataLength = tx_frame->data_len; - - if (tx_frame->id & CANARD_CAN_FRAME_RTR) { - TxHeader.TxFrameType = FDCAN_REMOTE_FRAME; - } else { - TxHeader.TxFrameType = FDCAN_DATA_FRAME; - } - - TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE; // unsure about this one - TxHeader.BitRateSwitch = FDCAN_BRS_OFF; // Disabling FDCAN (using CAN 2.0) - TxHeader.FDFormat = FDCAN_CLASSIC_CAN; // Disabling FDCAN (using CAN 2.0) - TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS; // unsure about this one - TxHeader.MessageMarker = 0; // unsure about this one - if (TxHeader.DataLength <= sizeof(TxData)) - { - memcpy(TxData, tx_frame->data, TxHeader.DataLength); - } - else - { - LOG_ERROR(CAN, "Data to transmit is larger than 8 byte frame size"); - return -CANARD_ERROR_INVALID_ARGUMENT; - } - - if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &TxHeader, TxData) == HAL_OK) { - // LOG_DEBUG(CAN, "Successfully sent message with id: %lu", TxHeader.Identifier); - return 1; - } - - LOG_DEBUG(CAN, "Failed at adding message with id: %lu to Tx Queue", TxHeader.Identifier); - - // This might be for many reasons including the Tx Fifo being full, the error can be read from hfdcan->ErrorCode - return 0; -} - -/** - * @brief CAN1 Initialization Function - * @param bitrate desired bitrate to run the CAN network at. - * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode - */ -int16_t canardSTM32CAN1_Init(uint32_t bitrate) -{ - RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0}; - struct Timings out_timings; - int16_t ErrorCode = 1; - - /* USER CODE BEGIN FDCAN1_Init 0 */ - - /* USER CODE END FDCAN1_Init 0 */ - - /* USER CODE BEGIN FDCAN1_Init 1 */ - - FDCAN_FilterTypeDef sFilterConfig; - sFilterConfig.IdType = FDCAN_EXTENDED_ID; - sFilterConfig.FilterIndex = 0; - sFilterConfig.FilterType = FDCAN_FILTER_DUAL; - sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0; - sFilterConfig.FilterID1 = 0x0; - sFilterConfig.FilterID2 = 0x1FFFFFFFU; - /* USER CODE END FDCAN1_Init 1 */ - hfdcan1.Instance = FDCAN1; - hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC; // Initialize in CAN2.0 mode not CAN_FD - hfdcan1.Init.Mode = FDCAN_MODE_NORMAL; - hfdcan1.Init.AutoRetransmission = DISABLE; - hfdcan1.Init.TransmitPause = DISABLE; - hfdcan1.Init.ProtocolException = DISABLE; - - ErrorCode = canardSTM32ComputeTimings(bitrate, &out_timings); - if (ErrorCode != 1) - { - LOG_ERROR(CAN, "Unable to calculate timings, Error Code:%d", ErrorCode); - return -CANARD_ERROR_INTERNAL; - } - - hfdcan1.Init.NominalPrescaler = out_timings.prescaler; - hfdcan1.Init.NominalSyncJumpWidth = out_timings.sjw; - hfdcan1.Init.NominalTimeSeg1 = out_timings.bs1; - hfdcan1.Init.NominalTimeSeg2 = out_timings.bs2; - LOG_DEBUG(CAN, "Prescaler: %d, SJW: %d, BS1: %d, BS2: %d", out_timings.prescaler, out_timings.sjw, out_timings.bs1, out_timings.bs2); - - hfdcan1.Init.RxFifo0ElmtsNbr = 30; - hfdcan1.Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_8; - hfdcan1.Init.RxBuffersNbr = 1; - hfdcan1.Init.RxBufferSize = FDCAN_DATA_BYTES_8; - hfdcan1.Init.StdFiltersNbr = 0; - hfdcan1.Init.ExtFiltersNbr = 1; - hfdcan1.Init.TxFifoQueueElmtsNbr = 32; - hfdcan1.Init.TxEventsNbr = 0; - hfdcan1.Init.TxBuffersNbr = 5; - hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION; - hfdcan1.Init.TxElmtSize = FDCAN_DATA_BYTES_8; - LOG_DEBUG(CAN, "In CAN Init"); - - /** Initializes the peripherals clock - */ - PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_FDCAN; - PeriphClkInitStruct.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL; - if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) - { - LOG_DEBUG(CAN, "Unable to configure peripheral clock"); - return -CANARD_ERROR_INTERNAL; - } - - /* FDCAN1 clock enable */ - __HAL_RCC_FDCAN_CLK_ENABLE(); - - canardSTM32GPIO_Init(); // Set up the pins for CAN and optional listen only mode - - if (HAL_FDCAN_Init(&hfdcan1) != HAL_OK) - { - LOG_ERROR(CAN, "Failed CAN Init"); - return -CANARD_ERROR_INTERNAL; - } - /* USER CODE BEGIN FDCAN1_Init 2 */ - if (HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig) != HAL_OK) { - LOG_ERROR(CAN, "Failed Config Filter"); - return -CANARD_ERROR_INTERNAL; - } - if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE) != HAL_OK) { - LOG_ERROR(CAN, "Failed to config FDCAN filter"); - return -CANARD_ERROR_INTERNAL; - } - - if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK) { - LOG_ERROR(CAN, "Failed to Start"); - return -CANARD_ERROR_INTERNAL; - } - return CANARD_OK; -} +/* --- Initialization --- */ /** * @brief GPIO Initialization Function @@ -240,13 +41,13 @@ static void canardSTM32GPIO_Init(void) // Set up the Rx and Tx pins for CAN1 and if present, the standby or listen only pin. #if defined(CAN1_TX) && defined(CAN1_RX) IOInit(IOGetByTag(IO_TAG(CAN1_TX)), OWNER_DRONECAN, RESOURCE_CAN_TX, 0); - IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_TX)), IOCFG_AF_PP, GPIO_AF9_FDCAN1); // How do I make the alternate function crossplatform? + IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_TX)), IOCFG_AF_PP, GPIO_AF9_FDCAN1); IOInit(IOGetByTag(IO_TAG(CAN1_RX)), OWNER_DRONECAN, RESOURCE_CAN_RX, 0); - IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_RX)), IOCFG_AF_PP, GPIO_AF9_FDCAN1); // How do I make the alternate function crossplatform? + IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_RX)), IOCFG_AF_PP, GPIO_AF9_FDCAN1); #endif - #ifdef CAN1_STANDBY +#ifdef CAN1_STANDBY // Initialize the standby or listen only pin. Set default state to enable CAN. // TODO: Tie the pin state to a configuration option so we can turn CAN on and off. @@ -255,6 +56,14 @@ static void canardSTM32GPIO_Init(void) IOLo(IOGetByTag(IO_TAG(CAN1_STANDBY))); #endif } + +/** + * @brief Compute FDCAN bit-timing parameters (prescaler, SJW, BS1, BS2) for a + * given target bitrate, targeting an ~87.5% sample point location. + * @param target_bitrate Desired CAN bus bitrate in bits per second. + * @param out_timings Pointer to a Timings struct to receive the computed values. + * @retval true on success, false if no valid timing solution exists for the bitrate. + */ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings *out_timings) { if (target_bitrate < 1) { @@ -280,7 +89,7 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi * 250 kbps 16 17 * 125 kbps 16 17 */ - const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17; + const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17; // TODO: F7 uses 18 here — revisit both alongside SJW tuning LOG_DEBUG(CAN, "Baudrate: %lu", target_bitrate); LOG_DEBUG(CAN, "Max Quanta per bit: %i", max_quanta_per_bit); @@ -311,7 +120,7 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi } const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum); - if ((prescaler < 1U) || (prescaler > 1024U)) { + if ((prescaler < 1U) || (prescaler > 512U)) { // FDCAN NBTP.NBRP is 9-bit: valid range 1-512 return false; // No solution } LOG_DEBUG(CAN, "Prescaler: %lu", prescaler); @@ -376,9 +185,209 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi return true; } +/** + * @brief CAN1 Initialization Function + * @param bitrate desired bitrate to run the CAN network at. + * @retval CANARD_OK (0) on success, negative CANARD_ERROR on failure. + */ +int16_t canardSTM32CAN1_Init(uint32_t bitrate) +{ + RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0}; + struct Timings out_timings; + + FDCAN_FilterTypeDef sFilterConfig; + sFilterConfig.IdType = FDCAN_EXTENDED_ID; + sFilterConfig.FilterIndex = 0; + sFilterConfig.FilterType = FDCAN_FILTER_DUAL; + sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0; + sFilterConfig.FilterID1 = 0x0; + sFilterConfig.FilterID2 = 0x1FFFFFFFU; // FILTER_DUAL matches two IDs, but ConfigGlobalFilter below + // accepts all non-matching frames — this filter is a no-op + hfdcan1.Instance = FDCAN1; + hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC; // Initialize in CAN2.0 mode not CAN_FD + hfdcan1.Init.Mode = FDCAN_MODE_NORMAL; + // AutoRetransmission=ENABLE: hardware retries on arbitration loss without software intervention. + // Unlike the F7 (ISR + 32-deep SW queue), the H7 has only the HW TX FIFO (no SW queue). + // FDCAN TX FIFO maintains strict submission order regardless of retransmission, so frame + // ordering is preserved. Risk on a heavily loaded bus: a retrying frame can delay later + // FIFO entries but cannot reorder them. Original code used DISABLE (software-controlled + // retry via polling). TODO: evaluate whether DISABLE is safer for the H7 polling model. + hfdcan1.Init.AutoRetransmission = ENABLE; + hfdcan1.Init.TransmitPause = DISABLE; + hfdcan1.Init.ProtocolException = DISABLE; + + if (!canardSTM32ComputeTimings(bitrate, &out_timings)) + { + LOG_ERROR(CAN, "Failed to compute CAN timings for bitrate %lu", (unsigned long)bitrate); + return -CANARD_ERROR_INTERNAL; + } + + hfdcan1.Init.NominalPrescaler = out_timings.prescaler; + hfdcan1.Init.NominalSyncJumpWidth = out_timings.sjw; + hfdcan1.Init.NominalTimeSeg1 = out_timings.bs1; + hfdcan1.Init.NominalTimeSeg2 = out_timings.bs2; + LOG_DEBUG(CAN, "Prescaler: %d, SJW: %d, BS1: %d, BS2: %d", out_timings.prescaler, out_timings.sjw, out_timings.bs1, out_timings.bs2); + + hfdcan1.Init.RxFifo0ElmtsNbr = 30; + hfdcan1.Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_8; + hfdcan1.Init.RxBuffersNbr = 1; + hfdcan1.Init.RxBufferSize = FDCAN_DATA_BYTES_8; + hfdcan1.Init.StdFiltersNbr = 0; + hfdcan1.Init.ExtFiltersNbr = 1; + hfdcan1.Init.TxFifoQueueElmtsNbr = 32; + hfdcan1.Init.TxEventsNbr = 0; + hfdcan1.Init.TxBuffersNbr = 5; + hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION; + hfdcan1.Init.TxElmtSize = FDCAN_DATA_BYTES_8; + LOG_DEBUG(CAN, "In CAN Init"); + + /** Initializes the peripherals clock + */ + PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_FDCAN; + PeriphClkInitStruct.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL; + if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) + { + LOG_DEBUG(CAN, "Unable to configure peripheral clock"); + return -CANARD_ERROR_INTERNAL; + } + + /* FDCAN1 clock enable */ + __HAL_RCC_FDCAN_CLK_ENABLE(); + + canardSTM32GPIO_Init(); // Set up the pins for CAN and optional listen only mode + + if (HAL_FDCAN_Init(&hfdcan1) != HAL_OK) + { + LOG_ERROR(CAN, "Failed CAN Init"); + return -CANARD_ERROR_INTERNAL; + } + if (HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig) != HAL_OK) { + LOG_ERROR(CAN, "Failed Config Filter"); + return -CANARD_ERROR_INTERNAL; + } + if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE) != HAL_OK) { + LOG_ERROR(CAN, "Failed to config FDCAN filter"); + return -CANARD_ERROR_INTERNAL; + } + + if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK) { + LOG_ERROR(CAN, "Failed to Start"); + return -CANARD_ERROR_INTERNAL; + } + return CANARD_OK; +} + +/* --- RX --- */ + +/** + * @brief Process CAN message from RxLocation FIFO into rx_frame + * @param hfdcan pointer to an FDCAN_HandleTypeDef structure that contains + * the configuration information for the specified FDCAN. + * @param RxLocation Location of the received message to be read. + * This parameter can be a value of @arg FDCAN_Rx_location. + * @param rx_frame pointer to a CanardCANFrame structure where the received CAN message will be + * stored. + * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode + */ +int16_t canardSTM32Receive(CanardCANFrame *const rx_frame) { + if (rx_frame == NULL) { + return -CANARD_ERROR_INVALID_ARGUMENT; + } + + FDCAN_RxHeaderTypeDef RxHeader; + uint8_t RxData[8]; + + if (HAL_FDCAN_GetRxMessage(&hfdcan1, FDCAN_RX_FIFO0, &RxHeader, RxData) == HAL_OK) { + + // Process ID to canard format + rx_frame->id = RxHeader.Identifier; + + if (RxHeader.IdType == FDCAN_EXTENDED_ID) { // canard will only process the message if it is extended ID + rx_frame->id |= CANARD_CAN_FRAME_EFF; + } + + if (RxHeader.RxFrameType == FDCAN_REMOTE_FRAME) { // canard won't process the message if it is a remote frame + rx_frame->id |= CANARD_CAN_FRAME_RTR; + } + + rx_frame->data_len = RxHeader.DataLength; + memcpy(rx_frame->data, RxData, RxHeader.DataLength); + + // assume a single interface + rx_frame->iface_id = 0; + + return 1; + } + + // Either no CAN msg to be read, or an error that can be read from hfdcan->ErrorCode + return 0; +} + +/* --- TX --- */ + +/** + * @brief Process tx_frame CAN message into Tx FIFO/Queue and transmit it + * @param tx_frame pointer to a CanardCANFrame structure that contains the CAN message to + * transmit. + * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode + */ +int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { + if (tx_frame == NULL) { + return -CANARD_ERROR_INVALID_ARGUMENT; + } + + if (tx_frame->id & CANARD_CAN_FRAME_ERR) { + return -CANARD_ERROR_INVALID_ARGUMENT; // unsupported frame format + } + + FDCAN_TxHeaderTypeDef TxHeader; + uint8_t TxData[8]; + + // Process canard id to STM FDCAN header format + if (tx_frame->id & CANARD_CAN_FRAME_EFF) { + TxHeader.IdType = FDCAN_EXTENDED_ID; + TxHeader.Identifier = tx_frame->id & CANARD_CAN_EXT_ID_MASK; + } else { + TxHeader.IdType = FDCAN_STANDARD_ID; + TxHeader.Identifier = tx_frame->id & CANARD_CAN_STD_ID_MASK; + } + + TxHeader.DataLength = tx_frame->data_len; + + if (tx_frame->id & CANARD_CAN_FRAME_RTR) { + TxHeader.TxFrameType = FDCAN_REMOTE_FRAME; + } else { + TxHeader.TxFrameType = FDCAN_DATA_FRAME; + } + + TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE; // node is not in error-passive state; sender sets ESI to active + TxHeader.BitRateSwitch = FDCAN_BRS_OFF; // Disabling FDCAN (using CAN 2.0) + TxHeader.FDFormat = FDCAN_CLASSIC_CAN; // Disabling FDCAN (using CAN 2.0) + TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS; // no TX event FIFO allocated; event tracking not needed + TxHeader.MessageMarker = 0; // only meaningful when TX events are enabled; unused here + memcpy(TxData, tx_frame->data, TxHeader.DataLength); + + if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &TxHeader, TxData) == HAL_OK) { + return 1; + } + + LOG_DEBUG(CAN, "Failed at adding message with id: %lu to Tx Queue", TxHeader.Identifier); + + // This might be for many reasons including the Tx Fifo being full, the error can be read from hfdcan->ErrorCode + return 0; +} + +/* --- Diagnostics --- */ + +/** + * @brief Read FDCAN protocol status (bus-off, error passive) into a + * canardProtocolStatus_t struct. + * @param pProtocolStat Pointer to the struct to populate with current status. + */ void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){ FDCAN_ProtocolStatusTypeDef protocolStatus = {}; + memset(pProtocolStat, 0, sizeof(*pProtocolStat)); HAL_FDCAN_GetProtocolStatus(&hfdcan1, &protocolStatus); pProtocolStat->BusOff = protocolStatus.BusOff; pProtocolStat->ErrorPassive = protocolStatus.ErrorPassive; @@ -386,17 +395,37 @@ void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){ LOG_DEBUG(CAN, "ErrorPassive: %lu", protocolStatus.ErrorPassive); } +/** + * @brief Return the current number of frames pending in the software TX queue. + * The H7 FDCAN driver transmits directly into the hardware TX FIFO with no + * intermediate software queue, so this always returns 0. + * @retval 0 (no software TX queue on H7). + */ +int32_t canardSTM32GetTxQueueFillLevel(void) { + return 0; +} + +/** + * @brief Return the current number of frames waiting in the FDCAN RX FIFO0. + * @retval Number of frames available to read. + */ int32_t canardSTM32GetRxFifoFillLevel(void){ return (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1, FDCAN_RX_FIFO0)); } +/** + * @brief Recover the FDCAN peripheral from a bus-off condition by clearing + * the INIT bit in CCCR, which restarts the CAN participation. + */ void canardSTM32RecoverFromBusOff(void){ CLEAR_BIT(hfdcan1.Instance->CCCR, FDCAN_CCCR_INIT); // Clear INIT bit to recover from Bus-Off } -/* - get a 16 byte unique ID for this node, this should be based on the CPU unique ID or other unique ID - */ +/** + * @brief Populate a 16-byte unique node ID from the STM32 96-bit factory UID. + * The 12 UID bytes are copied into the first 12 bytes; the remainder is zero-padded. + * @param id Output buffer of exactly 16 bytes to receive the unique ID. + */ void canardSTM32GetUniqueID(uint8_t id[16]) { uint32_t HALUniqueIDs[3]; // Make Unique ID out of the 96-bit STM32 UID and fill the rest with 0s @@ -405,4 +434,4 @@ void canardSTM32GetUniqueID(uint8_t id[16]) { HALUniqueIDs[1] = HAL_GetUIDw1(); HALUniqueIDs[2] = HAL_GetUIDw2(); memcpy(id, HALUniqueIDs, 12); -} \ No newline at end of file +} diff --git a/src/main/drivers/nvic.h b/src/main/drivers/nvic.h index cb484937d3d..8ff16cd2998 100644 --- a/src/main/drivers/nvic.h +++ b/src/main/drivers/nvic.h @@ -12,6 +12,7 @@ #define NVIC_PRIO_TIMER 3 #define NVIC_PRIO_TIMER_DMA 3 #define NVIC_PRIO_SDIO 3 +#define NVIC_PRIO_CAN 4 #define NVIC_PRIO_USB 5 #define NVIC_PRIO_SERIALUART 5 #define NVIC_PRIO_VCP 7 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 6f809432648..e7205a2fe21 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -127,6 +127,10 @@ bool cliMode = false; #include "sensors/esc_sensor.h" #endif +#ifdef USE_DRONECAN +#include "drivers/dronecan/libcanard/canard_stm32_driver.h" +#endif + #include "telemetry/telemetry.h" #include "build/debug.h" @@ -4327,6 +4331,29 @@ static void cliStatus(char *cmdline) } } +#ifdef USE_DRONECAN +static void cliDronecan(char *cmdline) +{ + UNUSED(cmdline); + static const char * const lecNames[] = { + "None", "Stuff", "Form", "ACK", "BitR", "BitD", "CRC", "SW" + }; + canardProtocolStatus_t stat; + canardSTM32GetProtocolStatus(&stat); + int32_t txFill = canardSTM32GetTxQueueFillLevel(); + int32_t rxFill = canardSTM32GetRxFifoFillLevel(); + cliPrintLine("DroneCAN CAN peripheral status:"); + cliPrintLinef(" BusOff: %s", stat.BusOff ? "YES" : "no"); + cliPrintLinef(" ErrorPassive: %s", stat.ErrorPassive ? "YES" : "no"); + cliPrintLinef(" TEC: %u", (unsigned)stat.tec); + cliPrintLinef(" REC: %u", (unsigned)stat.rec); + cliPrintLinef(" LEC: %s (%u)", lecNames[stat.lec & 0x7], (unsigned)stat.lec); + cliPrintLinef(" TX queue: %ld (hwm: %u)", (long)txFill, (unsigned)stat.tx_queue_hwm); + cliPrintLinef(" RX buffer: %ld (hwm: %u)", (long)rxFill, (unsigned)stat.rx_buffer_hwm); + cliPrintLinef(" TX dropped: %u", (unsigned)stat.tx_dropped); +} +#endif + static void cliTasks(char *cmdline) { UNUSED(cmdline); @@ -4885,6 +4912,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), CLI_COMMAND_DEF("diff", "list configuration changes from default", "[master|battery_profile|control_profile|mixer_profile|rates|all] {showdefaults}", cliDiff), +#ifdef USE_DRONECAN + CLI_COMMAND_DEF("dronecan", "show DroneCAN CAN peripheral debug status", NULL, cliDronecan), +#endif CLI_COMMAND_DEF("dump", "dump configuration", "[master|battery_profile|control_profile|mixer_profile|rates|all] {showdefaults}", cliDump), #ifdef USE_RX_ELERES