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

fix(flash): 修改角度存储类型为float并更新读写逻辑

Browse Source 
将FlashEEPROM中角度数据的存储类型从u16改为float，并相应地更新了读写函数。
新增readFloat和writeFloat函数以支持浮点数存储，同时调整了虚拟地址类型为u8。
修改了save_angle和read_angle函数实现，确保角度值能正确保存与读取。
优化了格式化函数中的数据解析方式，提高数据存储可靠性。
This commit is contained in:
cbb
2025-12-17 02:34:08 +08:00
parent 02572df688
commit d6cf143e87
3 changed files with 169 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/device/Can.cpp
+1 -1
View File
@@ -696,7 +696,7 @@ void CAN_transmiter::communication_check() {
FOC.target_velocity = target_velocity_union.f; FOC.target_velocity = target_velocity_union.f;
} else if (receive_message.rx_efid == 0x00010301) { } else if (receive_message.rx_efid == 0x00010301) {
motor_reset(); motor_reset();
flashEEP.save_angle((u16)FOC.rt_angle, false); flashEEP.save_angle(FOC.rt_angle, false);
} else { } else {
printf("Unknown CAN ID: %X\n", receive_message.rx_efid); printf("Unknown CAN ID: %X\n", receive_message.rx_efid);
} }
Robot_V2/sys/flash.cpp
+140 -42
View File
@@ -2,8 +2,10 @@
#include "ang_sensor.h" #include "ang_sensor.h"
#include "core_cmFunc.h" #include "core_cmFunc.h"
#include "gd32c10x_fmc.h" #include "gd32c10x_fmc.h"
#include "math.h"
#include <Can.h> #include <Can.h>
#include <cstdio> #include <cstdio>
#include <cstring>
#include <stdint.h> #include <stdint.h>
FlashEEPROM flashEEP = FlashEEPROM(); FlashEEPROM flashEEP = FlashEEPROM();
@@ -23,15 +25,15 @@ FlashEEPROM::FlashEEPROM() {
*/ */
u8 FlashEEPROM::init() { u8 FlashEEPROM::init() {
// 读取保存的参数 // 读取保存的参数
u16 angle; float angle;
// 尝试从Flash读取参数，如果不存在则使用默认值 // 尝试从Flash读取参数，如果不存在则使用默认值
if (read(ADDR_ANGLE_DATA, &angle) != FLASH_EEPROM_OK) { if (readFloat(ADDR_ANGLE_DATA, &angle) != FLASH_EEPROM_OK) {
angle = 0; // 默认值 angle = 0; // 默认值
std::printf("角度初始化为: %d\r\n", angle); std::printf("角度初始化为: %f\r\n", angle);
} else { } else {
std::printf("从Flash读取到角度数: %d\r\n", angle); std::printf("从Flash读取到角度数: %f\r\n", angle);
} }
KTH7823.angle_set((float)angle); KTH7823.angle_set(angle);
return 0; return 0;
} }
@@ -41,9 +43,9 @@ u8 FlashEEPROM::init() {
* @param data: 指向存储读取数据的指针 * @param data: 指向存储读取数据的指针
* @retval FlashEepromStatus: 操作状态 * @retval FlashEepromStatus: 操作状态
*/ */
FlashEepromStatus FlashEEPROM::read(u16 virtualAddr, u16 *data) const { FlashEepromStatus FlashEEPROM::read(u8 virtualAddr, u16 *data) const {
// 检查参数有效性 // 参数检查
if (data == nullptr || virtualAddr > 0xFFFE) { if (data == nullptr or virtualAddr > 0xFE) {
return FLASH_EEPROM_INVALID_ADDR; return FLASH_EEPROM_INVALID_ADDR;
} }
@@ -53,7 +55,7 @@ FlashEepromStatus FlashEEPROM::read(u16 virtualAddr, u16 *data) const {
u32 data_word = *((u32 *)address); u32 data_word = *((u32 *)address);
// 检查是否是有效数据（非擦除状态） // 检查是否是有效数据（非擦除状态）
if (data_word != 0xFFFFFFFF) { if (data_word != 0xFFFFFFFF) {
u16 addr = (u16)(data_word & 0xFFFF); u8 addr = (u8)(data_word & 0xFF);
// 找到匹配的虚拟地址 // 找到匹配的虚拟地址
if (addr == virtualAddr) { if (addr == virtualAddr) {
*data = (u16)((data_word >> 16) & 0xFFFF); *data = (u16)((data_word >> 16) & 0xFFFF);
@@ -65,15 +67,45 @@ FlashEepromStatus FlashEEPROM::read(u16 virtualAddr, u16 *data) const {
return FLASH_EEPROM_NOT_FOUND; return FLASH_EEPROM_NOT_FOUND;
} }
/**
* @brief 从Flash EEPROM读取float数据
* @param virtualAddr: 虚拟地址
* @param data: 指向存储读取数据的指针
* @retval FlashEepromStatus: 操作状态
*/
FlashEepromStatus FlashEEPROM::readFloat(u8 virtualAddr, float *data) const {
// 参数检查
if (data == nullptr or virtualAddr > 0xFE) {
return FLASH_EEPROM_INVALID_ADDR;
}
// 从Flash末尾开始向前搜索，找到最新的数据
for (u32 address = EEPROM_END_ADDRESS - SIZE_U32;
address >= EEPROM_START_ADDRESS; address -= SIZE_U32) {
u32 data_word = *((u32 *)address);
// 检查是否是有效数据（非擦除状态）
if (data_word != 0xFFFFFFFF) {
u8 addr = (u8)(data_word & 0xFF);
// 找到匹配的虚拟地址
if (addr == virtualAddr) {
// 使用完整32位数据解释为float
memcpy(data, &data_word, sizeof(float));
return FLASH_EEPROM_OK;
}
}
}
// 未找到指定虚拟地址的数据
return FLASH_EEPROM_NOT_FOUND;
}
/** /**
* @brief 向Flash EEPROM写入数据 * @brief 向Flash EEPROM写入数据
* @param virtualAddr: 虚拟地址 * @param virtualAddr: 虚拟地址
* @param data: 要写入的数据 * @param data: 要写入的数据
* @retval FlashEepromStatus: 操作状态 * @retval FlashEepromStatus: 操作状态
*/ */
FlashEepromStatus FlashEEPROM::write(u16 virtualAddr, u16 data) { FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) {
// 参数检查 // 参数检查
if (virtualAddr > 0xFFFE) { if (virtualAddr > 0xFE) {
return FLASH_EEPROM_INVALID_ADDR; return FLASH_EEPROM_INVALID_ADDR;
} }
// 查找下一个可写入的位置 // 查找下一个可写入的位置
@@ -97,6 +129,7 @@ FlashEepromStatus FlashEEPROM::write(u16 virtualAddr, u16 data) {
// 如果有足够空间，直接写入 // 如果有足够空间，直接写入
if (next_address < EEPROM_END_ADDRESS) { if (next_address < EEPROM_END_ADDRESS) {
// 新格式: [addr:8bit][reserved:8bit][data:16bit]
u32 data_word = ((u32)data << 16) | (u32)virtualAddr; u32 data_word = ((u32)data << 16) | (u32)virtualAddr;
// 使用GD32库函数进行安全编程 // 使用GD32库函数进行安全编程
@@ -119,7 +152,8 @@ FlashEepromStatus FlashEEPROM::write(u16 virtualAddr, u16 data) {
} }
// 格式化后在页面开头写入数据 // 格式化后在页面开头写入数据
u32 new_data_word = ((u32)data << 16) | (virtualAddr & 0xFFFF); // 新格式: [addr:8bit][reserved:8bit][data:16bit]
u32 new_data_word = ((u32)data << 16) | (u32)virtualAddr;
fmc_state_enum fmc_status = fmc_state_enum fmc_status =
fmc_word_program(EEPROM_START_ADDRESS, new_data_word); fmc_word_program(EEPROM_START_ADDRESS, new_data_word);
@@ -131,6 +165,84 @@ FlashEepromStatus FlashEEPROM::write(u16 virtualAddr, u16 data) {
} }
} }
/**
* @brief 向Flash EEPROM写入float数据
* @param virtualAddr: 虚拟地址
* @param data: 要写入的数据
* @retval FlashEepromStatus: 操作状态
*/
FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) {
// 参数检查
if (virtualAddr > 0xFE) {
return FLASH_EEPROM_INVALID_ADDR;
}
// 查找下一个可写入的位置
u32 next_address = EEPROM_START_ADDRESS;
// 遍历Flash页面寻找下一个可写入位置
for (u32 address = EEPROM_START_ADDRESS; address < EEPROM_END_ADDRESS;
address += SIZE_U32) {
// 安全读取Flash数据
volatile u32 *flash_ptr = (volatile u32 *)address;
u32 data_word = *flash_ptr;
// 找到空位置
if (data_word == 0xFFFFFFFF) {
next_address = address;
break;
}
// 更新下一个可能的写入位置（如果需要格式化）
next_address = address + SIZE_U32;
}
// 如果有足够空间，直接写入
if (next_address < EEPROM_END_ADDRESS) {
u32 float_bits;
u32 data_word;
// 将完整的float数据复制到32位变量中
memcpy(&data_word, &data, sizeof(float));
// 插入虚拟地址到低8位
data_word = (data_word & 0xFFFFFF00) | (u32)virtualAddr;
// 使用GD32库函数进行安全编程
fmc_unlock();
fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR);
// 确保使用对齐地址
fmc_state_enum fmc_status = fmc_word_program(next_address, data_word);
fmc_lock();
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;
}
// 格式化后在页面开头写入数据
u32 float_bits;
u32 data_word;
// 将完整的float数据复制到32位变量中
memcpy(&data_word, &data, sizeof(float));
// 插入虚拟地址到低8位
data_word = (data_word & 0xFFFFFF00) | (u32)virtualAddr;
fmc_state_enum fmc_status =
fmc_word_program(EEPROM_START_ADDRESS, data_word);
if (fmc_status != FMC_READY) {
return FLASH_EEPROM_WRITE_ERROR;
}
return FLASH_EEPROM_OK;
}
}
/** /**
* @brief 格式化Flash EEPROM页面 * @brief 格式化Flash EEPROM页面
* @param None * @param None
@@ -150,24 +262,25 @@ FlashEepromStatus FlashEEPROM::format() {
// 检查是否是有效数据项 // 检查是否是有效数据项
if (data_word != 0xFFFFFFFF) { if (data_word != 0xFFFFFFFF) {
u16 addr = (u16)(data_word & 0xFFFF); u8 addr = (u8)(data_word & 0xFF);
u16 value = (u16)((data_word >> 16) & 0xFFFF); u32 value = (data_word & 0xFFFFFF00); // 保留高24位
// 查找是否已经有相同虚拟地址的数据项 // 查找是否已经有相同虚拟地址的数据项
uint8_t found = 0; uint8_t found = 0;
for (u16 i = 0; i < valid_count; i++) { for (u16 i = 0; i < valid_count; i++) {
u16 existing_addr = (u16)(valid_entries[i] & 0xFFFF); u8 existing_addr = (u8)(valid_entries[i] & 0xFF);
if (existing_addr == addr) { if (existing_addr == addr) {
// 更新为最新的数据 // 更新为最新的数据
valid_entries[i] = ((u32)value << 16) | addr; valid_entries[i] = value | addr;
found = 1; found = 1;
break; break;
} }
} }
// 如果是新的虚拟地址，添加到有效数据列表 // 如果是新的虚拟地址/类型组合，添加到有效数据列表
if (!found && valid_count < (EEPROM_MAX_SIZE / SIZE_U32)) { if (!found && valid_count < (EEPROM_MAX_SIZE / SIZE_U32)) {
valid_entries[valid_count] = ((u32)value << 16) | addr; valid_entries[valid_count] =
((u32)value << 16) | ((u32)value << 8) | addr;
valid_count++; valid_count++;
} }
} }
@@ -191,17 +304,6 @@ FlashEepromStatus FlashEEPROM::format() {
return FLASH_EEPROM_OK; return FLASH_EEPROM_OK;
} }
/**
* @brief 更新参数并保存到Flash
* @param kp 新的KP值
* @param ki 新的KI值
* @param kd 新的KD值
*/
void FlashEEPROM::update_pid_parameters(u16 kp, u16 ki, u16 kd) {
write(ADDR_MOTOR_KP, kp);
write(ADDR_MOTOR_KI, ki);
write(ADDR_MOTOR_KD, kd);
}
/** /**
* @brief 更新系统配置并保存到Flash * @brief 更新系统配置并保存到Flash
@@ -216,25 +318,26 @@ void FlashEEPROM::update_system_config(u16 config) {
* @param angle 角度数据 * @param angle 角度数据
* @return FlashEepromStatus: 操作状态 * @return FlashEepromStatus: 操作状态
*/ */
FlashEepromStatus FlashEEPROM::save_angle(u16 angle, bool saved) { FlashEepromStatus FlashEEPROM::save_angle(float angle, bool saved) {
u16 angle_read = 0; float angle_read = 0;
FlashEepromStatus status = FLASH_EEPROM_ERROR; FlashEepromStatus status = FLASH_EEPROM_ERROR;
// 读取保存的参数 // 读取保存的参数
if (!angle_saved) { if (!angle_saved) {
// 写入参数 // 写入参数
status = write(ADDR_ANGLE_DATA, angle); status = writeFloat(ADDR_ANGLE_DATA, angle);
if (read(ADDR_ANGLE_DATA, &angle_read) != FLASH_EEPROM_OK) if (readFloat(ADDR_ANGLE_DATA, &angle_read) != FLASH_EEPROM_OK)
angle_read = 0; // 读不到角度值，存储失败 angle_read = 0; // 读不到角度值，存储失败
if (angle_read != angle) { if (fabsf(angle_read - angle) > 0.01 * angle) {
status = FLASH_EEPROM_WRITE_ERROR; status = FLASH_EEPROM_WRITE_ERROR;
CAN0_transmiter.EEPROMStatus = 1; // 设置EEPROM错误状态 CAN0_transmiter.EEPROMStatus = 1; // 设置EEPROM错误状态
} else { } else {
angle_saved = saved; // 标记已保存过 angle_saved = saved; // 标记已保存过
CAN0_transmiter.EEPROMStatus = 0; // 设置EEPROM状态 CAN0_transmiter.EEPROMStatus = 0; // 设置EEPROM状态
} }
printf("read:%f\r\n", angle_read);
} }
return status; return status;
} }
@@ -244,11 +347,11 @@ FlashEepromStatus FlashEEPROM::save_angle(u16 angle, bool saved) {
* @return float: 角度数据 * @return float: 角度数据
*/ */
float FlashEEPROM::read_angle() { float FlashEEPROM::read_angle() {
u16 angle = 0; float angle = 0;
if (read(ADDR_ANGLE_DATA, &angle) != FLASH_EEPROM_OK) { if (readFloat(ADDR_ANGLE_DATA, &angle) != FLASH_EEPROM_OK) {
angle = 0; angle = 0;
} }
return (float)angle; // 默认返回0度 return angle; // 默认返回0度
} }
/** /**
@@ -277,11 +380,6 @@ void flash_eeprom_example_main(void) {
// 初始化Flash EEPROM // 初始化Flash EEPROM
flashEEP.init(); flashEEP.init();
// 模拟参数更新
std::printf("\r\n--- 更新PID参数 ---\r\n");
flashEEP.update_pid_parameters(120, 80, 30);
// 模拟系统配置更新 // 模拟系统配置更新
std::printf("\r\n--- 更新系统配置 ---\r\n"); std::printf("\r\n--- 更新系统配置 ---\r\n");
flashEEP.update_system_config(0x03); flashEEP.update_system_config(0x03);
Robot_V2/sys/flash.h
+28 -26
View File
@@ -5,59 +5,61 @@
#include <cstdint> #include <cstdint>
#include <stdint.h> #include <stdint.h>
// 修改后 - 强制4字节对齐 // 修改后 - 强制4字节对齐
#define ALIGN_4BYTE(addr) ((addr) & 0xFFFFFFFC) // 4字节对齐 #define ALIGN_4BYTE(addr) ((addr) & 0xFFFFFFFC) // 4字节对齐
// EEPROM操作状态 // EEPROM操作状态
typedef enum { typedef enum {
FLASH_EEPROM_OK = 0, // 操作成功 FLASH_EEPROM_OK = 0, // 操作成功
FLASH_EEPROM_ERROR, // 操作失败 FLASH_EEPROM_ERROR, // 操作失败
FLASH_EEPROM_NOT_FOUND, // 未找到指定数据 FLASH_EEPROM_NOT_FOUND, // 未找到指定数据
FLASH_EEPROM_INVALID_ADDR, // 无效地址 FLASH_EEPROM_INVALID_ADDR, // 无效地址
FLASH_EEPROM_WRITE_ERROR, // 写入错误 FLASH_EEPROM_WRITE_ERROR, // 写入错误
FLASH_EEPROM_ERASE_ERROR, // 擦除错误 FLASH_EEPROM_ERASE_ERROR, // 擦除错误
FLASH_EEPROM_ALIGNMENT_ERROR,// 对齐错误 FLASH_EEPROM_ALIGNMENT_ERROR, // 对齐错误
} FlashEepromStatus; } FlashEepromStatus;
// Flash EEPROM类 // Flash EEPROM类
class FlashEEPROM { class FlashEEPROM {
public: public:
// EEPROM emulation constants // EEPROM emulation constants
static constexpr uint32_t EEPROM_START_ADDRESS = static constexpr u32 EEPROM_START_ADDRESS =
0x0801F800; // Flash最后一页(2KB) 0x0801F800; // Flash最后一页(2KB)
static constexpr uint32_t EEPROM_END_ADDRESS = 0x08020000; // Flash结束地址 static constexpr u32 EEPROM_END_ADDRESS = 0x08020000; // Flash结束地址
static constexpr uint32_t EEPROM_PAGE_SIZE = 0x800; // 页大小2KB static constexpr u32 EEPROM_PAGE_SIZE = 0x800; // 页大小2KB
static constexpr uint32_t EEPROM_MAX_SIZE = 0x800; // EEPROM最大容量 static constexpr u32 EEPROM_MAX_SIZE = 0x800; // EEPROM最大容量
// 定义用户自定义的虚拟地址 // 定义用户自定义的虚拟地址
static constexpr uint16_t ADDR_MOTOR_KP = 0x0010; // 电机KP参数地址 static constexpr u8 ADDR_MOTOR_KP = 0x10; // 电机KP参数地址
static constexpr uint16_t ADDR_MOTOR_KI = 0x0011; // 电机KI参数地址 static constexpr u8 ADDR_MOTOR_KI = 0x11; // 电机KI参数地址
static constexpr uint16_t ADDR_MOTOR_KD = 0x0012; // 电机KD参数地址 static constexpr u8 ADDR_MOTOR_KD = 0x12; // 电机KD参数地址
static constexpr uint16_t ADDR_SYSTEM_CONFIG = 0x0020; // 系统配置参数地址 static constexpr u8 ADDR_SYSTEM_CONFIG = 0x20; // 系统配置参数地址
static constexpr uint16_t ADDR_ERROR_COUNT = 0x0030; // 错误计数地址 static constexpr u8 ADDR_ERROR_COUNT = 0x30; // 错误计数地址
// 角度数据存储地址 static constexpr u8 ADDR_ANGLE_DATA = 0x01; // 角度数据存储地址
static constexpr u16 ADDR_ANGLE_DATA = 0x0013;
bool angle_saved = false; bool angle_saved = false;
// 构造函数 // 构造函数
FlashEEPROM(); FlashEEPROM();
// 初始化Flash EEPROM模拟 // 初始化Flash EEPROM模拟
u8 init(); u8 init();
// 从Flash EEPROM读取数据 // 从Flash EEPROM中读取数据
FlashEepromStatus read(uint16_t virtualAddr, uint16_t *data) const; FlashEepromStatus read(u8 virtualAddr, u16 *data) const;
// 向Flash EEPROM写入数据 // 向Flash EEPROM写入数据
FlashEepromStatus write(uint16_t virtualAddr, uint16_t data); FlashEepromStatus write(u8 virtualAddr, u16 data);
// Float数据读相关函数
FlashEepromStatus readFloat(u8 virtualAddr, float *data) const;
// Float数据存储相关函数
FlashEepromStatus writeFloat(u8 virtualAddr, float data);
// 格式化Flash EEPROM页面 // 格式化Flash EEPROM页面
FlashEepromStatus format(); FlashEepromStatus format();
void flash_eeprom_example_init(void); void flash_eeprom_example_init(void);
void update_pid_parameters(uint16_t kp, uint16_t ki, uint16_t kd); void update_system_config(u16 config);
void update_system_config(uint16_t config);
void increment_error_count(void); void increment_error_count(void);
FlashEepromStatus save_angle(u16 angle, bool saved); FlashEepromStatus save_angle(float angle, bool saved);
float read_angle(); float read_angle();
private: private: