diff --git a/Robot_V2/Algorithm/Filter.cpp b/Robot_V2/Algorithm/Filter.cpp index cbbf8a8..118433f 100644 --- a/Robot_V2/Algorithm/Filter.cpp +++ b/Robot_V2/Algorithm/Filter.cpp @@ -77,7 +77,10 @@ CurrentKalman::CurrentKalman(float initial_current, float L, float R, float T, : x(initial_current), P(1.0f), // 初始误差协方差 Q(Q), // 过程噪声 R(R_noise), // 测量噪声 - L(L), R_m(R), T(T) {} + L(L), R_m(R), T(T) { + + CircularBuffer buffer; + } /*! \brief 更新卡尔曼滤波器状态 \param[in] measured_current 测量电流值 diff --git a/Robot_V2/Algorithm/Filter.h b/Robot_V2/Algorithm/Filter.h index c715360..2d23541 100644 --- a/Robot_V2/Algorithm/Filter.h +++ b/Robot_V2/Algorithm/Filter.h @@ -66,9 +66,6 @@ class CurrentKalman { void setMeasurementNoise(float r); }; - extern CurrentKalman kalman; - extern CircularBuffer buffer; - #endif // __cplusplus #endif // _FILTER_H diff --git a/Robot_V2/RTE/Device/GD32C103CB/gd32c10x_can.c b/Robot_V2/RTE/Device/GD32C103CB/gd32c10x_can.c index 39b4ac4..ca75355 100644 --- a/Robot_V2/RTE/Device/GD32C103CB/gd32c10x_can.c +++ b/Robot_V2/RTE/Device/GD32C103CB/gd32c10x_can.c @@ -978,7 +978,8 @@ void can_message_receive(uint32_t can_periph, uint8_t fifo_number, can_receive_m receive_message->rx_data[7] = (uint8_t)(GET_RFIFOMDATA1_DB7(CAN_RFIFOMDATA1(can_periph, fifo_number))); } else { - CAN_ERROR_HANDLE("CAN FD function disabled \r\n"); + printf("CAN FD function disabled but FD frame received\r\n"); + // CAN_ERROR_HANDLE("CAN FD function disabled \r\n"); } } else { diff --git a/Robot_V2/Robot_V2.cproject.yml b/Robot_V2/Robot_V2.cproject.yml index 9afbc85..abb5113 100644 --- a/Robot_V2/Robot_V2.cproject.yml +++ b/Robot_V2/Robot_V2.cproject.yml @@ -1,4 +1,3 @@ -# A project translates into one executable or library. project: groups: - group: Source Files @@ -21,6 +20,7 @@ project: - file: device/adc.h - file: device/Timer.cpp - file: device/Timer.h + - file: device/test.cpp - group: Algorithm files: - file: Algorithm/Filter.cpp diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp index 69ef5b3..98b0f11 100644 --- a/Robot_V2/device/ADC.cpp +++ b/Robot_V2/device/ADC.cpp @@ -7,7 +7,7 @@ constexpr float ERROR_ADC_VAL = 255.0f; constexpr float ADC_SCALE = 3.3f / 4096.0f; constexpr float TEMP_FACTOR = 1000.0f / 4.3f; constexpr float VCC_FACTOR = 11.0f; -constexpr float CURRENT_FACTOR = 1.0f / 50.0f * 100.0f; +constexpr float CURRENT_FACTOR = 1.0f / 50.0f * 100.0f * 1000.0f; constexpr float MAX_TEMP = 150.0f; constexpr float MIN_TEMP = -40.0f; constexpr float MAX_CURRENT = 50.0f; @@ -83,7 +83,7 @@ float get_current(char type) { return ERROR_ADC_VAL; } } - + // 检查电流读数是否在有效范围内 if (current_latest < MIN_CURRENT || current_latest > MAX_CURRENT) { return ERROR_ADC_VAL; @@ -102,7 +102,7 @@ float get_vbus_voltage() { float vcc_latest = ERROR_ADC_VAL; for (int i = 0; i < 5; i++) { // 获取电压值 - vcc_latest = adc_value[6] * ADC_SCALE * VCC_FACTOR; + vcc_latest = adc_value[5] * ADC_SCALE * VCC_FACTOR; // 检查电压读数是否在有效范围内 if (vcc_latest > MAX_VCC || vcc_latest < MIN_VCC) { diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 80f9690..e5468c8 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -1,26 +1,41 @@ #include "Can.h" +#include "gd32c10x.h" #include "gd32c10x_can.h" -#include "gd32c10x_eval.h" #include +#include +#include #include +#include #include #include +#include + +#define DEV_CAN0_ID 0x0000 +#define DEV_CAN0_MASK 0x0000 +#define CANX CAN0 #define DEV_CAN_LOOPBACK_ID 0x0000 -#define DEV_CAN0_ID 0xaabb -#define DEV_CAN0_MASK 0x0000 +extern CAN_transmiter CAN0_transmiter; + +/*! + \brief initialize CAN GPIO + \param[in] none + \param[out] none + \retval ErrStatus: SUCCESS or ERROR +*/ u8 can_gpio_init() { /* enable CAN clock */ rcu_periph_clock_enable(RCU_CAN0); rcu_periph_clock_enable(RCU_GPIOA); - rcu_periph_clock_enable(RCU_AF); - /* configure CAN0 GPIO */ - gpio_init(GPIOA, GPIO_MODE_IPU, GPIO_OSPEED_MAX, GPIO_PIN_11); - gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_MAX, GPIO_PIN_12); - - /* enable the EN GPIO clock */ rcu_periph_clock_enable(RCU_GPIOB); + rcu_periph_clock_enable(RCU_AF); + + /* configure CAN0 GPIO */ + gpio_init(GPIOA, GPIO_MODE_IPU, GPIO_OSPEED_50MHZ, GPIO_PIN_11); // tx + gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_12); // rx + gpio_pin_remap_config(GPIO_CAN0_FULL_REMAP, DISABLE); + /* enable the EN GPIO clock */ gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_13); gpio_bit_reset(GPIOB, GPIO_PIN_13); return 0; @@ -33,8 +48,9 @@ u8 can_gpio_init() { \retval none */ void can_config(void) { + can_gpio_init(); can_parameter_struct can_parameter; - can_filter_parameter_struct can_filter; + can_filter_parameter_struct can_filter; can_struct_para_init(CAN_INIT_STRUCT, &can_parameter); /* initialize CAN register */ @@ -52,31 +68,31 @@ void can_config(void) { can_parameter.resync_jump_width = CAN_BT_SJW_1TQ; can_parameter.time_segment_1 = CAN_BT_BS1_7TQ; can_parameter.time_segment_2 = CAN_BT_BS2_2TQ; - can_parameter.prescaler = 12; + can_parameter.prescaler = 6; /* initialize CAN */ - can_init(CAN0, &can_parameter); + if (can_init(CAN0, &can_parameter) != SUCCESS) { + while (1) + printf("can init error \r\n"); + } + + /* initialize filter */ + /* configure filter mode */ + can_filter.filter_mode = CAN_FILTERMODE_MASK; + can_filter.filter_bits = CAN_FILTERBITS_32BIT; + /* configure filter mask */ + can_filter.filter_mask_high = 0x0000; + can_filter.filter_mask_low = 0x0000; + /* select receiver fifo */ + can_filter.filter_fifo_number = CAN_FIFO0; + can_filter.filter_enable = ENABLE; + /* configure filter number */ + can_filter.filter_number = 0; + can_filter_init(&can_filter); - /* initialize filter */ - /* configure filter mode */ - can_filter.filter_mode = CAN_FILTERMODE_MASK; - can_filter.filter_bits = CAN_FILTERBITS_32BIT; - /* configure filter ID */ - can_filter.filter_list_high = (uint16_t)(DEV_CAN_LOOPBACK_ID >> 13U); - can_filter.filter_list_low = ((uint16_t)(DEV_CAN_LOOPBACK_ID)) << 3U | (1U << 2); - /* configure filter mask */ - can_filter.filter_mask_high = 0x0000; - can_filter.filter_mask_low = 0x0000; - /* select receiver fifo */ - can_filter.filter_fifo_number = CAN_FIFO0; - can_filter.filter_enable = ENABLE; - can_filter_init(&can_filter); - /* configure CAN0 NVIC */ - nvic_irq_enable(CAN0_RX0_IRQn, 0, 0); + nvic_irq_enable(CAN0_RX0_IRQn, 1, 0); /* enable can receive FIFO0 not empty interrupt */ can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0); - - } /*! @@ -86,18 +102,16 @@ void can_config(void) { \retval none */ void canfd_config(void) { + can_gpio_init(); can_parameter_struct can_parameter; can_fdframe_struct can_fd_parameter; can_fd_tdc_struct can_fd_tdc_parameter; - can_filter_parameter_struct can_filter; + can_filter_parameter_struct can_filter; - /* enable CAN clock */ - rcu_periph_clock_enable(RCU_CAN0); + /* initialize CAN register */ + can_deinit(CAN0); - /* initialize CAN register */ - can_deinit(CAN0); - - /* initialize CAN parameters */ + /* initialize CAN */ can_parameter.time_triggered = DISABLE; can_parameter.auto_bus_off_recovery = DISABLE; can_parameter.auto_wake_up = DISABLE; @@ -109,8 +123,7 @@ void canfd_config(void) { can_parameter.resync_jump_width = CAN_BT_SJW_1TQ; can_parameter.time_segment_1 = CAN_BT_BS1_7TQ; can_parameter.time_segment_2 = CAN_BT_BS2_2TQ; - can_parameter.prescaler = 12; - /* initialize CAN */ + can_parameter.prescaler = 6; can_init(CAN0, &can_parameter); can_struct_para_init(CAN_FD_FRAME_STRUCT, &can_fd_parameter); @@ -129,17 +142,29 @@ void canfd_config(void) { can_fd_parameter.data_time_segment_2 = CAN_BT_BS2_1TQ; /* CAN-FD data segement prescaler should be the same as non-data segement * prescaler */ - can_fd_parameter.data_prescaler = 12; + can_fd_parameter.data_prescaler = 6; can_fd_init(CAN0, &can_fd_parameter); - /* initialize filter */ - can1_filter_start_bank(14); - can_filter_mask_mode_init(DEV_CAN0_ID, DEV_CAN0_MASK, CAN_EXTENDED_FIFO0, 0); + /* initialize filter */ + can_filter.filter_mode = CAN_FILTERMODE_MASK; + can_filter.filter_bits = CAN_FILTERBITS_32BIT; + /* configure filter ID */ + can_filter.filter_list_high = 0x0000; + can_filter.filter_list_low = 0x0000; + /* configure filter mask */ + can_filter.filter_mask_high = 0x0000; + can_filter.filter_mask_low = 0x0000; + /* select receiver fifo */ + can_filter.filter_fifo_number = CAN_FIFO0; + can_filter.filter_enable = ENABLE; + /* configure filter number */ + can_filter.filter_number = 0; + can_filter_init(&can_filter); - /* configure CAN0 NVIC */ - nvic_irq_enable(CAN0_RX0_IRQn, 0, 0); - /* enable can receive FIFO0 not empty interrupt */ - can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0); + /* configure CAN0 NVIC */ + nvic_irq_enable(CAN0_RX0_IRQn, 1, 0); + /* enable can receive FIFO0 not empty interrupt */ + can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0); } /*! @@ -149,109 +174,171 @@ void canfd_config(void) { \retval none */ void can_loopback_init(void) { + can_gpio_init(); can_parameter_struct can_parameter; - can_fdframe_struct can_fd_parameter; - can_fd_tdc_struct can_fd_tdc_parameter; - can_filter_parameter_struct can_filter; + can_filter_parameter_struct can_filter; + /* initialize CAN register */ + can_deinit(CANX); - /* enable CAN clock */ - rcu_periph_clock_enable(RCU_CAN0); - - /* initialize CAN register */ - can_deinit(CAN0); - - /* initialize CAN parameters */ + /* initialize CAN */ can_parameter.time_triggered = DISABLE; can_parameter.auto_bus_off_recovery = DISABLE; can_parameter.auto_wake_up = DISABLE; - can_parameter.auto_retrans = ENABLE; - can_parameter.rec_fifo_overwrite = ENABLE; - can_parameter.trans_fifo_order = ENABLE; - can_parameter.working_mode = CAN_LOOPBACK_MODE; + can_parameter.auto_retrans = DISABLE; + can_parameter.rec_fifo_overwrite = DISABLE; + can_parameter.trans_fifo_order = DISABLE; + can_parameter.working_mode = CAN_NORMAL_MODE; /* baudrate 1Mbps, sample piont at 80% */ can_parameter.resync_jump_width = CAN_BT_SJW_1TQ; can_parameter.time_segment_1 = CAN_BT_BS1_7TQ; can_parameter.time_segment_2 = CAN_BT_BS2_2TQ; - can_parameter.prescaler = 12; - /* initialize CAN */ - can_init(CAN0, &can_parameter); + can_parameter.prescaler = 6; + if (can_init(CANX, &can_parameter) != SUCCESS) { + while (1) + printf("can init error \r\n"); + } - can_struct_para_init(CAN_FD_FRAME_STRUCT, &can_fd_parameter); - can_fd_parameter.fd_frame = ENABLE; - can_fd_parameter.excp_event_detect = ENABLE; - can_fd_parameter.delay_compensation = ENABLE; - can_fd_tdc_parameter.tdc_filter = 0x04; - can_fd_tdc_parameter.tdc_mode = CAN_TDCMOD_CALC_AND_OFFSET; - can_fd_tdc_parameter.tdc_offset = 0x04; - can_fd_parameter.p_delay_compensation = &can_fd_tdc_parameter; - can_fd_parameter.iso_bosch = CAN_FDMOD_ISO; - can_fd_parameter.esi_mode = CAN_ESIMOD_HARDWARE; - /* FD mode data baudrate 2Mbps, sample piont at 80% */ - can_fd_parameter.data_resync_jump_width = CAN_BT_SJW_1TQ; - can_fd_parameter.data_time_segment_1 = CAN_BT_BS1_3TQ; - can_fd_parameter.data_time_segment_2 = CAN_BT_BS2_1TQ; - /* CAN-FD data segement prescaler should be the same as non-data segement - * prescaler */ - can_fd_parameter.data_prescaler = 12; - can_fd_init(CAN0, &can_fd_parameter); - - /* initialize filter */ - can1_filter_start_bank(14); - can_filter_mask_mode_init(DEV_CAN_LOOPBACK_ID, DEV_CAN0_MASK, CAN_EXTENDED_FIFO0, 0); + /* initialize filter */ + /* configure filter mode */ + can_filter.filter_mode = CAN_FILTERMODE_MASK; + can_filter.filter_bits = CAN_FILTERBITS_32BIT; + /* configure filter ID */ + can_filter.filter_list_high = (uint16_t)(DEV_CAN_LOOPBACK_ID >> 13U); + can_filter.filter_list_low = + ((uint16_t)(DEV_CAN_LOOPBACK_ID)) << 3U | (1U << 2); + /* configure filter mask */ + can_filter.filter_mask_high = 0x0000; + can_filter.filter_mask_low = 0x0000; + /* select receiver fifo */ + can_filter.filter_fifo_number = CAN_FIFO0; + can_filter.filter_enable = ENABLE; + /* configure filter number */ + can_filter.filter_number = 0; + can_filter_init(&can_filter); } /*! - \brief CAN0 loopback test + \brief CAN0 FD回环测试 \param[in] none \param[out] none \retval ErrStatus */ -void can_loopback_test(void) { - can_trasnmit_message_struct transmit_message; - can_receive_message_struct receive_message; - uint32_t timeout = 0xFFFF; - uint8_t transmit_mailbox = 0; +void canfd_loopback_test(void) { + can_trasnmit_message_struct transmit_message; + can_receive_message_struct receive_message; + uint32_t timeout = 0xFFFF; + uint8_t transmit_mailbox = 0; + std::mt19937 gen; + std::uniform_int_distribution dis(0, 255); - can_struct_para_init(CAN_FD_FRAME_STRUCT, &transmit_message); - transmit_message.tx_sfid = DEV_CAN_LOOPBACK_ID; - transmit_message.tx_efid = 0x00; - transmit_message.tx_ft = CAN_FT_DATA; - transmit_message.tx_ff = CAN_FF_EXTENDED; - transmit_message.tx_dlen = 12; - transmit_message.fd_flag = 1; - transmit_message.fd_brs = 1; - transmit_message.fd_esi = 0; + can_struct_para_init(CAN_FD_FRAME_STRUCT, &transmit_message); + transmit_message.tx_sfid = DEV_CAN_LOOPBACK_ID; + transmit_message.tx_efid = 0x00; + transmit_message.tx_ft = CAN_FT_DATA; + transmit_message.tx_ff = CAN_FF_EXTENDED; + transmit_message.tx_dlen = 7; + transmit_message.fd_flag = 1; + transmit_message.fd_brs = 1; + transmit_message.fd_esi = 0; int i = 0; - while (i++ dis(0, 0xff); + + can_struct_para_init(CAN_TX_MESSAGE_STRUCT, &transmit_message); + /* initialize transmit message */ + transmit_message.tx_sfid = DEV_CAN_LOOPBACK_ID; + transmit_message.tx_ft = CAN_FT_DATA; + transmit_message.tx_ff = CAN_FF_STANDARD; + transmit_message.tx_dlen = 7; + int i = 0; + while (i++ < transmit_message.tx_dlen) { + // transmit_message.tx_data[i] = dis(gen); + transmit_message.tx_data[i] = i; + } + /* transmit message */ + transmit_mailbox = can_message_transmit(CANX, &transmit_message); + /* waiting for transmit completed */ + while ((CAN_TRANSMIT_OK != can_transmit_states(CANX, transmit_mailbox)) && + (0 != timeout)) { + printf("sending:%x...\n", can_transmit_states(CANX, transmit_mailbox)); + timeout--; + } + timeout = 0xFF; + /* waiting for receive completed */ + while ((can_receive_message_length_get(CANX, CAN_FIFO0) < 1) && + (0 != timeout)) { + timeout--; + } + can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &receive_message); + can_message_receive(CANX, CAN_FIFO0, &receive_message); + /* check the receive message */ + if ((DEV_CAN_LOOPBACK_ID == receive_message.rx_sfid) && + (CAN_FF_STANDARD == receive_message.rx_ff) && + (transmit_message.tx_dlen == receive_message.rx_dlen)) { + i = transmit_message.tx_dlen; + while (i-- > 0) { + std::printf("rx %d:%x:%x\n", i, transmit_message.tx_data[i], + receive_message.rx_data[i]); + } + if (std::memcmp(receive_message.rx_data, transmit_message.tx_data, + transmit_message.tx_dlen) == 0) { + /* loopback test is success */ + std::printf("loopback success\n"); + } else { + std::printf("loopback failed\n"); + } + } else { + /* loopback test is failed */ + std::printf("loopback failed\n"); + } } /*! @@ -259,33 +346,14 @@ void can_loopback_test(void) { \note 初始化CAN通信相关参数和硬件配置 */ CAN_transmiter::CAN_transmiter() { - // 初始化成员变量 - controller_Mode = 0; - limit_velocity_union.f = 0; - limit_voltage = 0; - mcu_temp = 0; - motor_temp = 0; - Power_Voltage = 0; - rt_angle_union.f = 0; - Status_code = 0; - target_angle_union.f = 0; - Target_Control_Mode = 0; - target_velocity_union.f = 0; - u_current_union.f = 0; - w_current_union.f = 0; - v_current_union.f = 0; - // 初始化CAN硬件 - can_gpio_init(); - can_config(); + // can_config(); + canfd_config(); - printf("CAN_transmiter initialized successfully\r\n"); - - transmit_mailbox = CAN_NOMAILBOX; // 初始化为无mailbox状态 - timeout = 0xFFFF; // 默认超时时间 0xFFFF - - // 初始化发送消息结构体 - can_struct_para_init(CAN_TX_MESSAGE_STRUCT, &transmit_message); + // 初始化发送消息结构体 + can_struct_para_init(CAN_TX_MESSAGE_STRUCT, &transmit_message); + /* initialize receive message */ + can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &receive_message); } /*! @@ -293,142 +361,142 @@ CAN_transmiter::CAN_transmiter() { */ void CAN_transmiter::update(void) { limit_velocity_union.f = 0; - limit_voltage = 0; - mcu_temp = get_mcu_temp(); - motor_temp = get_drv_temp(); - Power_Voltage = get_vbus_voltage(); + limit_voltage = 0 * 1000; + mcu_temp = get_mcu_temp() * 100; + motor_temp = get_drv_temp() * 100; + Power_Voltage = get_vbus_voltage() * 1000; rt_angle_union.f = 0; rt_velocity_union.f = 0; Status_code = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 1 : 0; target_angle_union.f = 0; Target_Control_Mode = 0; target_velocity_union.f = 0; - u_current_union.f = get_current('U'); - w_current_union.f = get_current('W'); - v_current_union.f = get_current('V'); + u_current_union.f = get_current('U') * 1000; + w_current_union.f = get_current('W') * 1000; + v_current_union.f = get_current('V') * 1000; } /*! \brief CAN 数据发送 */ u8 CAN_transmiter::CAN_Send(u32 id, u8 length, u8 *data) { - // 检查数据长度是否符合CAN规范 - if (length > 8) { - printf("CAN data length exceeds maximum of 8 bytes\r\n"); - return 0; - } - - // 检查数据指针是否有效 - if (data == nullptr) { - printf("Invalid data pointer\r\n"); - return 0; - } - - /* 配置发送消息参数 */ - transmit_message.tx_ft = CAN_FT_DATA; // 数据帧 - transmit_message.tx_ff = CAN_FF_EXTENDED; // 扩展帧格式 - transmit_message.tx_sfid = id; // 设置帧ID - transmit_message.tx_dlen = length; // 设置数据长度 - - // 复制数据到发送缓冲区 - for (int i = 0; i < length && i < 8; i++) { - transmit_message.tx_data[i] = *(data + i); - } - - // 自动选择空闲mailbox发送消息，不需要人为指定 - transmit_mailbox = can_message_transmit(CAN0, &transmit_message); - - // 检查是否成功获取mailbox - if (CAN_NOMAILBOX == transmit_mailbox) { - // 没有可用的mailbox，可能所有mailbox都在使用中 - printf("No available mailbox for CAN transmission\r\n"); - return 0; - } - - // 等待发送完成，使用自动选择的mailbox进行状态检查 - while ((CAN_TRANSMIT_OK != can_transmit_states(CAN0, transmit_mailbox)) && - (0 != timeout)) { - timeout--; - } - - // 超时检查 - if (0 == timeout) { - printf("CAN transmission timeout for mailbox %d\r\n", transmit_mailbox); + timeout = 0xFFFF; + // 检查数据长度是否符合CAN规范 + if (length > 8) { + printf("CAN data length exceeds maximum of 8 bytes\r\n"); return 0; - } + } + + // 检查数据指针是否有效 + if (data == nullptr) { + printf("Invalid data pointer\r\n"); + return 0; + } + + /* 配置发送消息参数 */ + transmit_message.tx_ft = CAN_FT_DATA; // 数据帧 + transmit_message.tx_ff = CAN_FF_STANDARD; // 标准帧格式 + transmit_message.tx_sfid = id; // 设置帧ID + transmit_message.tx_dlen = length; // 设置数据长度 + + // 复制数据到发送缓冲区 + for (int i = 0; i < length && i < 8; i++) { + transmit_message.tx_data[i] = *(data + i); + } + // 自动选择空闲mailbox发送消息，不需要人为指定 + transmit_mailbox = can_message_transmit(CAN0, &transmit_message); + + // 检查是否成功获取mailbox + if (CAN_NOMAILBOX == transmit_mailbox) { + // 没有可用的mailbox，可能所有mailbox都在使用中 + printf("No available mailbox for CAN transmission\r\n"); + return 0; + } + + // 等待发送完成，使用自动选择的mailbox进行状态检查 + while ((CAN_TRANSMIT_OK != can_transmit_states(CAN0, transmit_mailbox)) && + (0 != timeout)) { + timeout--; + } + + // 超时检查 + if (0 == timeout) { + printf("CAN transmission timeout for mailbox %d\r\n", transmit_mailbox); + return 0; + } return 1; } - /*! \brief CANFD 数据发送 */ u8 CAN_transmiter::CANFD_Send(u32 id, u8 length, u8 *data) { - // 检查CANFD模式是否启用 - if (!(CAN_CTL(CAN0) & CAN_CTL_DFZ)) { - printf("CANFD mode not enabled\r\n"); - return 0; - } + timeout = 0xFFFF; + // 检查CANFD模式是否启用 + if (!(CAN_CTL(CAN0) & CAN_CTL_DFZ)) { + printf("CANFD mode not enabled\r\n"); + return 0; + } // 检查数据指针是否有效 - if (data == nullptr) { - printf("Invalid data pointer\r\n"); - return 0; - } - - // 检查数据长度是否符合CANFD规范 - if (length > 64) { - printf("CANFD data length exceeds maximum of 64 bytes\r\n"); - return 0; - } - - /* 配置发送消息参数 */ - transmit_message.tx_ft = CAN_FT_DATA; // 数据帧 - transmit_message.tx_ff = CAN_FF_EXTENDED; // 扩展帧格式 - transmit_message.tx_sfid = id; // 设置帧ID - transmit_message.tx_dlen = length; // 设置数据长度 - - // 设置CANFD特定参数 - transmit_message.fd_flag = CAN_FDF_FDFRAME; // CANFD帧格式 - transmit_message.fd_brs = ENABLE; // 使能比特率切换 - transmit_message.fd_esi = DISABLE; // 错误状态指示 - - // 复制数据到发送缓冲区 - for (int i = 0; i < length && i < 64; i++) { - transmit_message.tx_data[i] = *(data + i); - } - - // 自动选择空闲mailbox发送消息，不需要人为指定 - transmit_mailbox = can_message_transmit(CAN0, &transmit_message); - - // 检查是否成功获取mailbox - if (CAN_NOMAILBOX == transmit_mailbox) { - // 没有可用的mailbox，可能所有mailbox都在使用中 - printf("No available mailbox for CANFD transmission\r\n"); - return 0; - } - - // 等待发送完成，使用自动选择的mailbox进行状态检查 - while ((CAN_TRANSMIT_OK != can_transmit_states(CAN0, transmit_mailbox)) && - (0 != timeout)) { - timeout--; - } - - // 超时检查 - if (0 == timeout) { - printf("CANFD transmission timeout for mailbox %d\r\n", transmit_mailbox); + if (data == nullptr) { + printf("Invalid data pointer\r\n"); return 0; - } + } + + // 检查数据长度是否符合CANFD规范 + if (length > 64) { + printf("CANFD data length exceeds maximum of 64 bytes\r\n"); + return 0; + } + + /* 配置发送消息参数 */ + transmit_message.tx_ft = CAN_FT_DATA; // 数据帧 + transmit_message.tx_ff = CAN_FF_EXTENDED; // 扩展帧格式 + // transmit_message.tx_sfid = id; // 设置帧ID + transmit_message.tx_efid = id; // 扩展帧ID + transmit_message.tx_dlen = length; // 设置数据长度 + + // 设置CANFD特定参数 + transmit_message.fd_flag = CAN_FDF_FDFRAME; // CANFD帧格式 + transmit_message.fd_brs = ENABLE; // 使能比特率切换 + transmit_message.fd_esi = DISABLE; // 错误状态指示 + + // 复制数据到发送缓冲区 + for (int i = 0; i < length && i < 64; i++) { + transmit_message.tx_data[i] = *(data + i); + } + // 自动选择空闲mailbox发送消息，不需要人为指定 + transmit_mailbox = can_message_transmit(CAN0, &transmit_message); + + // 检查是否成功获取mailbox + if (CAN_NOMAILBOX == transmit_mailbox) { + // 没有可用的mailbox，可能所有mailbox都在使用中 + printf("No available mailbox for CANFD transmission\r\n"); + return 0; + } + + // 等待发送完成，使用自动选择的mailbox进行状态检查 + while ((CAN_TRANSMIT_OK != can_transmit_states(CAN0, transmit_mailbox)) && + (0 != timeout)) { + timeout--; + } + + // 超时检查 + if (0 == timeout) { + printf("CANFD transmission timeout for mailbox %d\r\n", + transmit_mailbox); + return 0; + } return 1; } - /** * @brief CAN发送函数 * @retval 0: 发送失败 * 1: 发送成功 */ -u8 CAN_transmiter::CAN_all_Send(){ +u8 CAN_transmiter::CAN_all_Send() { u32 success_flag = 0; constexpr u32 RT_Vel_Ang_FEED = 0x101; @@ -437,25 +505,32 @@ u8 CAN_transmiter::CAN_all_Send(){ constexpr u32 Status_FEED = 0x118; constexpr u32 Limit_Vel_Vol_FEED = 0x119; - //打包RT_Vel_Ang_FEED + // 打包RT_Vel_Ang_FEED pack_float_to_bytes(transmit_data, 0, rt_angle_union); pack_float_to_bytes(transmit_data, 4, rt_velocity_union); - //打包SET_Vel_Ang_FEED + success_flag += CAN_Send(RT_Vel_Ang_FEED, 8, transmit_data); + // 打包SET_Vel_Ang_FEED pack_float_to_bytes(transmit_data, 0, target_angle_union); pack_float_to_bytes(transmit_data, 4, target_velocity_union); - //打包Current_FEED - pack_float_to_bytes(transmit_data, 0, u_current_union); - pack_float_to_bytes(transmit_data, 4, w_current_union); - pack_float_to_bytes(transmit_data, 8, v_current_union); - //打包Status_FEED + success_flag += CAN_Send(SET_Vel_Ang_FEED, 8, transmit_data); + // 打包Current_FEED + pack_u16_to_bytes(transmit_data, 0, u16(u_current_union.f)); + pack_u16_to_bytes(transmit_data, 2, u16(w_current_union.f)); + pack_u16_to_bytes(transmit_data, 4, u16(v_current_union.f)); + success_flag += CAN_Send(Current_FEED, 6, transmit_data); + + // 打包Status_FEED transmit_data[0] = Status_code; transmit_data[1] = controller_Mode; pack_short_to_bytes(transmit_data, 2, motor_temp); pack_short_to_bytes(transmit_data, 4, mcu_temp); pack_short_to_bytes(transmit_data, 6, Power_Voltage); - //打包Limit_Vel_Vol_FEED + success_flag += CAN_Send(Status_FEED, 8, transmit_data); + // 打包Limit_Vel_Vol_FEED pack_float_to_bytes(transmit_data, 0, limit_velocity_union); pack_short_to_bytes(transmit_data, 4, limit_voltage); + success_flag += CAN_Send(Limit_Vel_Vol_FEED, 6, transmit_data); + return success_flag; } @@ -464,24 +539,22 @@ u8 CAN_transmiter::CAN_all_Send(){ * @retval 0: 发送失败 * 1: 发送成功 */ -u8 CAN_transmiter::CANFD_all_Send(){ +u8 CAN_transmiter::CANFD_all_Send() { u32 success_flag = 0; - constexpr u32 STATUS_ID = 0x18FF100; - constexpr u32 CONFIG_ID = 0x18FF101; - + constexpr u32 STATUS_ID = 0x18FF0100; + constexpr u32 CONFIG_ID = 0x18FF0101; // 打包status数据 pack_short_to_bytes(transmit_data, 0, mcu_temp); pack_short_to_bytes(transmit_data, 2, motor_temp); - pack_float_to_bytes(transmit_data, 4, u_current_union); - pack_float_to_bytes(transmit_data, 8, w_current_union); - pack_float_to_bytes(transmit_data, 12, v_current_union); - transmit_data[16] = Status_code; - pack_u16_to_bytes(transmit_data, 17, Power_Voltage); - pack_float_to_bytes(transmit_data, 19, rt_angle_union); - pack_float_to_bytes(transmit_data, 23, rt_velocity_union); - - success_flag += CANFD_Send(STATUS_ID, 27, transmit_data); + pack_u16_to_bytes(transmit_data, 4, u16(u_current_union.f)); + pack_u16_to_bytes(transmit_data, 6, u16(w_current_union.f)); + pack_u16_to_bytes(transmit_data, 8, u16(v_current_union.f)); + transmit_data[10] = Status_code; + pack_u16_to_bytes(transmit_data, 11, Power_Voltage); + pack_float_to_bytes(transmit_data, 13, rt_angle_union); + pack_float_to_bytes(transmit_data, 17, rt_velocity_union); + success_flag += CANFD_Send(STATUS_ID, 20, transmit_data); // 打包config数据 transmit_data[0] = controller_Mode; @@ -489,8 +562,65 @@ u8 CAN_transmiter::CANFD_all_Send(){ pack_float_to_bytes(transmit_data, 3, limit_velocity_union); pack_float_to_bytes(transmit_data, 7, target_angle_union); pack_float_to_bytes(transmit_data, 11, target_velocity_union); - - success_flag += CANFD_Send(CONFIG_ID, 15, transmit_data); + success_flag += CANFD_Send(CONFIG_ID, 16, transmit_data); return success_flag; } + +/** + * @brief CAN通信检查函数 + * @retval 0: 无新消息 + * 1: 有新消息 + */ +void CAN_transmiter::communication_check() { + uint8_t i = 0; + // CAN0 发送数据标志 + if (SET == can0_send_flag) { + // printf("\r\n can0 %X receive data:", receive_message.rx_efid); + CAN0_transmiter.update(); + // CAN0_transmiter.CAN_all_Send(); + CAN0_transmiter.CANFD_all_Send(); + can0_send_flag = RESET; + } + + // CAN0 接收数据标志 + if (SET == can0_receive_flag) { + // printf("\r\n can0 %X receive data:", receive_message.rx_efid); + // for (i = 0; i < receive_message.rx_dlen; i++) { + // printf(" %02x", receive_message.rx_data[i]); + // } + can0_receive_flag = RESET; + } +} + + +/** + * @brief CAN0中断服务程序 + * @retval 无 + */ +void CAN0_RX0_IRQHandler(void) { + // 检查是否是FIFO0消息挂起中断 + if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFL0) != RESET) { + // 清除中断标志 + can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFL0); + + // 读取接收到的数据 + can_message_receive(CAN0, CAN_FIFO0, &CAN0_transmiter.receive_message); + + // 接收标志位置1 + CAN0_transmiter.can0_receive_flag = SET; + } + + // 检查其他可能的中断源 + if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFF0) != RESET) { + // FIFO0满中断 + can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFF0); + printf("CAN FIFO0 full\n"); + } + + if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFO0) != RESET) { + // FIFO0溢出中断 + can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFO0); + printf("CAN FIFO0 overflow\n"); + } +} diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h index 0e7f6d9..5d1264d 100644 --- a/Robot_V2/device/Can.h +++ b/Robot_V2/device/Can.h @@ -18,28 +18,31 @@ typedef union { } float_union_t; // 添加辅助函数用于将short类型数据打包到u8数组中 -static inline void pack_short_to_bytes(u8* buffer, u16 offset, short value) { - buffer[offset] = (u8)(value & 0xFF); - buffer[offset + 1] = (u8)((value >> 8) & 0xFF); +static inline void pack_short_to_bytes(u8 *buffer, u16 offset, short value) { + buffer[offset] = (u8)(value & 0xFF); + buffer[offset + 1] = (u8)((value >> 8) & 0xFF); } // 添加辅助函数用于将u16类型数据打包到u8数组中 -static inline void pack_u16_to_bytes(u8* buffer, u16 offset, u16 value) { - buffer[offset] = (u8)(value & 0xFF); - buffer[offset + 1] = (u8)((value >> 8) & 0xFF); +static inline void pack_u16_to_bytes(u8 *buffer, u16 offset, u16 value) { + buffer[offset] = (u8)(value & 0xFF); + buffer[offset + 1] = (u8)((value >> 8) & 0xFF); } // 添加辅助函数用于将float类型数据打包到u8数组中 -static inline void pack_float_to_bytes(u8* buffer, u16 offset, float_union_t& float_union) { - buffer[offset] = float_union.b[0]; - buffer[offset + 1] = float_union.b[1]; - buffer[offset + 2] = float_union.b[2]; - buffer[offset + 3] = float_union.b[3]; +static inline void pack_float_to_bytes(u8 *buffer, u16 offset, + float_union_t &float_union) { + buffer[offset] = float_union.b[0]; + buffer[offset + 1] = float_union.b[1]; + buffer[offset + 2] = float_union.b[2]; + buffer[offset + 3] = float_union.b[3]; } -u8 can_gpio_init(void); -void can_config(void); -void canfd_config(void); +u8 can_gpio_init(void); // CAN引脚初始化 +void can_config(void); // CAN初始化 +void canfd_config(void);// CANFD初始化 +void CAN0_RX0_IRQHandler(void);// CAN接收中断处理函数 + void can_loopback_init(void); void can_loopback_test(void); @@ -50,42 +53,44 @@ void can_loopback_test(void); class CAN_transmiter { private: - u8 controller_Mode; - float_union_t limit_velocity_union; - u16 limit_voltage; - short mcu_temp; - short motor_temp; - u16 Power_Voltage; - float_union_t rt_angle_union; - float_union_t rt_velocity_union; - u8 Status_code; - float_union_t target_angle_union; - u8 Target_Control_Mode; - float_union_t target_velocity_union; - float_union_t u_current_union; - float_union_t w_current_union; - float_union_t v_current_union; + u8 controller_Mode = 0; // 电机模式 + float_union_t limit_velocity_union = {0}; // 限制速度 + u16 limit_voltage = 0; // 限制电压 + short mcu_temp = 0; // MCU温度 + short motor_temp = 0; // 电机温度 + u16 Power_Voltage = 0; // 电源电压 + float_union_t rt_angle_union = {0}; // 实时角度 + float_union_t rt_velocity_union = {0}; // 实时速度 + u8 Status_code = 0; // 状态码 + float_union_t target_angle_union = {0}; // 目标角度 + u8 Target_Control_Mode = 0; // 目标控制模式 + float_union_t target_velocity_union = {0}; // 目标速度 + float_union_t u_current_union = {0}; // U相电流 + float_union_t w_current_union = {0}; // W相电流 + float_union_t v_current_union = {0}; // V相电流 - u8 transmit_mailbox; // 初始化为无mailbox状态 - u32 timeout; // 默认超时为0xFFFF - u8 transmit_data[64]; // CANFD最大64字节 + can_trasnmit_message_struct transmit_message; // 发送消息结构体 + u8 transmit_mailbox = CAN_NOMAILBOX; // 初始化为无mailbox状态 + u32 timeout = 0XFFFF; // 默认超时为0xFFFF + u8 transmit_data[64]; // CANFD最大64字节 - can_trasnmit_message_struct transmit_message; - can_receive_message_struct receive_message; - - can_fdframe_struct can_fd_parameter; - can_fd_tdc_struct can_fd_tdc_parameter; + void update(); // 更新数据 + u8 CAN_Receive(); // CAN接收 + u8 CANFD_Receive(); // CANFD接收 + u8 CAN_Send(u32 ID, u8 length, u8 *data); // CAN发送 + u8 CANFD_Send(u32 ID, u8 length, u8 *data); // CANFD发送 + u8 CAN_all_Send(); // CAN发送 + u8 CANFD_all_Send(); // CANFD发送 public: // 添加构造函数声明 CAN_transmiter(); - u8 CAN_Send(u32 ID, u8 length, u8 *data); - u8 CANFD_Send(u32 ID, u8 length, u8 *data); - void update(); - u8 CAN_all_Send(); - u8 CANFD_all_Send(); - u8 CAN_Receive(); - u8 CANFD_Receive(); + + can_receive_message_struct receive_message; // 接收消息结构体 + FlagStatus can0_receive_flag; // CAN0接收标志 + FlagStatus can0_send_flag; // CAN0发送标志 + + void communication_check(); // 通信检查 }; #ifdef __cplusplus diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index b76e091..ced3cad 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -1,22 +1,31 @@ #include "Timer.h" +#include "Can.h" #include "adc.h" #include "gd32c10x.h" #include "gd32c10x_gpio.h" #include "gd32c10x_timer.h" #include "initial.h" +#include #include #include #include -constexpr int TIMER1_PRESCALER = 1199; -constexpr int TIMER1_PERIOD = 9; +// 配置定时器每100微秒中断一次 +// 系统时钟为120MHz，要得到100微秒的中断周期 +// 可以使用预分频器119，定时器时钟为120MHz/(119+1) = 1MHz +// 然后设置周期为99，即计数100次，1MHz/100 = 10kHz = 100微秒 +constexpr int TIMER1_PRESCALER = 119; // 120MHz / 120 = 1MHz +constexpr int TIMER1_PERIOD = 99; // 1MHz / 100 = 100us constexpr int TIMER0_PRESCALER = 5999; constexpr int TIMER0_PERIOD = 15999; constexpr int TIMER0_CH0_PULSE = 4000; constexpr int TIMER0_CH1_PULSE = 6000; constexpr int TIMER0_CH2_PULSE = 8000; + +static u32 total_us100_counter = 0; // 总计数器，每100微秒增加1 extern u16 adc_value[7]; +extern CAN_transmiter CAN0_transmiter; /** * @brief 设置引脚中断优先级 @@ -40,8 +49,8 @@ void nvic_config(void) { void timer1_config(void) { /* ---------------------------------------------------------------------------- TIMER1 Configuration: - TIMER1CLK = SystemCoreClock/12000 = 10KHz, the period is 1s(10000/10000 = - 1s). + TIMER1CLK = SystemCoreClock/120 = 1MHz, the period is 100us(100/1000000 = + 100us). ---------------------------------------------------------------------------- */ timer_parameter_struct timer_struct; @@ -55,11 +64,11 @@ void timer1_config(void) { timer_struct_para_init(&timer_struct); /* TIMER1配置参数设置 */ timer_struct.prescaler = - TIMER1_PRESCALER; /* 预分频值：12000分频(从0开始计数) */ + TIMER1_PRESCALER; /* 预分频值：120分频(从0开始计数) */ timer_struct.alignedmode = TIMER_COUNTER_EDGE; /* 边沿对齐模式 */ timer_struct.counterdirection = TIMER_COUNTER_UP; /* 向上计数模式 */ timer_struct.period = - TIMER1_PERIOD; /* 自动重装载值：10000次计数(从0开始计数) */ + TIMER1_PERIOD; /* 自动重装载值：100次计数(从0开始计数) */ timer_struct.clockdivision = TIMER_CKDIV_DIV1; /* 时钟分频值：1分频 */ timer_init(TIMER1, &timer_struct); @@ -92,35 +101,27 @@ void TIMER1_IRQHandler(void) { if (timer_interrupt_flag_get(TIMER1, TIMER_INT_UP) != RESET) { // 清除更新中断标志 timer_interrupt_flag_clear(TIMER1, TIMER_INT_UP); - - // 使用静态变量来跟踪定时器事件 - static u8 ms_counter = 0; - static u8 tens_counter = 0; - static u8 hundreds_counter = 0; - static u16 seconds_counter = 0; + // 增加总计数器 + total_us100_counter++; - // 增加计数器 - ms_counter++; - if (ms_counter >= 10) { - ms_counter = 0; - tens_counter++; + // 每100us执行一次的操作可以放在这里 + + if (total_us100_counter % 10 == 0) { // 每1ms执行的代码可以放在这里 - if (tens_counter >= 10) { - tens_counter = 0; - hundreds_counter++; - + CAN0_transmiter.can0_send_flag = SET; + if (total_us100_counter % 100 == 0) { // 每10ms执行的代码可以放在这里 + } - if (hundreds_counter >= 10) { - hundreds_counter = 0; - seconds_counter++; - // 每100ms执行的代码可以放在这里 + if (total_us100_counter % 1000 == 0) { + // 每100ms执行的代码可以放在这里 + } - if (seconds_counter >= 10) { - seconds_counter = 0; - // 每1s执行的代码可以放在这里 - } - } + if (total_us100_counter % 10000 == 0) { + // 每1s执行的代码可以放在这里 + + // 重置计数器避免溢出 + total_us100_counter = 0; } } } diff --git a/Robot_V2/device/test.cpp b/Robot_V2/device/test.cpp new file mode 100644 index 0000000..8f1b6ec --- /dev/null +++ b/Robot_V2/device/test.cpp @@ -0,0 +1,38 @@ +#include "Can.h" +#include "Filter.h" +#include "Timer.h" +#include "adc.h" +#include "core_cm4.h" +#include "device/tle5012b.h" +#include "initial.h" +#include "main.h" +#include "sys/initial.h" +#include "systick.h" +#include "tle5012b.h" +#include +#include + +constexpr int status_print = 0; + +void test_1111() { + // 初始化CAN回环测试 + // can_loopback_init(); + // can_config(); + if (status_print) { + /* value print */ + printf(" the temperature data is %2.2f degrees Celsius\r\n", + get_mcu_temp()); + printf(" the drv_temperature data is %2.2f degrees Celsius\r\n", + get_drv_temp()); + printf(" the U current data is %2.2f mA\r\n", get_current('U')); + printf(" the V current data is %2.2f mA\r\n", get_current('V')); + printf(" the W current data is %2.2f mA\r\n", get_current('W')); + printf(" the VBUS is %2.2f V\r\n", get_vbus_voltage()); + // printf(" the angle is %2.2f\r\n", tle5012b_read()); + + printf("\n----------------------------------------------------\n"); + // TLI5012B测试 + Test_TLI5012B(); + } + // can_loopback_test(); +} \ No newline at end of file diff --git a/Robot_V2/device/tle5012b.cpp b/Robot_V2/device/tle5012b.cpp index 9f8d0bc..1ec20e3 100644 --- a/Robot_V2/device/tle5012b.cpp +++ b/Robot_V2/device/tle5012b.cpp @@ -1,6 +1,9 @@ #include "tle5012b.h" +#include "core_cmInstr.h" #include "gd32c10x_gpio.h" +#include "gd32c10x_spi.h" #include "stdbool.h" +#include "systick.h" #define SPI_TX_OFF() \ do { \ @@ -64,21 +67,19 @@ static constexpr uint16_t MOD4_VALUE = 0x0080; /**< MOD4 register value: uint16_t spi_rw_half_word(uint32_t spi_periph, const uint16_t i_HalfWord) { uint32_t to; - uint16_t data = 0xFFFF; - bool ok = true; + uint16_t data = 0xFFFF; + bool ok = true; /* ---------- Send data ---------- */ to = 200; - - while ((RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE)) && --to); if (to == 0) ok = false; if (ok) { - if(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE)) + if(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE)) { - (void)spi_i2s_data_receive(spi_periph); /* throw Dummy */ + (void)spi_i2s_data_receive(spi_periph); /* throw Dummy */ } spi_i2s_data_transmit(spi_periph, i_HalfWord); /* wait rx succ*/ @@ -89,7 +90,7 @@ uint16_t spi_rw_half_word(uint32_t spi_periph, const uint16_t i_HalfWord) data = spi_i2s_data_receive(spi_periph); /* throw Dummy */ } - /* ---------- wait bus idle ---------- */ + /* ---------- wait bsu idle ---------- */ if (ok) { to = 200; @@ -100,6 +101,7 @@ uint16_t spi_rw_half_word(uint32_t spi_periph, const uint16_t i_HalfWord) return ok ? data : 0xFFFF; } + /** * @brief Write data to TLI5012B register * @param i_Cmd Command word to send @@ -111,12 +113,16 @@ uint16_t cdd_TLI5012_WriteReg(uint16_t i_Cmd, uint16_t i_Data) { uint16_t result = 0xFFFF; /* Default error code */ SPI_CS_ENABLE(); - SPI_TX_ON(); + __NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定 result = spi_rw_half_word(SPI0, i_Cmd); result = spi_rw_half_word(SPI0, i_Data); SPI_TX_OFF(); /* Configure MOSI as input to avoid conflicts */ - // result = spi_rw_half_word(SPI0, result); + __NOP();__NOP();__NOP();__NOP();__NOP();__NOP(); + __NOP();__NOP();__NOP();__NOP();__NOP();__NOP(); // 增加延时 + result = spi_rw_half_word(SPI0, DUMMY_BYTE); SPI_CS_DISABLE(); + SPI_TX_ON(); /* Restore MOSI as output */ + __NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定 return result; } @@ -131,12 +137,16 @@ uint16_t cdd_TLI5012_ReadReg(uint16_t i_Cmd, uint16_t * const i_Data) { uint16_t result = 0xFFFF; /* Default error code */ SPI_CS_ENABLE(); - SPI_TX_ON(); + __NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定 *i_Data = spi_rw_half_word(SPI0, i_Cmd); SPI_TX_OFF(); /* Configure MOSI as input to avoid conflicts */ - // *i_Data = spi_rw_half_word(SPI0, DUMMY_BYTE); - result = spi_rw_half_word(SPI0, result); + __NOP();__NOP();__NOP();__NOP();__NOP();__NOP(); + __NOP();__NOP();__NOP();__NOP();__NOP();__NOP(); // 增加延时 + *i_Data = spi_rw_half_word(SPI0, DUMMY_BYTE); + result = spi_rw_half_word(SPI0, DUMMY_BYTE); SPI_CS_DISABLE(); + SPI_TX_ON(); /* Restore MOSI as output */ + __NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定 return result; } @@ -146,16 +156,17 @@ void tle5012b_gpio_init(void) rcu_periph_clock_enable(RCU_GPIOB); rcu_periph_clock_enable(RCU_SPI0); rcu_periph_clock_enable(RCU_AF); - - /* JTAG-DP disabled and SW-DP enabled, so SPI0 can use PB3 and PB4 */ + /* 关闭 JTAG，仅保留 SWD（释放 PA15、PB3、PB4） */ gpio_pin_remap_config(GPIO_SWJ_SWDPENABLE_REMAP, ENABLE); + /* -------- SPI0 全 remap：PB3/4/5 + PA15 -------- */ gpio_pin_remap_config(GPIO_SPI0_REMAP, ENABLE); - /* SPI0 GPIO config: NSS/PA15, SCK/PB3, MOSI/PB5 */ - gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15); + /* PA15 → NSS, 推挽输出 */ + gpio_init(GPIOA, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15); + /* PB3 → SCK, PB5 → MOSI, 推挽输出 */ gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_3 | GPIO_PIN_5); - /* SPI0 GPIO config: MISO/PB4 */ - gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_4); + /* PB4 → MISO, 浮空输入 */ + gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_4 | GPIO_PIN_10); } /** @@ -168,21 +179,20 @@ void tle5012b_spi_init(void) spi_parameter_struct spi_init_str; spi_struct_para_init(&spi_init_str); - /* SPI0 parameter config */ + /* SPI参数：16bit, 主机, 全双工, CPOL=0 CPHA=1, MSB first */ spi_init_str.device_mode = SPI_MASTER; spi_init_str.trans_mode = SPI_TRANSMODE_FULLDUPLEX; spi_init_str.frame_size = SPI_FRAMESIZE_16BIT; - spi_init_str.clock_polarity_phase = SPI_CK_PL_LOW_PH_2EDGE; - spi_init_str.nss = SPI_NSS_SOFT; + spi_init_str.clock_polarity_phase = SPI_CK_PL_HIGH_PH_2EDGE; + spi_init_str.nss = SPI_NSS_SOFT; spi_init_str.endian = SPI_ENDIAN_MSB; - spi_init_str.prescale = SPI_PSC_128; + spi_init_str.prescale = SPI_PSC_16; /* 8MHz @ 128MHz */ spi_init(SPI0, &spi_init_str); - spi_nss_output_enable(SPI0); + spi_nss_output_enable(SPI0); spi_nssp_mode_enable(SPI0); - spi_enable(SPI0); -} - + spi_enable(SPI0); +} @@ -196,119 +206,134 @@ void cdd_TLI5012_Init(void) uint16_t u16Command = 0xFFFF; uint16_t u16Data = 0x0000; uint16_t u16Safe = 0x0000; - + /* ---------- Step 1: Read and configure STAT register ---------- */ /* Read STAT register to get current status */ u16Command = READ_STAT_VALUE; u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - printf("\r\n [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe); - + printf("\r\n @Joe [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe); + /* Write STAT register, set Slave_Number = 00 */ u16Command = 0x0001; u16Data &= 0x9FFF; /* Clear bits 13-14 (Slave_Number) */ u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data); - printf("\r\n [TLI5012Init] [STAT] write u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe); - + printf("\r\n @Joe [TLI5012Init] [STAT] write u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe); + /* Read STAT register again to verify write operation */ u16Command = READ_STAT_VALUE; u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - printf("\r\n [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe); - + printf("\r\n @Joe [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe); + /* ---------- Step 2: Read ACSTAT register ---------- */ u16Command = READ_ACSTAT_VALUE; u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - printf("\r\n [TLI5012Init] read [ACSTAT]: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe); - + printf("\r\n @Joe [TLI5012Init] read [ACSTAT]: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe); + /* ---------- Step 3: Configure MOD1 register ---------- */ /* Read current MOD1 register value */ u16Command = RD_REG | WRITE_MOD1_VALUE; u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - printf("\r\n [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); + printf("\r\n @Joe [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); + + /* Write MOD1 register with configuration value */ + u16Command = WRITE_MOD1_VALUE; + u16Data = MOD1_VALUE; + u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data); + printf("\r\n @Joe [TLI5012Init] [MOD1] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); + + /* Verify MOD1 register write operation */ + u16Command = RD_REG | WRITE_MOD1_VALUE; + u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); + printf("\r\n @Joe [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); + + /* ---------- Step 4: Configure MOD2 register ---------- */ + /* Read current MOD2 register value */ + u16Command = RD_REG | WRITE_MOD2_VALUE; + u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); + printf("\r\n @Joe [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); + + /* Write MOD2 register with configuration value */ + u16Command = WRITE_MOD2_VALUE; + u16Data = MOD2_VALUE; + u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data); + printf("\r\n @Joe [TLI5012Init] [MOD2] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); + + /* Verify MOD2 register write operation */ + u16Command = RD_REG | WRITE_MOD2_VALUE; + u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); + printf("\r\n @Joe [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - // /* Write MOD1 register with configuration value */ - // u16Command = WRITE_MOD1_VALUE; - // u16Data = MOD1_VALUE; - // u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data); - // printf("\r\n [TLI5012Init] [MOD1] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - // /* Verify MOD1 register write operation */ - // u16Command = RD_REG | WRITE_MOD1_VALUE; - // u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - // printf("\r\n [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - // /* ---------- Step 4: Configure MOD2 register ---------- */ - // /* Read current MOD2 register value */ - // u16Command = RD_REG | WRITE_MOD2_VALUE; - // u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - // printf("\r\n [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - // /* Write MOD2 register with configuration value */ - // u16Command = WRITE_MOD2_VALUE; - // u16Data = MOD2_VALUE; - // u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data); - // printf("\r\n [TLI5012Init] [MOD2] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - // /* Verify MOD2 register write operation */ - // u16Command = RD_REG | WRITE_MOD2_VALUE; - // u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - // printf("\r\n [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - // /* ---------- Step 5: Configure MOD3 register ---------- */ - // /* Read current MOD3 register value */ - // u16Command = RD_REG | WRITE_MOD3_VALUE; - // u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - // printf("\r\n [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - // /* Write MOD3 register with configuration value */ - // u16Command = WRITE_MOD3_VALUE; - // u16Data = MOD3_VALUE; - // u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data); - // printf("\r\n [TLI5012Init] [MOD3] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - // /* Verify MOD3 register write operation */ - // u16Command = RD_REG | WRITE_MOD3_VALUE; - // u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - // printf("\r\n [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - // /* Optional: Configure MOD3 register with SSC_OD = 1 (commented out) */ - - // u16Command = 0x5091; - // u16Data |= 0x0004; // Set SSC_OD bit - // u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data); - // printf("\r\n [TLI5012Init] [MOD3] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - - // /* ---------- Step 6: Configure MOD4 register ---------- */ - // /* Read current MOD4 register value */ - // u16Command = RD_REG | WRITE_MOD4_VALUE; - // u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - // printf("\r\n [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - // /* Write MOD4 register with configuration value */ - // u16Command = WRITE_MOD4_VALUE; - // u16Data = MOD4_VALUE; - // u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data); - // printf("\r\n [TLI5012Init] [MOD4] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - // /* Verify MOD4 register write operation */ - // u16Command = RD_REG | WRITE_MOD4_VALUE; - // u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - // printf("\r\n [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); + /* ---------- Step 5: Configure MOD3 register ---------- */ + /* Read current MOD3 register value */ + u16Command = RD_REG | WRITE_MOD3_VALUE; + u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); + printf("\r\n @Joe [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); + + /* Write MOD3 register with configuration value */ + u16Command = WRITE_MOD3_VALUE; + u16Data = MOD3_VALUE; + u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data); + printf("\r\n @Joe [TLI5012Init] [MOD3] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); + + /* Verify MOD3 register write operation */ + u16Command = RD_REG | WRITE_MOD3_VALUE; + u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); + printf("\r\n @Joe [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); + + /* ---------- Step 6: Configure MOD4 register ---------- */ + /* Read current MOD4 register value */ + u16Command = RD_REG | WRITE_MOD4_VALUE; + u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); + printf("\r\n @Joe [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); + + /* Write MOD4 register with configuration value */ + u16Command = WRITE_MOD4_VALUE; + u16Data = MOD4_VALUE; + u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data); + printf("\r\n @Joe [TLI5012Init] [MOD4] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); + + /* Verify MOD4 register write operation */ + u16Command = RD_REG | WRITE_MOD4_VALUE; + u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); + printf("\r\n @Joe [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); } void Test_TLI5012B(void) { - uint16_t u16Command = READ_ANGLE_VALUE; + uint16_t u16Command = 0xFFFF; uint16_t u16Data = 0x0000; uint16_t u16Safe = 0x0000; - float angle = 0; + float angle = 0; u16Command = READ_ANGLE_VALUE; u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); + + // 输出原始读取的数据，用于诊断 + printf("\r\n @Joe RAW AVAL = 0x%04X, SafeWord = 0x%04X \r\n", u16Data, u16Safe); + + // 检查是否有错误标志位 + if (u16Safe != 0xFFFF && u16Safe != 0x0000) { + printf("\r\n @Joe Warning: Non-ideal SafeWord detected \r\n"); + } + + // 检查错误位（bit 15） + if (u16Data & 0x8000) { + printf("\r\n @Joe Error: Communication error detected (bit 15 set) \r\n"); + } + + // 提取角度值(15位) u16Data = u16Data & 0x7FFF; - printf ( "\r\n AVAL = 0x%04X, SafeWord = 0x%04X \r\n", u16Data, u16Safe); - // angle = 360*( ((int16_t)(u16Data << 1)) >> 1)/32768; - angle = ( 360*( ( (int16_t)(u16Data << 1) ) >> 1 ) ) >> 15; - printf ( "\r\n angle = %f, AVAL = %d \r\n", angle, (((int16_t)(u16Data << 1)) >> 1)); -} + printf ( "\r\n @Joe AVAL = 0x%04X, SafeWord = 0x%04X \r\n", u16Data, u16Safe); + + // 将15位角度值转换为角度(0-360度) + // 0x7FFF = 32767 对应 360度 + angle = ((float)u16Data * 360.0f) / 32767.0f; + printf ( "\r\n @Joe angle = %f, AVL = %d \r\n", angle, (int16_t)u16Data); + + // 添加额外的传感器状态检查 + uint16_t status_data, status_safe; + u16Command = READ_STAT_VALUE; + status_safe = cdd_TLI5012_ReadReg(u16Command, &status_data); + printf("\r\n @Joe STATUS = 0x%04X, Status SafeWord = 0x%04X \r\n", status_data, status_safe); +} \ No newline at end of file diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 2c96d15..e2026eb 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -3,11 +3,11 @@ #include "Filter.h" #include "Timer.h" #include "adc.h" +#include "core_cm4.h" #include "device/tle5012b.h" #include "initial.h" #include "sys/initial.h" #include "systick.h" -#include "core_cm4.h" #include "tle5012b.h" #include #include @@ -15,45 +15,26 @@ // 全局变量定义 CurrentKalman kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f); -CircularBuffer buffer; CAN_transmiter CAN0_transmiter; - int main(void) { - systick_config(); // 系统时钟初始化 - USART0_init(); // 调试串口初始化 - User_timer_init(); // Timer初始化 - adc_config(); // ADC初始化 - dma_config(); // DMA采样ADC初始化 - - tle5012b_gpio_init(); // TLI5012B初始化 + systick_config(); // 系统时钟初始化 + USART0_init(); // 调试串口初始化 + User_timer_init(); // Timer初始化 + adc_config(); // ADC初始化 + dma_config(); // DMA采样ADC初始化 + + tle5012b_gpio_init(); // TLI5012B初始化 tle5012b_spi_init(); - cdd_TLI5012_Init(); // 初始化TLE5012B传感器 - - watchdog_init(1000); - // 初始化CAN回环测试 - // can_loopback_init(); + // cdd_TLI5012_Init(); // 初始化TLE5012B传感器 + + watchdog_init(5000); while (1) { - /* clear the FWDGT reset flag */ - fwdgt_counter_reload(); + /* clear the FWDGT reset flag */ + fwdgt_counter_reload(); - // can_loopback_test(); - /* value print */ - // printf(" the temperature data is %2.2f degrees Celsius\r\n", - // get_mcu_temp()); - // printf(" the drv_temperature data is %2.2f degrees Celsius\r\n", - // get_drv_temp()); - // printf(" the U current data is %2.2f mA\r\n",get_current('U')); - // printf(" the V current data is %2.2f mA\r\n",get_current('V')); - // printf(" the W current data is %2.2f mA\r\n",get_current('W')); - // printf(" the VBUS is %2.2f V\r\n",get_vbus_voltage()); - // printf(" the angle is %2.2f\r\n", tle5012b_read()); - - printf("\n----------------------------------------------------\n"); + CAN0_transmiter.communication_check(); + // delay_1ms(2); - delay_1ms(700); - // TLI5012B测试 - Test_TLI5012B(); - Set_Pwm(0, 1500); } -} \ No newline at end of file +} diff --git a/Robot_V2/sys/systick.cpp b/Robot_V2/sys/systick.cpp index 2b794da..e427419 100644 --- a/Robot_V2/sys/systick.cpp +++ b/Robot_V2/sys/systick.cpp @@ -56,7 +56,19 @@ void systick_config(void) /* configure the systick handler priority */ NVIC_SetPriority(SysTick_IRQn, 0x00U); } - +/*! + \brief delay a time in seconds + \param[in] count: count in seconds + \param[out] none + \retval none +*/ +void delay_1s(uint32_t count) +{ + uint32_t i; + for(i = 0; i < count; i++){ + delay_1ms(1000); + } +} /*! \brief delay a time in milliseconds \param[in] count: count in milliseconds @@ -66,8 +78,8 @@ void systick_config(void) void delay_1ms(uint32_t count) { delay = count; - while(0U != delay){ + fwdgt_counter_reload(); } } diff --git a/Robot_V2/sys/systick.h b/Robot_V2/sys/systick.h index 3441d4c..ebc8ee8 100644 --- a/Robot_V2/sys/systick.h +++ b/Robot_V2/sys/systick.h @@ -44,6 +44,8 @@ extern "C" { void systick_config(void); /* delay a time in milliseconds */ void delay_1ms(uint32_t count); +/* delay a time in seconds */ +void delay_1s(uint32_t count); /* delay decrement */ void delay_decrement(void);