443 lines
12 KiB
C
443 lines
12 KiB
C
#include "app_param_store.h"
|
||
|
||
#include <string.h>
|
||
|
||
#include "Com_debug.h"
|
||
#include "app_CiA402.h"
|
||
#include "Int_EEPROM24xx.h"
|
||
#include "crc.h"
|
||
|
||
/* 参数块格式：
|
||
* 固定头(magic/version/len) + 业务参数 + CRC32
|
||
* 用于上电校验，防止读取到脏数据。
|
||
*/
|
||
#define APP_PARAM_MAGIC 0x5250414DU /* "MPAR" */
|
||
#define APP_PARAM_VERSION 0x0001u
|
||
#define APP_PARAM_EEPROM_ADDR 0x0000u
|
||
#define APP_PARAM_AUTOSAVE_DEBOUNCE_MS 500u
|
||
#define APP_PARAM_THRESHOLD_DEFAULT_MM 0.1f
|
||
|
||
/* Log level: 0=off, 1=error, 2=info */
|
||
#define APP_PARAM_STORE_LOG_LEVEL 2
|
||
#if APP_PARAM_STORE_LOG_LEVEL >= 2
|
||
#define APP_PS_LOGI(...) debug_printf(__VA_ARGS__)
|
||
#else
|
||
#define APP_PS_LOGI(...) ((void)0)
|
||
#endif
|
||
#if APP_PARAM_STORE_LOG_LEVEL >= 1
|
||
#define APP_PS_LOGE(...) debug_printf(__VA_ARGS__)
|
||
#else
|
||
#define APP_PS_LOGE(...) ((void)0)
|
||
#endif
|
||
|
||
typedef struct
|
||
{
|
||
/* blob = 二进制数据块（Binary Large Object）：
|
||
* 这里表示“整块写入/读出 EEPROM 的参数记录”。
|
||
*/
|
||
uint32_t magic;
|
||
uint16_t version;
|
||
uint16_t payload_len;
|
||
uint16_t acc_mm_s2;
|
||
uint16_t dec_mm_s2;
|
||
float step_loss_threshold_mm;
|
||
uint32_t crc32;
|
||
} App_ParamBlob_t;
|
||
|
||
static uint32_t g_eeprom_i2c_last_err = 0u;
|
||
static App_RunParams_t g_last_seen_params = {0};
|
||
static uint8_t g_last_seen_valid = 0u;
|
||
static uint8_t g_autosave_dirty = 0u;
|
||
static uint32_t g_autosave_due_ms = 0u;
|
||
|
||
/* 使用STM32硬件CRC外设计算CRC32。 */
|
||
static uint32_t App_ParamStore_Crc32(const uint8_t *data, uint32_t len)
|
||
{
|
||
uint32_t idx = 0u;
|
||
uint32_t word = 0u;
|
||
uint32_t crc = 0u;
|
||
uint8_t first_word = 1u;
|
||
|
||
if ((data == 0) || (len == 0u))
|
||
{
|
||
return 0u;
|
||
}
|
||
|
||
__HAL_CRC_DR_RESET(&hcrc);
|
||
|
||
while (idx < len)
|
||
{
|
||
uint8_t b0 = data[idx++];
|
||
uint8_t b1 = (idx < len) ? data[idx++] : 0u;
|
||
uint8_t b2 = (idx < len) ? data[idx++] : 0u;
|
||
uint8_t b3 = (idx < len) ? data[idx++] : 0u;
|
||
|
||
word = ((uint32_t)b0) |
|
||
((uint32_t)b1 << 8) |
|
||
((uint32_t)b2 << 16) |
|
||
((uint32_t)b3 << 24);
|
||
|
||
if (first_word != 0u)
|
||
{
|
||
crc = HAL_CRC_Calculate(&hcrc, &word, 1u);
|
||
first_word = 0u;
|
||
}
|
||
else
|
||
{
|
||
crc = HAL_CRC_Accumulate(&hcrc, &word, 1u);
|
||
}
|
||
}
|
||
|
||
return crc;
|
||
|
||
}
|
||
|
||
static bool App_ParamStore_Validate(const App_ParamBlob_t *blob)
|
||
{
|
||
uint32_t calc_crc;
|
||
|
||
/* 先做结构字段检查，再做CRC校验。 */
|
||
if (blob->magic != APP_PARAM_MAGIC)
|
||
{
|
||
return false;
|
||
}
|
||
if (blob->version != APP_PARAM_VERSION)
|
||
{
|
||
return false;
|
||
}
|
||
if (blob->payload_len != (uint16_t)(sizeof(App_ParamBlob_t) - sizeof(uint32_t)))
|
||
{
|
||
return false;
|
||
}
|
||
if ((blob->acc_mm_s2 == 0u) || (blob->dec_mm_s2 == 0u))
|
||
{
|
||
return false;
|
||
}
|
||
if ((blob->step_loss_threshold_mm <= 0.0f) || (blob->step_loss_threshold_mm > SOFT_LIMIT_MAX_MM))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
calc_crc = App_ParamStore_Crc32((const uint8_t *)blob, (uint32_t)(sizeof(App_ParamBlob_t) - sizeof(uint32_t)));
|
||
return (calc_crc == blob->crc32);
|
||
}
|
||
|
||
static uint32_t App_ParamStore_CalcBlobCrc(const App_ParamBlob_t *blob)
|
||
{
|
||
return App_ParamStore_Crc32((const uint8_t *)blob, (uint32_t)(sizeof(App_ParamBlob_t) - sizeof(uint32_t)));
|
||
}
|
||
|
||
static void App_ParamStore_ApplyThresholdToOD(float threshold_mm)
|
||
{
|
||
/* 将丢步阈值同步到OD，CANopen运行时需加锁访问。 */
|
||
OD_entry_t *entry = OD_find(OD, CIA402_INDEX_FOLLOWING_ERROR_WINDOW);
|
||
if (entry == 0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
if (CO != 0)
|
||
{
|
||
CO_LOCK_OD(CO->CANmodule);
|
||
(void)OD_set_f32(entry, 0, threshold_mm, true);
|
||
CO_UNLOCK_OD(CO->CANmodule);
|
||
}
|
||
else
|
||
{
|
||
(void)OD_set_f32(entry, 0, threshold_mm, true);
|
||
}
|
||
}
|
||
|
||
static void App_ParamStore_ApplyAccDecToOD(uint16_t acc_mm_s2, uint16_t dec_mm_s2)
|
||
{
|
||
/* 将加减速度同步到OD，保持网关与本地参数一致。 */
|
||
OD_entry_t *entry_acc = OD_find(OD, CIA402_INDEX_PROFILE_ACC);
|
||
OD_entry_t *entry_dec = OD_find(OD, CIA402_INDEX_PROFILE_DEC);
|
||
if ((entry_acc == 0) || (entry_dec == 0))
|
||
{
|
||
return;
|
||
}
|
||
|
||
if (CO != 0)
|
||
{
|
||
CO_LOCK_OD(CO->CANmodule);
|
||
(void)OD_set_u32(entry_acc, 0, (uint32_t)acc_mm_s2, true);
|
||
(void)OD_set_u32(entry_dec, 0, (uint32_t)dec_mm_s2, true);
|
||
CO_UNLOCK_OD(CO->CANmodule);
|
||
}
|
||
else
|
||
{
|
||
(void)OD_set_u32(entry_acc, 0, (uint32_t)acc_mm_s2, true);
|
||
(void)OD_set_u32(entry_dec, 0, (uint32_t)dec_mm_s2, true);
|
||
}
|
||
}
|
||
|
||
static bool App_ParamStore_ReadCurrentFromOD(App_RunParams_t *params)
|
||
{
|
||
OD_entry_t *entry_acc = OD_find(OD, CIA402_INDEX_PROFILE_ACC);
|
||
OD_entry_t *entry_dec = OD_find(OD, CIA402_INDEX_PROFILE_DEC);
|
||
OD_entry_t *entry_ferr = OD_find(OD, CIA402_INDEX_FOLLOWING_ERROR_WINDOW);
|
||
ODR_t odr_acc = ODR_DEV_INCOMPAT;
|
||
ODR_t odr_dec = ODR_DEV_INCOMPAT;
|
||
ODR_t odr_ferr = ODR_DEV_INCOMPAT;
|
||
uint32_t acc_raw = 0u;
|
||
uint32_t dec_raw = 0u;
|
||
float32_t ferr_raw = 0.0f;
|
||
float ferr_mm = APP_PARAM_THRESHOLD_DEFAULT_MM;
|
||
|
||
if ((params == 0) || (entry_acc == 0) || (entry_dec == 0) || (entry_ferr == 0))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (CO != 0)
|
||
{
|
||
CO_LOCK_OD(CO->CANmodule);
|
||
odr_acc = OD_get_u32(entry_acc, 0, &acc_raw, true);
|
||
odr_dec = OD_get_u32(entry_dec, 0, &dec_raw, true);
|
||
odr_ferr = OD_get_f32(entry_ferr, 0, &ferr_raw, true);
|
||
CO_UNLOCK_OD(CO->CANmodule);
|
||
}
|
||
else
|
||
{
|
||
odr_acc = OD_get_u32(entry_acc, 0, &acc_raw, true);
|
||
odr_dec = OD_get_u32(entry_dec, 0, &dec_raw, true);
|
||
odr_ferr = OD_get_f32(entry_ferr, 0, &ferr_raw, true);
|
||
}
|
||
|
||
if ((odr_acc != ODR_OK) || (odr_dec != ODR_OK) || (odr_ferr != ODR_OK))
|
||
{
|
||
APP_PS_LOGE("[EEPROM] read OD failed, odr_acc=%d odr_dec=%d odr_ferr=%d",
|
||
(int)odr_acc, (int)odr_dec, (int)odr_ferr);
|
||
return false;
|
||
}
|
||
|
||
if (acc_raw == 0u)
|
||
{
|
||
acc_raw = 1u;
|
||
}
|
||
if (dec_raw == 0u)
|
||
{
|
||
dec_raw = 1u;
|
||
}
|
||
if (acc_raw > 65535u)
|
||
{
|
||
acc_raw = 65535u;
|
||
}
|
||
if (dec_raw > 65535u)
|
||
{
|
||
dec_raw = 65535u;
|
||
}
|
||
|
||
ferr_mm = (float)ferr_raw;
|
||
if (!(ferr_mm == ferr_mm) || (ferr_mm <= 0.0f))
|
||
{
|
||
ferr_mm = APP_PARAM_THRESHOLD_DEFAULT_MM;
|
||
}
|
||
if (ferr_mm > SOFT_LIMIT_MAX_MM)
|
||
{
|
||
ferr_mm = SOFT_LIMIT_MAX_MM;
|
||
}
|
||
|
||
params->acc_mm_s2 = (uint16_t)acc_raw;
|
||
params->dec_mm_s2 = (uint16_t)dec_raw;
|
||
params->step_loss_threshold_mm = ferr_mm;
|
||
return true;
|
||
}
|
||
|
||
static uint8_t App_ParamStore_ParamsEqual(const App_RunParams_t *a, const App_RunParams_t *b)
|
||
{
|
||
float diff;
|
||
|
||
if ((a == 0) || (b == 0))
|
||
{
|
||
return 0u;
|
||
}
|
||
|
||
if ((a->acc_mm_s2 != b->acc_mm_s2) || (a->dec_mm_s2 != b->dec_mm_s2))
|
||
{
|
||
return 0u;
|
||
}
|
||
|
||
diff = a->step_loss_threshold_mm - b->step_loss_threshold_mm;
|
||
if (diff < 0.0f)
|
||
{
|
||
diff = -diff;
|
||
}
|
||
return (diff <= 0.0001f) ? 1u : 0u;
|
||
}
|
||
|
||
void App_ParamStore_Init(void)
|
||
{
|
||
App_RunParams_t current;
|
||
|
||
/* EEPROM驱动初始化：参数由Int_EEPROM24xx内部宏配置。 */
|
||
if (Int_EEPROM24xx_Init() != INT_EEPROM_OK)
|
||
{
|
||
g_eeprom_i2c_last_err = Int_EEPROM24xx_GetLastHalError();
|
||
APP_PS_LOGE("[EEPROM] init failed");
|
||
}
|
||
|
||
if (App_ParamStore_ReadCurrentFromOD(¤t))
|
||
{
|
||
g_last_seen_params = current;
|
||
g_last_seen_valid = 1u;
|
||
}
|
||
else
|
||
{
|
||
g_last_seen_valid = 0u;
|
||
}
|
||
g_autosave_dirty = 0u;
|
||
g_autosave_due_ms = 0u;
|
||
}
|
||
|
||
bool App_ParamStore_Read(App_RunParams_t *params)
|
||
{
|
||
App_ParamBlob_t blob;
|
||
uint32_t calc_crc;
|
||
|
||
if (params == 0)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
/* 从固定地址读出完整参数块并校验。 */
|
||
if (Int_EEPROM24xx_Read(APP_PARAM_EEPROM_ADDR, (uint8_t *)&blob, (uint16_t)sizeof(blob)) != INT_EEPROM_OK)
|
||
{
|
||
g_eeprom_i2c_last_err = Int_EEPROM24xx_GetLastHalError();
|
||
APP_PS_LOGE("[EEPROM] read failed, i2c_err=0x%08lX", (unsigned long)g_eeprom_i2c_last_err);
|
||
return false;
|
||
}
|
||
|
||
if (!App_ParamStore_Validate(&blob))
|
||
{
|
||
calc_crc = App_ParamStore_CalcBlobCrc(&blob);
|
||
APP_PS_LOGI("[EEPROM] invalid blob: magic=0x%08lX ver=%u len=%u acc=%u dec=%u ferr=%.4f crc=0x%08lX calc=0x%08lX",
|
||
(unsigned long)blob.magic,
|
||
(unsigned int)blob.version,
|
||
(unsigned int)blob.payload_len,
|
||
(unsigned int)blob.acc_mm_s2,
|
||
(unsigned int)blob.dec_mm_s2,
|
||
(double)blob.step_loss_threshold_mm,
|
||
(unsigned long)blob.crc32,
|
||
(unsigned long)calc_crc);
|
||
return false;
|
||
}
|
||
|
||
params->acc_mm_s2 = blob.acc_mm_s2;
|
||
params->dec_mm_s2 = blob.dec_mm_s2;
|
||
params->step_loss_threshold_mm = blob.step_loss_threshold_mm;
|
||
return true;
|
||
}
|
||
|
||
bool App_ParamStore_LoadAndApply(void)
|
||
{
|
||
App_RunParams_t p;
|
||
|
||
/* 上电读取成功后，应用到运行参数与OD对象。 */
|
||
if (!App_ParamStore_Read(&p))
|
||
{
|
||
APP_PS_LOGI("[EEPROM] no valid params in EEPROM, keep defaults");
|
||
return false;
|
||
}
|
||
|
||
stepper_1.acc = p.acc_mm_s2;
|
||
stepper_1.dec = p.dec_mm_s2;
|
||
App_ParamStore_ApplyAccDecToOD(p.acc_mm_s2, p.dec_mm_s2);
|
||
App_ParamStore_ApplyThresholdToOD(p.step_loss_threshold_mm);
|
||
|
||
APP_PS_LOGI("[EEPROM] loaded acc=%u dec=%u ferr=%.4f",
|
||
(unsigned int)p.acc_mm_s2,
|
||
(unsigned int)p.dec_mm_s2,
|
||
(double)p.step_loss_threshold_mm);
|
||
g_last_seen_params = p;
|
||
g_last_seen_valid = 1u;
|
||
g_autosave_dirty = 0u;
|
||
return true;
|
||
}
|
||
|
||
bool App_ParamStore_Save(const App_RunParams_t *params)
|
||
{
|
||
App_ParamBlob_t blob;
|
||
|
||
/* 写入前先做业务范围检查。 */
|
||
if (params == 0)
|
||
{
|
||
return false;
|
||
}
|
||
if ((params->acc_mm_s2 == 0u) || (params->dec_mm_s2 == 0u))
|
||
{
|
||
return false;
|
||
}
|
||
if ((params->step_loss_threshold_mm <= 0.0f) || (params->step_loss_threshold_mm > SOFT_LIMIT_MAX_MM))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
memset(&blob, 0, sizeof(blob));
|
||
blob.magic = APP_PARAM_MAGIC;
|
||
blob.version = APP_PARAM_VERSION;
|
||
blob.payload_len = (uint16_t)(sizeof(App_ParamBlob_t) - sizeof(uint32_t));
|
||
blob.acc_mm_s2 = params->acc_mm_s2;
|
||
blob.dec_mm_s2 = params->dec_mm_s2;
|
||
blob.step_loss_threshold_mm = params->step_loss_threshold_mm;
|
||
blob.crc32 = App_ParamStore_Crc32((const uint8_t *)&blob, (uint32_t)(sizeof(App_ParamBlob_t) - sizeof(uint32_t)));
|
||
|
||
/* 写入固定地址，供下次上电加载。 */
|
||
if (Int_EEPROM24xx_Write(APP_PARAM_EEPROM_ADDR, (const uint8_t *)&blob, (uint16_t)sizeof(blob)) != INT_EEPROM_OK)
|
||
{
|
||
g_eeprom_i2c_last_err = Int_EEPROM24xx_GetLastHalError();
|
||
APP_PS_LOGE("[EEPROM] save failed, i2c_err=0x%08lX", (unsigned long)g_eeprom_i2c_last_err);
|
||
return false;
|
||
}
|
||
|
||
APP_PS_LOGI("[EEPROM] saved acc=%u dec=%u ferr=%.4f",
|
||
(unsigned int)params->acc_mm_s2,
|
||
(unsigned int)params->dec_mm_s2,
|
||
(double)params->step_loss_threshold_mm);
|
||
return true;
|
||
}
|
||
|
||
void App_ParamStore_Process(void)
|
||
{
|
||
App_RunParams_t current;
|
||
uint32_t now_ms;
|
||
|
||
if (!App_ParamStore_ReadCurrentFromOD(¤t))
|
||
{
|
||
return;
|
||
}
|
||
|
||
now_ms = HAL_GetTick();
|
||
|
||
if (g_last_seen_valid == 0u)
|
||
{
|
||
g_last_seen_params = current;
|
||
g_last_seen_valid = 1u;
|
||
return;
|
||
}
|
||
|
||
if (App_ParamStore_ParamsEqual(¤t, &g_last_seen_params) == 0u)
|
||
{
|
||
g_last_seen_params = current;
|
||
g_autosave_dirty = 1u;
|
||
g_autosave_due_ms = now_ms + APP_PARAM_AUTOSAVE_DEBOUNCE_MS;
|
||
return;
|
||
}
|
||
|
||
if (g_autosave_dirty == 0u)
|
||
{
|
||
return;
|
||
}
|
||
|
||
if ((int32_t)(now_ms - g_autosave_due_ms) < 0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
if (App_ParamStore_Save(&g_last_seen_params))
|
||
{
|
||
g_autosave_dirty = 0u;
|
||
}
|
||
}
|