cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(PID): 调整速度与角度环PID参数

Browse Source 
更新了FOC控制器中的速度环和角度环PID参数以改善控制响应，并微调电流环参数提升稳定性。

feat(Filter): 改进卡尔曼滤波器初始化逻辑及速度计算方式
引入系统运行时间计数器，修正编码器角度和速度卡尔曼滤波器的初始化流程，并基于实际时间间隔优化角速度估算精度。

refactor(Filter): 移动成员变量声明顺序
调整`EncoderAngleKalman`类中`last_angle`成员变量的访问权限和声明位置，增强封装性和可读性。

feat(Flash): 新增Flash EEPROM功能支持
添加FlashEEPROM类及相关头文件，支持电机角度等关键数据掉电保存，并在CAN通信复位时进行角度存储操作。

refactor(Timer): 优化定时器配置和中断优先级设置
删除冗余的TIMER3定义，修改TIMER1中断服务函数可见性，并调整NVIC子优先级分配，确保中断响应合理有序。

feat(AngleSensor): 完善角度传感器初始化与角度设定接口
重构角度传感器初始化方法并新增角度设置函数，支持从Flash读取初始角度值并与减速比结合应用。

feat(Health): 实现母线电压监测与保护策略
新增电源管理函数，在母线电压异常下降时自动关闭电机输出，防止因供电不足导致设备损坏。
This commit is contained in:
cbb
2025-12-16 22:39:39 +08:00
parent 839a32ff04
commit 7371f97882
19 changed files with 516 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2.csolution.yml
+1 -1
View File
@@ -30,7 +30,7 @@ solution:
target-set:
- set:
images:
- project-context: Robot_V2.Debug
- project-context: Robot_V2.Release
debugger:
name: ST-Link@pyOCD
port: 3333
Robot_V2/Algorithm/DengFOC.cpp
+1 -1
View File
@@ -172,7 +172,7 @@ float FOC_Controlroller::velocity_open_loop() {
const float Uq = voltage_power_supply / 2.0f;
setPhaseVoltage(Uq, 0.0f);
CAN0_transmiter.testfloat_union.f =
// CAN0_transmiter.testfloat_union.f =
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
return limit_voltage;
Robot_V2/Algorithm/DengFOC.h
+3 -3
View File
@@ -43,12 +43,12 @@ class FOC_Controlroller {
float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
PIDController vel_loop = PIDController{0.13f, 0.4f, 2.8e-4, 1000000,
PIDController vel_loop = PIDController{0.028f, 0.2f, 1e-4, 1000000,
voltage_power_supply}; // 12V
PIDController angle_loop = PIDController{25.0f, 30.0f, 0.15f, 10000000,
PIDController angle_loop = PIDController{20.0f, 100.0f, 0.2f, 10000000,
voltage_power_supply}; // 12V
PIDController current_loop =
PIDController{0.01f, 0.001f, 0, 100000, voltage_power_supply}; // 12V
PIDController{0.006f, 0.02f, 0, 100000, voltage_power_supply}; // 12V
void setPhaseVoltage(float Uq, float Ud);
float cal_Iq_Id(float current_a, float current_b, float current_c);
Robot_V2/Algorithm/Filter.cpp
+11 -8
View File
@@ -7,8 +7,9 @@
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
EncoderAngleKalman Angle_kalman(KTH7823.getAngle(), 1.0f, 1e-4f, 0.25f);
EncoderVelocityKalman Vel_kalman(Angle_kalman.getAngle(), 1e-5f, 1e-6f, 0.02f);
extern u32 system_runtime_us;
EncoderAngleKalman Angle_kalman(0, 1.0f, 1e-4f, 0.25f);
EncoderVelocityKalman Vel_kalman(0, 1e-5f, 1e-6f, 0.02f);
constexpr u8 SIZE_FLOAT = sizeof(float);
@@ -30,7 +31,6 @@ float medium_filter(float values[], u8 size) {
return sorted[size / 2]; // 返回中值
}
// EncoderVelocityKalman类的实现
/*!
\brief 构造函数，初始化编码器速度卡尔曼滤波器参数
@@ -100,7 +100,6 @@ EncoderAngleKalman::EncoderAngleKalman(float initial_angle,
measurement_noise),
last_angle(initial_angle), initialized(true) {}
/*!
\brief 更新卡尔曼滤波器状态
\param[in] measured_angle 测量的角度值 (degree)
@@ -127,16 +126,20 @@ float EncoderAngleKalman::getAngle() const { return kalman.getState(); }
\brief 获取当前角度估计值
\retval 当前角度估计值 (degree)
*/
float EncoderAngleKalman::getVelocity() const { return (kalman.getState()-last_angle)*3.75e3; }
float EncoderAngleKalman::getVelocity() const {
static u32 last_us100_counter = system_runtime_us;
float vel = (kalman.getState() - last_angle) * 1e4 *
(system_runtime_us - last_us100_counter);
last_us100_counter = system_runtime_us;
return vel;
}
/*!
\brief 设置过程噪声协方差
\param[in] q 过程噪声协方差值
\retval none
*/
void EncoderAngleKalman::setProcessNoise(float q) {
kalman.setProcessNoise(q);
}
void EncoderAngleKalman::setProcessNoise(float q) { kalman.setProcessNoise(q); }
/*!
\brief 设置测量噪声协方差
Robot_V2/Algorithm/Filter.h
+1 -1
View File
@@ -138,9 +138,9 @@ class EncoderAngleKalman {
private:
KalmanFilter<float> kalman;
bool initialized;
float last_angle;
public:
float last_angle;
EncoderAngleKalman(float initial_angle, float initial_error_covariance,
float process_noise, float measurement_noise);
Robot_V2/RTE/Device/GD32C103CB/gd32c10x_fmc.c
+3 -1
View File
@@ -33,6 +33,7 @@ OF SUCH DAMAGE.
*/
#include "gd32c10x_fmc.h"
#include "stdio.h"
/* FMC mask */
#define LOW_8BITS_MASK ((uint32_t)0x000000FFU)
@@ -329,6 +330,7 @@ fmc_state_enum fmc_doubleword_program(uint32_t address, uint64_t data)
fmc_state_enum fmc_word_program(uint32_t address, uint32_t data)
{
fmc_state_enum fmc_state = fmc_ready_wait(FMC_TIMEOUT_COUNT);
printf("fmc_status: 0x%x\r\n", fmc_state);
if(FMC_READY == fmc_state){
/* set the PG bit to start program */
@@ -988,7 +990,7 @@ void fmc_interrupt_flag_clear(uint32_t flag)
static fmc_state_enum fmc_state_get(void)
{
fmc_state_enum fmc_state = FMC_READY;
if((uint32_t)0x00U != (FMC_STAT & FMC_STAT_BUSY)){
fmc_state = FMC_BUSY;
}else{
Robot_V2/Robot_V2.cproject.yml
+2
View File
@@ -12,6 +12,8 @@ project:
- file: sys/initial.h
- file: sys/health.h
- file: sys/health.cpp
- file: sys/flash.h
- file: sys/flash.cpp
- group: device
files:
- file: device/Can.cpp
Robot_V2/device/Can.cpp
+9 -5
View File
@@ -3,6 +3,8 @@
#include "Filter.h"
#include "adc.h"
#include "ang_sensor.h"
#include "core_cmFunc.h"
#include "flash.h"
#include "gd32c10x.h"
#include "gd32c10x_can.h"
#include "gd32c10x_dma.h"
@@ -27,6 +29,7 @@ extern FOC_Controlroller FOC;
extern sampler sampler;
extern angle_sensor KTH7823;
extern EncoderVelocityKalman Vel_kalman;
extern FlashEEPROM flashEEP;
// 添加DMA相关变量
dma_parameter_struct dma_rx_parameter;
@@ -628,9 +631,10 @@ u8 CAN_transmiter::CANFD_all_Send() {
pack_u16_to_bytes(transmit_data, 4, u16(u_current_union.f));
pack_u16_to_bytes(transmit_data, 6, u16(v_current_union.f));
pack_u16_to_bytes(transmit_data, 8, u16(w_current_union.f));
transmit_data[10] = (transmit_data[10] & ~0x07) |
((MotorStatus & 0x01) | ((MagStatus & 0x01) << 1) |
((ThermalStatus & 0x01) << 2));
transmit_data[10] =
(transmit_data[10] & ~0x07) |
((MotorStatus & 0x01) | ((MagStatus & 0x01) << 1) |
((ThermalStatus & 0x01) << 2) | ((EEPROMStatus & 0x01) << 3));
pack_u16_to_bytes(transmit_data, 11, Power_Voltage);
pack_float_to_bytes(transmit_data, 13, rt_angle_union);
@@ -692,8 +696,8 @@ void CAN_transmiter::communication_check() {
FOC.target_velocity = target_velocity_union.f;
} else if (receive_message.rx_efid == 0x00010301) {
motor_reset();
}
else {
flashEEP.save_angle(666, false);
} else {
printf("Unknown CAN ID: %X\n", receive_message.rx_efid);
}
can0_receive_flag = RESET;
Robot_V2/device/Can.h
+1
View File
@@ -110,6 +110,7 @@ class CAN_transmiter {
bool MagStatus = 0; // 磁状态码
bool ThermalStatus = 0; // 热能状态码
bool EEPROMStatus = 0; // EEPROM状态码
void communication_check(); // 通信检查
};
Robot_V2/device/Timer.cpp
+4 -13
View File
@@ -14,8 +14,6 @@
constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz
constexpr int TIMER2_PERIOD = 99; // 1MHz / 100 = 100us
constexpr int TIMER3_PRESCALER = 119; // 120MHz / 120 = 1MHz
constexpr int TIMER3_PERIOD = 9999; // 1MHz / 10000 = 100Hz = 10ms
constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz
constexpr int TIMER1_PERIOD = 5999; // 120MHz / (0+1) / (5999+1) = 20kHz
@@ -25,13 +23,13 @@ constexpr int TIMER1_CH2_PULSE = 0;
constexpr int TIMER1_CH3_COMPARE_VALUE =
(TIMER1_PERIOD + 1) * 0.5 - 1; // PWM周期50%位置触发中断
static u32 total_us100_counter = 0; // 总计数器，每100微秒增加1
volatile u32 total_us100_counter = 0; // 总计数器，每100微秒增加1
extern u16 adc_value[7];
extern FOC_Controlroller FOC;
extern TaskScheduler scheduler;
extern CAN_transmiter CAN0_transmiter;
// 添加系统运行时间计数器
static volatile std::u64 system_runtime_us = 0;
extern u32 cnt_test;
/**
* @brief 设置引脚中断优先级
@@ -44,7 +42,7 @@ void nvic_config(void) {
nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2);
/* 使能TIMER2中断，设置抢占优先级为1，子优先级为1 */
nvic_irq_enable(TIMER2_IRQn, 0, 1);
nvic_irq_enable(TIMER1_IRQn, 0, 1);
nvic_irq_enable(TIMER1_IRQn, 0, 2);
}
/**
@@ -99,13 +97,6 @@ void User_timer_init(void) {
nvic_config();
}
/*!
\brief 获取系统运行时间(毫秒)
\param[in] none
\param[out] none
\retval 系统运行时间，单位为毫秒
*/
uint64_t get_system_runtime_us(void) { return system_runtime_us; }
/**
* @brief TIMER2中断处理函数
@@ -277,7 +268,7 @@ void timer1_pwm_config(void) {
* @brief TIMER1中断处理函数
* @return void
*/
extern "C" void TIMER1_IRQHandler(void) {
void TIMER1_IRQHandler(void) {
// 检查CH3比较中断标志并处理
if (timer_interrupt_flag_get(TIMER1, TIMER_INT_FLAG_CH3) != RESET) {
// 清除CH3比较中断标志
Robot_V2/device/ang_sensor.cpp
+19 -8
View File
@@ -1,12 +1,15 @@
#include "ang_sensor.h"
#include "Filter.h"
#include "core_cmFunc.h"
#include "gd32c10x_can.h"
#include "gd32c10x_dma.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_spi.h"
#include "initial.h"
#include "flash.h"
#include <cstdio>
#include <cstring>
#include "Can.h"
#include <stdint.h>
#define ANG_SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
@@ -28,8 +31,10 @@ volatile bool spi_dma_tx_complete = false;
volatile bool spi_dma_rx_complete = false;
angle_sensor KTH7823; // 角度传感器初始化
extern FlashEEPROM flashEEP;
extern EncoderAngleKalman Angle_kalman;
extern EncoderVelocityKalman Vel_kalman;
extern CAN_transmiter CAN0_transmiter; // CAN初始化
/**
* @brief SPI DMA initialization
@@ -194,21 +199,27 @@ void KTH7823_spi_init(void) {
spi_enable(SPI0);
}
/**
* @brief KTH7823 初始化
* @ 角度传感器类 初始化
* @return 0:初始化成功 1:初始化失败
*/
void angle_sensor::KTH7823_init(void) {
u8 angle_sensor::init(void) {
KTH7823_gpio_init(); // GPIO初始化
KTH7823_spi_init(); // SPI初始化
spi_dma_init(); // DMA初始化
}
/**
* @brief 角度传感器构造函数
*/
u8 angle_sensor::init(void) {
KTH7823_init();
return 0;
}
/**
* @ 角度传感器类 设置当前角度
* @param angle 输出级角度值
* @return 0成功/1失败
*/
u8 angle_sensor::angle_set(float angle){
m_cumulativeAngle = flashEEP.read_angle()*REDUCTION_RATIO; // 从Flash读取上次保存的角度
return 0;
}
/**
* @brief 读取传感器数据
Robot_V2/device/ang_sensor.h
+1 -1
View File
@@ -21,9 +21,9 @@ class angle_sensor {
float getAngle(void) const;
float getRawAngle(void) const;
float getcurrent_sensorangle(void) const;
u8 angle_set(float angle);
protected:
void KTH7823_init(void);
float m_cumulativeAngle;
float current_angle_deg;
float current_angle_rad;
Robot_V2/main.cpp
+20 -15
View File
@@ -7,16 +7,20 @@
#include "ang_sensor.h"
#include "core_cm4.h"
#include "core_cmFunc.h"
#include "motor.h"
#include "flash.h"
#include "health.h"
#include "initial.h"
#include "motor.h"
#include "systick.h"
#include <cstdio>
#include <stdint.h>
TaskScheduler scheduler = {0};
volatile u32 system_runtime_us = 0;
volatile u32 cnt_test = 0;
int main(void) {
systick_config(); // 系统时钟初始化
unused_pin_init(); // 未使用引脚初始化
USART0_init(); // 调试串口初始化
User_timer_init(); // 任务Timer初始化
motor_init(); // 电机驱动初始化
@@ -28,27 +32,27 @@ int main(void) {
extern EncoderAngleKalman Angle_kalman; // 角度滤波
extern ThermalController thermalController; // 热量控制器
extern FOC_Controlroller FOC; // FOC控制
extern FlashEEPROM flashEEP; // EEPROM类
u8 init_status = 0;
init_status += KTH7823.init(); // 角度传感器初始化
init_status += sampler.init(); // 采样器初始化
init_status += CAN0_transmiter.init(); // CAN初始化
init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化
init_status += Vel_kalman.init(0.0f); // 速度滤波初始化
while (init_status) { // 初始化检测
init_status += flashEEP.init(); // EEPROM初始化
init_status += KTH7823.init(); // 角度传感器初始化
init_status += sampler.init(); // 采样器初始化
init_status += CAN0_transmiter.init(); // CAN初始化
init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化
init_status += Vel_kalman.init(0.0f); // 速度滤波初始化
while (init_status) { // 初始化检测
std::printf("初始化失败\r\n");
}
u32 cnt_test = 0;
watchdog_init(500); // 看门狗初始化
angsens_MG_check(); // 角度传感器磁感应强度测试
angsens_MG_check(); // 角度传感器磁感应强度测试
while (1) {
__disable_irq();
u32 currentTasks =
scheduler.taskFlags; // 保存当前任务标志快照以提高效率并确保一致性
scheduler.taskFlags &= ~currentTasks; // 一次性清除所有已处理的任务标志
__enable_irq();
cnt_test++;
// 按照优先级和重要性处理任务
if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值
@@ -59,11 +63,11 @@ int main(void) {
FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle());
}
if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度传感器值
CAN0_transmiter.testfloat_union.f = (Angle_kalman.getstate()-Angle_kalman.last_angle)*10000.0f; // 速度滤波前的速度值，单位: deg/s
FOC.rt_velocity = Vel_kalman.update(Angle_kalman.getVelocity());
}
if (currentTasks &
(1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级)
system_runtime_us++;
switch (FOC.control_mode) {
case 0: {
FOC.drv_hold(0.95); // 占空比置0
@@ -71,6 +75,8 @@ int main(void) {
}
case 1: {
FOC.angle_control(FOC.target_angle); // 角度闭环控制
// FOC.angle_control_with_velocity_limit(FOC.target_angle,
// FOC.limit_velocity); // 角度闭环控制，带速度限制
break;
}
case 2: {
@@ -102,11 +108,10 @@ int main(void) {
angsens_MG_check();
}
if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值
sampler.update_vbus_voltage();
FOC.update_power();
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();
@@ -114,4 +119,4 @@ int main(void) {
}
}
return 0;
}
}
Robot_V2/sys/flash.cpp
+344
View File
@@ -0,0 +1,344 @@
#include "flash.h"
#include "ang_sensor.h"
#include "core_cmFunc.h"
#include "gd32c10x_fmc.h"
#include <Can.h>
#include <cstdio>
#include <stdint.h>
FlashEEPROM flashEEP = FlashEEPROM();
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
// FlashEEPROM类实现
FlashEEPROM::FlashEEPROM() {
// 构造函数可以为空，或者做一些初始化工作
}
/**
* @brief 初始化Flash EEPROM模拟
* @param None
* @retval None
*/
u8 FlashEEPROM::init() {
// 解锁Flash
fmc_unlock();
// 读取保存的参数
u16 angle;
// 尝试从Flash读取参数，如果不存在则使用默认值
if (read(ADDR_ANGLE_DATA, &angle) != FLASH_EEPROM_OK) {
angle = 0; // 默认值
std::printf("角度初始化为: %d\r\n", angle);
} else {
std::printf("从Flash读取到角度数: %d\r\n", angle);
}
KTH7823.angle_set((float)angle);
// save_angle(200, false); // 重置保存标志以允许再次保存
return 0;
}
/**
* @brief 从Flash EEPROM读取数据
* @param virtualAddr: 虚拟地址
* @param data: 指向存储读取数据的指针
* @retval FlashEepromStatus: 操作状态
*/
FlashEepromStatus FlashEEPROM::read(uint16_t virtualAddr,
uint16_t *data) const {
// 检查参数有效性
if (data == nullptr || virtualAddr == 0xFFFF) {
return FLASH_EEPROM_ERROR;
}
// 从Flash末尾开始向前搜索，找到最新的数据
for (uint32_t address = EEPROM_END_ADDRESS - sizeof(uint32_t);
address >= EEPROM_START_ADDRESS; address -= sizeof(uint32_t)) {
uint32_t data_word = *((uint32_t *)address);
// 检查是否是有效数据（非擦除状态）
if (data_word != 0xFFFFFFFF) {
uint16_t addr = (uint16_t)(data_word & 0xFFFF);
// 找到匹配的虚拟地址
if (addr == virtualAddr) {
*data = (uint16_t)((data_word >> 16) & 0xFFFF);
return FLASH_EEPROM_OK;
}
}
}
// 未找到指定虚拟地址的数据
return FLASH_EEPROM_NOT_FOUND;
}
/**
* @brief 向Flash EEPROM写入数据
* @param virtualAddr: 虚拟地址
* @param data: 要写入的数据
* @retval FlashEepromStatus: 操作状态
*/
FlashEepromStatus FlashEEPROM::write(uint16_t virtualAddr, uint16_t data) {
// 参数检查
if (virtualAddr == 0xFFFF) {
return FLASH_EEPROM_INVALID_ADDR;
}
// 查找下一个可写入的位置
uint32_t next_address = EEPROM_START_ADDRESS;
// 遍历Flash页面寻找下一个可写入位置
for (uint32_t address = EEPROM_START_ADDRESS; address < EEPROM_END_ADDRESS;
address += sizeof(uint32_t)) {
uint32_t data_word = *((uint32_t *)address);
// 如果找到已使用的地址且与要写入的地址相同，检查数据是否需要更新
if ((data_word & 0xFFFF) == virtualAddr) {
uint16_t old_data = (uint16_t)((data_word >> 16) & 0xFFFF);
if (old_data == data) {
// 数据相同，无需更新
return FLASH_EEPROM_OK;
}
}
// 找到空位置
if (data_word == 0xFFFFFFFF) {
next_address = address;
break;
}
// 更新下一个可能的写入位置（如果需要格式化）
next_address = address + sizeof(uint32_t);
}
// 如果有足够空间，直接写入
if (next_address < EEPROM_END_ADDRESS) {
u32 new_data_word = ((u32)data << 16) | (virtualAddr & 0xFFFF);
fmc_state_enum fmc_status =
fmc_word_program(next_address, new_data_word);
if (fmc_status != FMC_READY) {
return FLASH_EEPROM_WRITE_ERROR;
}
return FLASH_EEPROM_OK;
} else {
// 空间不足，需要格式化页面
FlashEepromStatus status = format();
if (status != FLASH_EEPROM_OK) {
return status;
}
// 格式化后在页面开头写入数据
uint32_t new_data_word =
((uint32_t)data << 16) | (virtualAddr & 0xFFFF);
fmc_state_enum fmc_status =
fmc_word_program(EEPROM_START_ADDRESS, new_data_word);
if (fmc_status != FMC_READY) {
return FLASH_EEPROM_WRITE_ERROR;
}
return FLASH_EEPROM_OK;
}
}
/**
* @brief 格式化Flash EEPROM页面
* @param None
* @retval FlashEepromStatus: 操作状态
*/
FlashEepromStatus FlashEEPROM::format() {
fmc_state_enum fmc_status;
// 创建临时缓冲区存储唯一地址的最新数据
uint32_t valid_entries[EEPROM_MAX_SIZE / sizeof(uint32_t)];
uint16_t valid_count = 0;
// 收集有效的数据项（去重，保留最新的）
for (uint32_t address = EEPROM_START_ADDRESS; address < EEPROM_END_ADDRESS;
address += sizeof(uint32_t)) {
uint32_t data_word = *((uint32_t *)address);
// 检查是否是有效数据项
if (data_word != 0xFFFFFFFF) {
uint16_t addr = (uint16_t)(data_word & 0xFFFF);
uint16_t value = (uint16_t)((data_word >> 16) & 0xFFFF);
// 查找是否已经有相同虚拟地址的数据项
uint8_t found = 0;
for (uint16_t i = 0; i < valid_count; i++) {
uint16_t existing_addr = (uint16_t)(valid_entries[i] & 0xFFFF);
if (existing_addr == addr) {
// 更新为最新的数据
valid_entries[i] = ((uint32_t)value << 16) | addr;
found = 1;
break;
}
}
// 如果是新的虚拟地址，添加到有效数据列表
if (!found && valid_count < (EEPROM_MAX_SIZE / sizeof(uint32_t))) {
valid_entries[valid_count] = ((uint32_t)value << 16) | addr;
valid_count++;
}
}
}
// 擦除整个页面
fmc_status = fmc_page_erase(EEPROM_START_ADDRESS);
if (fmc_status != FMC_READY) {
return FLASH_EEPROM_ERASE_ERROR;
}
// 重新写入有效的数据项
for (uint16_t i = 0; i < valid_count; i++) {
fmc_status = fmc_word_program(
EEPROM_START_ADDRESS + i * sizeof(uint32_t), valid_entries[i]);
if (fmc_status != FMC_READY) {
return FLASH_EEPROM_WRITE_ERROR;
}
}
return FLASH_EEPROM_OK;
}
/**
* @brief 更新参数并保存到Flash
* @param kp 新的KP值
* @param ki 新的KI值
* @param kd 新的KD值
*/
void FlashEEPROM::update_pid_parameters(uint16_t kp, uint16_t ki, uint16_t kd) {
write(ADDR_MOTOR_KP, kp);
write(ADDR_MOTOR_KI, ki);
write(ADDR_MOTOR_KD, kd);
}
/**
* @brief 更新系统配置并保存到Flash
* @param config 新的系统配置值
*/
void FlashEEPROM::update_system_config(uint16_t config) {
write(ADDR_SYSTEM_CONFIG, config);
}
/**
* @brief 保存角度数据到Flash
* @param angle 角度数据
* @return FlashEepromStatus: 操作状态
*/
FlashEepromStatus FlashEEPROM::save_angle(float angle,
bool saved) { // 解锁Flash
// 解锁Flash
fmc_unlock();
// 读取保存的参数
u16 angle2;
// 尝试从Flash读取参数，如果不存在则使用默认值
if (read(ADDR_ANGLE_DATA, &angle2) != FLASH_EEPROM_OK) {
angle = 0; // 默认值
std::printf("角度初始化为: %d\r\n", angle2);
} else {
std::printf("从Flash读取到角度数: %d\r\n", angle2);
}
// 读取保存的参数
if (!angle_saved) {
printf("saved_angle: %u\r\n", (u16)angle);
FlashEepromStatus status = write(ADDR_ANGLE_DATA, (u16)angle);
if (status == FLASH_EEPROM_OK) {
angle_saved = saved; // 标记已保存过
CAN0_transmiter.EEPROMStatus = 0; // 设置EEPROM状态
} else {
CAN0_transmiter.EEPROMStatus = 1; // 设置EEPROM错误状态
}
CAN0_transmiter.testfloat_union.f = status; // CAN发送测试
return status;
}
return FLASH_EEPROM_OK; // 已保存过，直接返回
}
/**
* @brief 读取角度数据从Flash
* @return float: 角度数据
*/
float FlashEEPROM::read_angle() {
u16 angle = 0;
if (read(ADDR_ANGLE_DATA, &angle) == FLASH_EEPROM_OK) {
return (float)angle;
}
return 0.0f; // 默认返回0度
}
/**
* @brief 增加错误计数并保存到Flash
*/
void FlashEEPROM::increment_error_count(void) {
uint16_t error_count = 0;
// 读取当前错误计数
FlashEepromStatus read_status = read(ADDR_ERROR_COUNT, &error_count);
if (read_status == FLASH_EEPROM_OK ||
read_status == FLASH_EEPROM_NOT_FOUND) {
// 增加错误计数（如果是NOT_FOUND，则error_count已经是0）
error_count++;
// 保存更新后的错误计数
write(ADDR_ERROR_COUNT, error_count);
}
}
/**
* @brief 示例主函数 - 展示Flash EEPROM的使用方法
*/
void flash_eeprom_example_main(void) {
std::printf("\r\n=== Flash EEPROM 使用示例 ===\r\n");
// 初始化Flash EEPROM
flashEEP.init();
// 模拟参数更新
std::printf("\r\n--- 更新PID参数 ---\r\n");
flashEEP.update_pid_parameters(120, 80, 30);
// 模拟系统配置更新
std::printf("\r\n--- 更新系统配置 ---\r\n");
flashEEP.update_system_config(0x03);
// 模拟错误发生
std::printf("\r\n--- 模拟错误计数增加 ---\r\n");
flashEEP.increment_error_count();
flashEEP.increment_error_count();
// 再次读取参数验证保存是否成功
std::printf("\r\n--- 验证保存的数据 ---\r\n");
uint16_t kp_value, ki_value, kd_value;
uint16_t system_config;
uint16_t error_count;
// if (flashEEP.read(ADDR_MOTOR_KP, &kp_value) == FLASH_EEPROM_OK) {
// std::printf("当前KP值: %d\r\n", kp_value);
// }
// if (flashEEP.read(ADDR_MOTOR_KI, &ki_value) == FLASH_EEPROM_OK) {
// std::printf("当前KI值: %d\r\n", ki_value);
// }
// if (flashEEP.read(ADDR_MOTOR_KD, &kd_value) == FLASH_EEPROM_OK) {
// std::printf("当前KD值: %d\r\n", kd_value);
// }
// if (flashEEP.read(ADDR_SYSTEM_CONFIG, &system_config) == FLASH_EEPROM_OK)
// { std::printf("当前系统配置: 0x%02X\r\n", system_config);
// }
// if (flashEEP.read(ADDR_ERROR_COUNT, &error_count) == FLASH_EEPROM_OK) {
// std::printf("当前错误计数: %d\r\n", error_count);
// }
std::printf("\r\n=== 示例结束 ===\r\n");
}
Robot_V2/sys/flash.h
+64
View File
@@ -0,0 +1,64 @@
#ifndef __FLASH_H
#define __FLASH_H
#include "gd32c10x.h"
#include <cstdint>
// EEPROM操作状态
typedef enum {
FLASH_EEPROM_OK = 0, // 操作成功
FLASH_EEPROM_ERROR, // 操作失败
FLASH_EEPROM_NOT_FOUND, // 未找到指定数据
FLASH_EEPROM_INVALID_ADDR, // 无效地址
FLASH_EEPROM_WRITE_ERROR, // 写入错误
FLASH_EEPROM_ERASE_ERROR // 擦除错误
} FlashEepromStatus;
// Flash EEPROM类
class FlashEEPROM {
public:
// EEPROM emulation constants
static constexpr uint32_t EEPROM_START_ADDRESS =
0x0801F800; // Flash最后一页(2KB)
static constexpr uint32_t EEPROM_END_ADDRESS = 0x08020000; // Flash结束地址
static constexpr uint32_t EEPROM_PAGE_SIZE = 0x800; // 页大小2KB
static constexpr uint32_t EEPROM_MAX_SIZE = 0x800; // EEPROM最大容量
// 定义用户自定义的虚拟地址
static constexpr uint16_t ADDR_MOTOR_KP = 0x0010; // 电机KP参数地址
static constexpr uint16_t ADDR_MOTOR_KI = 0x0011; // 电机KI参数地址
static constexpr uint16_t ADDR_MOTOR_KD = 0x0012; // 电机KD参数地址
static constexpr uint16_t ADDR_SYSTEM_CONFIG = 0x0020; // 系统配置参数地址
static constexpr uint16_t ADDR_ERROR_COUNT = 0x0030; // 错误计数地址
// 角度数据存储地址
static constexpr u16 ADDR_ANGLE_DATA = 0x0021;
bool angle_saved = false;
// 构造函数
FlashEEPROM();
// 初始化Flash EEPROM模拟
u8 init();
// 从Flash EEPROM读取数据
FlashEepromStatus read(uint16_t virtualAddr, uint16_t *data) const;
// 向Flash EEPROM写入数据
FlashEepromStatus write(uint16_t virtualAddr, uint16_t data);
// 格式化Flash EEPROM页面
FlashEepromStatus format();
void flash_eeprom_example_init(void);
void update_pid_parameters(uint16_t kp, uint16_t ki, uint16_t kd);
void update_system_config(uint16_t config);
void increment_error_count(void);
FlashEepromStatus save_angle(float angle,bool saved);
float read_angle();
private:
};
void flash_eeprom_example_main(void);
#endif /* __FLASH_H */
Robot_V2/sys/health.cpp
+15
View File
@@ -2,11 +2,13 @@
#include "Can.h"
#include "DengFOC.h"
#include "adc.h"
#include "flash.h"
#include <stdint.h>
extern sampler sampler;
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controlroller FOC;
extern FlashEEPROM flashEEP;
ThermalController thermalController; // 热量控制器初始化
#define max(a, b, c) \
@@ -85,4 +87,17 @@ void angsens_MG_check(void) {
bool MGH = gpio_input_bit_get(GPIOB, GPIO_PIN_10);
bool MGL = gpio_input_bit_get(GPIOB, GPIO_PIN_11);
CAN0_transmiter.MagStatus = MGH or MGL;
}
void power_manage(void) {
float last_vbus = sampler.get_vbus_voltage();
sampler.update_vbus_voltage();
FOC.update_power();
float vbus = sampler.get_vbus_voltage();
if ((vbus < 22.0f) and (last_vbus > vbus) and vbus > 8.0f) {
FOC.shut_down();
// flashEEP.save_angle(FOC.rt_angle,false);
} else {
FOC.resume();
}
}
Robot_V2/sys/health.h
+1
View File
@@ -35,5 +35,6 @@ class ThermalController {
};
void angsens_MG_check(void);
void power_manage(void);
#endif
Robot_V2/sys/initial.cpp
+15
View File
@@ -1,4 +1,6 @@
#include "initial.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_rcu.h"
#include <stdint.h>
@@ -31,3 +33,16 @@ int fputc(int ch, FILE *f) throw() {
;
return ch;
}
void unused_pin_init() {
rcu_periph_clock_enable(RCU_GPIOA); // 使能GPIOA时钟
rcu_periph_clock_enable(RCU_GPIOB); // 使能GPIOB时钟
rcu_periph_clock_enable(RCU_GPIOC); // 使能GPIOC时钟
gpio_init(GPIOA, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, GPIO_PIN_8|GPIO_PIN_11|GPIO_PIN_12);
gpio_init(GPIOB, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, GPIO_PIN_6|GPIO_PIN_7|GPIO_PIN_15);
gpio_init(GPIOC, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, GPIO_PIN_13|GPIO_PIN_14|GPIO_PIN_15);
gpio_bit_reset(GPIOA, GPIO_PIN_8|GPIO_PIN_11|GPIO_PIN_12);
gpio_bit_reset(GPIOB, GPIO_PIN_6|GPIO_PIN_7|GPIO_PIN_15);
gpio_bit_reset(GPIOC, GPIO_PIN_13|GPIO_PIN_14|GPIO_PIN_15);
}
Robot_V2/sys/initial.h
+1
View File
@@ -19,6 +19,7 @@ int fputc(int ch, FILE *f); // 声明fputc函数以重载标准库实现
u8 USART0_init(void);
u8 watchdog_init(u32 time);
void unused_pin_init();
// 任务标志定义
typedef enum {
TASK_MOTOR_CONTROL = 0,