From c0b5f6902b721ed79247caf063ff4b021cc81129 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Tue, 20 Jan 2026 01:28:52 +0800 Subject: [PATCH] =?UTF-8?q?docs(config):=20=E6=96=B0=E5=A2=9E=E8=BD=AF?= =?UTF-8?q?=E4=BB=B6=E5=92=8C=E7=A1=AC=E4=BB=B6=E7=89=88=E6=9C=AC=E5=8F=B7?= =?UTF-8?q?=E5=AE=9A=E4=B9=89=20-=20=E5=AE=9A=E4=B9=89SOFTWARE=5FVERSION?= =?UTF-8?q?=2026.1.19.1=20-=20=E5=AE=9A=E4=B9=89HARDWARE=5FVERSION=2025.12?= =?UTF-8?q?.01.0=20-=20=E5=AE=9A=E4=B9=89BOOT=5FVERSION=2026.1.12.0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat(motor-control): 实现FOC控制算法速度环优化 refactor(can-protocol): 升级CANFD通信协议支持版本管理 - 实现版本号管理和CANFD_VERSION_ID协议 - 扩展CMD_ID消息解析支持PID参数动态配置 - 添加版本管理器实例并实现版本信息发送功能 - 优化CANFD异步发送数据长度判断逻辑 fix(angle-sensor): 修正角度传感器方向和偏移校准 - 添加ANGLESENS_OFFSET补偿参数 - 反转角度传感器读数方向(DIR = -1.0f) - 在角度计算中应用负向因子和偏移量 chore(memory-layout): 调整Flash存储布局和版本配置 - 扩展BOOTLOADER_SIZE从16KB到24KB - 更新APPLATEST_START_ADDR和APPBACK_START_ADDR偏移 - 重新分配OTA配置参数存储区域 - 增加EEP_DEBUG调试开关 refactor(pid-controller): 增强PID控制器功能接口 - 添加set_P_percent方法支持百分比参数设置 - 格式化PID计算代码提升可读性 - 保留积分和微分环节的约束保护机制 --- Robot_V2/Algorithm/FOC.cpp | 22 +-- Robot_V2/Algorithm/FOC.h | 24 ++- Robot_V2/Algorithm/pid.cpp | 30 ++-- Robot_V2/Algorithm/pid.h | 1 + .../Device/GD32C103CB/regions_GD32C103CB.h | 2 +- .../RTE/Device/GD32C103CB/system_gd32c10x.c | 2 +- Robot_V2/device/Can.cpp | 49 ++++-- Robot_V2/device/ang_sensor.cpp | 3 +- Robot_V2/main.cpp | 6 +- Robot_V2/sys/config.h | 57 +++++-- Robot_V2/sys/flash.cpp | 99 +++++++---- Robot_V2/sys/initial.cpp | 157 +++++++++++++++++- Robot_V2/sys/initial.h | 50 +++++- 13 files changed, 393 insertions(+), 109 deletions(-) diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp index a093f79..7fa485c 100644 --- a/Robot_V2/Algorithm/FOC.cpp +++ b/Robot_V2/Algorithm/FOC.cpp @@ -9,12 +9,14 @@ #include #include #include +#include "Filter.h" #include FOC_Controlroller FOC; // FOC控制初始化 extern sampler sampler; extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; +extern EncoderVelocityKalman Vel_kalman; // 速度滤波 // 初始变量及函数定义 #define _constrain(amt, low, high) \ @@ -54,7 +56,7 @@ inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b, I_q = I_beta * cos_el - I_alpha * sin_el; // arm_park_f32(I_alpha, I_beta, &I_d, &I_q, sin_el, cos_el); - return I_q * 1000.0f; // 返回Iq值，乘以1000以转换为mA + return I_q * 1000.0f - KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA } /** @@ -156,17 +158,17 @@ void FOC_Controlroller::update_power(void) { */ float FOC_Controlroller::velocity_open_loop() { // 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化 - // 预计算常量 1e-4 = 100微秒控制周期 - shaft_angle = _normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR * - 1.0 / TIMER2_FREQUENCY); + shaft_angle = _normalizeAngle(shaft_angle + + target_velocity * ANGLE_FACTOR * + REDUCTION_RATIO * 1.0 / TIMER2_FREQUENCY); // 因为在开环控制中，Ud始终为0，只控制Uq const float Uq = voltage_power_supply / 2.0f; setPhaseVoltage(Uq, 0.0f); - // CAN0_transmiter.testfloat_union.f = - cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), - sampler.get_current('W')); + CAN0_transmiter.testfloat_union.f = shaft_angle; + // cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), + // sampler.get_current('W')); return limit_voltage; } @@ -221,8 +223,8 @@ float FOC_Controlroller::angle_control_with_velocity_limit() { _constrain(target_velocity, -limit_velocity, limit_velocity); // 使用速度PID控制器计算最终输出电压 - float Uq = vel_inVelAngloop(target_velocity - rt_velocity); - // CAN0_transmiter.testfloat_union.f = target_velocity - rt_velocity; + float Uq = vel_inVelAngloop(rt_velocity - target_velocity); + CAN0_transmiter.testfloat_union.f = shaft_angle; // 获取电机轴角度 shaft_angle = KTH7823.getcurrent_sensorangle(); @@ -241,7 +243,7 @@ float FOC_Controlroller::angle_control_with_velocity_limit() { */ float FOC_Controlroller::velocity_control() { // PID 控制计算 - float Uq = vel_loop((target_velocity - rt_velocity) * 25); + float Uq = vel_loop((rt_velocity - target_velocity) * 25); // 获取电机轴角度 shaft_angle = KTH7823.getcurrent_sensorangle(); diff --git a/Robot_V2/Algorithm/FOC.h b/Robot_V2/Algorithm/FOC.h index e882a55..7e83c68 100644 --- a/Robot_V2/Algorithm/FOC.h +++ b/Robot_V2/Algorithm/FOC.h @@ -37,24 +37,23 @@ class FOC_Controlroller { void resume(void); void drv_stop(void); float drv_hold(void); + PIDController vel_loop = PIDController{ + vel_loop_p * 100, 0.20f, 5e-5, 1000000, voltage_power_supply}; // 12V + PIDController angle_loop = + PIDController{angle_loop_p * 100, 100.0f, 0.2f, 10000000, + voltage_power_supply}; // 12V + PIDController current_loop = PIDController{ + current_loop_p * 100, 5e-7f, 7e-8, 500, 24}; // 12V + PIDController vel_inVelAngloop = PIDController{ + vel_angloop_p * 100, 3.0f, 5e-4, 1000000, voltage_power_supply}; + PIDController ang_inVelAngloop = + PIDController{ang_angloop_p * 100, 0.0f, 5e-4, 1000000, 999}; protected: float shaft_angle = 0; float Ualpha, Ubeta = 0; float zero_electric_angle = 0; float v_half = voltage_power_supply * 0.5f; // 电源电压的一半 - - PIDController vel_loop = - PIDController{0.01f, 0.20f, 5e-5, 1000000, voltage_power_supply}; // 12V - PIDController angle_loop = PIDController{20.0f, 100.0f, 0.2f, 10000000, - voltage_power_supply}; // 12V - PIDController current_loop = - PIDController{0.006f, 0.02f, 0, 100000, voltage_power_supply}; // 12V - PIDController vel_inVelAngloop = - PIDController{0.12f, 3.0f, 5e-4, 1000000, voltage_power_supply}; - PIDController ang_inVelAngloop = - PIDController{100.0f, 0.0f, 5e-4, 1000000, 999}; - void setPhaseVoltage(float Uq, float Ud); float cal_Iq_Id(float current_a, float current_b, float current_c); }; @@ -65,5 +64,4 @@ typedef float (FOC_Controlroller::*FOCFunctionPtr)(void); inline float _normalizeAngle(float angle); inline float _electricalAngle(); - #endif \ No newline at end of file diff --git a/Robot_V2/Algorithm/pid.cpp b/Robot_V2/Algorithm/pid.cpp index 61595c1..e650a96 100644 --- a/Robot_V2/Algorithm/pid.cpp +++ b/Robot_V2/Algorithm/pid.cpp @@ -18,25 +18,27 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit) float PIDController::operator()(float error) { // P环 float proportional = P * error; - + // Tustin 散点积分（I环） // 添加积分限幅，防止积分饱和 - float integral = integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev); + float integral = + integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev); integral = _constrain(integral, -limit, limit); - + // D环（微分环节） float derivative = D * (error - error_prev) / TIME_SLICE; - + // 将P,I,D三环的计算值加起来 float output = proportional + integral + derivative; output = _constrain(output, -limit, limit); // 输出斜坡限制 - // 简化的斜坡限制（单行逻辑） - if (output_ramp > 0) { - float max_delta = output_ramp * TIME_SLICE; - output = output_prev + fmaxf(-max_delta, fminf(max_delta, output - output_prev)); - } + // 简化的斜坡限制（单行逻辑） + if (output_ramp > 0) { + float max_delta = output_ramp * TIME_SLICE; + output = output_prev + + fmaxf(-max_delta, fminf(max_delta, output - output_prev)); + } // 保存值（为了下一次循环） integral_prev = integral; output_prev = output; @@ -57,4 +59,12 @@ void PIDController::set_PID(float P, float I, float D) { this->P = P; this->I = I; this->D = D; -} \ No newline at end of file +} + +/** + * @brief 设置P参数 + * + * @param P: P参数 + * @return void + */ +void PIDController::set_P_percent(float pp) { this->P = pp; } \ No newline at end of file diff --git a/Robot_V2/Algorithm/pid.h b/Robot_V2/Algorithm/pid.h index 695b51d..d8fb2f1 100644 --- a/Robot_V2/Algorithm/pid.h +++ b/Robot_V2/Algorithm/pid.h @@ -9,6 +9,7 @@ public: float operator() (float error); void set_PID(float P, float I, float D); + void set_P_percent(float pp); float P; //!< 比例增益(P环增益) float I; //!< 积分增益（I环增益） diff --git a/Robot_V2/RTE/Device/GD32C103CB/regions_GD32C103CB.h b/Robot_V2/RTE/Device/GD32C103CB/regions_GD32C103CB.h index 13fc0b9..54b2c27 100644 --- a/Robot_V2/RTE/Device/GD32C103CB/regions_GD32C103CB.h +++ b/Robot_V2/RTE/Device/GD32C103CB/regions_GD32C103CB.h @@ -14,7 +14,7 @@ // Base address <0x0-0xFFFFFFFF:8> // Defines base address of memory region. Default: 0x08000000 // Contains Startup and Vector Table -#define __ROM0_BASE 0x08011000 +#define __ROM0_BASE 0x08012000 // Region size [bytes] <0x0-0xFFFFFFFF:8> // Defines size of memory region. Default: 0x00020000 #define __ROM0_SIZE 0x0000C000 diff --git a/Robot_V2/RTE/Device/GD32C103CB/system_gd32c10x.c b/Robot_V2/RTE/Device/GD32C103CB/system_gd32c10x.c index 66e2e72..9142dca 100644 --- a/Robot_V2/RTE/Device/GD32C103CB/system_gd32c10x.c +++ b/Robot_V2/RTE/Device/GD32C103CB/system_gd32c10x.c @@ -42,7 +42,7 @@ #define __SYS_OSC_CLK (__IRC8M) /* main oscillator frequency */ /* Vector Table base offset */ -#define APP_BASE ((uint32_t)0x08011000U) /*!< main FLASH base address */ +#define APP_BASE ((uint32_t)0x08012000U) /*!< main FLASH base address */ #define VECT_TAB_OFFSET (uint32_t)APP_BASE-FLASH_BASE /* This value must be a multiple of 0x200. */ /* select a system clock by uncommenting the following line */ diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 598fc8e..693538b 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -10,6 +10,7 @@ #include "gd32c10x_dma.h" #include "gd32c10x_fmc.h" #include "gd32c10x_gpio.h" +#include "initial.h" #include "motor.h" #include "systick.h" #include @@ -29,6 +30,7 @@ extern angle_sensor KTH7823; extern EncoderAngleKalman Angle_kalman; // 角度滤波 extern EncoderVelocityKalman Vel_kalman; // 速度滤波 extern FlashEEPROM flashEEP; +extern VersionManager Version; // 声明外部版本管理器实例 // 添加DMA相关变量 dma_parameter_struct dma_rx_parameter; @@ -574,48 +576,65 @@ void CAN_transmiter::communication_check() { // printf(" %02x", receive_message.rx_data[i]); // } if (receive_message.rx_efid == CANFD_CMD_ID) { + // 控制模式 FOC.control_mode = receive_message.rx_data[0]; - if (float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000 != - FOC.limit_voltage) { - FOC.limit_voltage = - float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / - 1000; - } - if (unpack_bytes_to_float(receive_message.rx_data, 3).f != - FOC.limit_velocity) { - FOC.limit_velocity = - unpack_bytes_to_float(receive_message.rx_data, 3).f; - } + // 目标角度 + FOC.limit_voltage = + float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000; + // 目标电流&目标速度 + FOC.limit_velocity = + unpack_bytes_to_float(receive_message.rx_data, 3).f; + FOC.target_current = FOC.limit_velocity; + // 目标角度 target_angle_union.f = unpack_bytes_to_float(receive_message.rx_data, 7).f; FOC.target_angle = target_angle_union.f; - + // 目标速度 target_velocity_union.f = unpack_bytes_to_float(receive_message.rx_data, 11).f; FOC.target_velocity = target_velocity_union.f; FOC.target_current = FOC.target_velocity * 10.0f; + + // 解析PID参数 + u16 P_config = unpack_bytes_to_u16(receive_message.rx_data, 15); + FOC.current_loop.set_P_percent(current_loop_p * P_config); + P_config = unpack_bytes_to_u16(receive_message.rx_data, 17); + FOC.vel_loop.set_P_percent(vel_loop_p * P_config); + P_config = unpack_bytes_to_u16(receive_message.rx_data, 19); + FOC.angle_loop.set_P_percent(angle_loop_p * P_config); + P_config = unpack_bytes_to_u16(receive_message.rx_data, 21); + FOC.vel_inVelAngloop.set_P_percent(vel_angloop_p * P_config); } else if (receive_message.rx_efid == CANFD_RESET_ID) { u8 first_byte = receive_message.rx_data[0]; u8 eigth_byte = receive_message.rx_data[7]; + // 电机重置 if (first_byte & 0x01) { motor_reset(); } + // EEPROM重置 if (first_byte >> 1 & 0x01) { - flashEEP.format(); + // flashEEP.format(); } + // 系统参数保存 if (first_byte >> 2 & 0x01) { flashEEP.save_system_config(false); } + // 角度传感器重置 if (first_byte >> 3 & 0x01) { KTH7823.angle_set(0); Angle_kalman.init(0); // 角度滤波 Vel_kalman.init(0); // 角度滤波 } + // 休眠 if (eigth_byte & 0x80 >> 7) { app_prepare_deep_sleep(true); // CAN_Sleep(); } + } else if (receive_message.rx_efid == CANFD_VERSION_ID) { + // 处理版本号请求，发送当前系统版本信息 + Version.sendVersionInfoViaCan(); } else if (receive_message.rx_efid == CANFD_BOOT_ID) { + flashEEP.save_system_config(false); fmc_unlock(); OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL); // 设置APP为不激活 fmc_lock(); @@ -805,9 +824,9 @@ u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) { valid_length = 16; } else if (length > 16 and length < 21) { valid_length = 20; - } else if (length > 20 and length < 24) { + } else if (length > 20 and length < 25) { valid_length = 24; - } else if (length > 28 and length < 33) { + } else if (length > 24 and length < 33) { valid_length = 32; } else if (length > 32 and length < 49) { valid_length = 48; diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp index c1d9d6c..a8f0056 100644 --- a/Robot_V2/device/ang_sensor.cpp +++ b/Robot_V2/device/ang_sensor.cpp @@ -1,6 +1,7 @@ #include "ang_sensor.h" #include "Can.h" #include "Filter.h" +#include "config.h" #include "flash.h" #include "gd32c10x_dma.h" #include "gd32c10x_gpio.h" @@ -222,7 +223,7 @@ inline float angle_sensor::read_sensorAngle(void) { if (result == 0) { // 单次浮点乘法优化：只计算一次度数，然后转换为弧度 - float angle_deg = (float)raw_data * ANGLESENS_FACTOR; + float angle_deg = -(float)raw_data * ANGLESENS_FACTOR + ANGLESENS_OFFSET; current_angle_deg = angle_deg; current_angle_rad = angle_deg * DEG2RAD; // M_PI/180.0f return angle_deg; diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 8b98faf..38392ee 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -24,7 +24,9 @@ extern EncoderAngleKalman Angle_kalman; // 角度滤波 extern ThermalController thermalController; // 热量控制器 extern FOC_Controlroller FOC; // FOC控制 extern FlashEEPROM flashEEP; // EEPROM类 +extern VersionManager Version; +// 全局版本管理器实例 TaskScheduler scheduler = {0}; volatile u32 motor_runtime_50us = 0; // 电机驱动计算轮次 volatile u32 cnt_test = 0; // 负载检测 @@ -38,6 +40,7 @@ int main(void) { angsens_MG_check(); // 角度传感器磁感应强度测试 watchdog_init(50); // 看门狗初始化 + while (1) { __disable_irq(); u32 currentTasks = @@ -57,6 +60,7 @@ u8 hardware_init(void) { USART0_init(); // 调试串口初始化 motor_init(); // 电机驱动初始化 User_timer_init(); // 任务Timer初始化 + u8 init_status = 0; init_status += KTH7823.init(); // 角度传感器初始化 init_status += sampler.init(); // 采样器初始化 @@ -117,7 +121,7 @@ void do_task(u32 currentTasks) { power_manage(); } if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信 - CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测 + // CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测 cnt_test = 0; CAN0_transmiter.can0_send_flag = SET; CAN0_transmiter.communication_check(); diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index d0d1fca..5307ed3 100644 --- a/Robot_V2/sys/config.h +++ b/Robot_V2/sys/config.h @@ -5,40 +5,64 @@ // 如果使用CMSIS DSP库，需要定义ARM_MATH_CM4 #define ARM_MATH_CM4 -#define EEP_DEBUG 0 +#define EEP_DEBUG 1 #define OTA_DEBUG 1 #define SLEEP_DEBUG 1 +// 版本号定义 - 可根据实际项目进行调整 +constexpr u16 SOFTWARE_VERSION_MAJOR = 26; +constexpr u16 SOFTWARE_VERSION_MINOR = 1; +constexpr u16 SOFTWARE_VERSION_PATCH = 19; +constexpr u16 SOFTWARE_VERSION_BUILD = 1; + +constexpr u16 HARDWARE_VERSION_MAJOR = 25; +constexpr u16 HARDWARE_VERSION_MINOR = 12; +constexpr u16 HARDWARE_VERSION_PATCH = 01; +constexpr u16 HARDWARE_VERSION_BUILD = 0; + +constexpr u16 BOOT_VERSION_MAJOR = 26; +constexpr u16 BOOT_VERSION_MINOR = 1; +constexpr u16 BOOT_VERSION_PATCH = 12; +constexpr u16 BOOT_VERSION_BUILD = 0; + // Flash分区配置 -constexpr u32 BOOTLOADER_SIZE = 0x4000; // 16KB +constexpr u32 BOOTLOADER_SIZE = 0x6000; // 24KB constexpr u32 BOOTLOADER_START_ADDR = 0x08000000; // BOOT起始地址 -constexpr u32 APPLATEST_START_ADDR = 0x08011000; // APP起始地址 -constexpr u32 APPBACK_START_ADDR = 0x08005000; // APPBACK起始地址 +constexpr u32 APPLATEST_START_ADDR = 0x08012000; // APP起始地址 +constexpr u32 APPBACK_START_ADDR = 0x08006000; // APPBACK起始地址 constexpr u32 APP_MAX_SIZE = 0xC000; // 48KB constexpr u32 FMC_PAGE_SIZE = 0x400; // 1KB -constexpr u32 SLEEP_CONFIG_START = 0x0801D400; // 睡眠配置起始地址 -constexpr u32 SLEEP_CONFIG_END = 0x0801D800; // 睡眠配置结束地址 +constexpr u32 SLEEP_CONFIG_START = 0x0801E400; // 睡眠配置起始地址 +constexpr u32 SLEEP_CONFIG_END = 0x0801E800; // 睡眠配置结束地址 constexpr u32 FLASH_EMPTY = 0xFFFFFFFF; // 空数据 constexpr u32 SLEEP_ENABLE = 0x11111111; // 睡眠使能 constexpr u32 SLEEP_DISABLE = 0x00000000; // 睡眠禁止 // BOOT 相关 constexpr u32 OTA_CONFIG_NUM = 6; -constexpr u32 n_APP_LATEST_AVAL_ADDR = 0x0801D000; // 最新APP可用地址 -constexpr u32 n_APP_BACKUP_AVAL_ADDR = 0x0801D004; // 备份APP可用地址 -constexpr u32 n_APP_LATEST_EN_ADDR = 0x0801D008; // 最新APP激活 -constexpr u32 n_APP_BACKUP_EN_ADDR = 0x0801D00C; // 备份APP激活 -constexpr u32 n_APP_LATEST_CHECK_ADDR = 0x0801D010; // 最新APP校验 -constexpr u32 n_APP_BACKUP_CHECK_ADDR = 0x0801D014; // 备份APP校验 +constexpr u32 n_APP_LATEST_AVAL_ADDR = 0x0801E000; // 最新APP可用地址 +constexpr u32 n_APP_BACKUP_AVAL_ADDR = 0x0801E004; // 备份APP可用地址 +constexpr u32 n_APP_LATEST_EN_ADDR = 0x0801E008; // 最新APP激活 +constexpr u32 n_APP_BACKUP_EN_ADDR = 0x0801E00C; // 备份APP激活 +constexpr u32 n_APP_LATEST_CHECK_ADDR = 0x0801E010; // 最新APP校验 +constexpr u32 n_APP_BACKUP_CHECK_ADDR = 0x0801E014; // 备份APP校验 constexpr u32 OTA_NOT_AVAL = 0xFFFFFFFF; constexpr u32 OTA_AVAL = 0x00000000; // 全零表示可用 // 参数配置 constexpr u8 PP = 11; -constexpr float DIR = 1.0; +constexpr float DIR = -1.0f; constexpr u16 REDUCTION_RATIO = 25; constexpr float CURRENT_OFFSET = -1.615f; // 当前电流传感器的零点偏移 constexpr float HOLD_PWM_DUTY = 0.95f; +constexpr u16 ANGLESENS_OFFSET = +0; +constexpr float KE_FACTOR = 14.0f; +// PID配置 +constexpr float current_loop_p = 6e-2f / 100; // 电流P +constexpr float vel_loop_p = 0.01f / 100; // 速度P +constexpr float angle_loop_p = 20.0f / 100; // 角度P +constexpr float vel_angloop_p = 0.12f / 100; // 速度 速度-角度P +constexpr float ang_angloop_p = 100.0f / 100; // 角度 速度-角度P // 常量定义 constexpr u8 SIZE_FLOAT = sizeof(float); @@ -62,12 +86,15 @@ constexpr u16 LUT_SCALE = (LUT_SIZE / (2.0f * PI)); constexpr u16 DEV_CAN0_ID = 0x0000; constexpr u16 DEV_CAN0_MASK = 0x0000; constexpr u16 DEV_CAN_LOOPBACK_ID = 0x0000; - -constexpr u32 CANFD_BOOT_ID = 0x00000000; +// 反馈ID constexpr u32 CANFD_STATUS_ID = 0x00010100; constexpr u32 CANFD_CONFIG_ID = 0x00010101; +constexpr u32 CANFD_VERSION_ID = 0x00010000; +// 命令ID constexpr u32 CANFD_CMD_ID = 0x00010200; constexpr u32 CANFD_RESET_ID = 0x00010301; +// OTA相关ID +constexpr u32 CANFD_BOOT_ID = 0x00000000; constexpr u32 CANFD_OTA_REQ_ID = 0x00000100; constexpr u32 CANFD_OTA_DATA_ID = 0x00000101; constexpr u32 CANFD_OTA_ACK_ID = 0x00000102; diff --git a/Robot_V2/sys/flash.cpp b/Robot_V2/sys/flash.cpp index 36720a2..a03d875 100644 --- a/Robot_V2/sys/flash.cpp +++ b/Robot_V2/sys/flash.cpp @@ -5,6 +5,7 @@ #include "gd32c10x.h" // GD32主头文件，确保外设函数声明可用 #include "gd32c10x_crc.h" #include "gd32c10x_fmc.h" +#include "systick.h" #include #include #include @@ -531,16 +532,20 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) { /** * @brief 设置OTA配置参数到Flash - * + * * 此函数用于更新存储在Flash中的OTA相关标志位（如APP可用、启用、校验等）。 * GD32的Flash特性是：只能将1写为0，不能将0写为1。若要将0变为1，必须先擦除整个页面。 * 因此，本函数通过读取同一页内的所有参数，擦除该页，然后重新写入有效的参数来实现“写1”操作。 - * + * * @param addr: 要设置的Flash地址 - * @param data: 要写入的数据 (通常为OTA_AVAL=0x00000000 或 OTA_NOT_AVAL=0xFFFFFFFF) + * @param data: 要写入的数据 (通常为OTA_AVAL=0x00000000 或 + * OTA_NOT_AVAL=0xFFFFFFFF) * @retval FLASH_EEPROM_OK (0) 表示成功, 其他值表示失败 */ u8 OTA_SET(u32 addr, u32 data) { +#if EEP_DEBUG + printf("OTA_prepare write%08x at %08x\r\n", data, addr); +#endif fmc_state_enum fmc_status; u32 temp_val[OTA_CONFIG_NUM]; // 临时数组，用于存储当前所有配置参数的值 volatile u32 *flash_ptr = (volatile u32 *)addr; @@ -562,7 +567,6 @@ u8 OTA_SET(u32 addr, u32 data) { } // 3. 对于需要将0改回1的操作，必须先擦除整个Flash页 - // 首先读取同一页内所有相关配置参数的当前状态 flash_ptr = (volatile u32 *)n_APP_LATEST_AVAL_ADDR; temp_val[0] = *flash_ptr; flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR; @@ -577,13 +581,13 @@ u8 OTA_SET(u32 addr, u32 data) { temp_val[5] = *flash_ptr; #if EEP_DEBUG - printf("OTA_SET - Start updating configuration\r\n"); + printf("OTA_prepare write%08x at %08x\r\n", data, addr); // 修复：完整输出所有6个配置项的值，便于调试 printf("LatestAval:0x%x, BackupAval:0x%x, LatestEn:0x%x, " "BackupEn:0x%x, LatestCheck:0x%x, BackupCheck:0x%x\r\n", - temp_val[0], temp_val[1], temp_val[2], temp_val[3], temp_val[4], temp_val[5]); + temp_val[0], temp_val[1], temp_val[2], temp_val[3], temp_val[4], + temp_val[5]); #endif - // 4. 擦除包含所有配置参数的Flash页 fmc_status = fmc_page_erase(n_APP_LATEST_AVAL_ADDR); if (fmc_status != FMC_READY) { @@ -593,32 +597,44 @@ u8 OTA_SET(u32 addr, u32 data) { return FLASH_EEPROM_ERASE_ERROR; } - // 5. 重新写入所有之前为有效(OTA_AVAL)的配置项 + // 5. 重新写入所有之前读取的配置项值（恢复原始状态） // 注意：不包括当前正在设置的地址，因为它将在最后单独处理 for (int i = 0; i < OTA_CONFIG_NUM; i++) { u32 current_addr = n_APP_LATEST_AVAL_ADDR + 4 * i; - if (temp_val[i] == OTA_AVAL && current_addr != addr) { - fmc_status = write_ADDRESS(current_addr, OTA_AVAL); + if (current_addr != addr and temp_val[i] != OTA_NOT_AVAL) { + fmc_status = write_ADDRESS(current_addr, temp_val[i]); if (fmc_status != FMC_READY) { #if EEP_DEBUG - printf("OTA_SET - Write backup value failed at 0x%08X!\r\n", current_addr); + printf("OTA_SET - Write backup value %08X failed at " + "0x%08X!status:%d\r\n", + temp_val[i], current_addr, fmc_status); #endif return FLASH_EEPROM_WRITE_ERROR; } } } - // 6. 最后，写入本次调用的目标数据 - // 即使addr为0（理论上无效），也检查一下以避免潜在问题 - if (addr != 0) { - fmc_status = write_ADDRESS(addr, data); - if (fmc_status != FMC_READY) { #if EEP_DEBUG - printf("OTA_SET - Final write to target address 0x%08X failed!\r\n", addr); + // 3. 对于需要将0改回1的操作，必须先擦除整个Flash页 + flash_ptr = (volatile u32 *)n_APP_LATEST_AVAL_ADDR; + temp_val[0] = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR; + temp_val[1] = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_LATEST_EN_ADDR; + temp_val[2] = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_BACKUP_EN_ADDR; + temp_val[3] = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_LATEST_CHECK_ADDR; + temp_val[4] = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR; + temp_val[5] = *flash_ptr; + // 修复：完整输出所有6个配置项的值，便于调试 + printf("LatestAval:0x%x, BackupAval:0x%x, LatestEn:0x%x, " + "BackupEn:0x%x, LatestCheck:0x%x, BackupCheck:0x%x\r\n", + temp_val[0], temp_val[1], temp_val[2], temp_val[3], temp_val[4], + temp_val[5]); #endif - return FLASH_EEPROM_WRITE_ERROR; - } - } + return FLASH_EEPROM_OK; } @@ -675,13 +691,16 @@ fmc_state_enum move_app_data() { // 数据大小: APP_MAX_SIZE (0xC000 = 48KB) fmc_state_enum fmc_status = FMC_READY; - +#if OTA_DEBUG + printf("move_app_data\r\n"); +#endif // 关闭全局中断以防止Flash操作期间的中断干扰 __disable_irq(); fmc_unlock(); // 首先擦除目标地址的Flash页面 - uint32_t num_pages = (APP_MAX_SIZE + FMC_PAGE_SIZE - 1) / FMC_PAGE_SIZE; // 向上取整 + uint32_t num_pages = + (APP_MAX_SIZE + FMC_PAGE_SIZE - 1) / FMC_PAGE_SIZE; // 向上取整 for (uint32_t i = 0; i < num_pages; i++) { uint32_t page_addr = APPBACK_START_ADDR + i * FMC_PAGE_SIZE; @@ -692,6 +711,7 @@ fmc_state_enum move_app_data() { fmc_lock(); #if OTA_DEBUG printf("move_app_data erase error\r\n"); + delay_1ms(1000); #endif __enable_irq(); // 重新开启中断 return fmc_status; @@ -701,28 +721,34 @@ fmc_state_enum move_app_data() { // 复制数据 - 按字（32位）进行复制以提高效率 uint32_t offset = 0; uint32_t write_count = 0; // 记录实际写入的字数 - const uint32_t* src_ptr = (const uint32_t*)APPLATEST_START_ADDR; - uint32_t* dst_ptr = (uint32_t*)APPBACK_START_ADDR; + const uint32_t *src_ptr = (const uint32_t *)APPLATEST_START_ADDR; + uint32_t *dst_ptr = (uint32_t *)APPBACK_START_ADDR; // 预计算源数据的CRC，避免重复访问内存 crc_deinit(); crc_data_register_reset(); - uint32_t src_crc = crc_block_data_calculate((uint32_t*)APPLATEST_START_ADDR, APP_MAX_SIZE/4); + uint32_t src_crc = crc_block_data_calculate( + (uint32_t *)APPLATEST_START_ADDR, APP_MAX_SIZE / 4); while (offset < APP_MAX_SIZE) { - uint32_t src_data = src_ptr[offset/4]; + uint32_t src_data = src_ptr[offset / 4]; // 检查源地址是否为有效的Flash数据（非0xFFFFFFFF表示有数据） if (src_data != 0xFFFFFFFF) { + fmc_unlock(); // 清除之前的标志位 - fmc_flag_clear(FMC_FLAG_END | FMC_FLAG_WPERR | FMC_FLAG_PGAERR | FMC_FLAG_PGERR); - - fmc_status = fmc_word_program(APPBACK_START_ADDR + offset, src_data); + fmc_flag_clear(FMC_FLAG_END | FMC_FLAG_WPERR | FMC_FLAG_PGAERR | + FMC_FLAG_PGERR); + fmc_status = + fmc_word_program(APPBACK_START_ADDR + offset, src_data); // 检查写入操作是否成功 if (fmc_status != FMC_READY) { #if OTA_DEBUG - printf("Flash write failed at offset 0x%08X with status: %d\r\n", offset, fmc_status); + printf( + "Flash write failed at offset 0x%08X with status: %d\r\n", + offset, fmc_status); + delay_1ms(1000); #endif fmc_lock(); __enable_irq(); // 重新开启中断 @@ -731,9 +757,9 @@ fmc_state_enum move_app_data() { write_count++; } offset += 4; - + // 每写入一定数量的数据后，喂狗一次 - if ((offset % 4096) == 0) { // 每4KB喂狗一次 + if ((offset % 4096) == 0) { // 每4KB喂狗一次 fwdgt_counter_reload(); } } @@ -745,15 +771,20 @@ fmc_state_enum move_app_data() { crc_data_register_reset(); // 确保CRC计算从初始状态开始 // 计算目标数据的CRC值 - uint32_t dst_crc = crc_block_data_calculate((uint32_t*)APPBACK_START_ADDR, APP_MAX_SIZE/4); + uint32_t dst_crc = crc_block_data_calculate((uint32_t *)APPBACK_START_ADDR, + APP_MAX_SIZE / 4); // 比较CRC值，确保数据完整性 if (src_crc != dst_crc) { #if OTA_DEBUG - printf("CRC mismatch: src_crc=0x%08X, dst_crc=0x%08X\r\n", src_crc, dst_crc); + printf("APPbackup CRC mismatch: src_crc=0x%08X, dst_crc=0x%08X\r\n", + src_crc, dst_crc); #endif return (fmc_state_enum)0xFF; // CRC校验失败 } +#if OTA_DEBUG + printf("APPbackup CRC OK\r\n"); +#endif // 设置备份应用可用标志 fmc_unlock(); OTA_SET(n_APP_BACKUP_AVAL_ADDR, OTA_AVAL); diff --git a/Robot_V2/sys/initial.cpp b/Robot_V2/sys/initial.cpp index e8d62ac..25811fe 100644 --- a/Robot_V2/sys/initial.cpp +++ b/Robot_V2/sys/initial.cpp @@ -6,11 +6,125 @@ #include "gd32c10x_fmc.h" #include "gd32c10x_gpio.h" #include "gd32c10x_rcu.h" +#include "initial.h" +#include #include -#include -#include +#include extern CAN_transmiter CAN0_transmiter; // CAN初始化 +VersionManager Version; + +VersionManager::VersionManager() { + // 初始化版本号 + setSoftwareVersion(SOFTWARE_VERSION_MAJOR, SOFTWARE_VERSION_MINOR, + SOFTWARE_VERSION_PATCH, SOFTWARE_VERSION_BUILD); + setHardwareVersion(HARDWARE_VERSION_MAJOR, HARDWARE_VERSION_MINOR, + HARDWARE_VERSION_PATCH, HARDWARE_VERSION_BUILD); + setBootVersion(BOOT_VERSION_MAJOR, BOOT_VERSION_MINOR, BOOT_VERSION_PATCH, + BOOT_VERSION_BUILD); +} + +VersionManager::VersionInfo VersionManager::getSoftwareVersion() const { + return softwareVersion; +} + +VersionManager::VersionInfo VersionManager::getHardwareVersion() const { + return hardwareVersion; +} + +VersionManager::VersionInfo VersionManager::getBootVersion() const { + return bootVersion; +} + +void VersionManager::setSoftwareVersion(u16 major, u16 minor, u16 patch, + u16 build) { + softwareVersion.major = major; + softwareVersion.minor = minor; + softwareVersion.patch = patch; + softwareVersion.build = build; +} + +void VersionManager::setHardwareVersion(u16 major, u16 minor, u16 patch, + u16 build) { + hardwareVersion.major = major; + hardwareVersion.minor = minor; + hardwareVersion.patch = patch; + hardwareVersion.build = build; +} + +void VersionManager::setBootVersion(u16 major, u16 minor, u16 patch, + u16 build) { + bootVersion.major = major; + bootVersion.minor = minor; + bootVersion.patch = patch; + bootVersion.build = build; +} + +char *VersionManager::versionToString(const VersionInfo &version, char *buffer, + size_t bufSize) const { + if (buffer == nullptr) { + return nullptr; + } + + snprintf(buffer, bufSize, "%d.%d.%d.%d", version.major, version.minor, + version.patch, version.build); + + return buffer; +} + +void VersionManager::sendVersionInfoViaCan() const { + // 准备发送版本信息 (ID: CANFD_VERSION_ID) + // 只使用必要的字节数（24字节）来传输三个版本的信息（每个版本6字节） + u8 versionData[24] = {0}; + + // 将版本信息打包到24字节数组中 + // 前8字节：软件版本信息 (4个uint16_t，每个占2字节) + versionData[0] = softwareVersion.major & 0xFF; // 低字节 + versionData[1] = (softwareVersion.major >> 8) & 0xFF; // 高字节 + versionData[2] = softwareVersion.minor & 0xFF; // 低字节 + versionData[3] = (softwareVersion.minor >> 8) & 0xFF; // 高字节 + versionData[4] = softwareVersion.patch & 0xFF; // 低字节 + versionData[5] = (softwareVersion.patch >> 8) & 0xFF; // 高字节 + versionData[6] = softwareVersion.build & 0xFF; // 低字节 + versionData[7] = (softwareVersion.build >> 8) & 0xFF; // 高字节 + + // 8-15字节：硬件版本信息 (4个uint16_t，每个占2字节) + versionData[8] = hardwareVersion.major & 0xFF; // 低字节 + versionData[9] = (hardwareVersion.major >> 8) & 0xFF; // 高字节 + versionData[10] = hardwareVersion.minor & 0xFF; // 低字节 + versionData[11] = (hardwareVersion.minor >> 8) & 0xFF; // 高字节 + versionData[12] = hardwareVersion.patch & 0xFF; // 低字节 + versionData[13] = (hardwareVersion.patch >> 8) & 0xFF; // 高字节 + versionData[14] = hardwareVersion.build & 0xFF; // 低字节 + versionData[15] = (hardwareVersion.build >> 8) & 0xFF; // 高字节 + + // 16-23字节：BOOT版本信息 (4个uint16_t，每个占2字节) + versionData[16] = bootVersion.major & 0xFF; // 低字节 + versionData[17] = (bootVersion.major >> 8) & 0xFF; // 高字节 + versionData[18] = bootVersion.minor & 0xFF; // 低字节 + versionData[19] = (bootVersion.minor >> 8) & 0xFF; // 高字节 + versionData[20] = bootVersion.patch & 0xFF; // 低字节 + versionData[21] = (bootVersion.patch >> 8) & 0xFF; // 高字节 + versionData[22] = bootVersion.build & 0xFF; // 低字节 + versionData[23] = (bootVersion.build >> 8) & 0xFF; // 高字节 + + // 使用CANFD发送函数发送版本信息 + CAN0_transmiter.CANFD_AsyncSend(CANFD_VERSION_ID, 24, versionData); +} + +void VersionManager::printVersions() const { + char versionStr[32]; + + printf( + "Software Version: %s\r\n", + versionToString(getSoftwareVersion(), versionStr, sizeof(versionStr))); + printf( + "Hardware Version: %s\r\n", + versionToString(getHardwareVersion(), versionStr, sizeof(versionStr))); + printf("BOOT Version: %s\r\n", + versionToString(getBootVersion(), versionStr, sizeof(versionStr))); +} + /* retarget the C library printf function to the USART */ u8 USART0_init() { gd_eval_com_init(EVAL_COM0); @@ -33,7 +147,7 @@ u8 watchdog_init(u32 time) { } int fputc(int ch, FILE *f) throw() { - usart_data_transmit(EVAL_COM0, (uint8_t)ch); + usart_data_transmit(EVAL_COM0, (u8)ch); while (RESET == usart_flag_get(EVAL_COM0, USART_FLAG_TBE)) ; return ch; @@ -97,23 +211,52 @@ u16 get_reset_cause(void) { } void OTA_initial(void) { +#if OTA_DEBUG + u32 temp_val[6]; + // 3. 对于需要将0改回1的操作，必须先擦除整个Flash页 + volatile u32 *flash_ptr1 = (volatile u32 *)n_APP_LATEST_AVAL_ADDR; + temp_val[0] = *flash_ptr1; + flash_ptr1 = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR; + temp_val[1] = *flash_ptr1; + flash_ptr1 = (volatile u32 *)n_APP_LATEST_EN_ADDR; + temp_val[2] = *flash_ptr1; + flash_ptr1 = (volatile u32 *)n_APP_BACKUP_EN_ADDR; + temp_val[3] = *flash_ptr1; + flash_ptr1 = (volatile u32 *)n_APP_LATEST_CHECK_ADDR; + temp_val[4] = *flash_ptr1; + flash_ptr1 = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR; + temp_val[5] = *flash_ptr1; + // 修复：完整输出所有6个配置项的值，便于调试 + printf("APP:CHECK LatestAval:0x%x, BackupAval:0x%x, LatestEn:0x%x, " + "BackupEn:0x%x, LatestCheck:0x%x, BackupCheck:0x%x\r\n", + temp_val[0], temp_val[1], temp_val[2], temp_val[3], temp_val[4], + temp_val[5]); +#endif // 确认当前APP可用 + volatile u32 *flash_ptr = (volatile u32 *)n_APP_LATEST_CHECK_ADDR; + u32 app_check = *flash_ptr; + if (app_check == OTA_NOT_AVAL) { // 新的APP刷入后第一次启动 +#if OTA_DEBUG + std::printf("APP enabled\r\n"); +#endif + move_app_data(); + } + fmc_unlock(); OTA_SET(n_APP_LATEST_CHECK_ADDR, OTA_AVAL); - OTA_SET(n_APP_LATEST_EN_ADDR, OTA_AVAL); fmc_lock(); + u8 transmit_data[2] = {0x0A, 0xAC}; // 确认APP可用 CAN0_transmiter.CANFD_AsyncSend(CANFD_OTA_ACK_ID, 2, transmit_data); - volatile u32 *flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR; + flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR; u32 back_aval = *flash_ptr; flash_ptr = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR; u32 back_check = *flash_ptr; - // 如果BACK不可用 - if (back_aval == OTA_NOT_AVAL) { + if (back_aval == OTA_NOT_AVAL) { // 如果BACK不可用 #if OTA_DEBUG std::printf("notaval\r\n"); #endif diff --git a/Robot_V2/sys/initial.h b/Robot_V2/sys/initial.h index 64e9491..b2b20f8 100644 --- a/Robot_V2/sys/initial.h +++ b/Robot_V2/sys/initial.h @@ -40,8 +40,56 @@ typedef struct { volatile u32 taskFlags; // 任务标志位(每个位代表一个任务) } TaskScheduler; +// 版本管理类 +class VersionManager { + public: + // 版本信息结构体 + typedef struct { + u16 major; + u16 minor; + u16 patch; + u16 build; + } VersionInfo; + + // 构造函数 + VersionManager(); + + // 获取软件版本 + VersionInfo getSoftwareVersion() const; + + // 获取硬件版本 + VersionInfo getHardwareVersion() const; + + // 获取BOOT版本 + VersionInfo getBootVersion() const; + + // 设置软件版本 + void setSoftwareVersion(u16 major, u16 minor, u16 patch, u16 build); + + // 设置硬件版本 + void setHardwareVersion(u16 major, u16 minor, u16 patch, u16 build); + + // 设置BOOT版本 + void setBootVersion(u16 major, u16 minor, u16 patch, u16 build); + + // 将版本信息转换为字符串 + char *versionToString(const VersionInfo &version, char *buffer, + size_t bufSize) const; + + // 打印所有版本信息 + void printVersions() const; + + // 通过CAN发送版本信息 + void sendVersionInfoViaCan() const; + + private: + VersionInfo softwareVersion; + VersionInfo hardwareVersion; + VersionInfo bootVersion; +}; + #ifdef __cplusplus } #endif -#endif +#endif \ No newline at end of file