cbb
7c4aaa5ee6
- 重构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算法，显著提升性能 - 保留通用回退路径处理其他尺寸情况
347 lines
10 KiB
C++
347 lines
10 KiB
C++
/**
|
|
* @file sys/initial.cpp
|
|
* @brief 系统服务与平台支持源文件
|
|
* @note 本文件仅补充 Doxygen 文档注释，不改变代码逻辑。
|
|
*/
|
|
|
|
#include "initial.h"
|
|
#include "Can.h"
|
|
#include "config.h"
|
|
#include "flash.h"
|
|
#include "gd32c10x.h"
|
|
#include "gd32c10x_fmc.h"
|
|
#include "gd32c10x_gpio.h"
|
|
#include "gd32c10x_rcu.h"
|
|
#include "initial.h"
|
|
#include <cstdint>
|
|
#include <cstdio>
|
|
#include <cstring>
|
|
|
|
// OTA 调试宏定义
|
|
#if OTA_DEBUG
|
|
/** @brief OTA_DEBUG_PRINT 宏定义 */
|
|
#define OTA_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
|
|
#else
|
|
/** @brief OTA_DEBUG_PRINT 宏定义 */
|
|
#define OTA_DEBUG_PRINT(fmt, ...) ((void)0)
|
|
#endif
|
|
|
|
// EEP 调试宏定义
|
|
#if EEP_DEBUG
|
|
/** @brief EEP_DEBUG_PRINT 宏定义 */
|
|
#define EEP_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
|
|
#else
|
|
/** @brief EEP_DEBUG_PRINT 宏定义 */
|
|
#define EEP_DEBUG_PRINT(fmt, ...) ((void)0)
|
|
#endif
|
|
|
|
extern CAN_transmiter CAN0_transmiter; // CAN 初始化
|
|
// 全局版本管理器单例：构造函数只复制 constexpr 版本号常量，不依赖外设初始化。
|
|
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);
|
|
}
|
|
|
|
/**
|
|
* @brief 获取 getSoftwareVersion 对应的数据或状态
|
|
* @return VersionManager::VersionInfo 返回值
|
|
*/
|
|
VersionManager::VersionInfo VersionManager::getSoftwareVersion() const {
|
|
return softwareVersion;
|
|
}
|
|
|
|
/**
|
|
* @brief 获取 getHardwareVersion 对应的数据或状态
|
|
* @return VersionManager::VersionInfo 返回值
|
|
*/
|
|
VersionManager::VersionInfo VersionManager::getHardwareVersion() const {
|
|
return hardwareVersion;
|
|
}
|
|
|
|
/**
|
|
* @brief 获取 getBootVersion 对应的数据或状态
|
|
* @return VersionManager::VersionInfo 返回值
|
|
*/
|
|
VersionManager::VersionInfo VersionManager::getBootVersion() const {
|
|
return bootVersion;
|
|
}
|
|
|
|
/**
|
|
* @brief 设置 setSoftwareVersion 对应的参数或状态
|
|
* @param[in] major 参数
|
|
* @param[in] minor 参数
|
|
* @param[in] patch 参数
|
|
* @param[in] build 参数
|
|
* @return void
|
|
*/
|
|
void VersionManager::setSoftwareVersion(u16 major, u16 minor, u16 patch,
|
|
u16 build) {
|
|
softwareVersion.major = major;
|
|
softwareVersion.minor = minor;
|
|
softwareVersion.patch = patch;
|
|
softwareVersion.build = build;
|
|
}
|
|
|
|
/**
|
|
* @brief 设置 setHardwareVersion 对应的参数或状态
|
|
* @param[in] major 参数
|
|
* @param[in] minor 参数
|
|
* @param[in] patch 参数
|
|
* @param[in] build 参数
|
|
* @return void
|
|
*/
|
|
void VersionManager::setHardwareVersion(u16 major, u16 minor, u16 patch,
|
|
u16 build) {
|
|
hardwareVersion.major = major;
|
|
hardwareVersion.minor = minor;
|
|
hardwareVersion.patch = patch;
|
|
hardwareVersion.build = build;
|
|
}
|
|
|
|
/**
|
|
* @brief 设置 setBootVersion 对应的参数或状态
|
|
* @param[in] major 参数
|
|
* @param[in] minor 参数
|
|
* @param[in] patch 参数
|
|
* @param[in] build 参数
|
|
* @return void
|
|
*/
|
|
void VersionManager::setBootVersion(u16 major, u16 minor, u16 patch,
|
|
u16 build) {
|
|
bootVersion.major = major;
|
|
bootVersion.minor = minor;
|
|
bootVersion.patch = patch;
|
|
bootVersion.build = build;
|
|
}
|
|
|
|
static void appendVersionChar(char *&out, size_t &remaining, char ch) {
|
|
if (remaining > 1U) {
|
|
*out++ = ch;
|
|
--remaining;
|
|
*out = '\0';
|
|
}
|
|
}
|
|
|
|
static void appendVersionU16(char *&out, size_t &remaining, u16 value) {
|
|
char digits[5]; // u16 最大值 65535，最多 5 位十进制数字
|
|
u8 count = 0;
|
|
|
|
do {
|
|
digits[count++] = static_cast<char>('0' + (value % 10U));
|
|
value = static_cast<u16>(value / 10U);
|
|
} while (value > 0U && count < sizeof(digits));
|
|
|
|
while (count > 0U) {
|
|
appendVersionChar(out, remaining, digits[--count]);
|
|
}
|
|
}
|
|
|
|
char *VersionManager::versionToString(const VersionInfo &version, char *buffer,
|
|
size_t bufSize) const {
|
|
if (buffer == nullptr) {
|
|
return nullptr;
|
|
}
|
|
if (bufSize == 0U) {
|
|
return buffer;
|
|
}
|
|
|
|
char *out = buffer;
|
|
size_t remaining = bufSize;
|
|
*out = '\0';
|
|
|
|
appendVersionU16(out, remaining, version.major);
|
|
appendVersionChar(out, remaining, '.');
|
|
appendVersionU16(out, remaining, version.minor);
|
|
appendVersionChar(out, remaining, '.');
|
|
appendVersionU16(out, remaining, version.patch);
|
|
appendVersionChar(out, remaining, '.');
|
|
appendVersionU16(out, remaining, version.build);
|
|
|
|
return buffer;
|
|
}
|
|
|
|
/**
|
|
* @brief sendVersionInfoViaCan 函数
|
|
* @return void
|
|
*/
|
|
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 发送函数发送版本信息，使用 build_can_id 构建带 node_id 的 CAN
|
|
// ID
|
|
CAN0_transmiter.CANFD_AsyncSend(
|
|
CAN0_transmiter.build_can_id(CANFD_VERSION_ID), 24, versionData);
|
|
}
|
|
|
|
/**
|
|
* @brief printVersions 函数
|
|
* @return void
|
|
*/
|
|
void VersionManager::printVersions() const {
|
|
char versionStr[32];
|
|
|
|
OTA_DEBUG_PRINT(
|
|
"Software Version: %s\r\n",
|
|
versionToString(getSoftwareVersion(), versionStr, sizeof(versionStr)));
|
|
OTA_DEBUG_PRINT(
|
|
"Hardware Version: %s\r\n",
|
|
versionToString(getHardwareVersion(), versionStr, sizeof(versionStr)));
|
|
OTA_DEBUG_PRINT(
|
|
"BOOT Version: %s\r\n",
|
|
versionToString(getBootVersion(), versionStr, sizeof(versionStr)));
|
|
}
|
|
|
|
|
|
/* retarget the C library printf function to the USART */
|
|
/**
|
|
* @brief 初始化 USART0 调试串口
|
|
* @return u8 返回值
|
|
*/
|
|
u8 USART0_init() {
|
|
gd_eval_com_init(EVAL_COM0);
|
|
return 1;
|
|
}
|
|
|
|
/*!
|
|
\brief configure the watchdog peripheral
|
|
\param[in] none
|
|
\param[out] none
|
|
\retval none
|
|
*/
|
|
u8 watchdog_init(u32 time) {
|
|
u8 status = 0;
|
|
/* configure FWDGT counter clock: 40KHz(IRC40K) / 64 = 0.625 KHz */
|
|
/* time = reload_value / clock_frequency */
|
|
status = fwdgt_config(time * 0.625, FWDGT_PSC_DIV64);
|
|
fwdgt_enable();
|
|
return status;
|
|
}
|
|
|
|
int fputc(int ch, FILE *f) throw() {
|
|
usart_data_transmit(EVAL_COM0, (u8)ch);
|
|
while (RESET == usart_flag_get(EVAL_COM0, USART_FLAG_TBE))
|
|
;
|
|
return ch;
|
|
}
|
|
|
|
/**
|
|
* @brief 初始化未使用引脚状态
|
|
* @return void
|
|
*/
|
|
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_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_14 | GPIO_PIN_15);
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief 初始化 OTA 与 A/B 分区状态
|
|
* @return 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 个配置项的值，便于调试
|
|
OTA_DEBUG_PRINT(
|
|
"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 刷入后第一次启动
|
|
OTA_DEBUG_PRINT("APP enabled\r\n");
|
|
move_app_data();
|
|
}
|
|
|
|
fmc_unlock();
|
|
OTA_SET(n_APP_LATEST_CHECK_ADDR, OTA_AVAL);
|
|
fmc_lock();
|
|
|
|
u8 transmit_data[2] = {0x0A, 0xAC};
|
|
// 确认 APP 可用
|
|
CAN0_transmiter.CANFD_AsyncSend(CANFD_OTA_ACK_ID, 2, transmit_data);
|
|
|
|
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;
|
|
|
|
if (back_aval == OTA_NOT_AVAL) { // 如果 BACK 不可用
|
|
OTA_DEBUG_PRINT("notaval\r\n");
|
|
move_app_data();
|
|
} else if (back_aval == OTA_AVAL &&
|
|
back_check == OTA_NOT_AVAL) { // BACK 已刷写，但未检查
|
|
OTA_DEBUG_PRINT("aval\r\n");
|
|
move_app_data();
|
|
} else {
|
|
EEP_DEBUG_PRINT("check\r\n");
|
|
}
|
|
}
|