feat(FOC): 优化FOC控制算法并修复多个控制环路问题

- 重构cal_Iq_Id函数，移除电角度参数传递，改用预计算的sin/cos值以避免重复三角函数计算，提升性能约15-300周期 - 修复角度同步Uq前馈学习中的运动方向判断逻辑，解决反转时的震荡问题和正转正常反转震荡的根本原因 - 实现低速缩放因子函数get_mode3_velocity_scale，改进静摩擦 hunting抑制效果 - 优化电压检测逻辑，将阻塞式ADC读取改为连续模式零等待，提升实时性能 - 修复初始化期间电压分母为零的问题，添加下限保护防止P增益无穷大 - 实现PID控制器测量微分功能，消除目标阶跃时的微分冲击 - 改进积分抗饱和策略，添加外部饱和泄放功能，优化到位稳定性能 perf(Filter): 优化中值滤波算法性能 - 针对ADC_FILTER_SIZE=6的固定场景实现最优排序网络，用12次比较替代通用std::nth_element算法，显著提升性能 - 保留通用回退路径处理其他尺寸情况
2026-09-01 02:46:25 +08:00
parent 76f7987238
commit 7c4aaa5ee6
30 changed files with 1127 additions and 783 deletions
@@ -491,17 +491,6 @@ FlashEepromStatus FlashEEPROM::save_system_config(bool saved) {
 	return FLASH_EEPROM_OK;
 }

-/**
- * @brief 读取角度数据从 Flash
- * @return float: 角度数据
- */
-float FlashEEPROM::read_angle() {
-	float angle = DEFAULT_VALUE;
-	if (readFloat(ADDR_ANGLE, &angle) != FLASH_EEPROM_OK) {
-		angle = DEFAULT_VALUE;
-	}
-	return angle;
-}

 /**
 * @brief 保存电角度零位校准角到 Flash
@@ -530,6 +519,35 @@ static u32 calculate_node_id_crc(u16 node_id, u32 counter) {
 	return crc_block_data_calculate(crc_data, 2);
 }

+/*!
+ * @brief 比较写入计数器新旧（有符号差值比较，容忍 u32 回绕）
+ * @param a: 计数器 a
+ * @param b: 计数器 b
+ * @retval bool: a 比 b 新则为 true
+ * @note       计数器写入时跳过 0，只要两次写入间隔不超过 2^31 次即可正确比较
+ */
+static bool node_id_counter_newer(u32 a, u32 b) {
+	return (int32_t)(a - b) > 0;
+}
+
+/*!
+ * @brief 擦除单个 1KB Flash 页
+ * @param page_addr: 页起始地址（1KB 对齐）
+ * @retval FlashEepromStatus: 操作状态
+ */
+static FlashEepromStatus erase_flash_page(u32 page_addr) {
+	fmc_unlock();
+	fmc_state_enum fmc_status = fmc_page_erase(page_addr);
+	if (fmc_status != FMC_READY) {
+		fmc_lock();
+		EEP_DEBUG_PRINT("Page erase failed at 0x%08X, status=%d\r\n",
+						page_addr, fmc_status);
+		return FLASH_EEPROM_ERASE_ERROR;
+	}
+	fmc_lock();
+	return FLASH_EEPROM_OK;
+}
+
 /*!
 * @brief 从指定地址读取节点 ID 数据结构
 * @param addr: Flash 地址
@@ -618,60 +636,87 @@ static FlashEepromStatus write_node_id_struct(u32 addr, u16 node_id, u32 counter
 }

 /*!
- * @brief 擦除节点 ID 存储页
+ * @brief 擦除节点 ID 存储区（主备份页 + 备用备份页，恢复默认值）
 * @retval FlashEepromStatus: 操作状态
+ * @note       仅用于恢复出厂/调试场景，会同时清除两份备份
 */
 FlashEepromStatus FlashEEPROM::eraseNodeId() {
-	fmc_state_enum fmc_status;
-	
-	EEP_DEBUG_PRINT("Erasing Node ID page at 0x%08X\r\n", NODE_ID_PAGE_ADDR);
-	
-	fmc_unlock();
-	fmc_status = fmc_page_erase(NODE_ID_PAGE_ADDR);
-	if (fmc_status != FMC_READY) {
-		fmc_lock();
-		EEP_DEBUG_PRINT("Node ID page erase failed, status=%d\r\n", fmc_status);
-		return FLASH_EEPROM_ERASE_ERROR;
+	EEP_DEBUG_PRINT("Erasing Node ID pages at 0x%08X / 0x%08X\r\n",
+					NODE_ID_PRIMARY_ADDR, NODE_ID_BACKUP_ADDR);
+
+	FlashEepromStatus status = erase_flash_page(NODE_ID_PRIMARY_ADDR);
+	if (status != FLASH_EEPROM_OK) {
+		return status;
 	}
-	fmc_lock();
-	
-	return FLASH_EEPROM_OK;
+	return erase_flash_page(NODE_ID_BACKUP_ADDR);
 }

 /*!
- * @brief 从 Flash 读取节点 ID
+ * @brief 从 Flash 读取节点 ID（双重备份回退机制）
 * @param node_id: 输出节点 ID 值
 * @retval FlashEepromStatus: 操作状态
+ * @note       读取主备份与备用备份两份记录：
+ *             - 两份均有效：取写入计数器更新的一份
+ *             - 仅一份有效：回退使用该份（另一份损坏或未写入）
+ *             - 均无效：返回 DATA_NOT_FOUND，由调用方使用默认值
 */
 FlashEepromStatus FlashEEPROM::readNodeId(u16 *node_id) const {
 	if (node_id == nullptr) {
 		return FLASH_EEPROM_INVALID_ADDR;
 	}

-	u16 id_value = 0;
-	u32 counter = 0;
+	u16 id_primary = 0, id_backup = 0;
+	u32 counter_primary = 0, counter_backup = 0;

-	// 从主存储区读取
-	FlashEepromStatus status = read_node_id_struct(NODE_ID_PRIMARY_ADDR, &id_value, &counter);
-	
-	if (status == FLASH_EEPROM_OK) {
-		*node_id = id_value;
-		EEP_DEBUG_PRINT("Node ID read from primary (counter=%u): %d\r\n", counter, *node_id);
-		return FLASH_EEPROM_OK;
+	FlashEepromStatus status_primary = read_node_id_struct(
+		NODE_ID_PRIMARY_ADDR, &id_primary, &counter_primary);
+	FlashEepromStatus status_backup = read_node_id_struct(
+		NODE_ID_BACKUP_ADDR, &id_backup, &counter_backup);
+
+	if (status_primary == FLASH_EEPROM_OK &&
+		status_backup == FLASH_EEPROM_OK) {
+		// 双备份均有效：取计数器更新的一份（计数器相同时优先主备份）
+		if (node_id_counter_newer(counter_backup, counter_primary)) {
+			*node_id = id_backup;
+			EEP_DEBUG_PRINT("Node ID read from backup (counter=%u): %d\r\n",
+							counter_backup, *node_id);
+		} else {
+			*node_id = id_primary;
+			EEP_DEBUG_PRINT("Node ID read from primary (counter=%u): %d\r\n",
+							counter_primary, *node_id);
+		}
+	} else if (status_primary == FLASH_EEPROM_OK) {
+		// 主备份有效，备用备份损坏/为空：回退主备份
+		*node_id = id_primary;
+		EEP_DEBUG_PRINT(
+			"Node ID fallback to primary (backup status=%d): %d\r\n",
+			status_backup, *node_id);
+	} else if (status_backup == FLASH_EEPROM_OK) {
+		// 主备份损坏/为空，备用备份有效：回退备用备份
+		*node_id = id_backup;
+		EEP_DEBUG_PRINT(
+			"Node ID fallback to backup (primary status=%d): %d\r\n",
+			status_primary, *node_id);
 	} else {
-		EEP_DEBUG_PRINT("Node ID read failed from primary: status=%d\r\n", status);
+		EEP_DEBUG_PRINT("Node ID not found in both slots (primary=%d, backup=%d)\r\n",
+						status_primary, status_backup);
 		return FLASH_EEPROM_DATA_NOT_FOUND;
 	}
+	return FLASH_EEPROM_OK;
 }

 /*!
- * @brief 保存节点 ID 到 Flash（每次写入前先擦除整个页面，写入后校验）
+ * @brief 保存节点 ID 到 Flash（乒乓式双重备份，写入后校验）
 * @param node_id: 要保存的节点 ID 值
 * @retval FlashEepromStatus: 操作状态
- * @note       由于Flash只能将1写为0，不能将0写为1，因此每次写入新数据前
- *             需要先擦除整个页面。简化方案：不再使用备用备份区，
- *             每次写入前擦除页面，然后写入主存储区。
- *             写入后立即校验数据，如果不对则再次写入，最多3次。
+ * @note       主备份与备用备份分属两个独立 1KB Flash 页。写入流程：
+ *             1. 读取两份备份，定位计数器最新的有效记录作为基准
+ *             2. 选择"非最新"的一页作为写入目标（乒乓交替）
+ *             3. 只擦除并写入目标页，另一页全程保有旧的有效记录，
+ *                掉电时至少一份数据存活
+ *             4. 写入后立即校验，失败则重试，最多3次
+ *             某一页损坏时无需专门修复：下一次写入目标即切换到该页，
+ *             擦除+重写天然完成自愈。
 */
 FlashEepromStatus FlashEEPROM::writeNodeId(u16 node_id) {
 	if (node_id > MAX_NODE_ID) {
@@ -679,34 +724,59 @@ FlashEepromStatus FlashEEPROM::writeNodeId(u16 node_id) {
 		return FLASH_EEPROM_INVALID_ADDR;
 	}

-	// 获取当前写入计数器（从主存储区读取）
-	u16 current_id = 0;
-	u32 current_counter = 0;
-	
-	FlashEepromStatus primary_status = read_node_id_struct(NODE_ID_PRIMARY_ADDR, &current_id, &current_counter);
-	
-	// 计数器 +1
-	u32 new_counter = current_counter + 1;
-	// 如果计数器溢出（0xFFFFFFFF + 1 = 0），则使用1
+	// 读取两份备份，确定当前最新有效记录（计数器基准 + 乒乓目标选择）
+	u16 id_primary = 0, id_backup = 0;
+	u32 counter_primary = 0, counter_backup = 0;
+
+	FlashEepromStatus status_primary = read_node_id_struct(
+		NODE_ID_PRIMARY_ADDR, &id_primary, &counter_primary);
+	FlashEepromStatus status_backup = read_node_id_struct(
+		NODE_ID_BACKUP_ADDR, &id_backup, &counter_backup);
+
+	bool primary_latest = false;
+	bool backup_latest = false;
+	if (status_primary == FLASH_EEPROM_OK &&
+		status_backup == FLASH_EEPROM_OK) {
+		// 两份均有效：计数器新的为最新（相同时视为主备份最新）
+		if (node_id_counter_newer(counter_backup, counter_primary)) {
+			backup_latest = true;
+		} else {
+			primary_latest = true;
+		}
+	} else if (status_primary == FLASH_EEPROM_OK) {
+		primary_latest = true;
+	} else if (status_backup == FLASH_EEPROM_OK) {
+		backup_latest = true;
+	}
+
+	// 计数器 +1（跳过 0；两份均无效时从 1 开始）
+	u32 new_counter = 1;
+	if (primary_latest) {
+		new_counter = counter_primary + 1;
+	} else if (backup_latest) {
+		new_counter = counter_backup + 1;
+	}
 	if (new_counter == 0) {
 		new_counter = 1;
 	}

-	// 先擦除整个页面
-	EEP_DEBUG_PRINT("Erasing Node ID page before write\r\n");
-	FlashEepromStatus status = eraseNodeId();
-	if (status != FLASH_EEPROM_OK) {
-		return status;
-	}
+	// 乒乓写入：目标为"非最新有效"的一页，最新有效的一页保持不动
+	u32 target_addr = primary_latest ? NODE_ID_BACKUP_ADDR : NODE_ID_PRIMARY_ADDR;

-	// 写入并校验，最多3次
+	// 写入并校验，最多3次（重试期间另一页的旧记录始终有效）
 	/** @brief MAX_WRITE_RETRY 宏定义 */
 	#define MAX_WRITE_RETRY 3
 	for (u8 retry = 0; retry < MAX_WRITE_RETRY; retry++) {
-		// 写入主存储区
-		EEP_DEBUG_PRINT("Writing to primary (counter=%u, retry=%d)\r\n", new_counter, retry);
-		FlashEepromStatus write_status = write_node_id_struct(NODE_ID_PRIMARY_ADDR, node_id, new_counter);
+		EEP_DEBUG_PRINT("Writing to 0x%08X (counter=%u, retry=%d)\r\n",
+						target_addr, new_counter, retry);

+		FlashEepromStatus status = erase_flash_page(target_addr);
+		if (status != FLASH_EEPROM_OK) {
+			continue;
+		}
+
+		FlashEepromStatus write_status =
+			write_node_id_struct(target_addr, node_id, new_counter);
 		if (write_status != FLASH_EEPROM_OK) {
 			EEP_DEBUG_PRINT("Node ID write failed: status=%d\r\n", write_status);
 			continue;
@@ -715,20 +785,18 @@ FlashEepromStatus FlashEEPROM::writeNodeId(u16 node_id) {
 		// 立即校验数据
 		u16 read_id = 0;
 		u32 read_counter = 0;
-		FlashEepromStatus read_status = read_node_id_struct(NODE_ID_PRIMARY_ADDR, &read_id, &read_counter);
-		
-		if (read_status == FLASH_EEPROM_OK && read_id == node_id && read_counter == new_counter) {
-			EEP_DEBUG_PRINT("Node ID saved and verified successfully: %d (counter=%u)\r\n", node_id, new_counter);
+		FlashEepromStatus read_status =
+			read_node_id_struct(target_addr, &read_id, &read_counter);
+
+		if (read_status == FLASH_EEPROM_OK && read_id == node_id &&
+			read_counter == new_counter) {
+			EEP_DEBUG_PRINT("Node ID saved and verified: %d (counter=%u, slot=0x%08X)\r\n",
+							node_id, new_counter, target_addr);
 			return FLASH_EEPROM_OK;
-		} else {
-			EEP_DEBUG_PRINT("Node ID verification failed: read_id=%d (expected=%d), read_counter=%u (expected=%u)\r\n",
-							read_id, node_id, read_counter, new_counter);
-			// 校验失败，重新擦除并写入
-			if (retry < MAX_WRITE_RETRY - 1) {
-				EEP_DEBUG_PRINT("Retrying...\r\n");
-				eraseNodeId();
-			}
 		}
+		EEP_DEBUG_PRINT("Node ID verify failed: read_id=%d (expected=%d), "
+						"read_counter=%u (expected=%u)\r\n",
+						read_id, node_id, read_counter, new_counter);
 	}

 	EEP_DEBUG_PRINT("Node ID save failed after %d retries\r\n", MAX_WRITE_RETRY);
@@ -741,7 +809,9 @@ FlashEepromStatus FlashEEPROM::writeNodeId(u16 node_id) {
 * @return FlashEepromStatus: 状态
 */
 FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) {
-	static u32 next_address;
+	// 每次调用重新扫描可写入位置，不跨调用残留旧地址
+	// （循环至少执行一次，next_address 必被赋值）
+	u32 next_address = EEPROM_START_ADDRESS;
 	// 遍历 Flash 页面寻找下一个可写入 crc 的位置
 	for (u32 address = EEPROM_DATA_START_ADDRESS - SIZE_U32;
 		 address > EEPROM_START_ADDRESS; address -= SIZE_U32) {
@@ -933,6 +1003,8 @@ void jumpToBOOT() {
 	uint32_t reset_handler = *(__IO uint32_t *)(BOOTLOADER_START_ADDR + 4);
 	typedef void (*AppEntryFunc)(void);
 	AppEntryFunc app_entry = (AppEntryFunc)reset_handler;
+	// 跳转前喂狗，给 BOOT 留出一个完整看门狗周期做早期初始化
+	fwdgt_counter_reload();
 	__set_MSP(msp_value);
 	// 7. 跳转
 	app_entry();