Compare commits
1 Commits
20251022-7
...
转速模式
| Author | SHA1 | Date | |
|---|---|---|---|
| 9a98344f85 |
@@ -19,8 +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;
|
||||
PID_t left_feed_pid;
|
||||
PID_t right_feed_pid;
|
||||
|
||||
|
||||
/**
|
||||
@@ -112,7 +112,7 @@ void handleVehicleState(MotorState *ctx, float speed, float torque)
|
||||
|
||||
|
||||
// 设置电机输出
|
||||
void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint16_t discharge_power)
|
||||
void setMotorOutput(float *out_torq, float max_torque, uint16_t left_feed_power,uint16_t right_feed_power, uint16_t discharge_power)
|
||||
{
|
||||
|
||||
float abs_left_front_speed = 0;
|
||||
@@ -121,44 +121,23 @@ void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint
|
||||
float abs_right_rear_speed = 0;
|
||||
|
||||
// 档位
|
||||
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 = (out_torq[0] >= 0) ? 1 : 2; // 1 表示前进，2 表示后退
|
||||
un_motor_output2.bit_data.gear = (out_torq[1] >= 0) ? 1 : 2;
|
||||
un_motor_output3.bit_data.gear = (out_torq[2] >= 0) ? 2 : 1; //20250717 2,3电机反相
|
||||
un_motor_output4.bit_data.gear = (out_torq[3] >= 0) ? 2 : 1;
|
||||
|
||||
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 == diff_data.motor_state[2])//把后两台电机反相
|
||||
{
|
||||
un_motor_output3.bit_data.gear = STATE_BACKWARD;
|
||||
}
|
||||
else if(STATE_BACKWARD == diff_data.motor_state[2])
|
||||
{
|
||||
un_motor_output3.bit_data.gear = STATE_FORWARD;
|
||||
}
|
||||
else
|
||||
{
|
||||
un_motor_output3.bit_data.gear = STATE_INIT;
|
||||
}
|
||||
|
||||
if(STATE_FORWARD == diff_data.motor_state[3])
|
||||
{
|
||||
un_motor_output4.bit_data.gear = STATE_BACKWARD;
|
||||
}
|
||||
else if(STATE_BACKWARD == diff_data.motor_state[3])
|
||||
{
|
||||
un_motor_output4.bit_data.gear = STATE_FORWARD;
|
||||
}
|
||||
else
|
||||
{
|
||||
un_motor_output4.bit_data.gear = STATE_INIT;
|
||||
}
|
||||
|
||||
abs_left_front_speed = fabsf(out_torq[0]); //根据挡位增加转矩方向
|
||||
abs_right_front_speed = fabsf(out_torq[1]);
|
||||
abs_left_rear_speed = fabsf(out_torq[2]);
|
||||
abs_right_rear_speed = fabsf(out_torq[3]);
|
||||
|
||||
|
||||
// 设置左右电机期望转速
|
||||
// un_motor_output1.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_left_speed) + 30000); // 20240921 增加偏移量 30000
|
||||
// un_motor_output2.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_right_speed) + 30000); // 20240921 增加偏移量 30000
|
||||
un_motor_output1.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_left_front_speed) + 30000); // 20240921 增加偏移量 30000
|
||||
un_motor_output2.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_right_front_speed) + 30000); // 20240921 增加偏移量 30000
|
||||
un_motor_output3.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_left_rear_speed) + 30000); // 20240921 增加偏移量 30000
|
||||
un_motor_output4.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_right_rear_speed) + 30000); // 20240921 增加偏移量 30000
|
||||
|
||||
|
||||
// 设置模式为扭矩模式
|
||||
un_motor_output1.bit_data.mode = MOTOR_MODE;
|
||||
@@ -167,31 +146,22 @@ void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint
|
||||
un_motor_output4.bit_data.mode = MOTOR_MODE;
|
||||
|
||||
// 设置扭矩
|
||||
un_motor_output1.bit_data.set_torque = (uint16_t)( (int16_t)abs_left_front_speed );
|
||||
un_motor_output2.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_front_speed );
|
||||
un_motor_output3.bit_data.set_torque = (uint16_t)( (int16_t)abs_left_rear_speed );
|
||||
un_motor_output4.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_rear_speed );
|
||||
|
||||
//设定转速
|
||||
un_motor_output1.bit_data.set_rotation_speed = 30000;
|
||||
un_motor_output2.bit_data.set_rotation_speed = 30000;
|
||||
un_motor_output3.bit_data.set_rotation_speed = 30000;
|
||||
un_motor_output4.bit_data.set_rotation_speed = 30000;
|
||||
un_motor_output1.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量
|
||||
un_motor_output2.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量
|
||||
un_motor_output3.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量
|
||||
un_motor_output4.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量
|
||||
|
||||
// 设置馈电功率
|
||||
un_motor_output1.bit_data.feed_power = feed_power;
|
||||
un_motor_output2.bit_data.feed_power = feed_power;
|
||||
un_motor_output3.bit_data.feed_power = feed_power;
|
||||
un_motor_output4.bit_data.feed_power = feed_power;
|
||||
|
||||
un_motor_output1.bit_data.feed_power = left_feed_power;
|
||||
un_motor_output2.bit_data.feed_power = right_feed_power;
|
||||
un_motor_output3.bit_data.feed_power = left_feed_power;
|
||||
un_motor_output4.bit_data.feed_power = right_feed_power;
|
||||
|
||||
// 设置放电功率
|
||||
un_motor_output1.bit_data.discharge_power = discharge_power;
|
||||
un_motor_output2.bit_data.discharge_power = discharge_power;
|
||||
un_motor_output3.bit_data.discharge_power = discharge_power;
|
||||
un_motor_output4.bit_data.discharge_power = discharge_power;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -322,93 +292,25 @@ void calculateCurrentState(float dt)
|
||||
diff_data.max_speed = calculateMaxSpeed();
|
||||
previous_speed = diff_data.speed;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 基于转速反比的双电机扭矩分配函数
|
||||
* @param rpm1 电机1当前转速（单位：rpm）
|
||||
* @param rpm2 电机2当前转速（单位：rpm）
|
||||
* @param total_torque 系统总需求扭矩（单位：Nm）
|
||||
* @param torque1 [out] 电机1分配到的扭矩（单位：Nm）
|
||||
* @param torque2 [out] 电机2分配到的扭矩（单位：Nm）
|
||||
* @note 分配原则：转速越高的电机分配扭矩越小，确保负载均衡
|
||||
* @brief 判断减速状态（最简逻辑） 如果同向或者有一个为0，或者都为0，那么判断绝对值大小，如果期望绝对值小于当前绝对值，那就为减速
|
||||
* @param target_speed 期望速度（带方向）
|
||||
* @param current_speed 当前速度（带方向）
|
||||
* @return 1:减速, 0:加速或保持
|
||||
*/
|
||||
void distributeTorque(float rpm1, float rpm2, float total_torque, float* torque1, float* torque2, float max_torque, float min_torque)
|
||||
uint8_t is_Decelerating(float target_speed, float current_speed, float des_yaw_rate)
|
||||
{
|
||||
|
||||
// 总扭矩为0时快速返回
|
||||
if (fabs(total_torque) < 0.001f) {
|
||||
*torque1 = 0.0f;
|
||||
*torque2 = 0.0f;
|
||||
return;
|
||||
// 特殊处理双零状态,双零表示刹车
|
||||
if ( (target_speed == 0.0f && current_speed == 0.0f) )//如果又减速的话也刹车 //|| (0 != des_yaw_rate)
|
||||
{
|
||||
return 2; // 驻车
|
||||
}
|
||||
|
||||
// // 保护条件：当两电机均静止时采用平均分配策略
|
||||
// if (fabs(rpm1) < 0.001f && fabs(rpm2) < 0.001f) {
|
||||
// *torque1 = total_torque / 2.0f;
|
||||
// *torque2 = total_torque / 2.0f;
|
||||
// return;
|
||||
// }
|
||||
|
||||
// 计算权重因子（与转速成反比关系）
|
||||
// 注：添加0.001f防止零转速时除零错误，fabs确保负转速正确处理
|
||||
float weight1 = 1.0f / (fabs(rpm1) + 0.001f);
|
||||
float weight2 = 1.0f / (fabs(rpm2) + 0.001f);
|
||||
|
||||
// 归一化计算分配比例
|
||||
float total_weight = weight1 + weight2;
|
||||
*torque1 = total_torque * (weight1 / total_weight);
|
||||
*torque2 = total_torque * (weight2 / total_weight);
|
||||
|
||||
// 独立限制单侧扭矩（修改核心逻辑）
|
||||
if (fabs(*torque1) > max_torque) {
|
||||
*torque1 = copysignf(max_torque, *torque1);
|
||||
}
|
||||
if (fabs(*torque2) > max_torque) {
|
||||
*torque2 = copysignf(max_torque, *torque2);
|
||||
}
|
||||
|
||||
// 仅对非零扭矩应用下限限制
|
||||
if (fabs(*torque1) < min_torque) {
|
||||
*torque1 = copysignf(min_torque, *torque1);
|
||||
}
|
||||
if ( fabs(*torque2) < min_torque) {
|
||||
*torque2 = copysignf(min_torque, *torque2);
|
||||
}
|
||||
|
||||
// 核心逻辑：方向相反 或 (同向/含零且期望绝对值 < 当前绝对值)
|
||||
return (signbit(target_speed) != signbit(current_speed)) ||
|
||||
(fabs(target_speed) < fabs(current_speed));
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 根据轮速差动态调整电机扭矩（带非负限制）
|
||||
* @param speed_left 左轮速度（单位：rpm或自定义）
|
||||
* @param speed_right 右轮速度（单位：rpm或自定义）
|
||||
* @param torque_left 左轮扭矩指针（单位：Nm或自定义）
|
||||
* @param torque_right 右轮扭矩指针（单位：Nm或自定义）
|
||||
* @param threshold 触发调整的速差阈值（单位同轮速）
|
||||
* @param k 扭矩调整系数（无量纲，建议0<k<1）
|
||||
* @note 函数会直接修改传入的扭矩值，并确保扭矩不小于0
|
||||
*/
|
||||
void adjust_torque_by_speed_diff(float speed_left, float speed_right,
|
||||
float* torque_left, float* torque_right,
|
||||
float threshold, float k) {
|
||||
// 计算轮速差绝对值
|
||||
float speed_diff = fabsf(speed_left - speed_right);
|
||||
|
||||
if (speed_diff > threshold) {
|
||||
// 计算需要减少的扭矩量（速差超出阈值部分×系数）
|
||||
float torque_reduction = (speed_diff - threshold) * k;
|
||||
|
||||
if (speed_left > speed_right) {
|
||||
// 左轮过快时减少左扭矩，并限制最小值为0
|
||||
*torque_left = fmaxf(*torque_left - torque_reduction, 0.0f);
|
||||
} else {
|
||||
// 右轮过快时减少右扭矩，并限制最小值为0
|
||||
*torque_right = fmaxf(*torque_right - torque_reduction, 0.0f);
|
||||
}
|
||||
}
|
||||
}
|
||||
// 计算左右电机速度
|
||||
void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max_speed, float *motor_speed)
|
||||
{
|
||||
@@ -427,17 +329,13 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
|
||||
float left_speed_mps = 0.0f;
|
||||
float right_speed_mps = 0.0f;
|
||||
|
||||
float max_torque = diff_data.max_Torq;//不需要限制，PID输出已经限制了
|
||||
// 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;
|
||||
}
|
||||
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);
|
||||
|
||||
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)
|
||||
@@ -471,13 +369,12 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
|
||||
motor_speed[2] = left_speed_mps;
|
||||
|
||||
motor_speed[1] = right_speed_mps;
|
||||
motor_speed[3] = right_speed_mps;
|
||||
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]);
|
||||
|
||||
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);
|
||||
@@ -493,8 +390,8 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
|
||||
float rotational_velocity = ((float)getParam("whl_bas") / 2.0f) * yaw_rate;
|
||||
|
||||
// 计算车辆左右线速度 (m/s)
|
||||
float left_speed_mps = linear_velocity_x - rotational_velocity; //20250316 为解决原地转向和直行转向相同，所以把左右输出的速度交换
|
||||
float right_speed_mps = linear_velocity_x + rotational_velocity;
|
||||
float left_speed_mps = linear_velocity_x + rotational_velocity; //20250316 为解决原地转向和直行转向相同，所以把左右输出的速度交换
|
||||
float right_speed_mps = linear_velocity_x - rotational_velocity;
|
||||
|
||||
// 计算轮子周长
|
||||
float wheel_circumference = (float)getParam("whl_dia") * M_PI;
|
||||
@@ -514,21 +411,77 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
|
||||
float max_motor_rpm = (float)getParam("max_rpm");
|
||||
left_motor_rpm = constrain(left_motor_rpm, -max_motor_rpm, max_motor_rpm);
|
||||
right_motor_rpm = constrain(right_motor_rpm, -max_motor_rpm, max_motor_rpm);
|
||||
|
||||
// 当电机转速小于50转时，设置为0
|
||||
if (fabsf(left_motor_rpm) < 50)//速度慢所以设置位10转
|
||||
{
|
||||
left_motor_rpm = 0;
|
||||
}
|
||||
if (fabsf(right_motor_rpm) < 50)//速度慢所以设置位10转
|
||||
{
|
||||
right_motor_rpm = 0;
|
||||
}
|
||||
// if (fabsf(left_motor_rpm) < 20)//速度慢所以设置位10转
|
||||
// {
|
||||
// left_motor_rpm = 0;
|
||||
// }
|
||||
// if (fabsf(right_motor_rpm) < 20)//速度慢所以设置位10转
|
||||
// {
|
||||
// right_motor_rpm = 0;
|
||||
// }
|
||||
// 左边电机方向反一下，因为电机安装反了，返回来的数据也要反一下
|
||||
// left_motor_rpm = -left_motor_rpm;
|
||||
// 返回计算结果
|
||||
*left_motor_speed = left_motor_rpm;
|
||||
*right_motor_speed = right_motor_rpm;
|
||||
motor_speed[0] = left_motor_rpm;//加速状态，没有负扭矩，要么前进加速要么后退加速
|
||||
motor_speed[2] = left_motor_rpm;
|
||||
|
||||
motor_speed[1] = right_motor_rpm;
|
||||
motor_speed[3] = right_motor_rpm;
|
||||
|
||||
diff_data.left_motor_state = is_Decelerating(left_motor_rpm, diff_data.left_motor_speed, diff_data.desired_yaw_rate);
|
||||
diff_data.right_motor_state = is_Decelerating(right_motor_rpm, diff_data.right_motor_speed, diff_data.desired_yaw_rate);
|
||||
|
||||
// printf(" left = %d,%d\n", diff_data.left_motor_state,diff_data.right_motor_state);
|
||||
|
||||
//馈电PID计算
|
||||
static float previous_time11 = 0.0f;
|
||||
|
||||
float time1 = (float)getCurrentTime();
|
||||
float dt = (time1 - previous_time11) / PERIOD_TICK;
|
||||
previous_time11 = time1;
|
||||
|
||||
float left_feed_pwoer = calculatePidOutput(&left_feed_pid, left_motor_rpm, diff_data.left_motor_speed, 0.0f, dt);//左右馈电PID
|
||||
float right_feed_pwoer = calculatePidOutput(&right_feed_pid, right_motor_rpm, diff_data.right_motor_speed, 0.0f, dt);
|
||||
|
||||
if(1 == diff_data.left_motor_state)//根据是否是刹车状态来确定是否设定馈电功率
|
||||
{
|
||||
diff_data.left_motor_feed_power = diff_data.max_feed_power;//20250723 修改为固定值最大值
|
||||
}
|
||||
else if(2 == diff_data.left_motor_state)
|
||||
{
|
||||
diff_data.left_motor_feed_power = diff_data.max_feed_power;
|
||||
}
|
||||
else
|
||||
{
|
||||
diff_data.left_motor_feed_power = 0.0f;
|
||||
}
|
||||
|
||||
if(1 == diff_data.right_motor_state)//根据是否是刹车状态来确定是否设定馈电功率
|
||||
{
|
||||
diff_data.right_motor_feed_power = diff_data.max_feed_power;//20250723 修改为固定值最大值
|
||||
}
|
||||
else if(2 == diff_data.left_motor_state)
|
||||
{
|
||||
diff_data.right_motor_feed_power = diff_data.max_feed_power;
|
||||
}
|
||||
else
|
||||
{
|
||||
diff_data.right_motor_feed_power = 0.0f;
|
||||
}
|
||||
|
||||
//限制最大馈电功率
|
||||
if(diff_data.left_motor_feed_power > diff_data.max_feed_power)
|
||||
{
|
||||
diff_data.left_motor_feed_power = diff_data.max_feed_power;
|
||||
}
|
||||
|
||||
if(diff_data.right_motor_feed_power > diff_data.max_feed_power)
|
||||
{
|
||||
diff_data.right_motor_feed_power = diff_data.max_feed_power;
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -613,86 +566,23 @@ 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) && ( 10 > fabs(diff_data.left_motor_speed) ) && ( 10 > fabs(diff_data.right_motor_speed) ) )//手柄回中，速度小的时候清0
|
||||
{
|
||||
resetPidIntegral(&speed_pid);
|
||||
resetPidIntegral(&yaw_rate_pid);
|
||||
output_speed = 0;
|
||||
output_yaw_rate = 0;
|
||||
}
|
||||
// if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) )//手柄回中，速度小的时候清0
|
||||
// {
|
||||
// resetPidIntegral(&speed_pid);
|
||||
// resetPidIntegral(&yaw_rate_pid);
|
||||
// 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);
|
||||
|
||||
computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, &diff_data.out_torq[0]);
|
||||
|
||||
|
||||
if( fabs(diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed) >= diff_data.diff_dead_zone )//如果超过系数
|
||||
{
|
||||
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( fabs(diff_data.right_front_motor_speed - diff_data.right_rear_motor_speed) >= diff_data.diff_dead_zone )//如果超过系数
|
||||
{
|
||||
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)//根据大小来限定值为分配扭矩。最小就是0扭矩。
|
||||
{
|
||||
diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, -out_torque[0], out_torque[0]);
|
||||
}
|
||||
else
|
||||
{
|
||||
diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, out_torque[0], -out_torque[0]);
|
||||
}
|
||||
|
||||
|
||||
if(out_torque[1] > 0)
|
||||
{
|
||||
diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, -out_torque[1], out_torque[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, out_torque[1], -out_torque[1]);
|
||||
}
|
||||
|
||||
|
||||
diff_data.out_torq[0] = (out_torque[0] + diff_data.left_diff_touue);//因为每一个电机都是相同的扭矩，所以扭矩和为2倍。
|
||||
diff_data.out_torq[2] = (out_torque[0] - diff_data.left_diff_touue);
|
||||
|
||||
diff_data.out_torq[1] = (out_torque[1] + diff_data.right_diff_touue);
|
||||
diff_data.out_torq[3] = (out_torque[1] - diff_data.right_diff_touue);
|
||||
|
||||
out_torque[0] = constrain(out_torque[0], -diff_data.max_Torq, diff_data.max_Torq); //限定最大扭矩
|
||||
out_torque[1] = constrain(out_torque[1], -diff_data.max_Torq, diff_data.max_Torq);
|
||||
out_torque[2] = constrain(out_torque[2], -diff_data.max_Torq, diff_data.max_Torq);
|
||||
out_torque[3] = constrain(out_torque[3], -diff_data.max_Torq, diff_data.max_Torq);
|
||||
|
||||
// printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral);
|
||||
|
||||
|
||||
|
||||
// 设置电机输出
|
||||
setMotorOutput(&diff_data.out_torq[0],
|
||||
diff_data.max_Torq,//
|
||||
(uint16_t)getParam("feedPwr"),
|
||||
diff_data.left_motor_feed_power,
|
||||
diff_data.right_motor_feed_power,
|
||||
(uint16_t)getParam("dispPwr"));
|
||||
// 发布左右电机期望转速,电源在工作状态才能发送
|
||||
if (power_data.current_state == POWER_WORKING)
|
||||
@@ -700,8 +590,7 @@ static void diffProcess(void *signal_id)
|
||||
publishMessage(&un_motor_output1, 1);
|
||||
publishMessage(&un_motor_output2, 1);
|
||||
publishMessage(&un_motor_output3, 1);
|
||||
publishMessage(&un_motor_output4, 1);
|
||||
|
||||
publishMessage(&un_motor_output4, 1);
|
||||
}
|
||||
|
||||
|
||||
@@ -711,45 +600,15 @@ 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_speed_diff);
|
||||
un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(diff_data.right_speed_diff);
|
||||
// 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);
|
||||
|
||||
}
|
||||
|
||||
/******************************************************************************
|
||||
Filter(); N个数中取两个
|
||||
******************************************************************************/
|
||||
int16_t Filter(int16_t *s,uint8_t Len)
|
||||
{
|
||||
uint8_t i,j;
|
||||
int16_t temp;
|
||||
//降序排序
|
||||
for(i=0;i<Len-1;i++)
|
||||
for(j=i+1;j<Len;j++)
|
||||
{
|
||||
if(*(s+i)>*(s+j))
|
||||
{
|
||||
*(s+i)=*(s+i)^*(s+j);
|
||||
*(s+j)=*(s+j)^*(s+i);
|
||||
*(s+i)=*(s+i)^*(s+j);
|
||||
}
|
||||
}
|
||||
temp=(*(s+Len/2)+*(s+(Len/2-1)))/2;//20210225修改为除以2，负数不能够右移
|
||||
return(temp);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 差速输入处理函数
|
||||
static void diffInput(void *signal_id)
|
||||
{
|
||||
float motor_speed_temp = 0.0f;
|
||||
|
||||
if (signal_id == &un_sw_sample)
|
||||
{
|
||||
diff_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch;
|
||||
@@ -767,7 +626,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, 1, 20, 5, 5, 0.5);
|
||||
diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.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;
|
||||
@@ -810,33 +669,56 @@ 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))//取速度较小的轮速
|
||||
if( fabs(diff_data.left_front_motor_speed) < 20)//速度死区
|
||||
{
|
||||
motor_speed_temp = diff_data.left_front_motor_speed;
|
||||
diff_data.left_front_motor_speed = 0;
|
||||
}
|
||||
else
|
||||
|
||||
if( fabs(diff_data.left_rear_motor_speed) < 20)//速度死区
|
||||
{
|
||||
motor_speed_temp = diff_data.left_rear_motor_speed;
|
||||
}
|
||||
diff_data.left_motor_speed = motor_speed_temp;
|
||||
diff_data.left_rear_motor_speed = 0;
|
||||
}
|
||||
|
||||
// if(fabs(diff_data.left_rear_motor_speed) > fabs(diff_data.left_front_motor_speed))//取速度较小的轮速
|
||||
// {
|
||||
// motor_speed_temp = diff_data.left_front_motor_speed;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// motor_speed_temp = diff_data.left_rear_motor_speed;
|
||||
// }
|
||||
// diff_data.left_motor_speed = motor_speed_temp;
|
||||
|
||||
diff_data.left_motor_speed = (diff_data.left_front_motor_speed + diff_data.left_rear_motor_speed)/2.0f;
|
||||
}
|
||||
else if( (signal_id == &un_motor_input2) || (signal_id == &un_motor_input4) )// 处理第二个电机速度信号（右电机）
|
||||
{
|
||||
diff_data.right_front_motor_speed = (float)((int16_t)(un_motor_input2.bit_data.speed - 30000)); // 20250502 1号控制器增加反相
|
||||
diff_data.right_rear_motor_speed = (float)((int16_t)(un_motor_input4.bit_data.speed - 30000));
|
||||
diff_data.right_front_motor_speed = (float)((int16_t)(un_motor_input2.bit_data.speed - 30000)); // 20250502 1号控制器增加反相
|
||||
|
||||
diff_data.right_rear_motor_speed = - diff_data.right_rear_motor_speed;//20250708 增加反相
|
||||
|
||||
if(fabs(diff_data.right_front_motor_speed) > fabs(diff_data.right_rear_motor_speed))//取速度较小的轮速
|
||||
{
|
||||
motor_speed_temp = diff_data.right_rear_motor_speed;
|
||||
}
|
||||
else
|
||||
{
|
||||
motor_speed_temp = diff_data.right_front_motor_speed;
|
||||
}
|
||||
diff_data.right_rear_motor_speed = (float)((int16_t)(un_motor_input4.bit_data.speed - 30000));
|
||||
diff_data.right_rear_motor_speed = - diff_data.right_rear_motor_speed;//20250708 增加反相
|
||||
|
||||
diff_data.right_motor_speed = motor_speed_temp;
|
||||
if( fabs(diff_data.right_front_motor_speed) < 20)//速度死区
|
||||
{
|
||||
diff_data.right_front_motor_speed = 0;
|
||||
}
|
||||
|
||||
if( fabs(diff_data.right_rear_motor_speed) < 20)//速度死区
|
||||
{
|
||||
diff_data.right_rear_motor_speed = 0;
|
||||
}
|
||||
|
||||
|
||||
// if(fabs(diff_data.right_front_motor_speed) > fabs(diff_data.right_rear_motor_speed))//取速度较小的轮速
|
||||
// {
|
||||
// motor_speed_temp = diff_data.right_rear_motor_speed;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// motor_speed_temp = diff_data.right_front_motor_speed;
|
||||
// }
|
||||
|
||||
diff_data.right_motor_speed = (diff_data.right_rear_motor_speed + diff_data.right_front_motor_speed)/2.0f;
|
||||
}
|
||||
|
||||
// 急停开关
|
||||
@@ -855,30 +737,26 @@ static void diffInput(void *signal_id)
|
||||
diff_data.desired_curvature = 0.0;
|
||||
}
|
||||
|
||||
// if (diff_data.emergency_stop_state == 1)//刹车 20241017 增加的扭矩限制
|
||||
// {
|
||||
// diff_data.max_Torq = 5;//20240403修改。刹车就是5N
|
||||
// }
|
||||
// else if ((0 == diff_data.desired_speed) && (0 == diff_data.desired_curvature) && (diff_data.left_motor_speed > -100) && (diff_data.left_motor_speed < 100)&& (((diff_data.right_motor_speed > -100) && (diff_data.right_motor_speed < 100))))//20240330只有当手柄回中，然后当前已经停止的状态才设置为最小停车扭矩
|
||||
// {
|
||||
// diff_data.max_Torq = 5;//停车 就为0 20250425 修改为5，解决手柄回中，震荡问题
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩
|
||||
// }
|
||||
if (diff_data.emergency_stop_state == 1)//刹车 20241017 增加的扭矩限制
|
||||
{
|
||||
diff_data.max_Torq = 5;//20240403修改。刹车就是5N
|
||||
}
|
||||
else if ((0 == diff_data.desired_speed) && (0 == diff_data.desired_curvature) && (diff_data.left_motor_speed > -100) && (diff_data.left_motor_speed < 100)&& (((diff_data.right_motor_speed > -100) && (diff_data.right_motor_speed < 100))))//20240330只有当手柄回中，然后当前已经停止的状态才设置为最小停车扭矩
|
||||
{
|
||||
diff_data.max_Torq = 5;//停车 就为0 20250425 修改为5，解决手柄回中，震荡问题
|
||||
}
|
||||
else
|
||||
{
|
||||
diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩
|
||||
}
|
||||
if((power_data.current_state == POWER_WORKING))//电机上电才运行
|
||||
{
|
||||
diffProcess(&diff_data);//计算左右电机期望转速
|
||||
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;
|
||||
resetPidIntegral(&yaw_rate_pid);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -892,7 +770,7 @@ void preChargeFinish(void *signal_id)
|
||||
|
||||
float out_torq[4] = {0.0f,0.0f,0.0f,0.0f};
|
||||
|
||||
setMotorOutput(out_torq, (uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr"));
|
||||
setMotorOutput(out_torq, (uint16_t)getParam("maxTorq"), 0,0, (uint16_t)getParam("dispPwr"));
|
||||
// 档位
|
||||
un_motor_output1.bit_data.gear = 0; // 0表示空挡
|
||||
un_motor_output2.bit_data.gear = 0;
|
||||
@@ -948,7 +826,7 @@ void diffParametersInit(void *signal_id)
|
||||
}
|
||||
|
||||
// 设置曲率 PID 控制器的参数
|
||||
setPidParameters(&Dec_front_speed_pid,
|
||||
setPidParameters(&left_feed_pid,
|
||||
getParam("mot_kp"),
|
||||
getParam("mot_ki"),
|
||||
getParam("mot_kd"),
|
||||
@@ -957,27 +835,29 @@ void diffParametersInit(void *signal_id)
|
||||
);
|
||||
|
||||
// 设置曲率 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
|
||||
setPidParameters(&right_feed_pid,
|
||||
left_feed_pid.kp,
|
||||
left_feed_pid.ki,
|
||||
left_feed_pid.kd,
|
||||
left_feed_pid.integral_limit,
|
||||
left_feed_pid.output_limit
|
||||
);
|
||||
|
||||
diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩
|
||||
diff_data.max_Torq = (float)getParam("maxTorq");
|
||||
|
||||
if(0 == (float)getParam("diff_sp"))//20250711 防止参数为0，影响计算。
|
||||
{
|
||||
diff_data.diff_dead_zone = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
diff_data.diff_dead_zone = (float)getParam("diff_sp");//参数读取设定最大扭矩
|
||||
}
|
||||
// diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩
|
||||
// diff_data.max_Torq = (float)getParam("maxTorq");
|
||||
|
||||
diff_data.max_feed_power = (uint16_t)getParam("feedPwr");
|
||||
|
||||
// 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("left_speed: %f, des_speed: %f,left_feed: %d\n", diff_data.left_motor_speed, diff_data.out_torq[0],diff_data.left_motor_feed_power); //left_motor_rpm, diff_data.left_motor_speed
|
||||
|
||||
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);
|
||||
@@ -996,7 +876,7 @@ void diffParametersInit(void *signal_id)
|
||||
|
||||
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]);
|
||||
printf(" left = %d,%d\n", diff_data.left_motor_state,diff_data.right_motor_state);
|
||||
|
||||
timerStart(&diff_app_timer,1000,1);//1s调用一次
|
||||
}
|
||||
@@ -1041,9 +921,9 @@ void diffAppInit(void)
|
||||
);
|
||||
|
||||
// 初始化减速 PID 控制器
|
||||
initializePid(&Dec_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
|
||||
initializePid(&left_feed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
|
||||
// 设置 PID 控制器的参数
|
||||
setPidParameters(&Dec_front_speed_pid,
|
||||
setPidParameters(&left_feed_pid,
|
||||
getParam("mot_kp"),
|
||||
getParam("mot_ki"),
|
||||
getParam("mot_kd"),
|
||||
@@ -1052,14 +932,14 @@ void diffAppInit(void)
|
||||
);
|
||||
|
||||
// 初始化加速 PID 控制器
|
||||
initializePid(&Acc_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
|
||||
initializePid(&right_feed_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
|
||||
setPidParameters(&right_feed_pid,
|
||||
left_feed_pid.kp,
|
||||
left_feed_pid.ki,
|
||||
left_feed_pid.kd,
|
||||
left_feed_pid.integral_limit,
|
||||
left_feed_pid.output_limit
|
||||
);
|
||||
|
||||
|
||||
|
||||
@@ -11,8 +11,8 @@ extern "C"
|
||||
#define SPEED_FITER_NUM 6
|
||||
|
||||
|
||||
#define SPEED_PID_MODE 0
|
||||
#define THROTTLE_PID_MODE 1
|
||||
#define SPEED_PID_MODE 1
|
||||
#define THROTTLE_PID_MODE 0
|
||||
|
||||
#define TURN_MIN_TOUQUE 1 //n*m
|
||||
|
||||
@@ -23,9 +23,8 @@ extern "C"
|
||||
#define TORQUE_HYSTERESIS_THRESHOLD 0.3f
|
||||
|
||||
|
||||
#define MOTOR_MODE TORQUE_MODE
|
||||
|
||||
|
||||
#define MOTOR_MODE SPEED_MODE
|
||||
|
||||
|
||||
#define ALPHA 0.1f // 滤波系数α∈[0.01,0.3]，0.2对应截止频率约10Hz（假设采样周期10ms）
|
||||
#define LOWPASS_FILTER(speed, prev) (ALPHA * (speed) + (1 - ALPHA) * (prev))
|
||||
@@ -71,15 +70,13 @@ typedef struct DiffData
|
||||
float out_left_motor_speed; // 输出左电机速度
|
||||
float out_right_motor_speed; // 输出右电机速度
|
||||
float out_torq[4]; //4个电机扭矩
|
||||
float max_Torq; // 最大扭矩限制
|
||||
float min_Torq; // 最小扭矩限制
|
||||
|
||||
float left_speed_diff; // 左侧转速差
|
||||
float right_speed_diff; // 右侧转速差
|
||||
|
||||
float left_diff_touue; // 左侧扭矩差
|
||||
float right_diff_touue; // 右侧扭矩差
|
||||
float diff_dead_zone; // 差速速度死区
|
||||
float max_Torq; // 最大扭矩限制
|
||||
float min_Torq; // 最小扭矩限制
|
||||
uint16_t left_motor_feed_power; // 左侧馈电功率
|
||||
uint16_t right_motor_feed_power; // 右侧馈电功率
|
||||
uint8_t left_motor_state; //左侧电机状态，1刹车，0停下或加速 2驻车
|
||||
uint8_t right_motor_state; //右侧电机状态，1刹车，0停下或加速
|
||||
uint16_t max_feed_power; //最大馈电功率
|
||||
|
||||
} DiffData;
|
||||
|
||||
|
||||
@@ -1,493 +1,464 @@
|
||||
#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);
|
||||
// whl_bas 轮胎直径
|
||||
// max_rpm 最大转速
|
||||
// whl_dia 轴距
|
||||
// max_acc 最大加速度
|
||||
// spd_kp 速度P
|
||||
// spd_ki 速度I
|
||||
// spd_kd 速度D
|
||||
// spd_il 速度积分限制
|
||||
// spd_ol 速度PID输出限制
|
||||
// crv_kp 转弯P
|
||||
// crv_ki 转弯I
|
||||
// crv_kd 转弯D
|
||||
// crv_il 转弯积分限制
|
||||
// crv_ol 转弯PID输出限制
|
||||
// brk_on 刹车刹紧时间
|
||||
// brk_off 刹车释放时间
|
||||
// maxTorq 最大扭矩
|
||||
// feedPwr 最大馈电功率
|
||||
// dispPwr 最大放电功率
|
||||
// VehMass 车重
|
||||
// gRatio 减速比
|
||||
// prCTime 预充电时间
|
||||
// brk_pos 刹车位置
|
||||
// pwr_sta 电源状态
|
||||
// high_sw 高压开关状态
|
||||
// stop_sw 急停开关状态
|
||||
// lightSt 灯光状态
|
||||
// pwr_btn 电源开关状态
|
||||
// sleepTm 休眠时间
|
||||
// wakeTm 唤醒时间
|
||||
// Ospd_kp 自主速度P
|
||||
// Ospd_ki 自主速度I
|
||||
// Ospd_kd 自主速度D
|
||||
// Ospd_il 自主速度积分限制
|
||||
// Ospd_ol 自主速度PID输出限制
|
||||
// Ocrv_kp 自主转弯P
|
||||
// Ocrv_ki 自主转弯I
|
||||
// Ocrv_kd 自主转弯D
|
||||
// Ocrv_il 自主转弯积分限制
|
||||
// Ocrv_ol 自主转弯PID输出限制
|
||||
// minTorq 输出扭矩死区
|
||||
// brk_rev 刹车方向
|
||||
// mot_kp 同侧扭矩P参数
|
||||
// mot_ki 同侧扭矩I参数
|
||||
// mot_kd 同侧扭矩d参数
|
||||
// mot_il 同侧扭矩积分限制
|
||||
// mot_ol 同侧扭矩输出限制
|
||||
// diff_sp 同侧扭矩速度差阈值
|
||||
// test 初始化测试参数
|
||||
// 订阅信号
|
||||
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");
|
||||
|
||||
// 清零 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());
|
||||
}
|
||||
|
||||
@@ -184,6 +184,9 @@ static void tempProcess(void *signal_id)
|
||||
|
||||
max_temp[0] = temp_data.current_temp[0];
|
||||
max_temp[1] = temp_data.current_temp[1];
|
||||
max_temp[2] = temp_data.current_temp[2];
|
||||
max_temp[3] = temp_data.current_temp[3];
|
||||
|
||||
|
||||
// printf("motor1 temp: %d, motor2 temp: %d\n", max_temp[0], max_temp[1]);
|
||||
handleTemperatureAlarm(max_temp[0], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[0]);
|
||||
@@ -239,19 +242,19 @@ static void tempInput(void *signal_id)
|
||||
// 填充数据
|
||||
if (signal_id == &un_motor_temp1)
|
||||
{
|
||||
temp_data.current_temp[0] = ( (int16_t)(un_motor_temp1.bit_data.controller_temp) - 40);//40偏移量
|
||||
temp_data.current_temp[0] = (int16_t)( (un_motor_temp1.bit_data.controller_temp) - 40 );//40偏移量
|
||||
}
|
||||
else if(signal_id == &un_motor_temp2)
|
||||
{
|
||||
temp_data.current_temp[1] = ( (int16_t)(un_motor_temp2.bit_data.controller_temp) - 40);
|
||||
temp_data.current_temp[1] = (int16_t)( (un_motor_temp2.bit_data.controller_temp) - 40 );
|
||||
}
|
||||
else if(signal_id == &un_motor_temp3)
|
||||
{
|
||||
temp_data.current_temp[2] = ( (int16_t)(un_motor_temp3.bit_data.controller_temp) - 40);
|
||||
temp_data.current_temp[2] = (int16_t)( (un_motor_temp3.bit_data.controller_temp) - 40 );
|
||||
}
|
||||
else if(signal_id == &un_motor_temp4)
|
||||
{
|
||||
temp_data.current_temp[3] = ( (int16_t)(un_motor_temp4.bit_data.controller_temp) - 40);
|
||||
temp_data.current_temp[3] = (int16_t)( (un_motor_temp4.bit_data.controller_temp) - 40 );
|
||||
}
|
||||
else{}
|
||||
}
|
||||
|
||||
@@ -407,7 +407,7 @@ void flexcan_Receive_callback_2(flexcan_handle_t *handle,
|
||||
un_motor_input4.arr[i] = buf->dataBuffer[i];
|
||||
}
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
|
||||
publishMessage(&un_motor_input4, 1);
|
||||
// publishMessage(&un_motor_input4, 1);
|
||||
}
|
||||
else if( RIGHT_REAR_MOTOR_INPUT2 == (buf->id) )
|
||||
{
|
||||
@@ -1176,7 +1176,7 @@ static void processUnGatherOutput(void *signal_id)
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_0, 0x15000002, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_gather_output, 8, 18);//<2F>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD>
|
||||
}
|
||||
|
||||
// un_gather_output.bit_data.vehicle_mode = power_data.current_state;
|
||||
|
||||
|
||||
static void processUltrasonicOutput(void *signal_id)
|
||||
@@ -1237,7 +1237,7 @@ void canSendAll(void *signal_id)
|
||||
|
||||
|
||||
motor_power_cnt ++;
|
||||
if(motor_power_cnt >= 1000)//1s<31><73><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
|
||||
if(motor_power_cnt >= 10)//1s<31><73><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
|
||||
{
|
||||
motor_power_cnt = 0;
|
||||
processMotorOutput3(CanData);
|
||||
@@ -1393,6 +1393,10 @@ void canInterfaceInit(void)
|
||||
|
||||
subscribe(&un_motor_output1, processMotorOutput1);
|
||||
subscribe(&un_motor_output2, processMotorOutput2);
|
||||
// subscribe(&un_motor_output3, processMotorOutput3);
|
||||
// subscribe(&un_motor_output4, processMotorOutput4);
|
||||
|
||||
|
||||
|
||||
subscribe(&un_inf_can_kgf_output1, processKgfOutput1);
|
||||
subscribe(&un_inf_can_kgf_output2, processKgfOutput2);
|
||||
@@ -1403,12 +1407,6 @@ void canInterfaceInit(void)
|
||||
subscribe(&un_ultrasonic_output1, processUltrasonicOutput); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// subscribe(&un_h_bridge_output2, processHBridgeOutput2);
|
||||
// subscribe(&un_lifter_output, processLifterOutput);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user