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

Compare commits

base: liumin:20251022-7-8号车程序
Branches Tags
liumin:main liumin:7-8car-chassis liumin:Pan-Tilt-Unit liumin:gimbal liumin:6-car liumin:1car liumin:4while4turn liumin:转速模式
liumin:20251022-7-8号车程序
..
compare: liumin:转速模式
Branches Tags
liumin:7-8car-chassis liumin:Pan-Tilt-Unit liumin:main liumin:gimbal liumin:6-car liumin:1car liumin:4while4turn liumin:转速模式
liumin:20251022-7-8号车程序

1 Commits
20251022-7 ... 转速模式

Author SHA1 Message Date
liumin
9a98344f85 转速模式增加馈电PID 2025-07-23 11:46:22 +08:00
5 changed files with 702 additions and 853 deletions
Expand all files Collapse all files

546
app/app_differential_drive.c
View File

@@ -19,8 +19,8 @@ DiffData diff_data;
PID_t speed_pid; PID_t speed_pid;
PID_t yaw_rate_pid; PID_t yaw_rate_pid;
PID_t Acc_front_speed_pid; PID_t left_feed_pid;
PID_t Dec_front_speed_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; 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; float abs_right_rear_speed = 0;
// 档位 // 档位
abs_left_front_speed = calculateTorqueOutput(diff_data.motor_state[0], out_torq[0]); //根据挡位增加转矩方向 un_motor_output1.bit_data.gear = (out_torq[0] >= 0) ? 1 : 2; // 1 表示前进，2 表示后退
abs_right_front_speed = calculateTorqueOutput(diff_data.motor_state[1], out_torq[1]); un_motor_output2.bit_data.gear = (out_torq[1] >= 0) ? 1 : 2;
abs_left_rear_speed = calculateTorqueOutput(diff_data.motor_state[2], out_torq[2]); un_motor_output3.bit_data.gear = (out_torq[2] >= 0) ? 2 : 1; //20250717 2,3电机反相
abs_right_rear_speed = calculateTorqueOutput(diff_data.motor_state[3], out_torq[3]); un_motor_output4.bit_data.gear = (out_torq[3] >= 0) ? 2 : 1;
un_motor_output1.bit_data.gear = diff_data.motor_state[0]; abs_left_front_speed = fabsf(out_torq[0]); //根据挡位增加转矩方向
un_motor_output2.bit_data.gear = diff_data.motor_state[1]; abs_right_front_speed = fabsf(out_torq[1]);
abs_left_rear_speed = fabsf(out_torq[2]);
if(STATE_FORWARD == diff_data.motor_state[2])//把后两台电机反相 abs_right_rear_speed = fabsf(out_torq[3]);
{
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;
}
// 设置左右电机期望转速 // 设置左右电机期望转速
// un_motor_output1.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_left_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_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; 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_output4.bit_data.mode = MOTOR_MODE;
// 设置扭矩 // 设置扭矩
un_motor_output1.bit_data.set_torque = (uint16_t)( (int16_t)abs_left_front_speed ); un_motor_output1.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量
un_motor_output2.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_front_speed ); un_motor_output2.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量
un_motor_output3.bit_data.set_torque = (uint16_t)( (int16_t)abs_left_rear_speed ); un_motor_output3.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量
un_motor_output4.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_rear_speed ); un_motor_output4.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量
//设定转速
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.feed_power = feed_power; un_motor_output1.bit_data.feed_power = left_feed_power;
un_motor_output2.bit_data.feed_power = feed_power; un_motor_output2.bit_data.feed_power = right_feed_power;
un_motor_output3.bit_data.feed_power = feed_power; un_motor_output3.bit_data.feed_power = left_feed_power;
un_motor_output4.bit_data.feed_power = feed_power; un_motor_output4.bit_data.feed_power = right_feed_power;
// 设置放电功率 // 设置放电功率
un_motor_output1.bit_data.discharge_power = discharge_power; un_motor_output1.bit_data.discharge_power = discharge_power;
un_motor_output2.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_output3.bit_data.discharge_power = discharge_power;
un_motor_output4.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(); diff_data.max_speed = calculateMaxSpeed();
previous_speed = diff_data.speed; previous_speed = diff_data.speed;
} }
/** /**
* @brief 基于转速反比的双电机扭矩分配函数 * @brief 判断减速状态（最简逻辑） 如果同向或者有一个为0，或者都为0，那么判断绝对值大小，如果期望绝对值小于当前绝对值，那就为减速
* @param rpm1 电机1当前转速（单位：rpm） * @param target_speed 期望速度（带方向）
* @param rpm2 电机2当前转速（单位：rpm） * @param current_speed 当前速度（带方向）
* @param total_torque 系统总需求扭矩（单位：Nm） * @return 1:减速, 0:加速或保持
* @param torque1 [out] 电机1分配到的扭矩（单位：Nm）
* @param torque2 [out] 电机2分配到的扭矩（单位：Nm）
* @note 分配原则：转速越高的电机分配扭矩越小，确保负载均衡
*/ */
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 ( (target_speed == 0.0f && current_speed == 0.0f) )//如果又减速的话也刹车 //|| (0 != des_yaw_rate)
if (fabs(total_torque) < 0.001f) { {
*torque1 = 0.0f; return 2; // 驻车
*torque2 = 0.0f;
return;
} }
// 核心逻辑：方向相反 或 (同向/含零且期望绝对值 < 当前绝对值)
// // 保护条件：当两电机均静止时采用平均分配策略 return (signbit(target_speed) != signbit(current_speed)) ||
// if (fabs(rpm1) < 0.001f && fabs(rpm2) < 0.001f) { (fabs(target_speed) < fabs(current_speed));
// *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);
}
} }
/**
* @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) 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 left_speed_mps = 0.0f;
float right_speed_mps = 0.0f; float right_speed_mps = 0.0f;
float max_torque = diff_data.max_Torq;//不需要限制，PID输出已经限制了 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); linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque);
yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque);
if( diff_data.min_Torq > fabs(linear_velocity_x) )//20250728 增加死区 解决手柄回中，不停车问题
{
linear_velocity_x = 0;
}
left_speed_mps = linear_velocity_x + yaw_rate; 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) 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[2] = left_speed_mps;
motor_speed[1] = right_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[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[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[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.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); // 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; float rotational_velocity = ((float)getParam("whl_bas") / 2.0f) * yaw_rate;
// 计算车辆左右线速度 (m/s) // 计算车辆左右线速度 (m/s)
float left_speed_mps = linear_velocity_x - rotational_velocity; //20250316 为解决原地转向和直行转向相同，所以把左右输出的速度交换 float left_speed_mps = linear_velocity_x + rotational_velocity; //20250316 为解决原地转向和直行转向相同，所以把左右输出的速度交换
float right_speed_mps = linear_velocity_x + rotational_velocity; float right_speed_mps = linear_velocity_x - rotational_velocity;
// 计算轮子周长 // 计算轮子周长
float wheel_circumference = (float)getParam("whl_dia") * M_PI; 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"); float max_motor_rpm = (float)getParam("max_rpm");
left_motor_rpm = constrain(left_motor_rpm, -max_motor_rpm, max_motor_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); right_motor_rpm = constrain(right_motor_rpm, -max_motor_rpm, max_motor_rpm);
// 当电机转速小于50转时，设置为0 // 当电机转速小于50转时，设置为0
if (fabsf(left_motor_rpm) < 50)//速度慢所以设置位10转 // if (fabsf(left_motor_rpm) < 20)//速度慢所以设置位10转
{ // {
left_motor_rpm = 0; // left_motor_rpm = 0;
} // }
if (fabsf(right_motor_rpm) < 50)//速度慢所以设置位10转 // if (fabsf(right_motor_rpm) < 20)//速度慢所以设置位10转
{ // {
right_motor_rpm = 0; // right_motor_rpm = 0;
} // }
// 左边电机方向反一下，因为电机安装反了，返回来的数据也要反一下 // 左边电机方向反一下，因为电机安装反了，返回来的数据也要反一下
// left_motor_rpm = -left_motor_rpm; // left_motor_rpm = -left_motor_rpm;
// 返回计算结果 // 返回计算结果
*left_motor_speed = left_motor_rpm; motor_speed[0] = left_motor_rpm;//加速状态，没有负扭矩，要么前进加速要么后退加速
*right_motor_speed = right_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 #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); // 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 // if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) )//手柄回中，速度小的时候清0
{ // {
resetPidIntegral(&speed_pid); // resetPidIntegral(&speed_pid);
resetPidIntegral(&yaw_rate_pid); // resetPidIntegral(&yaw_rate_pid);
output_speed = 0; // output_speed = 0;
output_yaw_rate = 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], setMotorOutput(&diff_data.out_torq[0],
diff_data.max_Torq,// diff_data.max_Torq,//
(uint16_t)getParam("feedPwr"), diff_data.left_motor_feed_power,
diff_data.right_motor_feed_power,
(uint16_t)getParam("dispPwr")); (uint16_t)getParam("dispPwr"));
// 发布左右电机期望转速,电源在工作状态才能发送 // 发布左右电机期望转速,电源在工作状态才能发送
if (power_data.current_state == POWER_WORKING) 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_output1, 1);
publishMessage(&un_motor_output2, 1); publishMessage(&un_motor_output2, 1);
publishMessage(&un_motor_output3, 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.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.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_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_right_out = (uint16_t)(int16_t)(diff_data.right_speed_diff);
publishMessage(&diff_data, 1); 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) static void diffInput(void *signal_id)
{ {
float motor_speed_temp = 0.0f;
if (signal_id == &un_sw_sample) if (signal_id == &un_sw_sample)
{ {
diff_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch; 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_speed = diff_data.desired_speed * 0.01f;
diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f; 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 = mapRemoteControlSpeed(diff_data.desired_curvature, 0.1, 2, 2, 1, 0.5);
diff_data.desired_curvature = -diff_data.desired_curvature; 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 = (float)((int16_t)(un_motor_input3.bit_data.speed - 30000));//20240921 增加偏移量
diff_data.left_rear_motor_speed = - diff_data.left_rear_motor_speed;//20250708 增加反相 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_rear_motor_speed = 0;
} }
diff_data.left_motor_speed = motor_speed_temp;
// 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) )// 处理第二个电机速度信号（右电机） 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_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_rear_motor_speed = - diff_data.right_rear_motor_speed;//20250708 增加反相 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 增加反相
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 = 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; diff_data.desired_curvature = 0.0;
} }
// if (diff_data.emergency_stop_state == 1)//刹车 20241017 增加的扭矩限制 if (diff_data.emergency_stop_state == 1)//刹车 20241017 增加的扭矩限制
// { {
// diff_data.max_Torq = 5;//20240403修改。刹车就是5N 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只有当手柄回中，然后当前已经停止的状态才设置为最小停车扭矩 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，解决手柄回中，震荡问题 diff_data.max_Torq = 5;//停车 就为0 20250425 修改为5，解决手柄回中，震荡问题
// } }
// else else
// { {
// diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩 diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩
// } }
if((power_data.current_state == POWER_WORKING))//电机上电才运行 if((power_data.current_state == POWER_WORKING))//电机上电才运行
{ {
diffProcess(&diff_data);//计算左右电机期望转速 diffProcess(&diff_data);//计算左右电机期望转速
} }
else else
{ {
resetPidIntegral(&speed_pid); resetPidIntegral(&speed_pid);
resetPidIntegral(&yaw_rate_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;
} }
} }
@@ -892,7 +770,7 @@ void preChargeFinish(void *signal_id)
float out_torq[4] = {0.0f,0.0f,0.0f,0.0f}; 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_output1.bit_data.gear = 0; // 0表示空挡
un_motor_output2.bit_data.gear = 0; un_motor_output2.bit_data.gear = 0;
@@ -948,7 +826,7 @@ void diffParametersInit(void *signal_id)
} }
// 设置曲率 PID 控制器的参数 // 设置曲率 PID 控制器的参数
setPidParameters(&Dec_front_speed_pid, setPidParameters(&left_feed_pid,
getParam("mot_kp"), getParam("mot_kp"),
getParam("mot_ki"), getParam("mot_ki"),
getParam("mot_kd"), getParam("mot_kd"),
@@ -957,27 +835,29 @@ void diffParametersInit(void *signal_id)
); );
// 设置曲率 PID 控制器的参数 // 设置曲率 PID 控制器的参数
setPidParameters(&Acc_front_speed_pid, setPidParameters(&right_feed_pid,
Dec_front_speed_pid.kp, left_feed_pid.kp,
Dec_front_speed_pid.ki, left_feed_pid.ki,
Dec_front_speed_pid.kd, left_feed_pid.kd,
Dec_front_speed_pid.integral_limit, left_feed_pid.integral_limit,
Dec_front_speed_pid.output_limit left_feed_pid.output_limit
); );
diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩 // diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩
diff_data.max_Torq = (float)getParam("maxTorq"); // diff_data.max_Torq = (float)getParam("maxTorq");
if(0 == (float)getParam("diff_sp"))//20250711 防止参数为0，影响计算。 diff_data.max_feed_power = (uint16_t)getParam("feedPwr");
{
diff_data.diff_dead_zone = 2;
}
else
{
diff_data.diff_dead_zone = (float)getParam("diff_sp");//参数读取设定最大扭矩
}
// 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("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);
@@ -996,7 +876,7 @@ void diffParametersInit(void *signal_id)
printf("remote_speed: %f, remote_yaw_rate: %f\n", deffspeed, deffcurvature); 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调用一次 timerStart(&diff_app_timer,1000,1);//1s调用一次
} }
@@ -1041,9 +921,9 @@ void diffAppInit(void)
); );
// 初始化减速 PID 控制器 // 初始化减速 PID 控制器
initializePid(&Dec_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); initializePid(&left_feed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
// 设置 PID 控制器的参数 // 设置 PID 控制器的参数
setPidParameters(&Dec_front_speed_pid, setPidParameters(&left_feed_pid,
getParam("mot_kp"), getParam("mot_kp"),
getParam("mot_ki"), getParam("mot_ki"),
getParam("mot_kd"), getParam("mot_kd"),
@@ -1052,14 +932,14 @@ void diffAppInit(void)
); );
// 初始化加速 PID 控制器 // 初始化加速 PID 控制器
initializePid(&Acc_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); initializePid(&right_feed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
// 设置 PID 控制器的参数 // 设置 PID 控制器的参数
setPidParameters(&Acc_front_speed_pid, setPidParameters(&right_feed_pid,
Dec_front_speed_pid.kp, left_feed_pid.kp,
Dec_front_speed_pid.ki, left_feed_pid.ki,
Dec_front_speed_pid.kd, left_feed_pid.kd,
Dec_front_speed_pid.integral_limit, left_feed_pid.integral_limit,
Dec_front_speed_pid.output_limit left_feed_pid.output_limit
); );

25
app/app_differential_drive.h
View File

@@ -11,8 +11,8 @@ extern "C"
#define SPEED_FITER_NUM 6 #define SPEED_FITER_NUM 6
#define SPEED_PID_MODE 0 #define SPEED_PID_MODE 1
#define THROTTLE_PID_MODE 1 #define THROTTLE_PID_MODE 0
#define TURN_MIN_TOUQUE 1 //n*m #define TURN_MIN_TOUQUE 1 //n*m
@@ -23,9 +23,8 @@ extern "C"
#define TORQUE_HYSTERESIS_THRESHOLD 0.3f #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 ALPHA 0.1f // 滤波系数α∈[0.01,0.3]，0.2对应截止频率约10Hz（假设采样周期10ms）
#define LOWPASS_FILTER(speed, prev) (ALPHA * (speed) + (1 - ALPHA) * (prev)) #define LOWPASS_FILTER(speed, prev) (ALPHA * (speed) + (1 - ALPHA) * (prev))
@@ -71,15 +70,13 @@ typedef struct DiffData
float out_left_motor_speed; // 输出左电机速度 float out_left_motor_speed; // 输出左电机速度
float out_right_motor_speed; // 输出右电机速度 float out_right_motor_speed; // 输出右电机速度
float out_torq[4]; //4个电机扭矩 float out_torq[4]; //4个电机扭矩
float max_Torq; // 最大扭矩限制 float max_Torq; // 最大扭矩限制
float min_Torq; // 最小扭矩限制 float min_Torq; // 最小扭矩限制
uint16_t left_motor_feed_power; // 左侧馈电功率
float left_speed_diff; // 左侧转速差 uint16_t right_motor_feed_power; // 右侧馈电功率
float right_speed_diff; // 右侧转速差 uint8_t left_motor_state; //左侧电机状态，1刹车，0停下或加速 2驻车
uint8_t right_motor_state; //右侧电机状态，1刹车，0停下或加速
float left_diff_touue; // 左侧扭矩差 uint16_t max_feed_power; //最大馈电功率
float right_diff_touue; // 右侧扭矩差
float diff_dead_zone; // 差速速度死区
} DiffData; } DiffData;

957
app/app_param_manage.c
View File

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

11
app/app_temp.c
View File

@@ -184,6 +184,9 @@ static void tempProcess(void *signal_id)
max_temp[0] = temp_data.current_temp[0]; max_temp[0] = temp_data.current_temp[0];
max_temp[1] = temp_data.current_temp[1]; 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]); // 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]); 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) 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) 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) 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) 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{} else{}
} }

16
interface_can.c
View File

@@ -407,7 +407,7 @@ void flexcan_Receive_callback_2(flexcan_handle_t *handle,
un_motor_input4.arr[i] = buf->dataBuffer[i]; un_motor_input4.arr[i] = buf->dataBuffer[i];
} }
//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD> //<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
publishMessage(&un_motor_input4, 1); // publishMessage(&un_motor_input4, 1);
} }
else if( RIGHT_REAR_MOTOR_INPUT2 == (buf->id) ) 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> (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> 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) static void processUltrasonicOutput(void *signal_id)
@@ -1237,7 +1237,7 @@ void canSendAll(void *signal_id)
motor_power_cnt ++; 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; motor_power_cnt = 0;
processMotorOutput3(CanData); processMotorOutput3(CanData);
@@ -1393,6 +1393,10 @@ void canInterfaceInit(void)
subscribe(&un_motor_output1, processMotorOutput1); subscribe(&un_motor_output1, processMotorOutput1);
subscribe(&un_motor_output2, processMotorOutput2); 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_output1, processKgfOutput1);
subscribe(&un_inf_can_kgf_output2, processKgfOutput2); 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_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_h_bridge_output2, processHBridgeOutput2);
// subscribe(&un_lifter_output, processLifterOutput); // subscribe(&un_lifter_output, processLifterOutput);