diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 42e03212b38..2c657af5daf 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -27,8 +27,8 @@ /* Private variables ---------------------------------------------------------*/ -CanardInstance canard; -uint8_t memory_pool[1024]; +static CanardInstance canard; +static uint8_t memory_pool[1024]; static struct uavcan_protocol_NodeStatus node_status; PG_REGISTER_WITH_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig, PG_DRONECAN_CONFIG, 0); @@ -41,162 +41,279 @@ PG_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig, static dronecanState_e dronecanState = STATE_DRONECAN_INIT; static uint8_t activeNodeCount = 0; static dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES]; +static volatile uint32_t txErrCount = 0; + +#if defined(STM32H7) +static inline void dronecanMaskTxISR(void) { NVIC_DisableIRQ(FDCAN1_IT0_IRQn); } +static inline void dronecanUnmaskTxISR(void) { NVIC_EnableIRQ(FDCAN1_IT0_IRQn); } +#elif defined(STM32F7) +static inline void dronecanMaskTxISR(void) { NVIC_DisableIRQ(CAN1_TX_IRQn); } +static inline void dronecanUnmaskTxISR(void) { NVIC_EnableIRQ(CAN1_TX_IRQn); } +#else +static inline void dronecanMaskTxISR(void) {} +static inline void dronecanUnmaskTxISR(void) {} +#endif -// 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 +/* Forward declarations ------------------------------------------------------*/ -// Canard Handlers ( Many have code copied from libcanard esc_node example: https://github.com/dronecan/libcanard/blob/master/examples/ESCNode/esc_node.c ) +static void processCanardTxQueueSafe(void); +static void process1HzTasks(timeUs_t timestamp_usec); +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); -void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_protocol_NodeStatus nodeStatus; +// ---- Public API ------------------------------------------------------------- - if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) { - LOG_DEBUG(CAN, "NodeStatus decode failed"); - return; - } +void dronecanInit(void) +{ + uint32_t bitrate = 500000; // At least define 500000 - uint8_t nodeId = transfer->source_node_id; - for (uint8_t i = 0; i < activeNodeCount; i++) { - if (nodeTable[i].nodeID == nodeId) { - // update health, mode, uptime, vendor_status_code, last_seen_ms - nodeTable[i].health = nodeStatus.health; - nodeTable[i].mode = nodeStatus.mode; - nodeTable[i].uptime_sec = nodeStatus.uptime_sec; - nodeTable[i].vendor_status_code = nodeStatus.vendor_specific_status_code; - nodeTable[i].last_seen_ms = millis(); - return; - } - } - // new node - if (activeNodeCount < DRONECAN_MAX_NODES) { - nodeTable[activeNodeCount].nodeID = nodeId; - nodeTable[activeNodeCount].health = nodeStatus.health; - nodeTable[activeNodeCount].mode = nodeStatus.mode; - nodeTable[activeNodeCount].uptime_sec = nodeStatus.uptime_sec; - nodeTable[activeNodeCount].vendor_status_code = nodeStatus.vendor_specific_status_code; - nodeTable[activeNodeCount].name_len = 0; - nodeTable[activeNodeCount].name[0] = 0; - nodeTable[activeNodeCount].last_seen_ms = millis(); - activeNodeCount++; + switch (dronecanConfig()->bitRateKbps){ + case DRONECAN_BITRATE_125KBPS: + bitrate = 125000; + break; + + case DRONECAN_BITRATE_250KBPS: + bitrate = 250000; + break; + + case DRONECAN_BITRATE_500KBPS: + bitrate = 500000; + break; + + case DRONECAN_BITRATE_1000KBPS: + bitrate = 1000000; + break; + + case DRONECAN_BITRATE_COUNT: + LOG_ERROR(SYSTEM, "Undefined bitrate set in configuration. 500kbps selected"); + bitrate = 500000; + break; + + default: + LOG_ERROR(SYSTEM, "Invalid bitrate setting, defaulting to 500kbps"); + bitrate = 500000; + break; + } + if(canardSTM32CAN1_Init(bitrate) != CANARD_OK) + { + LOG_ERROR(CAN, "Unable to initialize the CAN peripheral"); + dronecanState = STATE_DRONECAN_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"); + } } -void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_equipment_gnss_Auxiliary gnssAuxiliary; +void dronecanUpdate(timeUs_t currentTimeUs) +{ + static timeUs_t next_1hz_service_at = 0; + static timeUs_t busoffTimeUs = 0; + CanardCANFrame rx_frame; + int numMessagesToProcess = 0; + canardProtocolStatus_t protocolStatus = {}; + uint64_t timestamp; + int16_t rx_res; - 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); -} + switch(dronecanState) { + case STATE_DRONECAN_INIT: + next_1hz_service_at = currentTimeUs + 1000000ULL; // First 1Hz tick in 1 second + dronecanState = STATE_DRONECAN_NORMAL; + break; -void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_equipment_gnss_Fix gnssFix; + case STATE_DRONECAN_NORMAL: + processCanardTxQueueSafe(); - if (uavcan_equipment_gnss_Fix_decode(transfer, &gnssFix)) { - LOG_DEBUG(CAN, "GNSSFix decode failed"); - return; - } - dronecanGPSReceiveGNSSFix(&gnssFix); - LOG_DEBUG(CAN, "GNSS Fix received"); -} + for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--) + { + timestamp = millis() * 1000ULL; + rx_res = canardSTM32Receive(&rx_frame); -void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_equipment_gnss_Fix2 gnssFix2; + 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. + processCanardTxQueueSafe(); - if (uavcan_equipment_gnss_Fix2_decode(transfer, &gnssFix2)) { - LOG_DEBUG(CAN, "GNSSFix2 decode failed"); - return; - } - dronecanGPSReceiveGNSSFix2(&gnssFix2); - LOG_DEBUG(CAN, "GNSS Fix2 received"); -} + if (currentTimeUs >= next_1hz_service_at) + { + next_1hz_service_at += 1000000ULL; + process1HzTasks(currentTimeUs); + processCanardTxQueueSafe(); + + canardSTM32GetProtocolStatus(&protocolStatus); + if (protocolStatus.BusOff != 0 || protocolStatus.ErrorPassive != 0) { + LOG_DEBUG(CAN, "CAN status: BusOff=%" PRIu32 " ErrorPassive=%" PRIu32, protocolStatus.BusOff, protocolStatus.ErrorPassive); + } + + uint32_t rxDrops = canardSTM32GetAndClearRxDropCount(); + dronecanMaskTxISR(); + uint32_t txErrs = txErrCount; + txErrCount = 0; + dronecanUnmaskTxISR(); + if (rxDrops > 0) { + LOG_DEBUG(CAN, "RX drops: %" PRIu32, rxDrops); + } + if (txErrs > 0) { + LOG_DEBUG(CAN, "TX errors: %" PRIu32, txErrs); + } + + if (protocolStatus.BusOff != 0) { + dronecanState = STATE_DRONECAN_BUS_OFF; + busoffTimeUs = currentTimeUs; + } + } + break; -void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_equipment_gnss_RTCMStream gnssRTCMStream; + case STATE_DRONECAN_BUS_OFF: + if(currentTimeUs > (busoffTimeUs + 20000)) { // Wait 20ms: worst-case 128x11 recovery is 11.264ms at 125kbps + canardSTM32RecoverFromBusOff(); + busoffTimeUs = currentTimeUs; + canardSTM32GetProtocolStatus(&protocolStatus); + if(protocolStatus.BusOff == 0) { + dronecanState = STATE_DRONECAN_NORMAL; + } + } + break; + + case STATE_DRONECAN_FAILED: + break; + + default: + break; + + } - if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &gnssRTCMStream)) { - LOG_DEBUG(CAN, "RTCMStream decode failed"); - return; - } - LOG_DEBUG(CAN, "GNSS RTCM"); } -void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_equipment_power_BatteryInfo batteryInfo; +dronecanState_e dronecanGetState(void) +{ + return dronecanState; +} - if (uavcan_equipment_power_BatteryInfo_decode(transfer, &batteryInfo)) { - LOG_DEBUG(CAN, "BatteryInfo decode failed"); - return; - } - dronecanBatterySensorReceiveInfo(&batteryInfo); - LOG_DEBUG(CAN, "Battery Info"); +uint8_t dronecanGetNodeCount(void) +{ + return activeNodeCount; } -/* - handle a GetNodeInfo request -*/ +uint32_t dronecanGetBitrateKbps(void) +{ + switch (dronecanConfig()->bitRateKbps){ + case DRONECAN_BITRATE_125KBPS: + return 125; -// 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) { - LOG_DEBUG(CAN, "GetNodeInfo request from %d", transfer->source_node_id); + case DRONECAN_BITRATE_250KBPS: + return 250; - uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; - struct uavcan_protocol_GetNodeInfoResponse pkt; + case DRONECAN_BITRATE_500KBPS: + return 500; - memset(&pkt, 0, sizeof(pkt)); + case DRONECAN_BITRATE_1000KBPS: + return 1000; - node_status.uptime_sec = millis() / 1000ULL; - pkt.status = node_status; + case DRONECAN_BITRATE_COUNT: + default: + return 500; + } +} - // 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 +const dronecanNodeInfo_t *dronecanGetNode(uint8_t index) { + if (index < activeNodeCount) return &nodeTable[index]; + return NULL; +} - // should fill in hardware version - pkt.hardware_version.major = 1; - pkt.hardware_version.minor = 0; +// ---- ISR / HAL callbacks ---------------------------------------------------- - // just setting all 16 bytes to 1 for testing - canardSTM32GetUniqueID(pkt.hardware_version.unique_id); +#if defined(STM32H7) || defined(STM32F7) +/* Called from TX-complete ISR only. Already in interrupt context — no NVIC masking needed. + For main-loop use, call processCanardTxQueueSafe() instead. */ +static void processCanardTxQueue(void) { + // Transmitting + 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) { + txErrCount++; // logged from main loop at 1Hz + canardPopTxQueue(&canard); // Error - discard frame + } else if (tx_res > 0) { + canardPopTxQueue(&canard); // Success - remove from queue + } else { + // tx_res == 0: TX FIFO full, retry later + break; + } + } +} +#endif - uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); +#if defined(STM32H7) +void HAL_FDCAN_TxBufferCompleteCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t BufferIndexes) +{ + UNUSED(hfdcan); + UNUSED(BufferIndexes); + processCanardTxQueue(); +} +#endif +#if defined(STM32F7) +void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { UNUSED(hcan); processCanardTxQueue(); } +void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { UNUSED(hcan); processCanardTxQueue(); } +void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { UNUSED(hcan); processCanardTxQueue(); } +#endif - canardRequestOrRespond(ins, - transfer->source_node_id, - UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, - UAVCAN_PROTOCOL_GETNODEINFO_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &buffer[0], - total_size); +// ---- Internal helpers ------------------------------------------------------- + +static void processCanardTxQueueSafe(void) { + for (;;) { + dronecanMaskTxISR(); + const CanardCANFrame *tx_frame = canardPeekTxQueue(&canard); + if (tx_frame == NULL) { + dronecanUnmaskTxISR(); + break; + } + const int16_t tx_res = canardSTM32Transmit(tx_frame); // HAL register write, ~1µs + if (tx_res != 0) { + if (tx_res < 0) { + LOG_DEBUG(CAN, "Transmit error %d", tx_res); + } + canardPopTxQueue(&canard); + } + dronecanUnmaskTxISR(); + if (tx_res == 0) { + break; // HW TX full, ISR will refill when a slot opens + } + } } -// Canard Senders +// 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 /* 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; @@ -204,31 +321,53 @@ 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 // put whatever you like in here for display in GUI - node_status.vendor_specific_status_code = armingFlags; + node_status.vendor_specific_status_code = (uint16_t)(armingFlags & 0xFFFF); /* field is 16-bit by UAVCAN spec; bits 16-30 of armingFlags are not transmitted */ 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; - canardBroadcast(&canard, + dronecanMaskTxISR(); + const int16_t bc_res = canardBroadcast(&canard, UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, UAVCAN_PROTOCOL_NODESTATUS_ID, &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, buffer, len); - // PrintCanStatus(); + dronecanUnmaskTxISR(); + if (bc_res < 0) { + LOG_DEBUG(CAN, "NodeStatus broadcast failed: %d", bc_res); + } + +} + +/* + 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 + */ + dronecanMaskTxISR(); + canardCleanupStaleTransfers(&canard, timestamp_usec); + dronecanUnmaskTxISR(); + + /* + Transmit the node status message + */ + send_NodeStatus(); } -// Canard Util /* This callback is invoked by the library when it detects beginning of a new transfer on the bus that can be received by the local node. @@ -238,8 +377,7 @@ 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, @@ -295,253 +433,192 @@ bool shouldAcceptTransfer(const CanardInstance *ins, 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]); - // } +// Canard Handlers ( Many have code copied from libcanard esc_node example: https://github.com/dronecan/libcanard/blob/master/examples/ESCNode/esc_node.c ) - 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) { +static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_protocol_NodeStatus nodeStatus; - case UAVCAN_PROTOCOL_NODESTATUS_ID: - handle_NodeStatus(ins, transfer); - break; - + if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) { + LOG_DEBUG(CAN, "NodeStatus decode failed"); + return; + } - case UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID: - handle_GNSSAuxiliary(ins, transfer); - break; - - case UAVCAN_EQUIPMENT_GNSS_FIX_ID: - handle_GNSSFix(ins, transfer); - break; - - case UAVCAN_EQUIPMENT_GNSS_FIX2_ID: - handle_GNSSFix2(ins, transfer); - break; - - 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; + uint8_t nodeId = transfer->source_node_id; + for (uint8_t i = 0; i < activeNodeCount; i++) { + if (nodeTable[i].nodeID == nodeId) { + // update health, mode, uptime, vendor_status_code, last_seen_ms + nodeTable[i].health = nodeStatus.health; + nodeTable[i].mode = nodeStatus.mode; + nodeTable[i].uptime_sec = nodeStatus.uptime_sec; + nodeTable[i].vendor_status_code = nodeStatus.vendor_specific_status_code; + nodeTable[i].last_seen_ms = millis(); + return; } - } -} + } + // new node + if (activeNodeCount < DRONECAN_MAX_NODES) { + nodeTable[activeNodeCount].nodeID = nodeId; + nodeTable[activeNodeCount].health = nodeStatus.health; + nodeTable[activeNodeCount].mode = nodeStatus.mode; + nodeTable[activeNodeCount].uptime_sec = nodeStatus.uptime_sec; + nodeTable[activeNodeCount].vendor_status_code = nodeStatus.vendor_specific_status_code; + nodeTable[activeNodeCount].name_len = 0; + nodeTable[activeNodeCount].name[0] = 0; + nodeTable[activeNodeCount].last_seen_ms = millis(); + activeNodeCount++; + } +} -void processCanardTxQueue(void) { - // Transmitting - for (const CanardCANFrame *tx_frame ; (tx_frame = canardPeekTxQueue(&canard)) != NULL;) - { - const int16_t tx_res = canardSTM32Transmit(tx_frame); +static void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + if (gpsConfig()->provider != GPS_DRONECAN) return; + struct uavcan_equipment_gnss_Auxiliary gnssAuxiliary; - 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; - } + if (uavcan_equipment_gnss_Auxiliary_decode(transfer, &gnssAuxiliary)) { + LOG_DEBUG(CAN, "GNSSAuxiliary decode failed"); + return; } - + dronecanGPSReceiveGNSSAuxiliary(&gnssAuxiliary); } -/* - 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); +static void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + if (gpsConfig()->provider != GPS_DRONECAN) return; + struct uavcan_equipment_gnss_Fix gnssFix; - /* - Transmit the node status message - */ - send_NodeStatus(); + if (uavcan_equipment_gnss_Fix_decode(transfer, &gnssFix)) { + LOG_DEBUG(CAN, "GNSSFix decode failed"); + return; + } + dronecanGPSReceiveGNSSFix(&gnssFix); } -void dronecanInit(void) -{ - LOG_DEBUG(CAN, "dronecan Init"); - uint32_t bitrate = 500000; // At least define 500000 - - switch (dronecanConfig()->bitRateKbps){ - case DRONECAN_BITRATE_125KBPS: - bitrate = 125000; - break; +static void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + if (gpsConfig()->provider != GPS_DRONECAN) return; + struct uavcan_equipment_gnss_Fix2 gnssFix2; - case DRONECAN_BITRATE_250KBPS: - bitrate = 250000; - break; - - case DRONECAN_BITRATE_500KBPS: - bitrate = 500000; - break; + if (uavcan_equipment_gnss_Fix2_decode(transfer, &gnssFix2)) { + LOG_DEBUG(CAN, "GNSSFix2 decode failed"); + return; + } + dronecanGPSReceiveGNSSFix2(&gnssFix2); +} - case DRONECAN_BITRATE_1000KBPS: - bitrate = 1000000; - break; +static void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + UNUSED(transfer); + /* RTCM forwarding not yet implemented. Accepted in shouldAcceptTransfer for future use. */ +} - 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); +static void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_equipment_power_BatteryInfo batteryInfo; - // 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"); - } + 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; - 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; +// TODO: All the data in here is temporary for testing. If actually need to send valid data, edit accordingly. +static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) { + uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; + struct uavcan_protocol_GetNodeInfoResponse pkt; - case STATE_DRONECAN_NORMAL: - processCanardTxQueue(); + memset(&pkt, 0, sizeof(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); + node_status.uptime_sec = millis() / 1000ULL; + pkt.status = node_status; - 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(); + // 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 - if (currentTimeUs >= next_1hz_service_at) - { - next_1hz_service_at += 1000000ULL; - process1HzTasks(currentTimeUs); - processCanardTxQueue(); - } + // should fill in hardware version + pkt.hardware_version.major = 1; + pkt.hardware_version.minor = 0; - canardSTM32GetProtocolStatus(&protocolStatus); - if(protocolStatus.BusOff != 0) { - dronecanState = STATE_DRONECAN_BUS_OFF; - busoffTimeUs = currentTimeUs; - } - break; + // just setting all 16 bytes to 1 for testing + canardSTM32GetUniqueID(pkt.hardware_version.unique_id); - 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; - + 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); + + dronecanMaskTxISR(); + const int16_t rr_res = canardRequestOrRespond(ins, + transfer->source_node_id, + UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, + UAVCAN_PROTOCOL_GETNODEINFO_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + total_size); + dronecanUnmaskTxISR(); + if (rr_res < 0) { + LOG_DEBUG(CAN, "GetNodeInfo response failed: %d", rr_res); } - } -dronecanState_e dronecanGetState(void) -{ - return dronecanState; -} +/* + This callback is invoked by the library when a new message or request or response is received. +*/ +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) { -uint8_t dronecanGetNodeCount(void) -{ - return activeNodeCount; -} + case UAVCAN_PROTOCOL_NODESTATUS_ID: + handle_NodeStatus(ins, transfer); + break; -uint32_t dronecanGetBitrateKbps(void) -{ - switch (dronecanConfig()->bitRateKbps){ - case DRONECAN_BITRATE_125KBPS: - return 125; - case DRONECAN_BITRATE_250KBPS: - return 250; - - case DRONECAN_BITRATE_500KBPS: - return 500; + case UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID: + handle_GNSSAuxiliary(ins, transfer); + break; - case DRONECAN_BITRATE_1000KBPS: - return 1000; - - case DRONECAN_BITRATE_COUNT: - return 0; - } - return 0; -} + case UAVCAN_EQUIPMENT_GNSS_FIX_ID: + handle_GNSSFix(ins, transfer); + break; -const dronecanNodeInfo_t *dronecanGetNode(uint8_t index) { - if (index < activeNodeCount) return &nodeTable[index]; - return NULL; + case UAVCAN_EQUIPMENT_GNSS_FIX2_ID: + handle_GNSSFix2(ins, transfer); + break; + + case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID: + handle_GNSSRCTMStream(ins, transfer); + break; + + case UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID: + handle_BatteryInfo(ins, transfer); + break; + } + } } #endif diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h index b0212ec692d..202011b0483 100644 --- a/src/main/drivers/dronecan/dronecan.h +++ b/src/main/drivers/dronecan/dronecan.h @@ -14,7 +14,9 @@ typedef enum { typedef enum { STATE_DRONECAN_INIT, STATE_DRONECAN_NORMAL, - STATE_DRONECAN_BUS_OFF + STATE_DRONECAN_BUS_OFF, + STATE_DRONECAN_FAILED, + STATE_DRONECAN_COUNT } dronecanState_e; #define DRONECAN_MAX_NODES 32 // Reasonably expected number of devices on the bus. If this is regularly hit, we could go higher but it consumes more ram. diff --git a/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c b/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c index 8743e779dea..45c7639b5e4 100644 --- a/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c @@ -106,18 +106,12 @@ 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__ // SocketCAN implementations -/** - * @brief Initialize SocketCAN interface - * @param bitrate CAN bitrate in bps (for logging, actual rate set via ip link) - * @retval 0 on success, negative on error - */ static int16_t sitlCANInitSocketCAN(uint32_t bitrate) { struct sockaddr_can addr; struct ifreq ifr; @@ -163,9 +157,6 @@ static int16_t sitlCANInitSocketCAN(uint32_t bitrate) { return 0; } -/** - * @brief Convert libcanard frame to Linux CAN frame - */ static void sitlCANFrameToLinux(const CanardCANFrame *const src, struct can_frame *const dst) { memset(dst, 0, sizeof(struct can_frame)); @@ -176,6 +167,10 @@ static void sitlCANFrameToLinux(const CanardCANFrame *const src, struct can_fram dst->can_id = src->id & CANARD_CAN_STD_ID_MASK; } + if (src->id & CANARD_CAN_FRAME_RTR) { + dst->can_id |= CAN_RTR_FLAG; + } + // Copy data dst->can_dlc = src->data_len; if (src->data_len > 0) { @@ -183,9 +178,6 @@ static void sitlCANFrameToLinux(const CanardCANFrame *const src, struct can_fram } } -/** - * @brief Convert Linux CAN frame to libcanard frame - */ static void sitlCANFrameFromLinux(const struct can_frame *const src, CanardCANFrame *const dst) { memset(dst, 0, sizeof(CanardCANFrame)); @@ -196,6 +188,10 @@ static void sitlCANFrameFromLinux(const struct can_frame *const src, CanardCANFr dst->id = src->can_id & CANARD_CAN_STD_ID_MASK; } + if (src->can_id & CAN_RTR_FLAG) { + dst->id |= CANARD_CAN_FRAME_RTR; + } + // Copy data dst->data_len = src->can_dlc; if (src->can_dlc > 0) { @@ -203,11 +199,6 @@ static void sitlCANFrameFromLinux(const struct can_frame *const src, CanardCANFr } } -/** - * @brief Receive a CAN frame via SocketCAN - * @param rx_frame Pointer to frame structure to fill - * @retval 0 if no frame available, 1 if frame received, negative on error - */ static int16_t sitlCANReceiveSocketCAN(CanardCANFrame *const rx_frame) { struct can_frame frame; ssize_t nbytes; @@ -235,11 +226,6 @@ static int16_t sitlCANReceiveSocketCAN(CanardCANFrame *const rx_frame) { return 1; } -/** - * @brief Transmit a CAN frame via SocketCAN - * @param tx_frame Pointer to frame to transmit - * @retval 1 on success, 0 if busy, negative on error - */ static int16_t sitlCANTransmitSocketCAN(const CanardCANFrame* const tx_frame) { struct can_frame frame; ssize_t nbytes; @@ -262,16 +248,9 @@ static int16_t sitlCANTransmitSocketCAN(const CanardCANFrame* const tx_frame) { return 1; // Success } -/** - * @brief Get CAN protocol status from SocketCAN - * @param pProtocolStat Pointer to status structure to fill - */ +/* Always returns zeroes — SocketCAN provides no per-frame error counters via raw sockets. */ 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; + memset(pProtocolStat, 0, sizeof(*pProtocolStat)); } #endif // __linux__ @@ -280,7 +259,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; } @@ -304,7 +283,7 @@ int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { * @retval 1 on success, 0 if busy, negative on error */ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { - if (tx_frame == NULL) { + if (tx_frame == NULL || (tx_frame->id & CANARD_CAN_FRAME_ERR)) { return -CANARD_ERROR_INVALID_ARGUMENT; } @@ -348,8 +327,10 @@ int32_t canardSTM32GetRxFifoFillLevel(void) { #ifdef __linux__ if (can_mode == SITL_CAN_MODE_SOCKETCAN && can_socket >= 0) { int available; + /* FIONREAD on SOCK_RAW returns the byte size of the next pending datagram only, + so this yields 0 or 1 — SITL processes at most one frame per scheduler tick. */ if (ioctl(can_socket, FIONREAD, &available) == 0) { - return available / sizeof(struct can_frame); + return available / (int)sizeof(struct can_frame); } } #endif @@ -357,6 +338,14 @@ int32_t canardSTM32GetRxFifoFillLevel(void) { return 0; } +uint32_t canardSTM32GetAndClearRxDropCount(void) { + return 0; +} + +int32_t canardSTM32GetTxQueueFillLevel(void) { + return 0; +} + /** * @brief Recover from bus-off condition */ @@ -384,11 +373,11 @@ void canardSTM32GetUniqueID(uint8_t id[16]) { #ifdef __linux__ // Add process ID for uniqueness between multiple SITL instances - pid_t pid = getpid(); - id[4] = (pid >> 24) & 0xFF; - id[5] = (pid >> 16) & 0xFF; - id[6] = (pid >> 8) & 0xFF; - id[7] = pid & 0xFF; + uint32_t upid = (uint32_t)getpid(); + id[4] = (upid >> 24) & 0xFF; + id[5] = (upid >> 16) & 0xFF; + id[6] = (upid >> 8) & 0xFF; + id[7] = upid & 0xFF; // Add timestamp for additional uniqueness struct timespec ts; diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h b/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h index fe70cddf1d9..9b205f8bbaf 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h +++ b/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h @@ -12,16 +12,21 @@ typedef struct { uint32_t BusOff; uint32_t ErrorPassive; + uint8_t tec; + uint8_t rec; + uint8_t lec; } 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 canardSTM32GetTxQueueFillLevel(void); int32_t canardSTM32GetRxFifoFillLevel(void); +uint32_t canardSTM32GetAndClearRxDropCount(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..9c8f8b472b1 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -8,6 +8,7 @@ #include "common/log.h" #include "common/time.h" #include "drivers/io.h" +#include "drivers/nvic.h" #include "canard.h" #include "canard_stm32_driver.h" @@ -33,67 +34,108 @@ typedef struct { } RxFrame_t; static struct RxBuffer_t { - uint8_t writeIndex; - uint8_t readIndex; + volatile uint8_t writeIndex; + 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 int8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg); +static int8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg); +static uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf); static CAN_HandleTypeDef hcan1; -RxFrame_t rxMsg; +static volatile uint32_t rxDropCount = 0; -uint8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { - uint8_t next; - RxFrame_t *pCurrentRxMsg; +// ---- Public API ------------------------------------------------------------- - next = rxBuf->writeIndex + 1; - if(next >= RX_BUFFER_SIZE){ - next = 0; +/** + * @brief CAN1 (bxCAN) Initialization Function + * @param bitrate desired bitrate to run the CAN network at. + * @retval ret == 0: OK (CANARD_OK), ret < 0: CANARD_ERROR + */ +int16_t canardSTM32CAN1_Init(uint32_t bitrate) +{ + struct Timings out_timings; + + __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 = DISABLE; // ENABLE fills the TX FIFO on a degraded bus; DroneCAN reliability is handled at the application layer + hcan1.Init.ReceiveFifoLocked = DISABLE; + hcan1.Init.TransmitFifoPriority = DISABLE; + + if (!canardSTM32ComputeTimings(bitrate, &out_timings)) + { + LOG_ERROR(CAN, "Failed to compute CAN timings for bitrate %lu", (unsigned long)bitrate); + return -CANARD_ERROR_INTERNAL; } - if(next == rxBuf->readIndex) { - return -1; // rxBuf is full + hcan1.Init.Prescaler = out_timings.prescaler; + /* F7 bxCAN HAL ORs these directly into BTR; values must be pre-shifted to their register positions */ + 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(); + if (HAL_CAN_Init(&hcan1) != HAL_OK) + { + LOG_ERROR(CAN, "Failed CAN Init"); + return -CANARD_ERROR_INTERNAL; } - 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 (HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig) != HAL_OK) { + LOG_ERROR(CAN, "Failed Config Filter"); + return -CANARD_ERROR_INTERNAL; + } - if(rxBuf->writeIndex == rxBuf->readIndex){ - return -1; // Nothing to read + if (HAL_CAN_Start(&hcan1) != HAL_OK) { + LOG_ERROR(CAN, "Failed to Start"); + return -CANARD_ERROR_INTERNAL; } - next = rxBuf->readIndex + 1; - if (next >= RX_BUFFER_SIZE){ - next = 0; + 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; + } + if (HAL_CAN_ActivateNotification(&hcan1, CAN_IT_TX_MAILBOX_EMPTY) != HAL_OK) { + LOG_ERROR(CAN, "Failed to activate TX interrupt"); + return -CANARD_ERROR_INTERNAL; } - 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); + // Enable IRQs only after all ActivateNotification calls succeed + 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; } /** * @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 + * @retval ret == 0: OK (CANARD_OK), ret < 0: CANARD_ERROR */ -int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { +int16_t canardSTM32Receive(CanardCANFrame *const rx_frame) { RxFrame_t canRxFrame; if (rx_frame == NULL) { @@ -119,7 +161,7 @@ int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { rx_frame->iface_id = 0; return 1; } - // Either no CAN msg to be read, or an error that can be read from hfdcan->ErrorCode + // Either no CAN msg to be read, or an error that can be read from hcan1.ErrorCode return 0; } @@ -127,7 +169,7 @@ int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { * @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 + * @retval ret == 0: OK (CANARD_OK), ret < 0: CANARD_ERROR */ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { CAN_TxHeaderTypeDef txHeader = {}; @@ -164,109 +206,79 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { 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 + // TX failed (mailboxes full or bus 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; +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); + pProtocolStat->tec = (uint8_t)((esr >> 16) & 0xFF); + pProtocolStat->rec = (uint8_t)((esr >> 24) & 0xFF); + pProtocolStat->lec = (uint8_t)((esr >> 4) & 0x07); +} - /* CAN1 clock enable */ - __HAL_RCC_CAN1_CLK_ENABLE(); +int32_t canardSTM32GetTxQueueFillLevel(void){ + return 0; +} - // /* USER CODE BEGIN CAN1_MspInit 1 */ +int32_t canardSTM32GetRxFifoFillLevel(void){ + return rxBufferNumMessages(&RxBuffer); +} - 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); +void canardSTM32RecoverFromBusOff(void){ + // No-op: ABOM (CAN_MCR bit 6) is set in canardSTM32CAN1_Init, so hardware + // manages the full bus-off recovery sequence automatically. After 128x11 + // recessive bits, hardware cycles INRQ and clears ESR.BOFF without software + // intervention. See RM0410 ss40.7.6 and CAN_MCR.ABOM, CAN_ESR.BOFF. +} - 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); +/* + get a 16 byte unique ID for this node, this should be based on the CPU unique ID or other 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 + memset(id, 0, 16); + HALUniqueIDs[0] = *(uint32_t *)UID_BASE; + HALUniqueIDs[1] = *(uint32_t *)(UID_BASE + 4); + HALUniqueIDs[2] = *(uint32_t *)(UID_BASE + 8); + memcpy(id, HALUniqueIDs, 12); +} - // 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; - } +// ---- ISR / HAL callbacks ---------------------------------------------------- - 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; +void CAN1_RX0_IRQHandler(void) { + HAL_CAN_IRQHandler(&hcan1); +} + +void CAN1_TX_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) { + if (rxBufferPushFrame(&RxBuffer, &frame) != 0) { + rxDropCount++; // logged from main loop via canardSTM32GetAndClearRxDropCount() + } } - - // 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; +uint32_t canardSTM32GetAndClearRxDropCount(void) { + HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn); + uint32_t count = rxDropCount; + rxDropCount = 0; + HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); + return count; } +// ---- Private helpers -------------------------------------------------------- + /** * @brief GPIO Initialization Function * @param None @@ -278,9 +290,9 @@ 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 @@ -289,13 +301,14 @@ static void canardSTM32GPIO_Init(void) // TODO: Tie the pin state to a configuration option so we can turn CAN on and off. IOInit(IOGetByTag(IO_TAG(CAN1_STANDBY)), OWNER_DRONECAN, RESOURCE_CAN_STANDBY, 0); - IOConfigGPIO(IOGetByTag(IO_TAG(CAN1_STANDBY)), IOCFG_OUT_PP); // Do any boards use pullups, external/internal? + IOConfigGPIO(IOGetByTag(IO_TAG(CAN1_STANDBY)), IOCFG_OUT_PP); IOLo(IOGetByTag(IO_TAG(CAN1_STANDBY))); #endif } + static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings *out_timings) { - + if (target_bitrate < 1) { return false; } @@ -319,10 +332,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 : 17; static const int MaxSamplePointLocation = 900; /* @@ -336,7 +346,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,7 +362,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. @@ -404,54 +412,50 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi return false; } - LOG_DEBUG(CAN, "Timings: quanta/bit: %d, sample point location: %f%%", - (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; // Register value: hardware SJW = sjw+1 = 4 tq. F7 bxCAN needs wider SJW than H7 FDCAN. 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){ +static int8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { + uint8_t wi = rxBuf->writeIndex; // snapshot: only this ISR writes writeIndex + uint8_t next = wi + 1; + if (next >= RX_BUFFER_SIZE) { + next = 0; + } - 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); + if (next == rxBuf->readIndex) { + return -1; // rxBuf is full + } + memcpy(&rxBuf->rxMsg[wi], rxMsg, sizeof(RxFrame_t)); + __DMB(); // ensure frame data is visible to main loop before writeIndex advance + rxBuf->writeIndex = next; + return 0; } -int32_t canardSTM32GetRxFifoFillLevel(void){ - return rxBufferNumMessages(&RxBuffer); -} +static int8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { + uint8_t ri = rxBuf->readIndex; // snapshot: only main loop writes readIndex -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 -} + if (rxBuf->writeIndex == ri) { + return -1; // Nothing to read + } -/* - get a 16 byte unique ID for this node, this should be based on the CPU unique ID or other 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 - memset(id, 0, 16); - HALUniqueIDs[0] = *(uint32_t *)UID_BASE; - HALUniqueIDs[1] = *(uint32_t *)(UID_BASE + 4); - HALUniqueIDs[2] = *(uint32_t *)(UID_BASE + 8); - memcpy(id, HALUniqueIDs, 12); + uint8_t next = ri + 1; + if (next >= RX_BUFFER_SIZE) { + next = 0; + } + __DMB(); // ensure writeIndex read is complete before reading frame data written by ISR + memcpy(rxMsg, &rxBuf->rxMsg[ri], sizeof(RxFrame_t)); + rxBuf->readIndex = next; + return 0; } -void CAN1_RX0_IRQHandler(void) { - HAL_CAN_IRQHandler(&hcan1); -} +static uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf) { + if(rxBuf->writeIndex < rxBuf->readIndex) + return((rxBuf->writeIndex + RX_BUFFER_SIZE) - rxBuf->readIndex); -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 + return (rxBuf->writeIndex - rxBuf->readIndex); +} diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c index 77c1f94921f..acf902e7056 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c @@ -8,6 +8,7 @@ #include "common/log.h" #include "common/time.h" #include "drivers/io.h" +#include "drivers/nvic.h" #include "canard.h" #include "canard_stm32_driver.h" @@ -30,6 +31,91 @@ static void canardSTM32GPIO_Init(void); static FDCAN_HandleTypeDef hfdcan1; +// ---- Public API ------------------------------------------------------------- + +/** + * @brief CAN1 Initialization Function + * @param bitrate desired bitrate to run the CAN network at. + * @retval ret == 0: OK (CANARD_OK), ret < 0: CANARD_ERROR + */ +int16_t canardSTM32CAN1_Init(uint32_t bitrate) +{ + 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; + 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; // ENABLE fills the 32-slot TX FIFO on a degraded bus; DroneCAN reliability is handled at the application layer + hfdcan1.Init.TransmitPause = DISABLE; + hfdcan1.Init.ProtocolException = DISABLE; + + __HAL_RCC_FDCAN_CLK_ENABLE(); + + 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 = 0; + hfdcan1.Init.RxBufferSize = FDCAN_DATA_BYTES_8; + hfdcan1.Init.StdFiltersNbr = 0; + hfdcan1.Init.ExtFiltersNbr = 1; + hfdcan1.Init.TxFifoQueueElmtsNbr = 3; + hfdcan1.Init.TxEventsNbr = 0; + hfdcan1.Init.TxBuffersNbr = 0; + hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_QUEUE_OPERATION; + hfdcan1.Init.TxElmtSize = FDCAN_DATA_BYTES_8; + + 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_REJECT, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_REJECT_REMOTE, FDCAN_REJECT_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; + } + + if (HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_TX_COMPLETE, + FDCAN_TX_BUFFER0 | FDCAN_TX_BUFFER1 | FDCAN_TX_BUFFER2) != HAL_OK) { /* Must match TxFifoQueueElmtsNbr = 3 */ + LOG_ERROR(CAN, "Failed to activate interrupt notification"); + return -CANARD_ERROR_INTERNAL; + } + + /* FDCAN_IT_TX_COMPLETE routes to LINE0 by default (ILS resets to 0) */ + HAL_NVIC_SetPriority(FDCAN1_IT0_IRQn, NVIC_PRIO_CAN, 0); + HAL_NVIC_EnableIRQ(FDCAN1_IT0_IRQn); + + return CANARD_OK; +} + /** * @brief Process CAN message from RxLocation FIFO into rx_frame * @param hfdcan pointer to an FDCAN_HandleTypeDef structure that contains @@ -38,9 +124,9 @@ static FDCAN_HandleTypeDef hfdcan1; * 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 + * @retval ret == 0: OK (CANARD_OK), ret < 0: CANARD_ERROR */ -int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { +int16_t canardSTM32Receive(CanardCANFrame *const rx_frame) { if (rx_frame == NULL) { return -CANARD_ERROR_INVALID_ARGUMENT; } @@ -61,8 +147,12 @@ int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { rx_frame->id |= CANARD_CAN_FRAME_RTR; } - rx_frame->data_len = RxHeader.DataLength; - memcpy(rx_frame->data, RxData, RxHeader.DataLength); + /* FDCAN_DLC_BYTES_0..8 equal 0..8, so DataLength is the byte count in FDCAN_FRAME_CLASSIC mode. */ + if (RxHeader.DataLength > CANARD_CAN_FRAME_MAX_DATA_LEN) { + return -CANARD_ERROR_INVALID_ARGUMENT; /* should never happen in FDCAN_FRAME_CLASSIC mode */ + } + rx_frame->data_len = (uint8_t)RxHeader.DataLength; + memcpy(rx_frame->data, RxData, rx_frame->data_len); // assume a single interface rx_frame->iface_id = 0; @@ -78,7 +168,7 @@ int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { * @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 + * @retval ret == 0: OK (CANARD_OK), ret < 0: CANARD_ERROR */ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { if (tx_frame == NULL) { @@ -101,7 +191,7 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { TxHeader.Identifier = tx_frame->id & CANARD_CAN_STD_ID_MASK; } - TxHeader.DataLength = tx_frame->data_len; + TxHeader.DataLength = tx_frame->data_len; /* FDCAN_DLC_BYTES_0..8 == 0..8; valid only in FDCAN_FRAME_CLASSIC mode */ if (tx_frame->id & CANARD_CAN_FRAME_RTR) { TxHeader.TxFrameType = FDCAN_REMOTE_FRAME; @@ -109,11 +199,11 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { 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 + TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE; + TxHeader.BitRateSwitch = FDCAN_BRS_OFF; + TxHeader.FDFormat = FDCAN_CLASSIC_CAN; + TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS; + TxHeader.MessageMarker = 0; if (TxHeader.DataLength <= sizeof(TxData)) { memcpy(TxData, tx_frame->data, TxHeader.DataLength); @@ -125,111 +215,66 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { } 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; +void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){ + FDCAN_ProtocolStatusTypeDef protocolStatus = {}; + HAL_FDCAN_GetProtocolStatus(&hfdcan1, &protocolStatus); + pProtocolStat->BusOff = protocolStatus.BusOff; + pProtocolStat->ErrorPassive = protocolStatus.ErrorPassive; + /* HAL provides no accessor for ECR (TEC/REC); read directly. PSR fields (BusOff, ErrorPassive, LEC) come from HAL. */ + uint32_t ecr = hfdcan1.Instance->ECR; + pProtocolStat->tec = (uint8_t)(ecr & 0xFF); /* ECR[7:0] */ + pProtocolStat->rec = (uint8_t)((ecr >> 8) & 0x7F); /* ECR[14:8] */ + pProtocolStat->lec = (uint8_t)(protocolStatus.LastErrorCode & 0x07); +} - ErrorCode = canardSTM32ComputeTimings(bitrate, &out_timings); - if (ErrorCode != 1) - { - LOG_ERROR(CAN, "Unable to calculate timings, Error Code:%d", ErrorCode); - return -CANARD_ERROR_INTERNAL; - } +uint32_t canardSTM32GetAndClearRxDropCount(void) { + return 0; // H7 FIFO0 (30 slots) has no software ring buffer; hardware overflow is tracked via GetProtocolStatus +} - 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); +int32_t canardSTM32GetTxQueueFillLevel(void){ + return 0; +} - 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"); +int32_t canardSTM32GetRxFifoFillLevel(void){ + return (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1, FDCAN_RX_FIFO0)); +} - /** 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; - } +void canardSTM32RecoverFromBusOff(void){ + hfdcan1.Instance->TXBCR = 0xFFFFFFFFU; // Cancel all pending TX requests before recovery + /* H7 FDCAN does not set CCCR.INIT on bus-off entry (unlike F7 bxCAN ABOM). + Hardware runs the 128x11 recessive-bit sequence autonomously. This clear + is a defensive no-op in case software previously entered init mode. */ + CLEAR_BIT(hfdcan1.Instance->CCCR, FDCAN_CCCR_INIT); +} - /* FDCAN1 clock enable */ - __HAL_RCC_FDCAN_CLK_ENABLE(); +/* + get a 16 byte unique ID for this node, this should be based on the CPU unique ID or other 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 + memset(id, 0, 16); + HALUniqueIDs[0] = HAL_GetUIDw0(); + HALUniqueIDs[1] = HAL_GetUIDw1(); + HALUniqueIDs[2] = HAL_GetUIDw2(); + memcpy(id, HALUniqueIDs, 12); +} - 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; - } +// ---- ISR handler ------------------------------------------------------------ - if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK) { - LOG_ERROR(CAN, "Failed to Start"); - return -CANARD_ERROR_INTERNAL; - } - return CANARD_OK; +void FDCAN1_IT0_IRQHandler(void) { + HAL_FDCAN_IRQHandler(&hfdcan1); } +// ---- Private helpers -------------------------------------------------------- + /** * @brief GPIO Initialization Function * @param None @@ -240,9 +285,9 @@ 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 @@ -251,10 +296,11 @@ static void canardSTM32GPIO_Init(void) // TODO: Tie the pin state to a configuration option so we can turn CAN on and off. IOInit(IOGetByTag(IO_TAG(CAN1_STANDBY)), OWNER_DRONECAN, RESOURCE_CAN_STANDBY, 0); - IOConfigGPIO(IOGetByTag(IO_TAG(CAN1_STANDBY)), IOCFG_OUT_PP); // Do any boards use pullups, external/internal? + IOConfigGPIO(IOGetByTag(IO_TAG(CAN1_STANDBY)), IOCFG_OUT_PP); IOLo(IOGetByTag(IO_TAG(CAN1_STANDBY))); #endif } + static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings *out_timings) { if (target_bitrate < 1) { @@ -264,7 +310,7 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi /* * Hardware configuration */ - const uint32_t pclk = HAL_RCC_GetPCLK1Freq(); + const uint32_t pclk = HAL_RCCEx_GetPeriphCLKFreq(RCC_PERIPHCLK_FDCAN); static const int MaxBS1 = 16; static const int MaxBS2 = 8; @@ -281,8 +327,6 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi * 125 kbps 16 17 */ const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17; - LOG_DEBUG(CAN, "Baudrate: %lu", target_bitrate); - LOG_DEBUG(CAN, "Max Quanta per bit: %i", max_quanta_per_bit); static const int MaxSamplePointLocation = 900; @@ -297,7 +341,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. */ @@ -314,7 +357,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. @@ -365,44 +407,10 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi return false; } - LOG_DEBUG(CAN, "Timings: quanta/bit: %d, sample point location: %f%%", - (int)(1 + solution.bs1 + solution.bs2), (double)(solution.sample_point_permill) / (double)(10.0)); - out_timings->prescaler = (uint16_t)(prescaler); - out_timings->sjw = 8; // Not happy with this value, but 1MBPs with unshielded cable? + out_timings->sjw = 1; out_timings->bs1 = (uint8_t)(solution.bs1); // The HAL takes care of the 1 bs offset in the register so don't remove it here like AP does. out_timings->bs2 = (uint8_t)(solution.bs2); // The HAL takes care of the 1 bs offset in the register so don't remove it here like AP does. return true; } - -void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){ - FDCAN_ProtocolStatusTypeDef protocolStatus = {}; - - HAL_FDCAN_GetProtocolStatus(&hfdcan1, &protocolStatus); - pProtocolStat->BusOff = protocolStatus.BusOff; - pProtocolStat->ErrorPassive = protocolStatus.ErrorPassive; - LOG_DEBUG(CAN, "BusOff: %lu", protocolStatus.BusOff); - LOG_DEBUG(CAN, "ErrorPassive: %lu", protocolStatus.ErrorPassive); -} - -int32_t canardSTM32GetRxFifoFillLevel(void){ - return (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1, FDCAN_RX_FIFO0)); -} - -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 - */ -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 - memset(id, 0, 16); - HALUniqueIDs[0] = HAL_GetUIDw0(); - 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 27713297c86..124294e4e5f 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -125,6 +126,7 @@ bool cliMode = false; #include "sensors/temperature.h" #ifdef USE_DRONECAN #include "drivers/dronecan/dronecan.h" +#include "drivers/dronecan/libcanard/canard_stm32_driver.h" #endif #ifdef USE_ESC_SENSOR #include "sensors/esc_sensor.h" @@ -4200,11 +4202,12 @@ static void cliStatus(char *cmdline) #endif #ifdef USE_DRONECAN - static const char * const dronecanStateNames[] = {"INIT", "NORMAL", "BUS_OFF"}; + static const char * const dronecanStateNames[] = {"INIT", "NORMAL", "BUS_OFF", "FAILED"}; + STATIC_ASSERT(ARRAYLEN(dronecanStateNames) == STATE_DRONECAN_COUNT, dronecanStateNames_size_mismatch); cliPrintLinef("DroneCAN: nodeID=%d, bitrate=%u kbps, status=%s, nodes=%d", dronecanConfig()->nodeID, (unsigned)dronecanGetBitrateKbps(), - dronecanStateNames[dronecanGetState()], + dronecanStateNames[MIN((int)dronecanGetState(), (int)STATE_DRONECAN_COUNT - 1)], dronecanGetNodeCount() ); #endif @@ -4693,6 +4696,28 @@ static void printConfig(const char *cmdline, bool doDiff) restoreConfigs(); } +#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], (unsigned)stat.lec); + cliPrintLinef(" TX queue: %" PRId32, txFill); + cliPrintLinef(" RX buffer: %" PRId32, rxFill); +} +#endif + static void cliDump(char *cmdline) { printConfig(cmdline, false); @@ -4938,6 +4963,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 diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e5cfe7e81e8..9da0b461562 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1899,7 +1899,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF .nodeID = node->nodeID, .health = node->health, .mode = node->mode, - .last_seen_ms = node->last_seen_ms, + .last_seen_ms = millis() - node->last_seen_ms, }, sizeof(dronecanNodeStatus_t)); } } @@ -4608,7 +4608,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu sbufWriteU8(dst, node->mode); sbufWriteU32(dst, node->uptime_sec); sbufWriteU16(dst, node->vendor_status_code); - sbufWriteU32(dst, node->last_seen_ms); + sbufWriteU32(dst, millis() - node->last_seen_ms); sbufWriteU8(dst, node->name_len); sbufWriteDataSafe(dst, node->name, 32); found = true; diff --git a/src/main/io/gps_dronecan.c b/src/main/io/gps_dronecan.c index 849b5fe59b7..07e595cd208 100644 --- a/src/main/io/gps_dronecan.c +++ b/src/main/io/gps_dronecan.c @@ -56,7 +56,6 @@ static bool newDataReady; static uint16_t lastHDOP = 9999; -static uint16_t lastVDOP = 9999; void gpsRestartDronecan(void) { @@ -82,8 +81,6 @@ static uint8_t gpsMapFixType(uint8_t dronecanFixType) void dronecanGPSReceiveGNSSFix(const struct uavcan_equipment_gnss_Fix * pgnssFix) { - //const mspSensorGpsDataMessage_t * pkt = (const mspSensorGpsDataMessage_t *)bufferPtr; - gpsSolDRV.fixType = gpsMapFixType(pgnssFix->status); gpsSolDRV.numSat = pgnssFix->sats_used; gpsSolDRV.llh.lon = pgnssFix->longitude_deg_1e8 / 10; // convert to deg_1e7 @@ -134,8 +131,6 @@ void dronecanGPSReceiveGNSSFix(const struct uavcan_equipment_gnss_Fix * pgnssFix void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 * pgnssFix2) { - //const mspSensorGpsDataMessage_t * pkt = (const mspSensorGpsDataMessage_t *)bufferPtr; - gpsSolDRV.fixType = gpsMapFixType(pgnssFix2->status); gpsSolDRV.numSat = pgnssFix2->sats_used; gpsSolDRV.llh.lon = pgnssFix2->longitude_deg_1e8 / 10; // convert to deg_1e7 @@ -152,11 +147,9 @@ void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 * pgnssF gpsSolDRV.groundCourse = RADIANS_TO_DECIDEGREES(groundCourse); // TODO where to get EPH gpsSolDRV.eph = gpsConstrainEPE(pgnssFix-> / 10); // TODO where to get EPV gpsSolDRV.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10); - LOG_DEBUG(CAN, "Last HDOP %d", lastHDOP); if (pgnssFix2->pdop > 0){ gpsSolDRV.hdop = gpsConstrainHDOP(pgnssFix2->pdop * 100); // Only update if valid. } else if((9999 > lastHDOP) && (lastHDOP > 0)) { - LOG_DEBUG(CAN, "Updating gpsSolDRV"); gpsSolDRV.hdop = lastHDOP; } gpsSolDRV.flags.validVelNE = true; @@ -187,10 +180,10 @@ void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 * pgnssF void dronecanGPSReceiveGNSSAuxiliary(const struct uavcan_equipment_gnss_Auxiliary * pgnssAux) { - UNUSED(pgnssAux); - // No useful information I think... Placeholder until after testing. - lastVDOP = pgnssAux->vdop * 100; - lastHDOP = pgnssAux->hdop * 100; - + // DroneCAN float16 optional fields encode NaN when unpopulated; guard before use. + // gpsConstrainHDOP clamps to 9999 preventing uint16_t overflow for extreme DOP values. + if (!isnan(pgnssAux->hdop)) { + lastHDOP = gpsConstrainHDOP((uint32_t)(pgnssAux->hdop * 100)); + } } #endif \ No newline at end of file diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c index 0ef3e8b2b63..32f8c638fe2 100644 --- a/src/main/rx/fport.c +++ b/src/main/rx/fport.c @@ -149,8 +149,6 @@ static serialPort_t *fportPort; static void reportFrameError(uint8_t errorReason) { UNUSED(errorReason); - static volatile uint16_t frameErrors = 0; - frameErrors++; } // Receive ISR callback diff --git a/src/main/target/KAKUTEH7WING/target.h b/src/main/target/KAKUTEH7WING/target.h index 7626b997754..9601a4c77fe 100644 --- a/src/main/target/KAKUTEH7WING/target.h +++ b/src/main/target/KAKUTEH7WING/target.h @@ -154,6 +154,12 @@ #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART6 +// *************** CANBUS **************************** +#define USE_DRONECAN +#define CAN1_RX PD0 +#define CAN1_TX PD1 +// #define CAN1_STANDBY PD3 + // *************** ADC ***************************** #define USE_ADC #define ADC_INSTANCE ADC1 diff --git a/src/main/target/system_stm32h7xx.c b/src/main/target/system_stm32h7xx.c index 56a53cefcfb..eaf88057cfb 100644 --- a/src/main/target/system_stm32h7xx.c +++ b/src/main/target/system_stm32h7xx.c @@ -497,20 +497,33 @@ void SystemClock_Config(void) RCC_PeriphClkInit.I2c4ClockSelection = RCC_I2C4CLKSOURCE_D3PCLK1; HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphClkInit); -#ifdef USE_SDCARD_SDIO +#if defined(USE_SDCARD_SDIO) || defined(USE_DRONECAN) // PLL2M = HSE_VALUE / 1600000 pins the VCO input to exactly 1.6 MHz for any HSE. - // With N=500 this gives VCO=800 MHz: PLL2R/4=200 MHz (SDMMC), PLL2P/2=400 MHz. + // With N=500: VCO=800 MHz, PLL2R/4=200 MHz (SDMMC), PLL2Q/10=80 MHz (FDCAN). + // HSE_VALUE must be an exact multiple of 1600000. CMake sets it per-target via -DHSE_VALUE=; + // current targets use 8MHz (÷5) and 16MHz (÷10). If adding a target with a non-multiple HSE, + // this assert will fire — choose a different VCO input frequency. STATIC_ASSERT(HSE_VALUE % 1600000 == 0, HSE_not_a_multiple_of_1600000); - RCC_PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_SDMMC; RCC_PeriphClkInit.PLL2.PLL2M = HSE_VALUE / 1600000; RCC_PeriphClkInit.PLL2.PLL2N = 500; - RCC_PeriphClkInit.PLL2.PLL2P = 2; // 400Mhz - RCC_PeriphClkInit.PLL2.PLL2Q = 3; // 266Mhz - 133Mhz can be derived from this for for QSPI if flash chip supports the speed. - RCC_PeriphClkInit.PLL2.PLL2R = 4; // 200Mhz HAL LIBS REQUIRE 200MHZ SDMMC CLOCK, see HAL_SD_ConfigWideBusOperation, SDMMC_HSpeed_CLK_DIV, SDMMC_NSpeed_CLK_DIV + RCC_PeriphClkInit.PLL2.PLL2P = 2; + RCC_PeriphClkInit.PLL2.PLL2Q = 10; // 80 MHz for FDCAN + RCC_PeriphClkInit.PLL2.PLL2R = 4; // 200 MHz for SDMMC RCC_PeriphClkInit.PLL2.PLL2RGE = RCC_PLL2VCIRANGE_0; RCC_PeriphClkInit.PLL2.PLL2VCOSEL = RCC_PLL2VCOWIDE; RCC_PeriphClkInit.PLL2.PLL2FRACN = 0; + + uint32_t periphSel = 0; +#ifdef USE_SDCARD_SDIO + periphSel |= RCC_PERIPHCLK_SDMMC; RCC_PeriphClkInit.SdmmcClockSelection = RCC_SDMMCCLKSOURCE_PLL2; +#endif + +#ifdef USE_DRONECAN + periphSel |= RCC_PERIPHCLK_FDCAN; + RCC_PeriphClkInit.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL2; +#endif + RCC_PeriphClkInit.PeriphClockSelection = periphSel; if (HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphClkInit) != HAL_OK) { Error_Handler(); }