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

更新了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): 实现母线电压监测与保护策略新增电源管理函数，在母线电压异常下降时自动关闭电机输出，防止因供电不足导致设备损坏。
2025-12-16 22:39:39 +08:00
parent 839a32ff04
commit 7371f97882
19 changed files with 516 additions and 57 deletions
@@ -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");
+}