From febaad3dc6d4fae7408dd258f91a35406b919a4f Mon Sep 17 00:00:00 2001 From: liumin Date: Wed, 30 Jul 2025 11:28:53 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E5=8D=95=E4=B8=AA=E5=8F=82?= =?UTF-8?q?=E6=95=B0=E8=AF=BB=E5=8F=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/app_differential_drive.c | 10 +- app/app_param_manage.c | 939 ++++++++++++++++++----------------- 2 files changed, 483 insertions(+), 466 deletions(-) diff --git a/app/app_differential_drive.c b/app/app_differential_drive.c index b38f051..dd93c24 100644 --- a/app/app_differential_drive.c +++ b/app/app_differential_drive.c @@ -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 )//如果超过系数 diff --git a/app/app_param_manage.c b/app/app_param_manage.c index cf7f312..a53cb9b 100644 --- a/app/app_param_manage.c +++ b/app/app_param_manage.c @@ -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()); +}