修改单个参数读取
This commit is contained in:
@@ -431,8 +431,13 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
|
||||
// linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque);
|
||||
// yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque);
|
||||
|
||||
if( diff_data.min_Torq > fabs(linear_velocity_x) )//20250728 增加死区 解决手柄回中，不停车问题
|
||||
{
|
||||
linear_velocity_x = 0;
|
||||
}
|
||||
|
||||
left_speed_mps = linear_velocity_x + yaw_rate;
|
||||
right_speed_mps = linear_velocity_x - yaw_rate;
|
||||
right_speed_mps = linear_velocity_x - yaw_rate;
|
||||
|
||||
//扭矩分配
|
||||
if(max_torque < left_speed_mps)
|
||||
@@ -608,7 +613,7 @@ static void diffProcess(void *signal_id)
|
||||
// 限制输出速度在当前速度和最大加速度计算出来的速度之间
|
||||
// output_speed = constrain(output_speed, diff_data.speed - max_acceleration * dt, diff_data.speed + max_acceleration * dt);
|
||||
|
||||
if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) )//手柄回中，速度小的时候清0
|
||||
if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) && ( 10 > fabs(diff_data.left_motor_speed) ) && ( 10 > fabs(diff_data.right_motor_speed) ) )//手柄回中，速度小的时候清0
|
||||
{
|
||||
resetPidIntegral(&speed_pid);
|
||||
resetPidIntegral(&yaw_rate_pid);
|
||||
@@ -620,6 +625,7 @@ static void diffProcess(void *signal_id)
|
||||
float out_torque[4] = {0,0,0,0};
|
||||
// 使用差速车辆动力学模型计算左右电机的期望速度
|
||||
computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, out_torque);
|
||||
|
||||
|
||||
|
||||
if( fabs(diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed) >= diff_data.diff_dead_zone )//如果超过系数
|
||||
|
||||
@@ -1,464 +1,475 @@
|
||||
#include "app_config.h"
|
||||
#include "app_frm_monitor.h"
|
||||
#include "app_frm_signal.h"
|
||||
#include "app_frm_timer.h"
|
||||
#include "app_param_manage.h"
|
||||
|
||||
|
||||
#define E2_RESERVE_COUNT 0x20 //增加IP地址 修改为0x20 20250110
|
||||
|
||||
// 待发送的参数请求信号
|
||||
UnParamRequest un_param_request1 ;
|
||||
UnParamRequest un_param_request2;
|
||||
|
||||
RequestContext request_send ;
|
||||
RequestContext request_context ;
|
||||
|
||||
uint8_t read_write_e2_finished = 0;
|
||||
|
||||
// 定义全局信号实例，读写信号现在包括 offset 和 size
|
||||
ParamSignal param_signal = {
|
||||
.param_ptr = NULL, // 参数指针初始化为 NULL
|
||||
.type = READ_OPERATION, // 操作类型设置为读操作
|
||||
.offset = 0, // 整个数据块的偏移
|
||||
.size = sizeof(param_manager.arr) // 整个数据块的大小
|
||||
};
|
||||
|
||||
|
||||
// 全局变量：初始化参数名称结构体
|
||||
ParamNames param_names = {
|
||||
#define X(name) .name = #name,
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
};
|
||||
|
||||
|
||||
UnParamManager param_manager ;
|
||||
|
||||
static uint8_t is_param_initialized = 0;
|
||||
|
||||
// 打印所有参数的名称和值, 每行 4 个参数
|
||||
void printParams()
|
||||
{
|
||||
unsigned int param_count = 0;
|
||||
|
||||
#define X(name) \
|
||||
printf("%-8s: %-8.2f", param_names.name, param_manager.bit_data.name); \
|
||||
param_count++; \
|
||||
if (param_count % 4 == 0) { \
|
||||
printf("\n"); \
|
||||
} else { \
|
||||
printf(" "); \
|
||||
}
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
// 如果最后一行不足 4 个参数, 打印换行
|
||||
if (param_count % 4 != 0)
|
||||
{
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void writeByte24c02(uint16_t addr, uint8_t data)
|
||||
{
|
||||
if(0 != wrbyte_24c02(addr,data))
|
||||
{
|
||||
printf("E2PROM write error!\n");
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t readByte24c02(uint16_t addr)
|
||||
{
|
||||
return rdbyte_24c02(addr);
|
||||
}
|
||||
|
||||
// 定义一个通用的 EEPROM 访问函数
|
||||
uint8_t accessEeprom(size_t offset, void *data, size_t size, OperationType type)
|
||||
{
|
||||
if (data == NULL || size == 0)
|
||||
{
|
||||
return 1; // 返回错误状态，表示无效的参数
|
||||
}
|
||||
|
||||
uint8_t *byte_data = (uint8_t *)data; // 将 void* 转换为 uint8_t*，方便逐字节操作
|
||||
size_t index;
|
||||
|
||||
if (type == WRITE_OPERATION)
|
||||
{
|
||||
// 写入操作
|
||||
for (index = 0; index < size; index++)
|
||||
{
|
||||
writeByte24c02((uint16_t)(offset + index + E2_RESERVE_COUNT), byte_data[index]);
|
||||
udelay(4000);//写入一个字节延时4ms
|
||||
}
|
||||
// 校验
|
||||
for (index = 0; index < size; index++)
|
||||
{
|
||||
if (readByte24c02((uint16_t)(offset + index + E2_RESERVE_COUNT)) != byte_data[index])
|
||||
{
|
||||
return 2; // 返回错误状态，表示写入验证失败
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 读取操作
|
||||
for (index = 0; index < size; index++)
|
||||
{
|
||||
byte_data[index] = readByte24c02((uint16_t)(offset + index + E2_RESERVE_COUNT));
|
||||
}
|
||||
}
|
||||
return 0; // 返回状态，表示成功
|
||||
}
|
||||
|
||||
void handleParamOp(void *data)
|
||||
{
|
||||
ParamSignal *signal = (ParamSignal *)data;
|
||||
|
||||
if (signal->param_ptr == NULL)
|
||||
{
|
||||
// 操作整个参数管理器
|
||||
if (accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), signal->type) == 0)
|
||||
{
|
||||
read_write_e2_finished = 1;
|
||||
publishMessage(&read_write_e2_finished, 1); // 读写成功
|
||||
}
|
||||
else
|
||||
{
|
||||
read_write_e2_finished = 2;
|
||||
publishMessage(&read_write_e2_finished, 1); // 读写失败
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 根据信号中的偏移和大小操作单个参数
|
||||
if (accessEeprom(signal->offset, signal->param_ptr, signal->size, signal->type) == 0)
|
||||
{
|
||||
read_write_e2_finished = 1;
|
||||
publishMessage(&read_write_e2_finished, 1); // 读写成功
|
||||
}
|
||||
else
|
||||
{
|
||||
read_write_e2_finished = 2;
|
||||
publishMessage(&read_write_e2_finished, 1); // 读写失败
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
uint8_t calculateCRC(const uint8_t* data, uint32_t length) {
|
||||
uint8_t crc = 0;
|
||||
for (uint32_t i = 0; i < length; ++i) {
|
||||
crc += data[i]; // 简单的校验和，按字节累加
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
|
||||
float readParameter(const char *param_name) {
|
||||
float float_value = 0;
|
||||
unsigned int offset = 0;
|
||||
|
||||
#define X(name) \
|
||||
if (strcmp(param_name, param_names.name) == 0) { \
|
||||
accessEeprom(offset, ¶m_manager.bit_data.name,sizeof(param_manager.bit_data.name), READ_OPERATION);\
|
||||
memcpy(&float_value, ¶m_manager.bit_data.name, sizeof(param_manager.bit_data.name)); \
|
||||
return float_value; \
|
||||
} \
|
||||
offset += 4;
|
||||
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
printf("Parameter not found: %s\n", param_name);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void writeParameter(const char *param_name, const uint8_t *data) {
|
||||
unsigned int offset = 0;
|
||||
|
||||
#define X(name) \
|
||||
if (strcmp(param_name, param_names.name) == 0) { \
|
||||
memcpy(¶m_manager.bit_data.name, data, sizeof(param_manager.bit_data.name)); \
|
||||
accessEeprom(offset, ¶m_manager.bit_data.name,sizeof(param_manager.bit_data.name), WRITE_OPERATION);\
|
||||
return; \
|
||||
} \
|
||||
offset += 4;
|
||||
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
printf("Parameter not found: %s\n", param_name);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void sendParamRequestResponse(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port, uint8_t isWriteOperation) {
|
||||
// 准备响应帧
|
||||
paramRequest->bit_data.frame_header = 0xFF80;
|
||||
paramRequest->bit_data.frame_type = 0x002B;
|
||||
paramRequest->bit_data.frame_length = sizeof(StrParamRequest);
|
||||
paramRequest->bit_data.accumulated = 0;
|
||||
paramRequest->bit_data.request_id = isWriteOperation ? 98 : 99;
|
||||
|
||||
paramRequest->bit_data.crc = calculateCRC(paramRequest->arr, sizeof(paramRequest->arr) - 1);
|
||||
|
||||
request_send.param_request = paramRequest;
|
||||
request_send.sender_ip = sender_ip;
|
||||
request_send.sender_port = sender_port;
|
||||
// 发送信号，从UDP发送
|
||||
publishMessage(&request_send, 1);
|
||||
}
|
||||
|
||||
|
||||
void processReadAllParams(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) {
|
||||
uint8_t allParams[256][4]; // Size based on E2 size
|
||||
unsigned int i = 0;
|
||||
float param_value;
|
||||
uint8_t exceeded_max = 0; // 新增标志变量
|
||||
|
||||
// 清零 paramRequest
|
||||
memset(paramRequest, 0, sizeof(UnParamRequest));
|
||||
|
||||
accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION);
|
||||
|
||||
printf("Sending parameter data:\n");
|
||||
|
||||
#define X(name) \
|
||||
if (!exceeded_max) { \
|
||||
if (i < 256) { \
|
||||
strncpy((char *)paramRequest->bit_data.param_name[i], #name, sizeof(paramRequest->bit_data.param_name[i]) - 1); \
|
||||
paramRequest->bit_data.param_name[i][sizeof(paramRequest->bit_data.param_name[i]) - 1] = '\0'; \
|
||||
memcpy(allParams[i], ¶m_manager.bit_data.name, sizeof(param_manager.bit_data.name)); \
|
||||
memcpy(¶m_value, allParams[i], sizeof(float)); \
|
||||
printf("Parameter name: %-20s Value: %f\n", #name, param_value); \
|
||||
i++; \
|
||||
} else { \
|
||||
printf("Warning: Exceeded maximum number of parameters\n"); \
|
||||
exceeded_max = 1; \
|
||||
} \
|
||||
}
|
||||
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
// Pack all parameter data into paramRequest
|
||||
memcpy(paramRequest->bit_data.data, allParams, sizeof(allParams));
|
||||
|
||||
printf("Total parameters sent: %d\n", i);
|
||||
|
||||
// Send response
|
||||
sendParamRequestResponse(paramRequest, sender_ip, sender_port, 0);
|
||||
}
|
||||
|
||||
|
||||
void processWriteRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) {
|
||||
float value;
|
||||
printf("Processing write request.\n");
|
||||
|
||||
// 先发送信号，然后从结构体读数
|
||||
for (int i = 0; i < 256; ++i) {
|
||||
if (strlen((char *)paramRequest->bit_data.param_name[i]) > 0) {
|
||||
writeParameter(paramRequest->bit_data.param_name[i], paramRequest->bit_data.data[i]);
|
||||
printf("paramRequest->bit_data.param_name[i]:%s \n",paramRequest->bit_data.param_name[i]);
|
||||
memcpy(&value, paramRequest->bit_data.data[i], sizeof(float));
|
||||
printf("paramRequest->bit_data.data[i]:%f \n", value);
|
||||
}
|
||||
}
|
||||
|
||||
// 发送响应，发送所有参数
|
||||
processReadAllParams(paramRequest, sender_ip, sender_port);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void processReadRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) {
|
||||
// 处理读请求的逻辑
|
||||
printf("Processing read request.\n");
|
||||
|
||||
// 清零 paramRequest
|
||||
memset(paramRequest, 0, sizeof(UnParamRequest));
|
||||
|
||||
// 先发送信号，然后从结构体读数
|
||||
for (int i = 0; i < 256; ++i) {
|
||||
if (strlen((char *)paramRequest->bit_data.param_name[i]) > 0) {
|
||||
float readData = readParameter(paramRequest->bit_data.param_name[i]);
|
||||
memcpy(paramRequest->bit_data.data[i], &readData, sizeof(paramRequest->bit_data.data[i]));
|
||||
}
|
||||
}
|
||||
|
||||
// 发送响应
|
||||
sendParamRequestResponse(paramRequest, sender_ip, sender_port, 0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void OnParamSignal(void *data)
|
||||
{
|
||||
RequestContext *signal = (RequestContext *)data;
|
||||
|
||||
uint8_t *datagram = (uint8_t *)signal->param_request->arr;
|
||||
uint16_t request_id = ((uint16_t)datagram[7] << 8) | (uint16_t)datagram[8];// 大端模式
|
||||
|
||||
// 调试输出
|
||||
printf("Received request ID: 0x%04X\n", request_id);
|
||||
|
||||
// 计算CRC
|
||||
uint8_t calculatedCrc = calculateCRC(datagram, sizeof(UnParamRequest) - 1);
|
||||
uint8_t receivedCrc = datagram[sizeof(UnParamRequest) - 1];
|
||||
|
||||
// 比较CRC
|
||||
if (calculatedCrc != receivedCrc)
|
||||
{
|
||||
printf("CRC check failed, discarding data\n");
|
||||
printf("Calculated CRC: 0x%02X, Received CRC: 0x%02X\n", calculatedCrc, receivedCrc);
|
||||
return;
|
||||
}
|
||||
|
||||
printf("CRC check passed\n");
|
||||
|
||||
if (request_id == 100)
|
||||
{ // 读请求
|
||||
processReadRequestFrame(signal->param_request, signal->sender_ip, signal->sender_port);
|
||||
}
|
||||
else if (request_id == 101)
|
||||
{ // 写请求
|
||||
processWriteRequestFrame(signal->param_request, signal->sender_ip, signal->sender_port);
|
||||
}
|
||||
else if (request_id == 102)
|
||||
{ // 读取所有参数
|
||||
processReadAllParams(signal->param_request, signal->sender_ip, signal->sender_port);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("Unknown request ID.\n");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
float getParam(const char *param_name)
|
||||
{
|
||||
// 检查是否已初始化
|
||||
if (!is_param_initialized)
|
||||
{
|
||||
printf("Parameters not initialized, reinitializing\n");
|
||||
accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION);//Read all parameters from E2
|
||||
is_param_initialized = 1; // Mark as initialized
|
||||
printParams();
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// 检查参数名是否为空
|
||||
if (param_name == NULL)
|
||||
{
|
||||
printf("Error: Parameter name is empty\n");
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// 遍历所有参数
|
||||
#define X(name) \
|
||||
if (strcmp(param_name, #name) == 0) \
|
||||
{ \
|
||||
return param_manager.bit_data.name; \
|
||||
}
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
// 如果没有找到匹配的参数名
|
||||
printf("Error: Parameter %s not found\n", param_name);
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// setParam 函数
|
||||
uint8_t setParam(const char *param_name, float value)
|
||||
{
|
||||
// 检查参数名是否为空
|
||||
if (param_name == NULL)
|
||||
{
|
||||
printf("Error: Parameter name is empty\n");
|
||||
return 2; // 返回错误码
|
||||
}
|
||||
|
||||
// 参数名和值写入EEPROM，先转成字节数组
|
||||
uint8_t data[sizeof(float)];
|
||||
memcpy(data, &value, sizeof(float));
|
||||
writeParameter(param_name, data);
|
||||
|
||||
// 更新参数
|
||||
#define X(name) \
|
||||
if (strcmp(param_name, #name) == 0) \
|
||||
{ \
|
||||
memcpy(¶m_manager.bit_data.name, data, sizeof(param_manager.bit_data.name)); \
|
||||
}
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void paramAppInit(void)
|
||||
{
|
||||
// 初始化全局变量
|
||||
memset(&un_param_request1, 0, sizeof(UnParamRequest));
|
||||
memset(&un_param_request2, 0, sizeof(UnParamRequest));
|
||||
|
||||
// 正确初始化 RequestContext 结构体
|
||||
request_send.param_request = &un_param_request1;
|
||||
request_send.sender_ip = 0;
|
||||
request_send.sender_port = 0;
|
||||
|
||||
request_context.param_request = &un_param_request2;
|
||||
request_context.sender_ip = 0;
|
||||
request_context.sender_port = 0;
|
||||
|
||||
// 上电读取所有参数
|
||||
memset(param_manager.arr, 0, sizeof(param_manager.arr));
|
||||
accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION);
|
||||
|
||||
// 初始化每个参数
|
||||
// param_manager.bit_data.whl_bas = 1.5f; // 初始化轮距
|
||||
// param_manager.bit_data.max_rpm = 5500.0f; // 初始化最大转速
|
||||
// param_manager.bit_data.whl_dia = 0.6f; // 初始化轮直径
|
||||
// param_manager.bit_data.max_acc = 1.0f; // 初始化最大加速度
|
||||
// param_manager.bit_data.spd_kp = 5.0f; // 初始化速度控制 KP
|
||||
// param_manager.bit_data.spd_ki = 1.0f; // 初始化速度控制 KI
|
||||
// param_manager.bit_data.spd_kd = 0.0f; // 初始化速度控制 KD
|
||||
// param_manager.bit_data.spd_il = 5.0f; // 初始化速度控制 IL
|
||||
// param_manager.bit_data.spd_ol = 5.0f; // 初始化速度控制 OL
|
||||
// param_manager.bit_data.crv_kp = 1.0f; // 初始化曲线控制 KP
|
||||
// param_manager.bit_data.crv_ki = 0.0f; // 初始化曲线控制 KI
|
||||
// param_manager.bit_data.crv_kd = 0.0f; // 初始化曲线控制 KD
|
||||
// param_manager.bit_data.crv_il = 2.0f; // 初始化曲线控制 IL
|
||||
// param_manager.bit_data.crv_ol = 2.0f; // 初始化曲线控制 OL
|
||||
// param_manager.bit_data.brk_on = 1500.0f; // 初始化制动开启参数
|
||||
// param_manager.bit_data.brk_off = 800.0f; // 初始化制动关闭参数
|
||||
// param_manager.bit_data.maxTorq = 60.0f; // 初始化最大扭矩
|
||||
// param_manager.bit_data.feedPwr = 10000.0f; // 初始化馈电功率
|
||||
// param_manager.bit_data.dispPwr = 10000.0f; // 初始化显示功率
|
||||
// param_manager.bit_data.VehMass = 700.0f; // 初始化车辆质量
|
||||
// param_manager.bit_data.gRatio = 28.0f; // 初始化减速比
|
||||
// param_manager.bit_data.prCTime = 5.0f; // 初始化预充时间
|
||||
// param_manager.bit_data.brk_pos = 0.0f; // 初始化刹车位置, 0表示未刹车
|
||||
// param_manager.bit_data.pwr_sta = 0.0f; // 初始化电源状态
|
||||
// param_manager.bit_data.lightSt = 0.0f; // 初始化灯光状态
|
||||
// param_manager.bit_data.pwr_btn = 0.0f; // 初始化电源按钮状态
|
||||
// param_manager.bit_data.test = 0.0f; // 初始化测试参数
|
||||
|
||||
|
||||
// 订阅信号
|
||||
subscribe(¶m_signal, handleParamOp);
|
||||
subscribe(&request_context, OnParamSignal);// 接收到上位机读写参数信号
|
||||
|
||||
printParams();//打印所有参数
|
||||
|
||||
is_param_initialized = 1; // 标记初始化完成
|
||||
|
||||
printf("paramAPP init OK! %d\n",getCurrentTime());
|
||||
}
|
||||
#include "app_config.h"
|
||||
#include "app_frm_monitor.h"
|
||||
#include "app_frm_signal.h"
|
||||
#include "app_frm_timer.h"
|
||||
#include "app_param_manage.h"
|
||||
|
||||
|
||||
#define E2_RESERVE_COUNT 0x20 //增加IP地址 修改为0x20 20250110
|
||||
|
||||
// 待发送的参数请求信号
|
||||
UnParamRequest un_param_request1 ;
|
||||
UnParamRequest un_param_request2;
|
||||
|
||||
RequestContext request_send ;
|
||||
RequestContext request_context ;
|
||||
|
||||
uint8_t read_write_e2_finished = 0;
|
||||
|
||||
// 定义全局信号实例，读写信号现在包括 offset 和 size
|
||||
ParamSignal param_signal = {
|
||||
.param_ptr = NULL, // 参数指针初始化为 NULL
|
||||
.type = READ_OPERATION, // 操作类型设置为读操作
|
||||
.offset = 0, // 整个数据块的偏移
|
||||
.size = sizeof(param_manager.arr) // 整个数据块的大小
|
||||
};
|
||||
|
||||
|
||||
// 全局变量：初始化参数名称结构体
|
||||
ParamNames param_names = {
|
||||
#define X(name) .name = #name,
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
};
|
||||
|
||||
|
||||
UnParamManager param_manager ;
|
||||
|
||||
static uint8_t is_param_initialized = 0;
|
||||
|
||||
// 打印所有参数的名称和值, 每行 4 个参数
|
||||
void printParams()
|
||||
{
|
||||
unsigned int param_count = 0;
|
||||
|
||||
#define X(name) \
|
||||
printf("%-8s: %-8.2f", param_names.name, param_manager.bit_data.name); \
|
||||
param_count++; \
|
||||
if (param_count % 4 == 0) { \
|
||||
printf("\n"); \
|
||||
} else { \
|
||||
printf(" "); \
|
||||
}
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
// 如果最后一行不足 4 个参数, 打印换行
|
||||
if (param_count % 4 != 0)
|
||||
{
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void writeByte24c02(uint16_t addr, uint8_t data)
|
||||
{
|
||||
if(0 != wrbyte_24c02(addr,data))
|
||||
{
|
||||
printf("E2PROM write error!\n");
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t readByte24c02(uint16_t addr)
|
||||
{
|
||||
return rdbyte_24c02(addr);
|
||||
}
|
||||
|
||||
// 定义一个通用的 EEPROM 访问函数
|
||||
uint8_t accessEeprom(size_t offset, void *data, size_t size, OperationType type)
|
||||
{
|
||||
if (data == NULL || size == 0)
|
||||
{
|
||||
return 1; // 返回错误状态，表示无效的参数
|
||||
}
|
||||
|
||||
uint8_t *byte_data = (uint8_t *)data; // 将 void* 转换为 uint8_t*，方便逐字节操作
|
||||
size_t index;
|
||||
|
||||
if (type == WRITE_OPERATION)
|
||||
{
|
||||
// 写入操作
|
||||
for (index = 0; index < size; index++)
|
||||
{
|
||||
writeByte24c02((uint16_t)(offset + index + E2_RESERVE_COUNT), byte_data[index]);
|
||||
udelay(4000);//写入一个字节延时4ms
|
||||
}
|
||||
// 校验
|
||||
for (index = 0; index < size; index++)
|
||||
{
|
||||
if (readByte24c02((uint16_t)(offset + index + E2_RESERVE_COUNT)) != byte_data[index])
|
||||
{
|
||||
return 2; // 返回错误状态，表示写入验证失败
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 读取操作
|
||||
for (index = 0; index < size; index++)
|
||||
{
|
||||
byte_data[index] = readByte24c02((uint16_t)(offset + index + E2_RESERVE_COUNT));
|
||||
}
|
||||
}
|
||||
return 0; // 返回状态，表示成功
|
||||
}
|
||||
|
||||
void handleParamOp(void *data)
|
||||
{
|
||||
ParamSignal *signal = (ParamSignal *)data;
|
||||
|
||||
if (signal->param_ptr == NULL)
|
||||
{
|
||||
// 操作整个参数管理器
|
||||
if (accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), signal->type) == 0)
|
||||
{
|
||||
read_write_e2_finished = 1;
|
||||
publishMessage(&read_write_e2_finished, 1); // 读写成功
|
||||
}
|
||||
else
|
||||
{
|
||||
read_write_e2_finished = 2;
|
||||
publishMessage(&read_write_e2_finished, 1); // 读写失败
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 根据信号中的偏移和大小操作单个参数
|
||||
if (accessEeprom(signal->offset, signal->param_ptr, signal->size, signal->type) == 0)
|
||||
{
|
||||
read_write_e2_finished = 1;
|
||||
publishMessage(&read_write_e2_finished, 1); // 读写成功
|
||||
}
|
||||
else
|
||||
{
|
||||
read_write_e2_finished = 2;
|
||||
publishMessage(&read_write_e2_finished, 1); // 读写失败
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
uint8_t calculateCRC(const uint8_t* data, uint32_t length) {
|
||||
uint8_t crc = 0;
|
||||
for (uint32_t i = 0; i < length; ++i) {
|
||||
crc += data[i]; // 简单的校验和，按字节累加
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
|
||||
float readParameter(const char *param_name) {
|
||||
float float_value = 0;
|
||||
unsigned int offset = 0;
|
||||
|
||||
#define X(name) \
|
||||
if (strcmp(param_name, param_names.name) == 0) { \
|
||||
accessEeprom(offset, ¶m_manager.bit_data.name,sizeof(param_manager.bit_data.name), READ_OPERATION);\
|
||||
memcpy(&float_value, ¶m_manager.bit_data.name, sizeof(param_manager.bit_data.name)); \
|
||||
return float_value; \
|
||||
} \
|
||||
offset += 4;
|
||||
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
printf("Parameter not found: %s\n", param_name);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void writeParameter(const char *param_name, const uint8_t *data) {
|
||||
unsigned int offset = 0;
|
||||
|
||||
#define X(name) \
|
||||
if (strcmp(param_name, param_names.name) == 0) { \
|
||||
memcpy(¶m_manager.bit_data.name, data, sizeof(param_manager.bit_data.name)); \
|
||||
accessEeprom(offset, ¶m_manager.bit_data.name,sizeof(param_manager.bit_data.name), WRITE_OPERATION);\
|
||||
return; \
|
||||
} \
|
||||
offset += 4;
|
||||
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
printf("Parameter not found: %s\n", param_name);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void sendParamRequestResponse(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port, uint8_t isWriteOperation) {
|
||||
// 准备响应帧
|
||||
paramRequest->bit_data.frame_header = 0xFF80;
|
||||
paramRequest->bit_data.frame_type = 0x002B;
|
||||
paramRequest->bit_data.frame_length = sizeof(StrParamRequest);
|
||||
paramRequest->bit_data.accumulated = 0;
|
||||
paramRequest->bit_data.request_id = isWriteOperation ? 98 : 99;
|
||||
|
||||
paramRequest->bit_data.crc = calculateCRC(paramRequest->arr, sizeof(paramRequest->arr) - 1);
|
||||
|
||||
request_send.param_request = paramRequest;
|
||||
request_send.sender_ip = sender_ip;
|
||||
request_send.sender_port = sender_port;
|
||||
// 发送信号，从UDP发送
|
||||
publishMessage(&request_send, 1);
|
||||
}
|
||||
|
||||
|
||||
void processReadAllParams(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) {
|
||||
uint8_t allParams[256][4]; // Size based on E2 size
|
||||
unsigned int i = 0;
|
||||
float param_value;
|
||||
uint8_t exceeded_max = 0; // 新增标志变量
|
||||
|
||||
// 清零 paramRequest
|
||||
memset(paramRequest, 0, sizeof(UnParamRequest));
|
||||
|
||||
accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION);
|
||||
|
||||
printf("Sending parameter data:\n");
|
||||
|
||||
#define X(name) \
|
||||
if (!exceeded_max) { \
|
||||
if (i < 256) { \
|
||||
strncpy((char *)paramRequest->bit_data.param_name[i], #name, sizeof(paramRequest->bit_data.param_name[i]) - 1); \
|
||||
paramRequest->bit_data.param_name[i][sizeof(paramRequest->bit_data.param_name[i]) - 1] = '\0'; \
|
||||
memcpy(allParams[i], ¶m_manager.bit_data.name, sizeof(param_manager.bit_data.name)); \
|
||||
memcpy(¶m_value, allParams[i], sizeof(float)); \
|
||||
printf("Parameter name: %-20s Value: %f\n", #name, param_value); \
|
||||
i++; \
|
||||
} else { \
|
||||
printf("Warning: Exceeded maximum number of parameters\n"); \
|
||||
exceeded_max = 1; \
|
||||
} \
|
||||
}
|
||||
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
// Pack all parameter data into paramRequest
|
||||
memcpy(paramRequest->bit_data.data, allParams, sizeof(allParams));
|
||||
|
||||
printf("Total parameters sent: %d\n", i);
|
||||
|
||||
// Send response
|
||||
sendParamRequestResponse(paramRequest, sender_ip, sender_port, 0);
|
||||
}
|
||||
|
||||
|
||||
void processWriteRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) {
|
||||
float value;
|
||||
printf("Processing write request.\n");
|
||||
|
||||
// 先发送信号，然后从结构体读数
|
||||
for (int i = 0; i < 256; ++i) {
|
||||
if (strlen((char *)paramRequest->bit_data.param_name[i]) > 0) {
|
||||
writeParameter(paramRequest->bit_data.param_name[i], paramRequest->bit_data.data[i]);
|
||||
printf("paramRequest->bit_data.param_name[i]:%s \n",paramRequest->bit_data.param_name[i]);
|
||||
memcpy(&value, paramRequest->bit_data.data[i], sizeof(float));
|
||||
printf("paramRequest->bit_data.data[i]:%f \n", value);
|
||||
}
|
||||
}
|
||||
|
||||
// 发送响应，发送所有参数
|
||||
processReadAllParams(paramRequest, sender_ip, sender_port);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void processReadRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) {
|
||||
printf("Processing read request.\n");
|
||||
|
||||
// 先备份原始请求数据
|
||||
UnParamRequest originalRequest;
|
||||
memcpy(&originalRequest, paramRequest, sizeof(UnParamRequest));
|
||||
|
||||
// 清零响应数据
|
||||
memset(paramRequest, 0, sizeof(UnParamRequest));
|
||||
|
||||
// 处理客户端请求的参数
|
||||
for (int i = 0; i < 256; ++i) {
|
||||
if (strlen((char *)originalRequest.bit_data.param_name[i]) > 0) {
|
||||
// 复制参数名到响应
|
||||
strcpy((char *)paramRequest->bit_data.param_name[i],
|
||||
(char *)originalRequest.bit_data.param_name[i]);
|
||||
// 读取参数值
|
||||
float readData = readParameter(originalRequest.bit_data.param_name[i]);
|
||||
memcpy(paramRequest->bit_data.data[i], &readData, sizeof(float));
|
||||
|
||||
printf("Read parameter: %s = %f\n",
|
||||
originalRequest.bit_data.param_name[i], readData);
|
||||
}
|
||||
}
|
||||
|
||||
// 发送响应 - 直接传递 paramRequest
|
||||
sendParamRequestResponse(paramRequest, sender_ip, sender_port, 0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void OnParamSignal(void *data)
|
||||
{
|
||||
RequestContext *signal = (RequestContext *)data;
|
||||
|
||||
uint8_t *datagram = (uint8_t *)signal->param_request->arr;
|
||||
uint16_t request_id = ((uint16_t)datagram[7] << 8) | (uint16_t)datagram[8];// 大端模式
|
||||
|
||||
// 调试输出
|
||||
printf("Received request ID: 0x%04X\n", request_id);
|
||||
|
||||
// 计算CRC
|
||||
uint8_t calculatedCrc = calculateCRC(datagram, sizeof(UnParamRequest) - 1);
|
||||
uint8_t receivedCrc = datagram[sizeof(UnParamRequest) - 1];
|
||||
|
||||
// 比较CRC
|
||||
if (calculatedCrc != receivedCrc)
|
||||
{
|
||||
printf("CRC check failed, discarding data\n");
|
||||
printf("Calculated CRC: 0x%02X, Received CRC: 0x%02X\n", calculatedCrc, receivedCrc);
|
||||
return;
|
||||
}
|
||||
|
||||
printf("CRC check passed\n");
|
||||
|
||||
if (request_id == 100)
|
||||
{ // 读请求
|
||||
processReadRequestFrame(signal->param_request, signal->sender_ip, signal->sender_port);
|
||||
}
|
||||
else if (request_id == 101)
|
||||
{ // 写请求
|
||||
processWriteRequestFrame(signal->param_request, signal->sender_ip, signal->sender_port);
|
||||
}
|
||||
else if (request_id == 102)
|
||||
{ // 读取所有参数
|
||||
processReadAllParams(signal->param_request, signal->sender_ip, signal->sender_port);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("Unknown request ID.\n");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
float getParam(const char *param_name)
|
||||
{
|
||||
// 检查是否已初始化
|
||||
if (!is_param_initialized)
|
||||
{
|
||||
printf("Parameters not initialized, reinitializing\n");
|
||||
accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION);//Read all parameters from E2
|
||||
is_param_initialized = 1; // Mark as initialized
|
||||
printParams();
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// 检查参数名是否为空
|
||||
if (param_name == NULL)
|
||||
{
|
||||
printf("Error: Parameter name is empty\n");
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// 遍历所有参数
|
||||
#define X(name) \
|
||||
if (strcmp(param_name, #name) == 0) \
|
||||
{ \
|
||||
return param_manager.bit_data.name; \
|
||||
}
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
// 如果没有找到匹配的参数名
|
||||
printf("Error: Parameter %s not found\n", param_name);
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// setParam 函数
|
||||
uint8_t setParam(const char *param_name, float value)
|
||||
{
|
||||
// 检查参数名是否为空
|
||||
if (param_name == NULL)
|
||||
{
|
||||
printf("Error: Parameter name is empty\n");
|
||||
return 2; // 返回错误码
|
||||
}
|
||||
|
||||
// 参数名和值写入EEPROM，先转成字节数组
|
||||
uint8_t data[sizeof(float)];
|
||||
memcpy(data, &value, sizeof(float));
|
||||
writeParameter(param_name, data);
|
||||
|
||||
// 更新参数
|
||||
#define X(name) \
|
||||
if (strcmp(param_name, #name) == 0) \
|
||||
{ \
|
||||
memcpy(¶m_manager.bit_data.name, data, sizeof(param_manager.bit_data.name)); \
|
||||
}
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void paramAppInit(void)
|
||||
{
|
||||
// 初始化全局变量
|
||||
memset(&un_param_request1, 0, sizeof(UnParamRequest));
|
||||
memset(&un_param_request2, 0, sizeof(UnParamRequest));
|
||||
|
||||
// 正确初始化 RequestContext 结构体
|
||||
request_send.param_request = &un_param_request1;
|
||||
request_send.sender_ip = 0;
|
||||
request_send.sender_port = 0;
|
||||
|
||||
request_context.param_request = &un_param_request2;
|
||||
request_context.sender_ip = 0;
|
||||
request_context.sender_port = 0;
|
||||
|
||||
// 上电读取所有参数
|
||||
memset(param_manager.arr, 0, sizeof(param_manager.arr));
|
||||
accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION);
|
||||
|
||||
// 初始化每个参数
|
||||
// param_manager.bit_data.whl_bas = 1.5f; // 初始化轮距
|
||||
// param_manager.bit_data.max_rpm = 5500.0f; // 初始化最大转速
|
||||
// param_manager.bit_data.whl_dia = 0.6f; // 初始化轮直径
|
||||
// param_manager.bit_data.max_acc = 1.0f; // 初始化最大加速度
|
||||
// param_manager.bit_data.spd_kp = 5.0f; // 初始化速度控制 KP
|
||||
// param_manager.bit_data.spd_ki = 1.0f; // 初始化速度控制 KI
|
||||
// param_manager.bit_data.spd_kd = 0.0f; // 初始化速度控制 KD
|
||||
// param_manager.bit_data.spd_il = 5.0f; // 初始化速度控制 IL
|
||||
// param_manager.bit_data.spd_ol = 5.0f; // 初始化速度控制 OL
|
||||
// param_manager.bit_data.crv_kp = 1.0f; // 初始化曲线控制 KP
|
||||
// param_manager.bit_data.crv_ki = 0.0f; // 初始化曲线控制 KI
|
||||
// param_manager.bit_data.crv_kd = 0.0f; // 初始化曲线控制 KD
|
||||
// param_manager.bit_data.crv_il = 2.0f; // 初始化曲线控制 IL
|
||||
// param_manager.bit_data.crv_ol = 2.0f; // 初始化曲线控制 OL
|
||||
// param_manager.bit_data.brk_on = 1500.0f; // 初始化制动开启参数
|
||||
// param_manager.bit_data.brk_off = 800.0f; // 初始化制动关闭参数
|
||||
// param_manager.bit_data.maxTorq = 60.0f; // 初始化最大扭矩
|
||||
// param_manager.bit_data.feedPwr = 10000.0f; // 初始化馈电功率
|
||||
// param_manager.bit_data.dispPwr = 10000.0f; // 初始化显示功率
|
||||
// param_manager.bit_data.VehMass = 700.0f; // 初始化车辆质量
|
||||
// param_manager.bit_data.gRatio = 28.0f; // 初始化减速比
|
||||
// param_manager.bit_data.prCTime = 5.0f; // 初始化预充时间
|
||||
// param_manager.bit_data.brk_pos = 0.0f; // 初始化刹车位置, 0表示未刹车
|
||||
// param_manager.bit_data.pwr_sta = 0.0f; // 初始化电源状态
|
||||
// param_manager.bit_data.lightSt = 0.0f; // 初始化灯光状态
|
||||
// param_manager.bit_data.pwr_btn = 0.0f; // 初始化电源按钮状态
|
||||
// param_manager.bit_data.test = 0.0f; // 初始化测试参数
|
||||
|
||||
|
||||
// 订阅信号
|
||||
subscribe(¶m_signal, handleParamOp);
|
||||
subscribe(&request_context, OnParamSignal);// 接收到上位机读写参数信号
|
||||
|
||||
printParams();//打印所有参数
|
||||
|
||||
is_param_initialized = 1; // 标记初始化完成
|
||||
|
||||
printf("paramAPP init OK! %d\n",getCurrentTime());
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user