liumin/test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

扭矩模式，增加同侧差速PID

Browse Source
This commit is contained in:
liumin
2025-07-17 20:20:32 +08:00
parent eefa6daf38
commit cdc62d856d
5 changed files with 635 additions and 601 deletions
Download Patch File Download Diff File Expand all files Collapse all files

479
app/app_brake.c
View File

@@ -1,241 +1,238 @@
#include "app_config.h"
#include "interface.h"
#include "app_frm_monitor.h"
#include "app_frm_signal.h"
#include "app_frm_timer.h"
#include "app_param_manage.h"
#include "app_power.h"
#include "app_brake.h"
#include "app_differential_drive.h"
// 使用内联函数
static inline uint8_t setBrakeOn(void) { return 1; }
static inline uint8_t setBrakeOff(void) { return 0; }
BrakeSystem brake_data;
// 判断是否需要刹车
static uint8_t shouldApplyBrake()
{
return (brake_data.emergency_stop_switch ||
brake_data.remote_emergency_stop ||
(brake_data.mode_signal == 0 && brake_data.remote_fault) ||
(brake_data.mode_signal == 1 && brake_data.can_bus_fault));//20241021 修改不计算以太网故障
// (brake_data.mode_signal == 1 && (brake_data.can_bus_fault || brake_data.ethernet_fault)));
}
// 判断是否需要释放刹车
static uint8_t shouldReleaseBrake()
{
return (!brake_data.emergency_stop_switch &&
!brake_data.remote_emergency_stop &&
((brake_data.mode_signal == 0 && !brake_data.remote_fault) ||
(brake_data.mode_signal == 1 && !brake_data.can_bus_fault)));//20241021 修改不计算以太网故障
// (brake_data.mode_signal == 1 && (!brake_data.can_bus_fault && !brake_data.ethernet_fault))));
}
// 输出处理函数
static void brakeOutput(void *signal_id)
{
(void)signal_id;
// 根据电机状态，填充发送数据结构，发送信号
switch (brake_data.brake_motor_state)
{
case 1: // 电机前进状态
un_h_bridge_output.bit_data.channel_01 = setBrakeOn();
un_h_bridge_output.bit_data.channel_02 = setBrakeOff();
un_h_bridge_output1.bit_data.channel_01 = setBrakeOn();
un_h_bridge_output1.bit_data.channel_02 = setBrakeOff();
printf("Brake: Motor forward\n");
break;
case 2: // 电机后退状态
un_h_bridge_output.bit_data.channel_01 = setBrakeOff();
un_h_bridge_output.bit_data.channel_02 = setBrakeOn();
un_h_bridge_output1.bit_data.channel_01 = setBrakeOff();
un_h_bridge_output1.bit_data.channel_02 = setBrakeOn();
printf("Brake: Motor reverse\n");
break;
default:
un_h_bridge_output.bit_data.channel_01 = setBrakeOff();
un_h_bridge_output.bit_data.channel_02 = setBrakeOff();
un_h_bridge_output1.bit_data.channel_01 = setBrakeOff();
un_h_bridge_output1.bit_data.channel_02 = setBrakeOff();
printf("Brake: Motor off\n");
break;
}
publishMessage(&un_h_bridge_output, 1);
publishMessage(&un_inf_can_kgf_output1, 1);
}
// 修改刹车定时器处理函数
static void brakeTimerProcess(void *signal_id)
{
(void)signal_id;
//#ifdef OIL_BRAKE
switch (brake_data.state)
{
case BRAKE_STATE_IDLE:
if (shouldApplyBrake())
{
brake_data.state = BRAKE_STATE_APPLYING_BRAKE;
brake_data.brake_motor_state = 1;
if( 0 == brake_data.brake_direction)
{
brake_data.brake_motor_state = 1;
}
else
{
brake_data.brake_motor_state = 2;
}
brakeOutput(NULL);
timerStart(&brake_data.brake_apply_timer, (uint32_t)(getParam("brk_on")), 0);
}
break;
case BRAKE_STATE_BRAKE_ON:
if (shouldReleaseBrake() && power_data.current_state == POWER_WORKING)
{
brake_data.state = BRAKE_STATE_RELEASING_BRAKE;
brake_data.brake_motor_state = 2;
brakeOutput(NULL);
if( 0 == brake_data.brake_direction)
{
brake_data.brake_motor_state = 2;
}
else
{
brake_data.brake_motor_state = 1;
}
timerStart(&brake_data.brake_release_timer, (uint32_t)(getParam("brk_off")), 0);
}
break;
case BRAKE_STATE_APPLYING_BRAKE:
if (!brake_data.brake_apply_timer.active)
{
brake_data.state = BRAKE_STATE_BRAKE_ON;
brake_data.brake_motor_state = 0;
brakeOutput(NULL);
brake_data.brake_position = 1; // 刹车位置：1表示刹车
}
break;
case BRAKE_STATE_RELEASING_BRAKE:
if (!brake_data.brake_release_timer.active)
{
brake_data.state = BRAKE_STATE_IDLE;
brake_data.brake_motor_state = 0;
brakeOutput(NULL);
brake_data.brake_position = 0; // 刹车位置：0表示未刹车
}
break;
default:
printf("ERROR: Unknown state\n");
brake_data.state = BRAKE_STATE_IDLE;
break;
}
// 如果刹车位置有变化，存入EEPROM
if (brake_data.brake_position != brake_data.old_brake_position)
{
setParam("brk_pos", (float)brake_data.brake_position);
brake_data.old_brake_position = brake_data.brake_position;
printf("writeE2 brake_position = %d\n",brake_data.brake_position);
}
timerStart(&brake_data.brake_timer, 100, 1); // 周期调用
}
// 处理所有输入信号的函数
static void brakeInput(void *signal_id)
{
// BrakeSystem old_data = brake_data; // 定义并初始化old_data
// 填充数据
if (signal_id == &un_sw_sample)
{
brake_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch;
}
else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据
{
brake_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1;
brake_data.mode_signal = ((uint8_t)un_remote_control_input.bit_data.switch_c == 1) ? 1 : 0;
}
else if (signal_id == &can_fault_info)
{
brake_data.remote_fault = !can_fault_info.bit_data.remote_state;
brake_data.can_bus_fault = !can_fault_info.bit_data.motor1_state || !can_fault_info.bit_data.motor2_state || !can_fault_info.bit_data.navigator_state;
}
else if (signal_id == &ethernet_fault_Info)
{
brake_data.ethernet_fault = !ethernet_fault_Info.bit_data.auto_state && !ethernet_fault_Info.bit_data.manual_state;
}
}
void paramUpdate(void *signal_id)
{
brake_data.brake_direction = (uint8_t)getParam("brk_rev");
timerStart(&brake_data.brake_param_timer, 1000, 1);
}
// 修改APP模块的初始化函数
void brakeAppInit(void)
{
// 初始化
memset(&brake_data, 0, sizeof(BrakeSystem));
brake_data.state = BRAKE_STATE_IDLE;
// 初始化时恢复刹车位置
brake_data.brake_position = (uint8_t)getParam("brk_pos");
brake_data.old_brake_position = brake_data.brake_position;
// 根据刹车位置恢复刹车状态
if (brake_data.brake_position == 1)
{
brake_data.state = BRAKE_STATE_BRAKE_ON;
}
else
{
brake_data.state = BRAKE_STATE_IDLE;
}
// 初始化定时器
timerInit(&brake_data.brake_timer);
timerInit(&brake_data.brake_apply_timer);
timerInit(&brake_data.brake_release_timer);
timerInit(&brake_data.brake_param_timer);
// 订阅输入信号,处理刹车逻辑
subscribe(&un_sw_sample, brakeInput);
subscribe(&un_remote_control_input, brakeInput);
subscribe(&can_fault_info, brakeInput);
subscribe(&ethernet_fault_Info, brakeInput);
// 订阅定时器信号,用于状态机的定时处理
subscribe(&brake_data.brake_timer, brakeTimerProcess);
subscribe(&brake_data.brake_apply_timer, brakeTimerProcess);
subscribe(&brake_data.brake_release_timer, brakeTimerProcess);
subscribe(&brake_data.brake_param_timer, paramUpdate);
// 启动定时器，定期调用 brakeTimerProcess
timerStart(&brake_data.brake_timer, 500, 1);
timerStart(&brake_data.brake_param_timer, 1000, 1);
printf("app_brake: initial OK \n");
}
#include "app_config.h"
#include "interface.h"
#include "app_frm_monitor.h"
#include "app_frm_signal.h"
#include "app_frm_timer.h"
#include "app_param_manage.h"
#include "app_power.h"
#include "app_brake.h"
#include "app_differential_drive.h"
// 使用内联函数
static inline uint8_t setBrakeOn(void) { return 1; }
static inline uint8_t setBrakeOff(void) { return 0; }
BrakeSystem brake_data;
// 判断是否需要刹车
static uint8_t shouldApplyBrake()
{
return (brake_data.emergency_stop_switch ||
brake_data.remote_emergency_stop ||
(brake_data.mode_signal == 0 && brake_data.remote_fault) ||
(brake_data.mode_signal == 1 && brake_data.can_bus_fault));//20241021 修改不计算以太网故障
// (brake_data.mode_signal == 1 && (brake_data.can_bus_fault || brake_data.ethernet_fault)));
}
// 判断是否需要释放刹车
static uint8_t shouldReleaseBrake()
{
return (!brake_data.emergency_stop_switch &&
!brake_data.remote_emergency_stop &&
((brake_data.mode_signal == 0 && !brake_data.remote_fault) ||
(brake_data.mode_signal == 1 && !brake_data.can_bus_fault)));//20241021 修改不计算以太网故障
// (brake_data.mode_signal == 1 && (!brake_data.can_bus_fault && !brake_data.ethernet_fault))));
}
// 输出处理函数
static void brakeOutput(void *signal_id)
{
(void)signal_id;
// 根据电机状态，填充发送数据结构，发送信号
switch (brake_data.brake_motor_state)
{
case 1: // 电机前进状态
un_h_bridge_output.bit_data.channel_01 = setBrakeOn();
un_h_bridge_output.bit_data.channel_02 = setBrakeOff();
un_h_bridge_output1.bit_data.channel_01 = setBrakeOn();
un_h_bridge_output1.bit_data.channel_02 = setBrakeOff();
printf("Brake: Motor forward\n");
break;
case 2: // 电机后退状态
un_h_bridge_output.bit_data.channel_01 = setBrakeOff();
un_h_bridge_output.bit_data.channel_02 = setBrakeOn();
un_h_bridge_output1.bit_data.channel_01 = setBrakeOff();
un_h_bridge_output1.bit_data.channel_02 = setBrakeOn();
printf("Brake: Motor reverse\n");
break;
default:
un_h_bridge_output.bit_data.channel_01 = setBrakeOff();
un_h_bridge_output.bit_data.channel_02 = setBrakeOff();
un_h_bridge_output1.bit_data.channel_01 = setBrakeOff();
un_h_bridge_output1.bit_data.channel_02 = setBrakeOff();
printf("Brake: Motor off\n");
break;
}
publishMessage(&un_h_bridge_output, 1);
publishMessage(&un_inf_can_kgf_output1, 1);
}
// 修改刹车定时器处理函数
static void brakeTimerProcess(void *signal_id)
{
(void)signal_id;
//#ifdef OIL_BRAKE
switch (brake_data.state)
{
case BRAKE_STATE_IDLE:
if (shouldApplyBrake())
{
brake_data.state = BRAKE_STATE_APPLYING_BRAKE;
if( 0 == brake_data.brake_direction)
{
brake_data.brake_motor_state = 1;
}
else
{
brake_data.brake_motor_state = 2;
}
brakeOutput(NULL);
timerStart(&brake_data.brake_apply_timer, (uint32_t)(getParam("brk_on")), 0);
}
break;
case BRAKE_STATE_BRAKE_ON:
if (shouldReleaseBrake() && power_data.current_state == POWER_WORKING)
{
brake_data.state = BRAKE_STATE_RELEASING_BRAKE;
if( 0 == brake_data.brake_direction)
{
brake_data.brake_motor_state = 2;
}
else
{
brake_data.brake_motor_state = 1;
}
brakeOutput(NULL);
timerStart(&brake_data.brake_release_timer, (uint32_t)(getParam("brk_off")), 0);
}
break;
case BRAKE_STATE_APPLYING_BRAKE:
if (!brake_data.brake_apply_timer.active)
{
brake_data.state = BRAKE_STATE_BRAKE_ON;
brake_data.brake_motor_state = 0;
brakeOutput(NULL);
brake_data.brake_position = 1; // 刹车位置：1表示刹车
}
break;
case BRAKE_STATE_RELEASING_BRAKE:
if (!brake_data.brake_release_timer.active)
{
brake_data.state = BRAKE_STATE_IDLE;
brake_data.brake_motor_state = 0;
brakeOutput(NULL);
brake_data.brake_position = 0; // 刹车位置：0表示未刹车
}
break;
default:
printf("ERROR: Unknown state\n");
brake_data.state = BRAKE_STATE_IDLE;
break;
}
// 如果刹车位置有变化，存入EEPROM
if (brake_data.brake_position != brake_data.old_brake_position)
{
setParam("brk_pos", (float)brake_data.brake_position);
brake_data.old_brake_position = brake_data.brake_position;
printf("writeE2 brake_position = %d\n",brake_data.brake_position);
}
timerStart(&brake_data.brake_timer, 100, 1); // 周期调用
}
// 处理所有输入信号的函数
static void brakeInput(void *signal_id)
{
// BrakeSystem old_data = brake_data; // 定义并初始化old_data
// 填充数据
if (signal_id == &un_sw_sample)
{
brake_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch;
}
else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据
{
brake_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1;
brake_data.mode_signal = ((uint8_t)un_remote_control_input.bit_data.switch_c == 1) ? 1 : 0;
}
else if (signal_id == &can_fault_info)
{
brake_data.remote_fault = !can_fault_info.bit_data.remote_state;
brake_data.can_bus_fault = !can_fault_info.bit_data.motor1_state || !can_fault_info.bit_data.motor2_state || !can_fault_info.bit_data.navigator_state;
}
else if (signal_id == &ethernet_fault_Info)
{
brake_data.ethernet_fault = !ethernet_fault_Info.bit_data.auto_state && !ethernet_fault_Info.bit_data.manual_state;
}
}
void paramUpdate(void *signal_id)
{
brake_data.brake_direction = (uint8_t)getParam("brk_rev");
timerStart(&brake_data.brake_param_timer, 1000, 1);
}
// 修改APP模块的初始化函数
void brakeAppInit(void)
{
// 初始化
memset(&brake_data, 0, sizeof(BrakeSystem));
brake_data.state = BRAKE_STATE_IDLE;
// 初始化时恢复刹车位置
brake_data.brake_position = (uint8_t)getParam("brk_pos");
brake_data.old_brake_position = brake_data.brake_position;
// 根据刹车位置恢复刹车状态
if (brake_data.brake_position == 1)
{
brake_data.state = BRAKE_STATE_BRAKE_ON;
}
else
{
brake_data.state = BRAKE_STATE_IDLE;
}
// 初始化定时器
timerInit(&brake_data.brake_timer);
timerInit(&brake_data.brake_apply_timer);
timerInit(&brake_data.brake_release_timer);
timerInit(&brake_data.brake_param_timer);
// 订阅输入信号,处理刹车逻辑
subscribe(&un_sw_sample, brakeInput);
subscribe(&un_remote_control_input, brakeInput);
subscribe(&can_fault_info, brakeInput);
subscribe(&ethernet_fault_Info, brakeInput);
// 订阅定时器信号,用于状态机的定时处理
subscribe(&brake_data.brake_timer, brakeTimerProcess);
subscribe(&brake_data.brake_apply_timer, brakeTimerProcess);
subscribe(&brake_data.brake_release_timer, brakeTimerProcess);
subscribe(&brake_data.brake_param_timer, paramUpdate);
// 启动定时器，定期调用 brakeTimerProcess
timerStart(&brake_data.brake_timer, 500, 1);
timerStart(&brake_data.brake_param_timer, 1000, 1);
printf("app_brake: initial OK \n");
}

436
app/app_differential_drive.c
View File

@@ -19,7 +19,8 @@ DiffData diff_data;
PID_t speed_pid;
PID_t yaw_rate_pid;
PID_t Acc_front_speed_pid;
PID_t Dec_front_speed_pid;
/**
@@ -43,7 +44,7 @@ float calculateTorqueOutput(uint8_t gear, float input_torque)
else if (gear == STATE_BACKWARD)
{
output_torque = (-input_torque + OFFSET) * SCALE_FACTOR;
}
}
else
{
output_torque = DEFAULT_VALUE;
@@ -52,8 +53,62 @@ float calculateTorqueOutput(uint8_t gear, float input_torque)
return output_torque;
}
/**
* @brief 车辆状态控制状态机
* @note 根据车速和扭矩方向切换前进/后退状态，带扭矩回滞保护
* @param ctx 状态机上下文，包含当前状态(STATE_INIT/FORWARD/BACKWARD)
* @param speed 当前车速（单位：km/h），0表示静止状态
* @param torque 当前扭矩（单位：Nm），正数表示前进方向，负数表示后退方向
*/
void handleVehicleState(MotorState *ctx, float speed, float torque)
{
switch (*ctx)
{
// 初始状态：根据扭矩方向初始化
case STATE_INIT:
{
if (torque >= 0.0f)
{
*ctx = STATE_FORWARD; // 正扭矩进前进档
}
else
{
*ctx = STATE_BACKWARD; // 负扭矩进倒档
}
break;
}
// 前进状态：零速且反向扭矩超阈值切倒档
case STATE_FORWARD:
{
if ( (speed == 0.0f) && (torque <= -TORQUE_HYSTERESIS_THRESHOLD) )
{
*ctx = STATE_BACKWARD; // 满足条件切换
}
else
{
*ctx = STATE_FORWARD; // 否则保持
}
break;
}
// 倒车状态：零速且正向扭矩超阈值切前进
case STATE_BACKWARD:
{
if ( (speed == 0.0f) && (torque >= TORQUE_HYSTERESIS_THRESHOLD) )
{
*ctx = STATE_FORWARD; // 满足条件切换
}
else
{
*ctx = STATE_BACKWARD; // 否则保持
}
break;
}
default:; // 异常处理
}
}
// 设置电机输出
@@ -63,36 +118,22 @@ void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint
float abs_left_front_speed = 0;
float abs_right_front_speed = 0;
float abs_left_rear_speed = 0;
float abs_right_rear_speed = 0;
float abs_right_rear_speed = 0;
// 档位
if(diff_data.state != STATE_STATIC_TURN)
{
un_motor_output1.bit_data.gear = diff_data.state; // 1 表示前进，2 表示后退，0空挡
un_motor_output2.bit_data.gear = diff_data.state;
un_motor_output3.bit_data.gear = diff_data.state; // 1 表示前进，2 表示后退，0空挡
un_motor_output4.bit_data.gear = diff_data.state;
}
else//原地转向状态的话，根据扭矩大小确定方向
{
un_motor_output1.bit_data.gear = (out_torq[0] < 0) ? STATE_BACKWARD : STATE_FORWARD;
un_motor_output2.bit_data.gear = (out_torq[1] < 0) ? STATE_BACKWARD : STATE_FORWARD;
un_motor_output3.bit_data.gear = (out_torq[2] < 0) ? STATE_BACKWARD : STATE_FORWARD;
un_motor_output4.bit_data.gear = (out_torq[3] < 0) ? STATE_BACKWARD : STATE_FORWARD;
}
abs_left_front_speed = calculateTorqueOutput(un_motor_output1.bit_data.gear, out_torq[0]); //根据挡位增加转矩方向
abs_right_front_speed = calculateTorqueOutput(un_motor_output2.bit_data.gear, out_torq[1]);
abs_left_rear_speed = calculateTorqueOutput(un_motor_output3.bit_data.gear, out_torq[2]);
abs_right_rear_speed = calculateTorqueOutput(un_motor_output4.bit_data.gear, out_torq[3]);
abs_left_front_speed = calculateTorqueOutput(diff_data.motor_state[0], out_torq[0]); //根据挡位增加转矩方向
abs_right_front_speed = calculateTorqueOutput(diff_data.motor_state[1], out_torq[1]);
abs_left_rear_speed = calculateTorqueOutput(diff_data.motor_state[2], out_torq[2]);
abs_right_rear_speed = calculateTorqueOutput(diff_data.motor_state[3], out_torq[3]);
un_motor_output1.bit_data.gear = diff_data.motor_state[0];
un_motor_output2.bit_data.gear = diff_data.motor_state[1];
if(STATE_FORWARD == un_motor_output3.bit_data.gear)//把后两台电机反相
if(STATE_FORWARD == diff_data.motor_state[2])//把后两台电机反相
{
un_motor_output3.bit_data.gear = STATE_BACKWARD;
}
else if(STATE_BACKWARD == un_motor_output3.bit_data.gear)
else if(STATE_BACKWARD == diff_data.motor_state[2])
{
un_motor_output3.bit_data.gear = STATE_FORWARD;
}
@@ -101,11 +142,11 @@ void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint
un_motor_output3.bit_data.gear = STATE_INIT;
}
if(STATE_FORWARD == un_motor_output4.bit_data.gear)
if(STATE_FORWARD == diff_data.motor_state[3])
{
un_motor_output4.bit_data.gear = STATE_BACKWARD;
}
else if(STATE_BACKWARD == un_motor_output4.bit_data.gear)
else if(STATE_BACKWARD == diff_data.motor_state[3])
{
un_motor_output4.bit_data.gear = STATE_FORWARD;
}
@@ -382,14 +423,18 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
}
#if THROTTLE_PID_MODE
float max_torque = (float)getParam("maxTorq");
float left_speed_mps = 0.0f;
float right_speed_mps = 0.0f;
float max_torque = diff_data.max_Torq;
linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque);
yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque);
float left_speed_mps = linear_velocity_x + yaw_rate;
float right_speed_mps = linear_velocity_x - yaw_rate;
yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque);
left_speed_mps = linear_velocity_x + yaw_rate;
right_speed_mps = linear_velocity_x - yaw_rate;
//扭矩分配
if(max_torque < left_speed_mps)
{
@@ -413,36 +458,25 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
}
else{}
// printf("input_torq: left=%.1f right=%.1f yaw_rate=%.1f\n", left_speed_mps, right_speed_mps, yaw_rate);
// printf("input_torq: left=%.1f right=%.1f yaw_rate=%.1f\n", left_speed_mps, right_speed_mps, yaw_rate);
// adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],100, 5);
// adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],100, 5);
// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);
motor_speed[0] = left_speed_mps;
motor_speed[0] = left_speed_mps;//加速状态，没有负扭矩，要么前进加速要么后退加速
motor_speed[2] = left_speed_mps;
motor_speed[1] = right_speed_mps;
motor_speed[3] = right_speed_mps;
// adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],100, 5);
// adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],100, 5);
//
// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);
motor_speed[3] = right_speed_mps;
handleVehicleState(&diff_data.motor_state[0], diff_data.left_front_motor_speed, motor_speed[0]); //通过扭矩以及速度来判断挡位
handleVehicleState(&diff_data.motor_state[1], diff_data.right_front_motor_speed, motor_speed[1]);
handleVehicleState(&diff_data.motor_state[2], diff_data.left_rear_motor_speed, motor_speed[2]);
handleVehicleState(&diff_data.motor_state[3], diff_data.right_rear_motor_speed, motor_speed[3]);
// distributeTorque(diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed,2*left_speed_mps,&motor_speed[0],&motor_speed[2],diff_data.max_Torq,diff_data.min_Torq);
// distributeTorque(diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed,2*right_speed_mps,&motor_speed[1],&motor_speed[3],diff_data.max_Torq,diff_data.min_Torq);
// printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", motor_speed[0], motor_speed[1], motor_speed[2], motor_speed[3]);
// // 返回计算结果
// *left_motor_speed = left_speed_mps;
// *right_motor_speed = right_speed_mps;
#else
// 限制线速度和偏航率
@@ -535,108 +569,6 @@ float mapRemoteControlSpeed(
return output_speed;
}
/**
* @brief 状态机处理函数（修改后版本）
*/
void handleVehicleState(DiffData *ctx)
{
switch (ctx->state)
{
//-------------------------------------------
// 初始状态：根据期望速度方向跳转
//-------------------------------------------
case STATE_INIT:
{
if (ctx->desired_speed < 0.0f)
{
ctx->state = STATE_BACKWARD;
}
else if( (ctx->desired_speed == 0.0f) && (ctx->desired_curvature != 0) )//无速度有转向
{
ctx->state = STATE_STATIC_TURN;
}
else
{
ctx->state = STATE_FORWARD;
}
break;
}
//-------------------------------------------
// 前进状态：处理反向指令（新增else分支）
//-------------------------------------------
case STATE_FORWARD:
{
if( (ctx->desired_speed < 0.0f) && (ctx->speed == 0.0f) )
{
ctx->state = STATE_BACKWARD; // 零速时允许切换方向
}
else if( (ctx->desired_speed < 0.0f) && (ctx->speed != 0.0f) )
{
ctx->desired_speed = 0.0f; // 非零速时清空期望速度
ctx->state = STATE_FORWARD; // 显式保持当前状态
}
else if( (ctx->desired_speed == 0.0f) && (ctx->speed == 0.0f) && (ctx->desired_curvature != 0.0f) )
{
ctx->state = STATE_STATIC_TURN; // 原地转向
}
else
{
ctx->state = STATE_FORWARD; // 新增：其他情况保持前进状态
}
break;
}
//-------------------------------------------
// 倒车状态：处理正向指令（新增else分支）
//-------------------------------------------
case STATE_BACKWARD:
{
if ((ctx->desired_speed > 0.0f) && (ctx->speed == 0.0f))
{
ctx->state = STATE_FORWARD; // 零速时允许切换方向
}
else if ((ctx->desired_speed > 0.0f) && (ctx->speed != 0.0f))
{
ctx->desired_speed = 0.0f; // 非零速时清空期望速度
ctx->state = STATE_BACKWARD; // 显式保持当前状态
}
else if( (ctx->desired_speed == 0.0f) && (ctx->speed == 0.0f) && (ctx->desired_curvature != 0.0f) )
{
ctx->state = STATE_STATIC_TURN; // 原地转向
}
else
{
ctx->state = STATE_BACKWARD; // 新增：其他情况保持倒车状态
}
break;
}
//-------------------------------------------
// 原地转向状态
//-------------------------------------------
case STATE_STATIC_TURN:
{
if ((ctx->desired_speed > 0.0f) && (ctx->speed == 0.0f))
{
ctx->state = STATE_FORWARD; // 零速时允许切换方向
}
else if ((ctx->desired_speed < 0.0f) && (ctx->speed == 0.0f))
{
ctx->state = STATE_BACKWARD; // 显式保持当前状态
}
else
{
ctx->state = STATE_STATIC_TURN; // 原地转向
}
break;
}
default:;
}
}
// 差速处理函数
@@ -665,23 +597,17 @@ static void diffProcess(void *signal_id)
{
diff_data.desired_yaw_rate = diff_data.desired_curvature * diff_data.desired_speed;
}
handleVehicleState(&diff_data); //20250704 换挡函数 速度为0才能换挡
// printf("desired_speed: %f, desired_yaw: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate);
// 使用 PID 控制器计算输出速度和曲率
float output_speed = calculatePidOutput(&speed_pid, diff_data.desired_speed, diff_data.speed, 0.0f, dt);
float output_yaw_rate = calculatePidOutput(&yaw_rate_pid, diff_data.desired_yaw_rate, diff_data.yaw_rate, 0.0f, dt);
// 计算最大加速度，用函数计算
float max_acceleration = calculateMaxAcceleration();
// 限制输出速度在当前速度和最大加速度计算出来的速度之间
// 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
{
resetPidIntegral(&speed_pid);
@@ -689,38 +615,68 @@ static void diffProcess(void *signal_id)
output_speed = 0;
output_yaw_rate = 0;
}
// 使用差速车辆动力学模型计算左右电机的期望速度
float out_torque[4] = {0,0,0,0};
// 使用差速车辆动力学模型计算左右电机的期望速度
computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, out_torque);
if( ( (diff_data.left_front_motor_speed / diff_data.left_rear_motor_speed) >= diff_data.diff_dead_zone ) || ( (diff_data.left_front_motor_speed / diff_data.left_rear_motor_speed) <= (1/diff_data.diff_dead_zone) ) )//如果超过2倍，或者小于2倍
{
diff_data.left_speed_diff = diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed;
diff_data.left_diff_touue = calculatePidOutput(&Acc_front_speed_pid, 0.0f, diff_data.left_speed_diff, 0.0f, dt); //左侧转速差PID
}
else
{
diff_data.left_speed_diff = 0;
Acc_front_speed_pid.integral = 0;
diff_data.left_diff_touue = 0;
}
if( ( (diff_data.right_front_motor_speed / diff_data.right_rear_motor_speed) >= diff_data.diff_dead_zone ) || ( (diff_data.right_front_motor_speed / diff_data.right_rear_motor_speed) <= (1/diff_data.diff_dead_zone) ) )//如果超过2倍，或者小于2倍
{
diff_data.right_speed_diff = diff_data.right_front_motor_speed - diff_data.right_rear_motor_speed;
diff_data.right_diff_touue = calculatePidOutput(&Dec_front_speed_pid, 0.0f, diff_data.right_speed_diff, 0.0f, dt); //左侧转速差PID
}
else
{
diff_data.right_speed_diff = 0;
Dec_front_speed_pid.integral = 0;
diff_data.right_diff_touue = 0;
}
if(out_torque[0] > 0)
{
diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, -2*out_torque[0], 2*out_torque[0]);
}
else
{
diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, 2*out_torque[0], -2*out_torque[0]);
}
if(out_torque[1] > 0)
{
diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, -2*out_torque[1], 2*out_torque[1]);
}
else
{
diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, 2*out_torque[1], -2*out_torque[1]);
}
diff_data.out_torq[0] = (2*out_torque[0] + diff_data.left_diff_touue)/2.0f;//因为每一个电机都是相同的扭矩，所以扭矩和为2倍。
diff_data.out_torq[2] = (2*out_torque[0] - diff_data.left_diff_touue)/2.0f;
diff_data.out_torq[1] = (2*out_torque[1] + diff_data.right_diff_touue)/2.0f;
diff_data.out_torq[3] = (2*out_torque[1] - diff_data.right_diff_touue)/2.0f;
// printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral);
// if(diff_data.desired_yaw_rate != 0)//有转向的情况下下
// {
// if( (output_yaw_rate > -500) && (output_yaw_rate < 500) )//如果是转向输出在-500~500之间，那么开始原地转向扭矩太小，所以设定最小扭矩。
// {
// output_yaw_rate = 500;
// }
// }
// 使用差速车辆动力学模型计算左右电机的期望速度
computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, &diff_data.out_torq[0]);
// if( (left_speed < 200) && (left_speed > -200) )
// {
// left_speed = 0;
// }
//
// if( (right_speed < 200) && (right_speed > -200) )
// {
// right_speed = 0;
// }
// 设置电机输出
setMotorOutput(&diff_data.out_torq[0],
@@ -744,8 +700,8 @@ static void diffProcess(void *signal_id)
un_can_debug_output.bit_data.curvature = (uint8_t)(int8_t)(diff_data.yaw_rate*10);
un_can_debug_output.bit_data.desired_curvature = (uint8_t)(int8_t)(diff_data.desired_yaw_rate*10);
un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(diff_data.left_motor_speed);
un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(diff_data.right_motor_speed);
un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(diff_data.left_speed_diff);
un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(diff_data.right_speed_diff);
publishMessage(&diff_data, 1);
@@ -800,7 +756,7 @@ static void diffInput(void *signal_id)
diff_data.desired_speed = diff_data.desired_speed * 0.01f;
diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f;
// 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出
diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5);
diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 1, 20, 5, 5, 0.5);
diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0.1, 2, 2, 1, 0.5);
diff_data.desired_curvature = -diff_data.desired_curvature;
@@ -843,8 +799,6 @@ static void diffInput(void *signal_id)
diff_data.left_rear_motor_speed = (float)((int16_t)(un_motor_input3.bit_data.speed - 30000));//20240921 增加偏移量
diff_data.left_rear_motor_speed = - diff_data.left_rear_motor_speed;//20250708 增加反相
if(fabs(diff_data.left_rear_motor_speed) > fabs(diff_data.left_front_motor_speed))//取速度较小的轮速
{
motor_speed_temp = diff_data.left_front_motor_speed;
@@ -902,8 +856,20 @@ static void diffInput(void *signal_id)
// {
// diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩
// }
diffProcess(&diff_data);//计算左右电机期望转速
if((power_data.current_state == POWER_WORKING))//电机上电才运行
{
diffProcess(&diff_data);//计算左右电机期望转速
}
else
{
resetPidIntegral(&speed_pid);
resetPidIntegral(&yaw_rate_pid);
diff_data.motor_state[0] = STATE_INIT;
diff_data.motor_state[1] = STATE_INIT;
diff_data.motor_state[2] = STATE_INIT;
diff_data.motor_state[3] = STATE_INIT;
}
}
@@ -969,14 +935,44 @@ void diffParametersInit(void *signal_id)
getParam("crv_ol")
);
}
diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩
printf("desired_speed: %f, desired_yaw_rate: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate);
printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);
// printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);
// 设置曲率 PID 控制器的参数
setPidParameters(&Dec_front_speed_pid,
getParam("mot_kp"),
getParam("mot_ki"),
getParam("mot_kd"),
getParam("mot_il"),
getParam("mot_ol")
);
// 设置曲率 PID 控制器的参数
setPidParameters(&Acc_front_speed_pid,
Dec_front_speed_pid.kp,
Dec_front_speed_pid.ki,
Dec_front_speed_pid.kd,
Dec_front_speed_pid.integral_limit,
Dec_front_speed_pid.output_limit
);
diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩
diff_data.max_Torq = (float)getParam("maxTorq");
printf("LF_speed = %f,RF_speed = %f,LR_speed = %f,RR_speed = %f\n",diff_data.left_front_motor_speed,diff_data.right_front_motor_speed,diff_data.left_rear_motor_speed,diff_data.right_rear_motor_speed);
if(0 == (float)getParam("diff_sp"))//20250711 防止参数为0，影响计算。
{
diff_data.diff_dead_zone = 2;
}
else
{
diff_data.diff_dead_zone = (float)getParam("diff_sp");//参数读取设定最大扭矩
}
printf("desired_speed: %f, desired_yaw_rate: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate);
printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);
// printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);
printf("LF_speed = %f,RF_speed = %f,LR_speed = %f,RR_speed = %f\n",diff_data.left_front_motor_speed,diff_data.right_front_motor_speed,diff_data.left_rear_motor_speed,diff_data.right_rear_motor_speed);
// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);
printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", diff_data.out_torq[0], diff_data.out_torq[1], diff_data.out_torq[2], diff_data.out_torq[3]);
@@ -985,14 +981,15 @@ void diffParametersInit(void *signal_id)
float deffcurvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature));
// 单位转换
deffspeed = deffspeed * 0.01f;
deffcurvature = deffcurvature * 0.0001f;
deffcurvature = deffcurvature * 0.0001f;
printf(" car state = %d\n", diff_data.state);
printf("remote_speed: %f, remote_yaw_rate: %f\n", deffspeed, deffcurvature);
printf(" car state = %d,%d,%d,%d\n", diff_data.motor_state[0],diff_data.motor_state[1],diff_data.motor_state[2],diff_data.motor_state[3]);
timerStart(&diff_app_timer,1000,1);//1s调用一次
}
// 差速初始化函数
void diffAppInit(void)
{
@@ -1032,6 +1029,29 @@ void diffAppInit(void)
getParam("crv_ol")
);
// 初始化减速 PID 控制器
initializePid(&Dec_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
// 设置 PID 控制器的参数
setPidParameters(&Dec_front_speed_pid,
getParam("mot_kp"),
getParam("mot_ki"),
getParam("mot_kd"),
getParam("mot_il"),
getParam("mot_ol")
);
// 初始化加速 PID 控制器
initializePid(&Acc_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
// 设置 PID 控制器的参数
setPidParameters(&Acc_front_speed_pid,
Dec_front_speed_pid.kp,
Dec_front_speed_pid.ki,
Dec_front_speed_pid.kd,
Dec_front_speed_pid.integral_limit,
Dec_front_speed_pid.output_limit
);
subscribe(&diff_app_timer, diffParametersInit);
timerStart(&diff_app_timer,1000,1);//1s调用一次

21
app/app_differential_drive.h
View File

@@ -14,11 +14,15 @@ extern "C"
#define SPEED_PID_MODE 0
#define THROTTLE_PID_MODE 1
#define TURN_MIN_TOUQUE 1 //n*m
#define SPEED_MODE 0x01
#define TORQUE_MODE 0x02
#define TORQUE_HYSTERESIS_THRESHOLD 0.3f
#define MOTOR_MODE TORQUE_MODE
@@ -32,7 +36,6 @@ typedef enum
STATE_INIT, ///< 初始状态（转速为0且等待扭矩方向判定）
STATE_FORWARD, ///< 前进
STATE_BACKWARD, ///< 后退
STATE_STATIC_TURN ///< 原地转向
} MotorState;
typedef enum
@@ -44,8 +47,8 @@ typedef enum
typedef struct DiffData
{
ControlMode mode ; // 控制模式
MotorState state; //当前车辆状态
float desired_speed; // 期望速度
MotorState motor_state[4]; //当前车辆状态
float desired_speed; // 期望速度
float desired_curvature; // 期望曲率
float left_motor_speed; // 当前左电机速度
float right_motor_speed; // 当前右电机速度
@@ -69,7 +72,15 @@ typedef struct DiffData
float out_right_motor_speed; // 输出右电机速度
float out_torq[4]; //4个电机扭矩
float max_Torq; // 最大扭矩限制
float min_Torq; // 最小扭矩限制
float min_Torq; // 最小扭矩限制
float left_speed_diff; // 左侧转速差
float right_speed_diff; // 右侧转速差
float left_diff_touue; // 左侧扭矩差
float right_diff_touue; // 右侧扭矩差
float diff_dead_zone; // 差速速度死区
} DiffData;

298
app/app_param_manage.h
View File

@@ -1,146 +1,152 @@
#ifndef APP_PARAM_MANAGE_H
#define APP_PARAM_MANAGE_H
#ifdef __cplusplus
extern "C"
{
#endif
#include "app_config.h"
// 定义参数列表宏
#define PARAM_LIST \
X(whl_bas) \
X(max_rpm) \
X(whl_dia) \
X(max_acc) \
X(spd_kp) \
X(spd_ki) \
X(spd_kd) \
X(spd_il) \
X(spd_ol) \
X(crv_kp) \
X(crv_ki) \
X(crv_kd) \
X(crv_il) \
X(crv_ol) \
X(brk_on) \
X(brk_off) \
X(maxTorq) \
X(feedPwr) \
X(dispPwr) \
X(VehMass) \
X(gRatio) \
X(prCTime) \
X(brk_pos) \
X(pwr_sta) \
X(high_sw) \
X(stop_sw) \
X(lightSt) \
X(pwr_btn) \
X(sleepTm) \
X(wakeTm) \
X(Ospd_kp) \
X(Ospd_ki) \
X(Ospd_kd) \
X(Ospd_il) \
X(Ospd_ol) \
X(Ocrv_kp) \
X(Ocrv_ki) \
X(Ocrv_kd) \
X(Ocrv_il) \
X(Ocrv_ol) \
X(minTorq) \
X(brk_rev) \
X(test)
// 定义一个包含所有参数名称的结构体
typedef struct {
#define X(name) const char* name;
PARAM_LIST
#undef X
} ParamNames;
// 参数结构体，不能使用位域，不要超过E2的大小, 1K byte
typedef struct
{
#define X(name) float name;
PARAM_LIST
#undef X
} ParamData;
typedef union
{
ParamData bit_data; // 使用定义的结构体变量名
uint8_t arr[sizeof(ParamData)]; // 通过结构体类型确定大小
} UnParamManager;
// 定义信号操作类型
typedef enum
{
READ_OPERATION,
WRITE_OPERATION
} OperationType;
// 定义信号数据结构
typedef struct
{
void *param_ptr; // 参数数据的指针
OperationType type; // 操作类型
size_t offset; // 参数在结构体中的偏移
size_t size; // 参数大小
} ParamSignal;
#pragma pack(push, 1)
typedef struct _StrParamRequest {
//--------------------------------------------------
uint16_t frame_header; // 帧头 固定值0xFF80 (16位)
uint16_t frame_type; // 帧类型 固定值0x002A (16位)
uint16_t frame_length; // 帧长 根据参数数据的长度动态设置 (16位)
uint8_t accumulated; // 累加值 按帧累加 (8位)
uint16_t request_id; // 请求帧ID 请求ID 100表示读，101 表示写参数 (16位)
char param_name[256][8]; // 参数名称 标识要写入或读出的参数
uint8_t data[256][4]; // 数据 用于写入或读出的参数值，一个参数最大4字节 (8位*4)
uint8_t crc; // CRC 按字节累加之和 取低8位 (8位)
} StrParamRequest;
typedef union _UnParamRequest {
StrParamRequest bit_data; // 使用定义的结构体变量名
unsigned char arr[sizeof(StrParamRequest)]; // 通过结构体类型确定大小
} UnParamRequest;
typedef struct {
UnParamRequest *param_request; // 指向 UnParamRequest 的指针
uint32_t sender_ip; // 发送方的 IP 地址（使用标准的32位整数表示）
uint16_t sender_port; // 发送方的端口号（使用标准的16位整数表示）
} RequestContext;
#pragma pack(pop)
extern UnParamRequest un_param_request;// 声明用于参数响应的帧实例
extern RequestContext request_context;
// 声明全局信号实例
extern ParamNames param_names;
extern UnParamManager param_manager; // 全局参数管理实例
extern ParamSignal param_signal;
extern uint8_t read_write_e2_finished;
extern RequestContext request_send;// 待发送的参数请求信号
uint8_t access_eeprom(size_t offset, void *data, size_t size, OperationType type);
void paramAppInit(void);
// 在适当的位置添加以下函数声明
float getParam(const char *param_name);
uint8_t setParam(const char *param_name, float value);
void printParams(void);
void handleParamOp(void *data);
void OnParamSignal(void *data);
#ifdef __cplusplus
}
#endif
#endif // APP_PARAM_MANAGE_H
#ifndef APP_PARAM_MANAGE_H
#define APP_PARAM_MANAGE_H
#ifdef __cplusplus
extern "C"
{
#endif
#include "app_config.h"
// 定义参数列表宏
#define PARAM_LIST \
X(whl_bas) \
X(max_rpm) \
X(whl_dia) \
X(max_acc) \
X(spd_kp) \
X(spd_ki) \
X(spd_kd) \
X(spd_il) \
X(spd_ol) \
X(crv_kp) \
X(crv_ki) \
X(crv_kd) \
X(crv_il) \
X(crv_ol) \
X(brk_on) \
X(brk_off) \
X(maxTorq) \
X(feedPwr) \
X(dispPwr) \
X(VehMass) \
X(gRatio) \
X(prCTime) \
X(brk_pos) \
X(pwr_sta) \
X(high_sw) \
X(stop_sw) \
X(lightSt) \
X(pwr_btn) \
X(sleepTm) \
X(wakeTm) \
X(Ospd_kp) \
X(Ospd_ki) \
X(Ospd_kd) \
X(Ospd_il) \
X(Ospd_ol) \
X(Ocrv_kp) \
X(Ocrv_ki) \
X(Ocrv_kd) \
X(Ocrv_il) \
X(Ocrv_ol) \
X(minTorq) \
X(brk_rev) \
X(mot_kp) \
X(mot_ki) \
X(mot_kd) \
X(mot_il) \
X(mot_ol) \
X(diff_sp) \
X(test)
// 定义一个包含所有参数名称的结构体
typedef struct {
#define X(name) const char* name;
PARAM_LIST
#undef X
} ParamNames;
// 参数结构体，不能使用位域，不要超过E2的大小, 1K byte
typedef struct
{
#define X(name) float name;
PARAM_LIST
#undef X
} ParamData;
typedef union
{
ParamData bit_data; // 使用定义的结构体变量名
uint8_t arr[sizeof(ParamData)]; // 通过结构体类型确定大小
} UnParamManager;
// 定义信号操作类型
typedef enum
{
READ_OPERATION,
WRITE_OPERATION
} OperationType;
// 定义信号数据结构
typedef struct
{
void *param_ptr; // 参数数据的指针
OperationType type; // 操作类型
size_t offset; // 参数在结构体中的偏移
size_t size; // 参数大小
} ParamSignal;
#pragma pack(push, 1)
typedef struct _StrParamRequest {
//--------------------------------------------------
uint16_t frame_header; // 帧头 固定值0xFF80 (16位)
uint16_t frame_type; // 帧类型 固定值0x002A (16位)
uint16_t frame_length; // 帧长 根据参数数据的长度动态设置 (16位)
uint8_t accumulated; // 累加值 按帧累加 (8位)
uint16_t request_id; // 请求帧ID 请求ID 100表示读，101 表示写参数 (16位)
char param_name[256][8]; // 参数名称 标识要写入或读出的参数
uint8_t data[256][4]; // 数据 用于写入或读出的参数值，一个参数最大4字节 (8位*4)
uint8_t crc; // CRC 按字节累加之和 取低8位 (8位)
} StrParamRequest;
typedef union _UnParamRequest {
StrParamRequest bit_data; // 使用定义的结构体变量名
unsigned char arr[sizeof(StrParamRequest)]; // 通过结构体类型确定大小
} UnParamRequest;
typedef struct {
UnParamRequest *param_request; // 指向 UnParamRequest 的指针
uint32_t sender_ip; // 发送方的 IP 地址（使用标准的32位整数表示）
uint16_t sender_port; // 发送方的端口号（使用标准的16位整数表示）
} RequestContext;
#pragma pack(pop)
extern UnParamRequest un_param_request;// 声明用于参数响应的帧实例
extern RequestContext request_context;
// 声明全局信号实例
extern ParamNames param_names;
extern UnParamManager param_manager; // 全局参数管理实例
extern ParamSignal param_signal;
extern uint8_t read_write_e2_finished;
extern RequestContext request_send;// 待发送的参数请求信号
uint8_t access_eeprom(size_t offset, void *data, size_t size, OperationType type);
void paramAppInit(void);
// 在适当的位置添加以下函数声明
float getParam(const char *param_name);
uint8_t setParam(const char *param_name, float value);
void printParams(void);
void handleParamOp(void *data);
void OnParamSignal(void *data);
#ifdef __cplusplus
}
#endif
#endif // APP_PARAM_MANAGE_H

2
interface_can.c
View File

@@ -323,7 +323,7 @@ void flexcan_Receive_callback_1(flexcan_handle_t *handle,
un_motor_input3.arr[i] = buf->dataBuffer[i];
}
//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
publishMessage(&un_motor_input3, 1);
// publishMessage(&un_motor_input3, 1); //<2F>޸<EFBFBD>Ϊ
}
else if( LEFT_REAR_MOTOR2_INPUT2 == (buf->id) )
{