diff --git a/docs/Settings.md b/docs/Settings.md index 27e354dc9ce..0de2f3f70d8 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -5946,6 +5946,7 @@ Selection of pitot hardware. VIRTUAL only works if a GPS is enabled. | FAKE | | | MSP | | | DLVR-L10D | | +| MS5525 | | --- diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 42e03212b38..e79144a8c84 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -85,6 +85,7 @@ void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) { UNUSED(ins); + if (gpsConfig()->provider != GPS_DRONECAN) return; struct uavcan_equipment_gnss_Auxiliary gnssAuxiliary; if (uavcan_equipment_gnss_Auxiliary_decode(transfer, &gnssAuxiliary)) { @@ -92,11 +93,11 @@ void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) { return; } dronecanGPSReceiveGNSSAuxiliary(&gnssAuxiliary); - LOG_DEBUG(CAN, "GNSS Auxiliary: Sats=%d HDOP=%.1f", gnssAuxiliary.sats_used, (double)gnssAuxiliary.hdop); } void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) { UNUSED(ins); + if (gpsConfig()->provider != GPS_DRONECAN) return; struct uavcan_equipment_gnss_Fix gnssFix; if (uavcan_equipment_gnss_Fix_decode(transfer, &gnssFix)) { @@ -104,11 +105,11 @@ void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) { return; } dronecanGPSReceiveGNSSFix(&gnssFix); - LOG_DEBUG(CAN, "GNSS Fix received"); } void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) { UNUSED(ins); + if (gpsConfig()->provider != GPS_DRONECAN) return; struct uavcan_equipment_gnss_Fix2 gnssFix2; if (uavcan_equipment_gnss_Fix2_decode(transfer, &gnssFix2)) { @@ -116,18 +117,17 @@ void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) { return; } dronecanGPSReceiveGNSSFix2(&gnssFix2); - LOG_DEBUG(CAN, "GNSS Fix2 received"); } void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) { UNUSED(ins); + if (gpsConfig()->provider != GPS_DRONECAN) return; 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 handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) { @@ -139,7 +139,6 @@ void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) { return; } dronecanBatterySensorReceiveInfo(&batteryInfo); - LOG_DEBUG(CAN, "Battery Info"); } /* @@ -148,8 +147,6 @@ void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) { // 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); - uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; struct uavcan_protocol_GetNodeInfoResponse pkt; @@ -196,7 +193,6 @@ void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) { 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; @@ -300,13 +296,6 @@ bool shouldAcceptTransfer(const CanardInstance *ins, */ 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) { @@ -346,7 +335,6 @@ void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { break; case UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID: - LOG_DEBUG(CAN, "Battery Info"); handle_BatteryInfo(ins, transfer); break; } @@ -364,7 +352,6 @@ void processCanardTxQueue(void) { 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 @@ -392,7 +379,6 @@ void process1HzTasks(timeUs_t timestamp_usec) void dronecanInit(void) { - LOG_DEBUG(CAN, "dronecan Init"); uint32_t bitrate = 500000; // At least define 500000 switch (dronecanConfig()->bitRateKbps){ @@ -420,7 +406,7 @@ void dronecanInit(void) 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 + dronecanState = STATE_DRONECAN_FAILED; return; } /* @@ -463,8 +449,6 @@ void dronecanUpdate(timeUs_t currentTimeUs) 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); @@ -485,26 +469,35 @@ void dronecanUpdate(timeUs_t currentTimeUs) next_1hz_service_at += 1000000ULL; process1HzTasks(currentTimeUs); processCanardTxQueue(); - } - canardSTM32GetProtocolStatus(&protocolStatus); - if(protocolStatus.BusOff != 0) { - dronecanState = STATE_DRONECAN_BUS_OFF; - busoffTimeUs = currentTimeUs; + canardSTM32GetProtocolStatus(&protocolStatus); + if (protocolStatus.BusOff != 0 || protocolStatus.ErrorPassive != 0) { + LOG_DEBUG(CAN, "CAN status: BusOff=%" PRIu32 " ErrorPassive=%" PRIu32, protocolStatus.BusOff, protocolStatus.ErrorPassive); + } + if (protocolStatus.BusOff != 0) { + dronecanState = STATE_DRONECAN_BUS_OFF; + busoffTimeUs = currentTimeUs; + } } break; case STATE_DRONECAN_BUS_OFF: - if(currentTimeUs > (busoffTimeUs + 100000)) { // Wait 100 mS + 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; + canardSTM32GetProtocolStatus(&protocolStatus); + if(protocolStatus.BusOff == 0) { + dronecanState = STATE_DRONECAN_NORMAL; + } } break; - + + case STATE_DRONECAN_FAILED: + break; + + case STATE_DRONECAN_COUNT: + break; + } } 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_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index dd7d3e1b357..d630e9ab0ef 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -164,13 +164,10 @@ 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; } @@ -183,14 +180,10 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { */ 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; @@ -207,11 +200,15 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) hcan1.Init.TimeTriggeredMode = DISABLE; hcan1.Init.AutoBusOff = ENABLE; hcan1.Init.AutoWakeUp = DISABLE; - hcan1.Init.AutoRetransmission = ENABLE; + 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; - canardSTM32ComputeTimings(bitrate, &out_timings); + 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; @@ -219,31 +216,13 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) 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); - // 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()); + canardSTM32GPIO_Init(); 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; @@ -278,9 +257,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,7 +268,7 @@ 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 } @@ -320,9 +299,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 : 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); static const int MaxSamplePointLocation = 900; /* @@ -336,7 +312,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 +328,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,11 +378,8 @@ 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. @@ -419,8 +390,6 @@ void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){ 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); } int32_t canardSTM32GetRxFifoFillLevel(void){ @@ -428,8 +397,10 @@ int32_t canardSTM32GetRxFifoFillLevel(void){ } 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 + // 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. } /* diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c index 77c1f94921f..74f984edf2d 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c @@ -109,11 +109,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,12 +125,9 @@ 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; } @@ -142,35 +139,27 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { */ 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.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.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; - ErrorCode = canardSTM32ComputeTimings(bitrate, &out_timings); - if (ErrorCode != 1) + __HAL_RCC_FDCAN_CLK_ENABLE(); + + if (!canardSTM32ComputeTimings(bitrate, &out_timings)) { - LOG_ERROR(CAN, "Unable to calculate timings, Error Code:%d", ErrorCode); + LOG_ERROR(CAN, "Failed to compute CAN timings for bitrate %lu", (unsigned long)bitrate); return -CANARD_ERROR_INTERNAL; } @@ -188,23 +177,9 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) hfdcan1.Init.ExtFiltersNbr = 1; hfdcan1.Init.TxFifoQueueElmtsNbr = 32; hfdcan1.Init.TxEventsNbr = 0; - hfdcan1.Init.TxBuffersNbr = 5; + hfdcan1.Init.TxBuffersNbr = 0; 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 @@ -213,7 +188,6 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) 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; @@ -240,9 +214,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,7 +225,7 @@ 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 } @@ -264,7 +238,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 +255,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 +269,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 +285,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,11 +335,8 @@ 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. @@ -382,8 +349,6 @@ void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){ 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){ @@ -391,7 +356,8 @@ int32_t canardSTM32GetRxFifoFillLevel(void){ } void canardSTM32RecoverFromBusOff(void){ - CLEAR_BIT(hfdcan1.Instance->CCCR, FDCAN_CCCR_INIT); // Clear INIT bit to recover from Bus-Off + hfdcan1.Instance->TXBCR = 0xFFFFFFFFU; // Cancel all pending TX requests before recovery + CLEAR_BIT(hfdcan1.Instance->CCCR, FDCAN_CCCR_INIT); } /* diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 1976edaa8e0..97a5d14b681 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -4198,11 +4198,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 diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index d70f3edb1ca..3df125a1ab2 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)); } } @@ -4605,7 +4605,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/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 6ec0d1c4002..b6683072677 100644 --- a/src/main/target/system_stm32h7xx.c +++ b/src/main/target/system_stm32h7xx.c @@ -497,18 +497,36 @@ void SystemClock_Config(void) RCC_PeriphClkInit.I2c4ClockSelection = RCC_I2C4CLKSOURCE_D3PCLK1; HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphClkInit); -#ifdef USE_SDCARD_SDIO - RCC_PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_SDMMC; - RCC_PeriphClkInit.PLL2.PLL2M = 5; +#if defined(USE_SDCARD_SDIO) || defined(USE_DRONECAN) + // PLL2 provides SDMMC (200MHz via PLL2R) and FDCAN (80MHz via PLL2Q) + // VCO input = 1.6 MHz (HSE / M), VCO output = 800 MHz (1.6 * N=500) + // HSE_VALUE must be an exact multiple of 1600000 for integer division to give the correct M divider. + // CMake sets HSE_VALUE per-target via -DHSE_VALUE= (default 8MHz); the stm32h7xx_hal_conf.h + // fallback of 25MHz is never used for real hardware. Current targets: 8MHz (÷5) and 16MHz (÷10). + // If adding a new target with a non-multiple HSE, this assert will fire — choose a different VCO input. + STATIC_ASSERT(HSE_VALUE % 1600000 == 0, HSE_VALUE_must_be_a_multiple_of_1_6MHz_for_PLL2M_calculation); + RCC_PeriphClkInit.PLL2.PLL2M = HSE_VALUE / 1600000; RCC_PeriphClkInit.PLL2.PLL2N = 500; - RCC_PeriphClkInit.PLL2.PLL2P = 2; // 500Mhz - 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 - FDCAN + 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.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; - HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphClkInit); +#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(); + } #endif #ifdef USE_QUADSPI