diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_brake.c b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_brake.c index e7b346e..b2d3b7f 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_brake.c +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_brake.c @@ -16,6 +16,46 @@ static inline uint8_t setBrakeOff(void) { return 0; } BrakeSystem brake_data; +// 输出处理函数 +static void brakeOutput(void *signal_id) +{ + (void)signal_id; + // 根据电机状态，填充发送数据结构，发送信号 + switch (brake_data.brake_motor_state) + { + case 1: // 电机前进状态 + un_h_bridge_output.bit_data.channel_01 = setBrakeOn(); + un_h_bridge_output.bit_data.channel_04 = setBrakeOn(); + un_h_bridge_output.bit_data.channel_02 = setBrakeOff(); + un_h_bridge_output.bit_data.channel_03 = setBrakeOff(); + un_h_bridge_output.bit_data.sleep_01 = setBrakeOn(); + un_h_bridge_output.bit_data.sleep_02 = setBrakeOn(); // 正转 + printf("Brake: Motor forward\n"); + break; + + case 2: // 电机后退状态 + un_h_bridge_output.bit_data.channel_01 = setBrakeOff(); + un_h_bridge_output.bit_data.channel_04 = setBrakeOff(); + un_h_bridge_output.bit_data.channel_02 = setBrakeOn(); + un_h_bridge_output.bit_data.channel_03 = setBrakeOn(); + un_h_bridge_output.bit_data.sleep_01 = setBrakeOn(); + un_h_bridge_output.bit_data.sleep_02 = setBrakeOn(); // 反转 + printf("Brake: Motor reverse\n"); + break; + + default: + un_h_bridge_output.bit_data.channel_01 = setBrakeOff(); + un_h_bridge_output.bit_data.channel_04 = setBrakeOff(); + un_h_bridge_output.bit_data.channel_02 = setBrakeOff(); + un_h_bridge_output.bit_data.channel_03 = setBrakeOff(); + un_h_bridge_output.bit_data.sleep_01 = setBrakeOff(); + un_h_bridge_output.bit_data.sleep_02 = setBrakeOff(); // 关闭 + printf("Brake: Motor off\n"); + break; + } + publishMessage(&un_h_bridge_output, 1); +} + // 判断是否需要刹车 static uint8_t shouldApplyBrake() { @@ -36,63 +76,11 @@ static uint8_t shouldReleaseBrake() // (brake_data.mode_signal == 1 && (!brake_data.can_bus_fault && !brake_data.ethernet_fault)))); } - - -// 输出处理函数 -static void brakeOutput(void *signal_id) -{ - (void)signal_id; - // 根据电机状态，填充发送数据结构，发送信号 - switch (brake_data.brake_motor_state) - { - case 1: // 电机前进状态 - un_h_bridge_output.bit_data.channel_01 = setBrakeOn(); - un_h_bridge_output.bit_data.channel_04 = setBrakeOn(); - un_h_bridge_output.bit_data.channel_02 = setBrakeOff(); - un_h_bridge_output.bit_data.channel_03 = setBrakeOff(); - un_h_bridge_output.bit_data.sleep_01 = setBrakeOn(); - un_h_bridge_output.bit_data.sleep_02 = setBrakeOn(); // 正转 - un_inf_can_kgf_output1.bit_data.KGF13 = setBrakeOff(); // 抱闸继电器 - un_inf_can_kgf_output1.bit_data.KGF14 = setBrakeOff(); // 抱闸继电器 - printf("Brake: Motor forward\n"); - break; - - case 2: // 电机后退状态 - un_h_bridge_output.bit_data.channel_01 = setBrakeOff(); - un_h_bridge_output.bit_data.channel_04 = setBrakeOff(); - un_h_bridge_output.bit_data.channel_02 = setBrakeOn(); - un_h_bridge_output.bit_data.channel_03 = setBrakeOn(); - un_h_bridge_output.bit_data.sleep_01 = setBrakeOn(); - un_h_bridge_output.bit_data.sleep_02 = setBrakeOn(); // 反转 - un_inf_can_kgf_output1.bit_data.KGF13 = setBrakeOn(); // 抱闸继电器 - un_inf_can_kgf_output1.bit_data.KGF14 = setBrakeOn(); // 抱闸继电器 - - printf("Brake: Motor reverse\n"); - break; - - default: - un_h_bridge_output.bit_data.channel_01 = setBrakeOff(); - un_h_bridge_output.bit_data.channel_04 = setBrakeOff(); - un_h_bridge_output.bit_data.channel_02 = setBrakeOff(); - un_h_bridge_output.bit_data.channel_03 = setBrakeOff(); - un_h_bridge_output.bit_data.sleep_01 = setBrakeOff(); - un_h_bridge_output.bit_data.sleep_02 = setBrakeOff(); // 关闭 - printf("Brake: Motor off\n"); - break; - } - - publishMessage(&un_h_bridge_output, 1); - publishMessage(&un_inf_can_kgf_output1, 1); -} - - - // 修改刹车定时器处理函数 static void brakeTimerProcess(void *signal_id) { (void)signal_id; - -//#ifdef OIL_BRAKE + switch (brake_data.state) { case BRAKE_STATE_IDLE: @@ -140,16 +128,14 @@ static void brakeTimerProcess(void *signal_id) brake_data.state = BRAKE_STATE_IDLE; break; } + // 如果刹车位置有变化，存入EEPROM if (brake_data.brake_position != brake_data.old_brake_position) { setParam("brk_pos", (float)brake_data.brake_position); brake_data.old_brake_position = brake_data.brake_position; - - - printf("writeE2 brake_position = %d\n",brake_data.brake_position); - } + timerStart(&brake_data.brake_timer, 100, 1); // 周期调用 } diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_brake.h b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_brake.h index 8801633..cf77b6f 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_brake.h +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_brake.h @@ -10,9 +10,6 @@ extern "C" { #include "app_frm_signal.h" #include "app_frm_timer.h" -#define OIL_BRAKE 0 -#define ELECTROMAGNETIC_BRAKE 1 - // 定义刹车状态机状态 typedef enum { diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.c b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.c index 3649476..4290710 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.c +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.c @@ -19,51 +19,56 @@ DiffData diff_data; PID_t speed_pid; PID_t yaw_rate_pid; -PID_t Acc_front_speed_pid; -PID_t Dec_front_speed_pid; - // 设置电机输出 -void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint16_t discharge_power) +void setMotorOutput(float left_speed, float right_speed, uint16_t max_torque, uint16_t feed_power, uint16_t discharge_power) { -// // 档位 -// un_motor_output1.bit_data.gear = (left_speed >= 0) ? 1 : 2; // 1 表示前进，2 表示后退 -// un_motor_output2.bit_data.gear = (right_speed >= 0) ? 1 : 2; - -// // 计算绝对值并转换 - int16_t abs_left_front_speed = (int16_t)(out_torq[0]); - int16_t abs_right_front_speed = (int16_t)(out_torq[1]); - - int16_t abs_left_rear_speed = (int16_t)(out_torq[2]); - int16_t abs_right_rear_speed = (int16_t)(out_torq[3]*0.73); //20251107 修改新电机增加系数 解决不同步问题 + // 档位 + un_motor_output1.bit_data.gear = (left_speed >= 0) ? 1 : 2; // 1 表示前进，2 表示后退 + un_motor_output2.bit_data.gear = (right_speed >= 0) ? 1 : 2; + // 计算绝对值并转换 + float abs_left_speed = fabsf(left_speed); + float abs_right_speed = fabsf(right_speed); + // 设置左右电机期望转速 + 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.MotCon_1Signal3 = (uint16_t)(-abs_right_front_speed);//20250502方向原因，需要把1号电机控制器的左右电机反相 - un_motor_output2.bit_data.MotCon_1Signal4 = (uint16_t)(-abs_right_rear_speed); - - un_motor_output1.bit_data.MotCon_1Signal4 = (uint16_t)(-abs_left_front_speed); - un_motor_output2.bit_data.MotCon_1Signal3 = (uint16_t)abs_left_rear_speed; - + // 设置模式为恒速模式 + un_motor_output1.bit_data.mode = 0x01; + un_motor_output2.bit_data.mode = 0x01; -// // 设置模式为恒速模式 -// un_motor_output1.bit_data.mode = 0x01; -// un_motor_output2.bit_data.mode = 0x01; -// -// // 设置最大扭矩 -// un_motor_output1.bit_data.set_torque = (max_torque + 300) * 100; // 20240921 增加偏移量 -// un_motor_output2.bit_data.set_torque = (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_output1.bit_data.discharge_power = discharge_power; -// un_motor_output2.bit_data.discharge_power = discharge_power; + // 设置最大扭矩 + un_motor_output1.bit_data.set_torque = (max_torque + 300) * 100; // 20240921 增加偏移量 + un_motor_output2.bit_data.set_torque = (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_output1.bit_data.discharge_power = discharge_power; + un_motor_output2.bit_data.discharge_power = discharge_power; } +// 限制值在最小值和最大值之间 +float constrain(float value, float min_val, float max_val) +{ + if (value < min_val) + { + return min_val; + } + else if (value > max_val) + { + return max_val; + } + else + { + return value; + } +} + // 计算当前速度、角速度 uint8_t calculateCurrentSpeedYawRate(void) { @@ -76,17 +81,8 @@ uint8_t calculateCurrentSpeedYawRate(void) return 0; // 避免除以0的情况 } // 将电机转速 (RPM) 转换为线速度 (m/s)，考虑减速比 - - float left_speed_mps = (diff_data.left_motor_speed * wheel_circumference) / (60.0f * gear_ratio); float right_speed_mps = (diff_data.right_motor_speed * wheel_circumference) / (60.0f * gear_ratio); - -// float left_speed_mps = 0; -// float right_speed_mps = 0; - -// float left_speed_mps = 0; -// float right_speed_mps = 0; - // 计算当前速度 diff_data.speed = (left_speed_mps + right_speed_mps) / 2.0f; // 计算速度差 @@ -176,169 +172,17 @@ void calculateCurrentState(float dt) } -/** - * @brief 基于转速反比的双电机扭矩分配函数 - * @param rpm1 电机1当前转速（单位：rpm） - * @param rpm2 电机2当前转速（单位：rpm） - * @param total_torque 系统总需求扭矩（单位：Nm） - * @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) -{ - - // 总扭矩为0时快速返回 - if (fabs(total_torque) < 0.001f) { - *torque1 = 0.0f; - *torque2 = 0.0f; - return; - } - -// // 保护条件：当两电机均静止时采用平均分配策略 -// 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); - } - -} - - - -/** - * @brief 根据轮速差动态调整电机扭矩（带非负限制） - * @param speed_left 左轮速度（单位：rpm或自定义） - * @param speed_right 右轮速度（单位：rpm或自定义） - * @param torque_left 左轮扭矩指针（单位：Nm或自定义） - * @param torque_right 右轮扭矩指针（单位：Nm或自定义） - * @param threshold 触发调整的速差阈值（单位同轮速） - * @param k 扭矩调整系数（无量纲，建议0 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 *left_motor_speed, float *right_motor_speed) { // 防止速度过低导致不必要的计算 if (fabs(max_speed) < EPSILON) { - motor_speed[0] = 0.0f; - motor_speed[1] = 0.0f; - motor_speed[2] = 0.0f; - motor_speed[3] = 0.0f; + *left_motor_speed = 0.0f; + *right_motor_speed = 0.0f; return; } -#if THROTTLE_PID_MODE - float max_torque = (float)getParam("maxTorq"); - - linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque); - yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque); - - - float left_speed_mps = linear_velocity_x + yaw_rate; - float right_speed_mps = linear_velocity_x - yaw_rate; - -//扭矩分配 - if(max_torque < left_speed_mps) - { - right_speed_mps = right_speed_mps - (left_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差 - left_speed_mps = max_torque; - } - else if(-max_torque > left_speed_mps) - { - right_speed_mps = right_speed_mps - (left_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差 - left_speed_mps = -max_torque; - } - else if(max_torque < right_speed_mps) - { - left_speed_mps = left_speed_mps - (right_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差 - right_speed_mps = max_torque; - } - else if(-max_torque > right_speed_mps) - { - left_speed_mps = left_speed_mps - (right_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差 - right_speed_mps = -max_torque; - } - else{} - -// printf("input_torq: left=%.1f right=%.1f yaw_rate=%.1f\n", left_speed_mps, right_speed_mps, yaw_rate); - - motor_speed[0] = left_speed_mps; - motor_speed[2] = left_speed_mps; - - motor_speed[1] = right_speed_mps; - motor_speed[3] = right_speed_mps; - -// adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],100, 5); -// adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],100, 5); -// - -// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed); - - - - - - - - - -// distributeTorque(diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed,2*left_speed_mps,&motor_speed[0],&motor_speed[2],diff_data.max_Torq,diff_data.min_Torq); -// distributeTorque(diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed,2*right_speed_mps,&motor_speed[1],&motor_speed[3],diff_data.max_Torq,diff_data.min_Torq); - -// printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", motor_speed[0], motor_speed[1], motor_speed[2], motor_speed[3]); - -// // 返回计算结果 -// *left_motor_speed = left_speed_mps; -// *right_motor_speed = right_speed_mps; -#else - // 限制线速度和偏航率 linear_velocity_x = constrain(linear_velocity_x, -max_speed, max_speed); float max_yaw_rate = max_speed / ((float)getParam("whl_bas") / 2.0f); @@ -348,8 +192,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; + float right_speed_mps = linear_velocity_x - rotational_velocity; // 计算轮子周长 float wheel_circumference = (float)getParam("whl_dia") * M_PI; @@ -379,12 +223,10 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max right_motor_rpm = 0; } // 左边电机方向反一下，因为电机安装反了，返回来的数据也要反一下 - // left_motor_rpm = -left_motor_rpm; + left_motor_rpm = -left_motor_rpm; // 返回计算结果 *left_motor_speed = left_motor_rpm; *right_motor_speed = right_motor_rpm; - -#endif } // 映射遥控器速度，分为死区、低速区和高速区。 @@ -457,7 +299,7 @@ static void diffProcess(void *signal_id) diff_data.desired_yaw_rate = diff_data.desired_curvature * diff_data.desired_speed; } -// printf("desired_speed: %f, desired_yaw: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate); + // 使用 PID 控制器计算输出速度和曲率 float output_speed = calculatePidOutput(&speed_pid, diff_data.desired_speed, diff_data.speed, 0.0f, dt); float output_yaw_rate = calculatePidOutput(&yaw_rate_pid, diff_data.desired_yaw_rate, diff_data.yaw_rate, 0.0f, dt); @@ -467,105 +309,14 @@ static void diffProcess(void *signal_id) // 限制输出速度在当前速度和最大加速度计算出来的速度之间 // output_speed = constrain(output_speed, diff_data.speed - max_acceleration * dt, diff_data.speed + max_acceleration * dt); +// printf("output_speed: %f, output_yaw: %f\n", output_speed, output_yaw_rate); - 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; - } - -// printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral); - - -// if(diff_data.desired_yaw_rate != 0)//有转向的情况下下 -// { -// if( (output_yaw_rate > -500) && (output_yaw_rate < 500) )//如果是转向输出在-500~500之间，那么开始原地转向扭矩太小，所以设定最小扭矩。 -// { -// output_yaw_rate = 500; -// } -// } - - float out_torque[4] = {0,0,0,0}; // 使用差速车辆动力学模型计算左右电机的期望速度 - computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, &out_torque[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); - - - - - - -// if( (left_speed < 200) && (left_speed > -200) ) -// { -// left_speed = 0; -// } -// -// if( (right_speed < 200) && (right_speed > -200) ) -// { -// right_speed = 0; -// } - + computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, &diff_data.out_left_motor_speed, &diff_data.out_right_motor_speed); // 设置电机输出 - setMotorOutput(&diff_data.out_torq[0], + setMotorOutput(diff_data.out_left_motor_speed, + diff_data.out_right_motor_speed, diff_data.max_Torq,// (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr")); @@ -575,48 +326,13 @@ static void diffProcess(void *signal_id) publishMessage(&un_motor_output1, 1); publishMessage(&un_motor_output2, 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*(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 float left_speed_fiter[SPEED_FITER_NUM] = {0}; -// static uint8_t left_speed_cnt = 0; -// static float right_speed_fiter[SPEED_FITER_NUM] = {0}; -// static uint8_t right_speed_cnt = 0; - 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; @@ -634,8 +350,11 @@ static void diffInput(void *signal_id) diff_data.desired_speed = diff_data.desired_speed * 0.01f; diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f; // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出 - diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5); - diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0.1, 2, 2, 1, 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.02, 2, 2, 1, 0.5); + + diff_data.desired_curvature = -diff_data.desired_curvature;//20250316前进后退反相 + if(diff_data.desired_speed >= 0)//20250320 增加根据速度大小来决定方向，解决后退时转弯反向的问题 { @@ -649,14 +368,16 @@ static void diffInput(void *signal_id) } else if ( (signal_id == &un_manual_computer_input) && (diff_data.mode == MODE_AUTO) ) { +// printf(" un_manual_computer_input\n"); + diff_data.desired_speed = (float)((int16_t)(un_manual_computer_input.bit_data.set_speed)); diff_data.desired_curvature = (float)((int16_t)(un_manual_computer_input.bit_data.set_curvature)); // 单位转换 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, 0, 5, 10, 2.5, 5); -// diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1); + diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1); } else if ( (signal_id == &un_auto_computer_input) && (diff_data.mode == MODE_AUTO) ) { @@ -664,68 +385,19 @@ static void diffInput(void *signal_id) diff_data.desired_curvature = (float)((int16_t)(un_auto_computer_input.bit_data.set_curvature)); // 单位转换 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;// 20241016 增加转弯反相 // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出 diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0, 5, 10, 2.5, 5); -// diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1); + diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1); } else if (signal_id == &un_motor_input1)// 处理第一个电机速度信号（左电机） { - - diff_data.right_front_motor_speed = -(float)( (int16_t)(un_motor_input1.bit_data.MotCon_1Signal3) ) / 6.0; // 20250502 1号控制器增加反相 - diff_data.right_rear_motor_speed = -(float)( (int16_t)(un_motor_input2.bit_data.MotCon_1Signal4) ) /6.0; -// motor_speed_temp = (motor_speed_temp + (int16_t)un_motor_input2.bit_data.MotCon_1Signal4)/2/6; - - if(fabs(diff_data.right_rear_motor_speed) > fabs(diff_data.right_front_motor_speed))//取速度较小的轮速 - { - motor_speed_temp = diff_data.right_front_motor_speed; - } - else - { - motor_speed_temp = diff_data.right_rear_motor_speed; - } - -// diff_data.right_motor_speed = LOWPASS_FILTER(motor_speed_temp,right_speed_fiter[0]); - diff_data.right_motor_speed = motor_speed_temp;// -// right_speed_fiter[0] = diff_data.right_motor_speed; - - -// if(SPEED_FITER_NUM == right_speed_cnt)//取样4次后滤波 -// { -// right_speed_cnt = 0; -// diff_data.right_motor_speed = (float)Filter(right_speed_fiter,SPEED_FITER_NUM)/6.0f; -// } - -// printf("right_motor_speed = %f, motor_speed_temp = %d\n",diff_data.right_motor_speed,motor_speed_temp); - + diff_data.left_motor_speed = (float)((int16_t)(un_motor_input1.bit_data.speed - 30000));//20240921 增加偏移量 + diff_data.left_motor_speed = -diff_data.left_motor_speed;// 左边电机方向反一下，因为电机安装反了，返回来的数据也要反一下 } else if (signal_id == &un_motor_input2)// 处理第二个电机速度信号（右电机） { - diff_data.left_front_motor_speed = -(float)( (int16_t)(un_motor_input1.bit_data.MotCon_1Signal4) ) /6.0; // 20250502 1号控制器增加反相 - diff_data.left_rear_motor_speed = (float)( (int16_t) (un_motor_input2.bit_data.MotCon_1Signal3) ) / 6.0; - - if(fabs(diff_data.left_front_motor_speed) > fabs(diff_data.left_rear_motor_speed))//取速度较小的轮速 - { - motor_speed_temp = diff_data.left_rear_motor_speed; - } - else - { - motor_speed_temp = diff_data.left_front_motor_speed; - } - - diff_data.left_motor_speed = motor_speed_temp; // -// diff_data.left_motor_speed = LOWPASS_FILTER(motor_speed_temp,left_speed_fiter[0]);//低通滤波器 -// left_speed_fiter[0] = diff_data.left_motor_speed; - - -// left_speed_fiter[left_speed_cnt] = motor_speed_temp; -// left_speed_cnt ++; -// if(SPEED_FITER_NUM == left_speed_cnt)//取样4次后滤波 -// { -// left_speed_cnt = 0; -// diff_data.left_motor_speed = (float)Filter(left_speed_fiter,SPEED_FITER_NUM)/6.0f; -// } - + diff_data.right_motor_speed = (float)((int16_t)(un_motor_input2.bit_data.speed - 30000));//20240921 增加偏移量 } // 急停开关 @@ -766,13 +438,11 @@ static void diffInput(void *signal_id) void preChargeFinish(void *signal_id) { (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(0, 0, (uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr")); // 档位 -// un_motor_output1.bit_data.gear = 0; // 0表示空挡 -// un_motor_output2.bit_data.gear = 0; + un_motor_output1.bit_data.gear = 0; // 0表示空挡 + un_motor_output2.bit_data.gear = 0; publishMessage(&un_motor_output1, 1); publishMessage(&un_motor_output2, 1); } @@ -781,85 +451,28 @@ void preChargeFinish(void *signal_id) void diffParametersInit(void *signal_id) { (void)signal_id; // 标记变量为已使用，避免编译器警告 - - if(diff_data.mode == MODE_AUTO)//20250504 自动模式PID - { - setPidParameters(&speed_pid, - getParam("Ospd_kp"), - getParam("Ospd_ki"), - getParam("Ospd_kd"), - getParam("Ospd_il"), - getParam("Ospd_ol") - ); + setPidParameters(&speed_pid, + getParam("spd_kp"), + getParam("spd_ki"), + getParam("spd_kd"), + getParam("spd_il"), + getParam("spd_ol") + ); - setPidParameters(&yaw_rate_pid, - getParam("Ocrv_kp"), - getParam("Ocrv_ki"), - getParam("Ocrv_kd"), - getParam("Ocrv_il"), - getParam("Ocrv_ol") - ); - } - else//手动模式 - { - setPidParameters(&speed_pid, - getParam("spd_kp"), - getParam("spd_ki"), - getParam("spd_kd"), - getParam("spd_il"), - getParam("spd_ol") - ); - - setPidParameters(&yaw_rate_pid, - getParam("crv_kp"), - getParam("crv_ki"), - getParam("crv_kd"), - getParam("crv_il"), - getParam("crv_ol") - ); - } - - - // 设置曲率 PID 控制器的参数 - setPidParameters(&Dec_front_speed_pid, - getParam("mot_kp"), - getParam("mot_ki"), - getParam("mot_kd"), - getParam("mot_il"), - getParam("mot_ol") - ); - - // 设置曲率 PID 控制器的参数 - setPidParameters(&Acc_front_speed_pid, - Dec_front_speed_pid.kp, - Dec_front_speed_pid.ki, - Dec_front_speed_pid.kd, - Dec_front_speed_pid.integral_limit, - Dec_front_speed_pid.output_limit - ); - - 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");//参数读取设定最大扭矩 - - - 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); + setPidParameters(&yaw_rate_pid, + getParam("crv_kp"), + getParam("crv_ki"), + getParam("crv_kd"), + getParam("crv_il"), + getParam("crv_ol") + ); + 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("mode: %d\n", diff_data.mode); printf("left_motor_speed = %f\n",diff_data.left_motor_speed); printf("right_motor_speed = %f\n",diff_data.right_motor_speed); - - printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed); - printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", diff_data.out_torq[0], diff_data.out_torq[1], diff_data.out_torq[2], diff_data.out_torq[3]); + printf("desired_cc_speed: %d, desired_cc_yaw: %d\n", un_manual_computer_input.bit_data.set_speed, un_manual_computer_input.bit_data.set_curvature); float deffspeed = (float)((int16_t)(un_remote_control_input.bit_data.speed)); float deffcurvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature)); @@ -867,17 +480,7 @@ void diffParametersInit(void *signal_id) deffspeed = deffspeed * 0.01f; deffcurvature = deffcurvature * 0.0001f; - printf("remote speed = %f, remote curvature = %f\n", deffspeed, deffcurvature); - - - deffspeed = (float)((int16_t)(un_manual_computer_input.bit_data.set_speed)); - deffcurvature = (float)((int16_t)(un_manual_computer_input.bit_data.set_curvature)); - // 单位转换 - deffspeed = deffspeed * 0.01f; - deffcurvature = deffcurvature * 0.0001f; - - printf("manual speed = %f, manual curvature = %f\n", deffspeed, deffcurvature); - + printf("remote speed = %f, remote curvature = %f\n", deffspeed, deffcurvature); timerStart(&diff_app_timer,1000,1);//1s调用一次 } @@ -922,28 +525,6 @@ void diffAppInit(void) getParam("crv_ol") ); - // 初始化减速 PID 控制器 - initializePid(&Dec_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); - // 设置 PID 控制器的参数 - setPidParameters(&Dec_front_speed_pid, - getParam("mot_kp"), - getParam("mot_ki"), - getParam("mot_kd"), - getParam("mot_il"), - getParam("mot_ol") - ); - - // 初始化加速 PID 控制器 - initializePid(&Acc_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); - // 设置 PID 控制器的参数 - setPidParameters(&Acc_front_speed_pid, - Dec_front_speed_pid.kp, - Dec_front_speed_pid.ki, - Dec_front_speed_pid.kd, - Dec_front_speed_pid.integral_limit, - Dec_front_speed_pid.output_limit - ); - subscribe(&diff_app_timer, diffParametersInit); timerStart(&diff_app_timer,1000,1);//1s调用一次 diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.h b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.h index dd6451c..378e8b2 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.h +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.h @@ -8,20 +8,7 @@ extern "C" #include "app_config.h" -#define SPEED_FITER_NUM 6 - - -#define SPEED_PID_MODE 0 -#define THROTTLE_PID_MODE 1 - - -#define ALPHA 0.1f // 滤波系数α∈[0.01,0.3]，0.2对应截止频率约10Hz（假设采样周期10ms） -#define LOWPASS_FILTER(speed, prev) (ALPHA * (speed) + (1 - ALPHA) * (prev)) - - - - typedef enum { MODE_MANUAL, // 手动模式 @@ -35,10 +22,6 @@ typedef struct DiffData float desired_curvature; // 期望曲率 float left_motor_speed; // 当前左电机速度 float right_motor_speed; // 当前右电机速度 - float left_front_motor_speed; // 当前左前电机速度 - float right_front_motor_speed; // 当前右前电机速度 - float left_rear_motor_speed; // 当前左后电机速度 - float right_rear_motor_speed; // 当前右后电机速度 float speed; // 当前车速 float curvature; // 当前曲率 float yaw_rate; // 当前角速度 @@ -53,17 +36,7 @@ typedef struct DiffData uint8_t emergency_stop_state; // 急停状态 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; // 差速速度死区 - + uint16_t max_Torq; // 最大扭矩限制 } DiffData; diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_frm_signal.h b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_frm_signal.h index cac6043..f598bfa 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_frm_signal.h +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_frm_signal.h @@ -10,7 +10,7 @@ extern "C" #include "app_dependence.h" #define MAX_SIGNALS 500u // 每个优先级的最大信号数量 -#define MAX_SUBSCRIBERS 100u // 不同信号的订阅者数量 +#define MAX_SUBSCRIBERS 50u // 不同信号的订阅者数量 #define MAX_CALLBACKS 25u // 每个信号最多支持多少订阅者 #define PRIORITY_LEVELS 2u // 优先级层次 diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_light.c b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_light.c index 78a7b8f..a8fc1e2 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_light.c +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_light.c @@ -110,28 +110,40 @@ static void lightOutput(void *signal_id) switch (i) {//正常所有灯光熄灭 case LIGHT_HEAD://头灯，前面4个灯 - un_inf_can_kgf_output2.bit_data.KGF07 = state_value; - un_inf_can_kgf_output2.bit_data.KGF09 = state_value; + un_inf_can_kgf_output1.bit_data.KGF06 = state_value;//左前红灯 + un_inf_can_kgf_output1.bit_data.KGF09 = state_value;//左前黄灯 +// un_inf_can_kgf_output2.bit_data.KGF08 = state_value;//右前红灯 +// un_inf_can_kgf_output2.bit_data.KGF09 = state_value;//右前黄灯 break; case LIGHT_TAIL://尾灯，后面4个灯 - un_inf_can_kgf_output2.bit_data.KGF11 = state_value; - un_inf_can_kgf_output2.bit_data.KGF13 = state_value; +// un_inf_can_kgf_output2.bit_data.KGF10 = state_value;//左后红灯 +// un_inf_can_kgf_output2.bit_data.KGF11 = state_value;//左后黄灯 +// un_inf_can_kgf_output2.bit_data.KGF13 = state_value;//右后红灯 +// un_inf_can_kgf_output2.bit_data.KGF14 = state_value;//右后黄灯 break; case LIGHT_LEFT_TURN://左转向，左边4个灯 - un_inf_can_kgf_output2.bit_data.KGF08 = state_value; - un_inf_can_kgf_output2.bit_data.KGF12 = state_value; + un_inf_can_kgf_output1.bit_data.KGF06 = state_value;//左前红灯 + un_inf_can_kgf_output1.bit_data.KGF09 = state_value;//左前黄灯 +// un_inf_can_kgf_output2.bit_data.KGF10 = state_value;//左后红灯 +// un_inf_can_kgf_output2.bit_data.KGF11 = state_value;//左后黄灯 break; case LIGHT_RIGHT_TURN://右转向灯，右边4个灯 - un_inf_can_kgf_output2.bit_data.KGF10 = state_value; - un_inf_can_kgf_output2.bit_data.KGF14 = state_value; +// un_inf_can_kgf_output2.bit_data.KGF08 = state_value;//右前红灯 +// un_inf_can_kgf_output2.bit_data.KGF09 = state_value;//右前黄灯 +// un_inf_can_kgf_output2.bit_data.KGF13 = state_value;//右后红灯 +// un_inf_can_kgf_output2.bit_data.KGF14 = state_value;//右后黄灯 break; case LIGHT_BRAKE://刹车灯，四个黄灯 - un_inf_can_kgf_output2.bit_data.KGF11 = state_value; - un_inf_can_kgf_output2.bit_data.KGF13 = state_value; + un_inf_can_kgf_output1.bit_data.KGF09 = state_value;//左前黄灯 +// un_inf_can_kgf_output2.bit_data.KGF11 = state_value;//左后黄灯 +// un_inf_can_kgf_output2.bit_data.KGF09 = state_value;//右前黄灯 +// un_inf_can_kgf_output2.bit_data.KGF14 = state_value;//右后黄灯 break; case LIGHT_ALARM://报警灯，四个红灯 - un_inf_can_kgf_output2.bit_data.KGF11 = state_value; - un_inf_can_kgf_output2.bit_data.KGF13 = state_value; + un_inf_can_kgf_output1.bit_data.KGF06 = state_value;//左前红灯 + un_inf_can_kgf_output2.bit_data.KGF08 = state_value;//右前红灯 +// un_inf_can_kgf_output2.bit_data.KGF10 = state_value;//左后红灯 +// un_inf_can_kgf_output2.bit_data.KGF13 = state_value;//右后红灯 break; } } diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_param_manage.h b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_param_manage.h index 9f9bf6d..9963107 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_param_manage.h +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_param_manage.h @@ -35,32 +35,10 @@ extern "C" X(prCTime) \ X(brk_pos) \ X(pwr_sta) \ - X(high_sw) \ - X(stop_sw) \ X(lightSt) \ X(pwr_btn) \ X(sleepTm) \ X(wakeTm) \ - X(Ospd_kp) \ - X(Ospd_ki) \ - X(Ospd_kd) \ - X(Ospd_il) \ - X(Ospd_ol) \ - X(Ocrv_kp) \ - X(Ocrv_ki) \ - X(Ocrv_kd) \ - X(Ocrv_il) \ - X(Ocrv_ol) \ - X(minTorq) \ - X(minYpos) \ - X(maxYpos) \ - X(mot_kp) \ - X(mot_ki) \ - X(mot_kd) \ - X(mot_il) \ - X(mot_ol) \ - X(diff_sp) \ - X(turn_sp) \ X(test) // 定义一个包含所有参数名称的结构体 diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_power (conflicted copy 2025-07-26 160926).c b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_power (conflicted copy 2025-07-26 160926).c new file mode 100644 index 0000000..b6ed680 --- /dev/null +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_power (conflicted copy 2025-07-26 160926).c @@ -0,0 +1,429 @@ +#include "app_config.h" +#include "interface.h" +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" +#include "app_param_manage.h" + +#include "app_power.h" + + +// 定义按钮状态枚举 +typedef enum { + BUTTON_STATE_INITIAL, + BUTTON_STATE_SHORT_PRESS, + BUTTON_STATE_SHORT_PRESS_DETECTED, + BUTTON_STATE_WAIT_FOR_LONG_PRESS, + BUTTON_STATE_LONG_PRESS, + BUTTON_STATE_LONG_PRESS_WAIT +} ButtonState; + +// 定义按钮结构体 +typedef struct { + ButtonState state; + uint32_t press_start_time; + uint32_t release_start_time; + uint8_t is_power_on; + Timer timer; + uint8_t old_is_power_on; + Timer timer1; + +} PowerButton; + +// 全局变量 +PowerSystem power_data; +static PowerButton power_button = {BUTTON_STATE_INITIAL, 0, 0, 0, {0},0}; + +// 电源按钮处理函数 +static void handlePowerButton(void) +{ + switch (power_button.state) + { + case BUTTON_STATE_INITIAL: + if (power_data.remote_power_switch == app_close()) + { + power_button.state = BUTTON_STATE_SHORT_PRESS_DETECTED; + timerStart(&power_button.timer, 500, 0); // 启动短按定时器，500ms + } + break; + + case BUTTON_STATE_SHORT_PRESS_DETECTED: + if (power_data.remote_power_switch == app_open()) + { + if (power_button.timer.active) // 定时器未到期,短按完成，启动等待长按定时器 + { + power_button.state = BUTTON_STATE_WAIT_FOR_LONG_PRESS; + timerStart(&power_button.timer, 500, 0); // 启动等待长按定时器，500ms + } + } + else if (!power_button.timer.active)// 短按定时器到期，按键仍被按下，视为无效，重置为初始状态 + { + power_button.state = BUTTON_STATE_INITIAL; + } + break; + + case BUTTON_STATE_WAIT_FOR_LONG_PRESS: + if (power_data.remote_power_switch == app_close())// 检测是否在等待时间内进行长按 + { + power_button.state = BUTTON_STATE_LONG_PRESS; + timerStart(&power_button.timer, 1000, 0); // 启动长按定时器，1000ms + } + else if (!power_button.timer.active) // 等待长按超时，重置为初始状态 + { + power_button.state = BUTTON_STATE_INITIAL; + } + break; + + case BUTTON_STATE_LONG_PRESS: + + if (!power_button.timer.active)// 长按完成，切换电源状态 20250423 修改不需要判断松开按键就打开控制器 + { + power_button.is_power_on = !power_button.is_power_on; + printf("PowerButton: is_power_on = %d\n", power_button.is_power_on); + power_button.state = BUTTON_STATE_LONG_PRESS_WAIT; + } + else if(power_data.remote_power_switch == app_open()) + { + power_button.state = BUTTON_STATE_INITIAL; + printf("Long press for short duration"); + } + else + break; + + case BUTTON_STATE_LONG_PRESS_WAIT: + if (power_data.remote_power_switch == app_open())// 检测按键释放 + { + power_button.state = BUTTON_STATE_INITIAL; + printf("Release the button"); + } + default: + power_button.state = BUTTON_STATE_INITIAL; + break; + } +} + + +// 输出处理函数 +static void powerOutput(void *signal_id) +{ + (void)signal_id; + + // 根据当前状态,控制各个设备的电源 + switch (power_data.current_state) + { + case POWER_PRE_CHARGE: + publishMessage(&power_data.pre_charge_finish, 1);//发布预充完成信号，100ms发送一次，直到预充完成 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOn(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 计算机 自己 + un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + break; + + case POWER_NEUTRAL: + publishMessage(&power_data.pre_charge_finish, 1);//20250316增加，发送空挡信号，保证电机控制器高压上电后，发送空挡信号 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOn(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOn(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 计算机 自己 + un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + break; + + + case POWER_STANDBY: + // 初始状态,只开启基本设备 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOff(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOff(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 计算机 自己 + un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + break; + + case POWER_WORKING: + // 工作状态,除预充继电器外所有设备开启 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOn(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOn(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 计算机 自己 + un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + break; + + case POWER_EMERGENCY: + // 急停状态,断开高压 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 计算机 自己 + un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + break; + + case POWER_SLEEP: + // 休眠状态,关闭所有设备 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOff(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOff(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 计算机 自己 + un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + break; + + default: + break; + } + publishMessage(&power_data, 1); + publishMessage(&un_inf_can_kgf_output1, 1); + publishMessage(&un_inf_can_kgf_output2, 1); +} + +static void wakeupProcess(void *signal_id) +{ + (void)signal_id; + + un_gather_output.bit_data.sleep_duration = (uint16_t)getParam("sleepTm"); + un_gather_output.bit_data.wakeup_interval = (uint16_t)getParam("wakeTm"); + + if(un_gather_output.bit_data.sleep_duration < 5)//最小值限定 + { + un_gather_output.bit_data.sleep_duration = 5; + } + + if(un_gather_output.bit_data.wakeup_interval < 5)//最小值限定 + { + un_gather_output.bit_data.wakeup_interval = 5; + } + + un_gather_output.bit_data.vehicle_mode = power_data.current_state; + + publishMessage(&un_gather_output, 1); + timerStart(&power_data.timer1, 500, 1); //周期调用 +} + + +// 定时器处理函数 +static void powerTimerProcess(void *signal_id) +{ + (void)signal_id; + + // 调用电源按钮处理函数 + handlePowerButton(); + // 电源按钮状态有变化，保存到参数 + if (power_button.is_power_on != power_button.old_is_power_on) + { + setParam("pwr_btn", (float)power_button.is_power_on); + power_button.old_is_power_on = power_button.is_power_on; + } + + // 状态转换逻辑 + switch (power_data.current_state) + { + case POWER_PRE_CHARGE: + if (!power_data.timer_pre_charge.active) // 预充时间到 + { + power_data.current_state = POWER_NEUTRAL; // 工作 + power_data.pre_charge_finish = 1; // 预充完成 + printf("Power: Transitioning from PRE_CHARGE to POWER_NEUTRAL state\n"); + } + break; + case POWER_NEUTRAL://20250316增加，发送空挡信号 + if (power_data.neutral_cnt >= 5) // 运行5次 + { + power_data.neutral_cnt = 0; + power_data.current_state = POWER_WORKING; // 工作 + power_data.pre_charge_finish = 1; // 预充完成 + printf("Power: Transitioning from POWER_NEUTRAL to WORKING state\n"); + } + else + { + power_data.neutral_cnt ++; + power_data.current_state = POWER_NEUTRAL; // 空挡 + power_data.pre_charge_finish = 1; // 预充完成 + } + break; + + case POWER_STANDBY: + if (power_data.high_voltage_switch == app_open()) // 高压开关断开 + { + power_data.current_state = POWER_SLEEP; // 休眠 + printf("Power: Transitioning from STANDBY to SLEEP state\n"); + } + else if (power_button.is_power_on == app_close() && power_data.emergency_stop == app_close()) // 遥控器电源开关闭合且急停开关闭合 + { + power_data.current_state = POWER_EMERGENCY; // 急停 + printf("Power: Transitioning from STANDBY to EMERGENCY state\n"); + } + break; + + case POWER_WORKING: + if (power_data.high_voltage_switch == app_open()) // 高压开关断开 + { + power_data.current_state = POWER_SLEEP; // 休眠 + printf("Power: Transitioning from STANDBY to SLEEP state\n"); + } + else if (power_data.emergency_stop == app_close()) // 急停开关闭合 + { + power_data.current_state = POWER_EMERGENCY; // 急停 + printf("Power: Transitioning from WORKING to EMERGENCY state\n"); + + printf("emergency_stop_switch: %d, remote_emergency_stop: %d\n", power_data.emergency_stop_switch, power_data.remote_emergency_stop); //打印状态 + printf("remote_stop: %d\n", un_remote_control_input.bit_data.switch_b); + } + break; + case POWER_EMERGENCY: + if (power_data.high_voltage_switch == app_open()) // 高压开关断开 + { + power_data.current_state = POWER_SLEEP; // 休眠 + printf("Power: Transitioning from EMERGENCY to SLEEP state\n"); + } + else if (power_button.is_power_on == app_open()) // 遥控器电源开关断开 + { + power_data.current_state = POWER_STANDBY; // 待机 + printf("Power: Transitioning from EMERGENCY to STANDBY state\n"); + } + else if (power_data.emergency_stop == app_open()) // 急停断开 + { + power_data.current_state = POWER_PRE_CHARGE; // 预充 + timerStart(&power_data.timer_pre_charge, (uint32_t)(getParam("prCTime") * 1000), 1); // 启动预充定时器 + printf("Power: Transitioning from EMERGENCY to PRE_CHARGE state\n"); + } + break; + case POWER_SLEEP: + if (power_data.high_voltage_switch == app_close()) // 高压开关闭合 + { + power_data.current_state = POWER_STANDBY; // 待机 + printf("Power: Transitioning from SLEEP to STANDBY state\n"); + } + break; + default: + power_data.current_state = POWER_STANDBY; // 待机 + break; + } + + powerOutput(NULL); // 输出 + + // 电源状态有变化，记录到参数 + if (power_data.old_state != power_data.current_state) + { + power_data.old_state = power_data.current_state; + setParam("pwr_sta", (float)power_data.current_state); + } + + timerStart(&power_data.timer, 100, 1); //周期调用 +} + + + + +// 处理所有输入信号的函数 +static void powerInput(void *signal_id) +{ + //不能直接赋值，用memcpy + PowerSystem old_data; + memcpy(&old_data, &power_data, sizeof(PowerSystem)); + + // 填充数据 + if (signal_id == &un_sw_sample) + { + power_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch;//急停开关 + power_data.high_voltage_switch = (uint8_t)un_sw_sample.bit_data.High_voltage_switch;//高压开关 + } + else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据 + { + power_data.remote_power_switch = (uint8_t)un_remote_control_input.bit_data.switch_d; // 遥控器电源开关 + power_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1;// 遥控器急停开关 + } + + // 急停开关 + power_data.emergency_stop = (uint8_t)( (power_data.emergency_stop_switch == app_close()) || (power_data.remote_emergency_stop == app_close()) ); + +} + + +// APP模块的初始化 +void powerAppInit(void) +{ + // 初始化变量 + memset(&power_data, 0, sizeof(PowerSystem)); + power_data.current_state = POWER_STANDBY; + // 初始化时恢复电源状态 + power_data.current_state = (PowerState)getParam("pwr_sta"); + power_data.old_state = power_data.current_state; + // 恢复电源按钮状态 + power_button.is_power_on = (uint8_t)getParam("pwr_btn"); + power_button.old_is_power_on = power_button.is_power_on; + + // 订阅输入信号 + subscribe(&un_sw_sample, powerInput); // 急停开关、高压开关 + subscribe(&un_remote_control_input, powerInput); // 遥控器电源开关 + // 定时器 + timerInit(&power_data.timer); + subscribe(&power_data.timer, powerTimerProcess); + timerStart(&power_data.timer, 500, 1); // 周期调用 + //定时器唤醒 + timerInit(&power_data.timer1); + subscribe(&power_data.timer1, wakeupProcess); + timerStart(&power_data.timer1, 500, 1); // 周期调用 + //预充定时器 + timerInit(&power_data.timer_pre_charge); + subscribe(&power_data.timer_pre_charge, powerTimerProcess); + + printf("app_power: initial OK\n"); +} diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_power.c b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_power.c index 16629f6..49bd4a0 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_power.c +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_power.c @@ -43,7 +43,7 @@ static void handlePowerButton(void) if (power_data.remote_power_switch == app_close()) { power_button.state = BUTTON_STATE_SHORT_PRESS_DETECTED; - timerStart(&power_button.timer, 500, 1); // 启动短按定时器，500ms + timerStart(&power_button.timer, 500, 0); // 启动短按定时器，500ms } break; @@ -53,7 +53,7 @@ static void handlePowerButton(void) if (power_button.timer.active) // 定时器未到期,短按完成，启动等待长按定时器 { power_button.state = BUTTON_STATE_WAIT_FOR_LONG_PRESS; - timerStart(&power_button.timer, 500, 1); // 启动等待长按定时器，500ms + timerStart(&power_button.timer, 500, 0); // 启动等待长按定时器，500ms } } else if (!power_button.timer.active)// 短按定时器到期，按键仍被按下，视为无效，重置为初始状态 @@ -66,7 +66,7 @@ static void handlePowerButton(void) if (power_data.remote_power_switch == app_close())// 检测是否在等待时间内进行长按 { power_button.state = BUTTON_STATE_LONG_PRESS; - timerStart(&power_button.timer, 1000, 1); // 启动长按定时器，1000ms + timerStart(&power_button.timer, 1000, 0); // 启动长按定时器，1000ms } else if (!power_button.timer.active) // 等待长按超时，重置为初始状态 { @@ -113,15 +113,14 @@ static void powerOutput(void *signal_id) { case POWER_PRE_CHARGE: publishMessage(&power_data.pre_charge_finish, 1);//发布预充完成信号，100ms发送一次，直到预充完成 - un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOn(); // 预充继电器 - un_inf_can_kgf_output1.bit_data.KGF05 = setPowerOn(); // 上装转台继电器 - un_inf_can_kgf_output1.bit_data.KGF06 = setPowerOn(); // 上装转台继电器 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOn(); // 预充继电器 un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器 un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器 - un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 导航计算机 - un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 导航计算机 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 计算机 自己 + un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机 @@ -129,20 +128,18 @@ static void powerOutput(void *signal_id) un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 - break; case POWER_NEUTRAL: publishMessage(&power_data.pre_charge_finish, 1);//20250316增加，发送空挡信号，保证电机控制器高压上电后，发送空挡信号 un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 - un_inf_can_kgf_output1.bit_data.KGF05 = setPowerOn(); // 上装转台继电器 - un_inf_can_kgf_output1.bit_data.KGF06 = setPowerOn(); // 上装转台继电器 un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOn(); // 高压继电器 un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOn(); // 高压继电器 un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器 un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器 - un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 导航计算机 - un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 导航计算机 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 计算机 自己 + un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机 @@ -156,14 +153,13 @@ static void powerOutput(void *signal_id) case POWER_STANDBY: // 初始状态,只开启基本设备 un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 - un_inf_can_kgf_output1.bit_data.KGF05 = setPowerOff(); // 上装转台继电器 - un_inf_can_kgf_output1.bit_data.KGF06 = setPowerOff(); // 上装转台继电器 un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOff(); // 低压继电器 un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOff(); // 低压继电器 - un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOff(); // 导航计算机 - un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOff(); // 导航计算机 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 计算机 自己 + un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机 @@ -176,14 +172,13 @@ static void powerOutput(void *signal_id) case POWER_WORKING: // 工作状态,除预充继电器外所有设备开启 un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 - un_inf_can_kgf_output1.bit_data.KGF05 = setPowerOn(); // 上装转台继电器 - un_inf_can_kgf_output1.bit_data.KGF06 = setPowerOn(); // 上装转台继电器 un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOn(); // 高压继电器 un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOn(); // 高压继电器 un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器 un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器 - un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 导航计算机 - un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 导航计算机 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 计算机 自己 + un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机 @@ -196,14 +191,13 @@ static void powerOutput(void *signal_id) case POWER_EMERGENCY: // 急停状态,断开高压 un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 - un_inf_can_kgf_output1.bit_data.KGF05 = setPowerOn(); // 上装转台继电器 - un_inf_can_kgf_output1.bit_data.KGF06 = setPowerOn(); // 上装转台继电器 un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器 un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器 - un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 导航计算机 - un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 导航计算机 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 计算机 自己 + un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机 @@ -216,14 +210,13 @@ static void powerOutput(void *signal_id) case POWER_SLEEP: // 休眠状态,关闭所有设备 un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 - un_inf_can_kgf_output1.bit_data.KGF05 = setPowerOff(); // 上装转台继电器 - un_inf_can_kgf_output1.bit_data.KGF06 = setPowerOff(); // 上装转台继电器 un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOff(); // 低压继电器 un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOff(); // 低压继电器 - un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOff(); // 导航计算机 - un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOff(); // 导航计算机 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 计算机 自己 + un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机 @@ -285,27 +278,62 @@ static void powerTimerProcess(void *signal_id) case POWER_PRE_CHARGE: if (!power_data.timer_pre_charge.active) // 预充时间到 { - power_data.current_state = POWER_WORKING; // 工作 + power_data.current_state = POWER_NEUTRAL; // 工作 power_data.pre_charge_finish = 1; // 预充完成 - printf("Power: Transitioning from PRE_CHARGE to WORKING state\n"); + printf("Power: Transitioning from PRE_CHARGE to POWER_NEUTRAL state\n"); } break; + case POWER_NEUTRAL://20250316增加，发送空挡信号 + if (power_data.neutral_cnt >= 5) // 运行5次 + { + power_data.neutral_cnt = 0; + power_data.current_state = POWER_WORKING; // 工作 + power_data.pre_charge_finish = 1; // 预充完成 + printf("Power: Transitioning from POWER_NEUTRAL to WORKING state\n"); + } + else + { + power_data.neutral_cnt ++; + power_data.current_state = POWER_NEUTRAL; // 空挡 + power_data.pre_charge_finish = 1; // 预充完成 + } + break; + case POWER_STANDBY: - if (power_button.is_power_on == app_close() && power_data.emergency_stop == app_close()) // 遥控器电源开关闭合且急停开关闭合 + if (power_data.high_voltage_switch == app_open()) // 高压开关断开 + { + power_data.current_state = POWER_SLEEP; // 休眠 + printf("Power: Transitioning from STANDBY to SLEEP state\n"); + } + else if (power_button.is_power_on == app_close() && power_data.emergency_stop == app_close()) // 遥控器电源开关闭合且急停开关闭合 { power_data.current_state = POWER_EMERGENCY; // 急停 printf("Power: Transitioning from STANDBY to EMERGENCY state\n"); } break; + case POWER_WORKING: - if (power_data.emergency_stop == app_close()) // 急停开关闭合 + if (power_data.high_voltage_switch == app_open()) // 高压开关断开 + { + power_data.current_state = POWER_SLEEP; // 休眠 + printf("Power: Transitioning from STANDBY to SLEEP state\n"); + } + else if (power_data.emergency_stop == app_close()) // 急停开关闭合 { power_data.current_state = POWER_EMERGENCY; // 急停 - printf("Power: Transitioning from WORKING to EMERGENCY state\n"); + printf("Power: Transitioning from WORKING to EMERGENCY state\n"); + + printf("emergency_stop_switch: %d, remote_emergency_stop: %d\n", power_data.emergency_stop_switch, power_data.remote_emergency_stop); //打印状态 + printf("remote_stop: %d\n", un_remote_control_input.bit_data.switch_b); } break; case POWER_EMERGENCY: - if (power_button.is_power_on == app_open()) // 遥控器电源开关断开 + if (power_data.high_voltage_switch == app_open()) // 高压开关断开 + { + power_data.current_state = POWER_SLEEP; // 休眠 + printf("Power: Transitioning from EMERGENCY to SLEEP state\n"); + } + else if (power_button.is_power_on == app_open()) // 遥控器电源开关断开 { power_data.current_state = POWER_STANDBY; // 待机 printf("Power: Transitioning from EMERGENCY to STANDBY state\n"); @@ -317,13 +345,13 @@ static void powerTimerProcess(void *signal_id) printf("Power: Transitioning from EMERGENCY to PRE_CHARGE state\n"); } break; -// case POWER_SLEEP: -// if (power_data.high_voltage_switch == app_close()) // 高压开关闭合 -// { -// power_data.current_state = POWER_STANDBY; // 待机 -// printf("Power: Transitioning from SLEEP to STANDBY state\n"); -// } -// break; + case POWER_SLEEP: + if (power_data.high_voltage_switch == app_close()) // 高压开关闭合 + { + power_data.current_state = POWER_STANDBY; // 待机 + printf("Power: Transitioning from SLEEP to STANDBY state\n"); + } + break; default: power_data.current_state = POWER_STANDBY; // 待机 break; @@ -362,22 +390,10 @@ static void powerInput(void *signal_id) power_data.remote_power_switch = (uint8_t)un_remote_control_input.bit_data.switch_d; // 遥控器电源开关 power_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1;// 遥控器急停开关 } + // 急停开关 power_data.emergency_stop = (uint8_t)( (power_data.emergency_stop_switch == app_close()) || (power_data.remote_emergency_stop == app_close()) ); - - // 急停开关有变化，记录到参数 - if (power_data.old_emergency_stop != power_data.emergency_stop) - { - power_data.old_emergency_stop = power_data.emergency_stop; - setParam("stop_sw", (float)power_data.emergency_stop); - } - - // 高压开关状态有变化，记录到参数 - if (power_data.old_high_voltage_switch != power_data.high_voltage_switch) - { - power_data.old_high_voltage_switch = power_data.high_voltage_switch; - setParam("high_sw", (float)power_data.high_voltage_switch); - } + } @@ -393,12 +409,6 @@ void powerAppInit(void) // 恢复电源按钮状态 power_button.is_power_on = (uint8_t)getParam("pwr_btn"); power_button.old_is_power_on = power_button.is_power_on; - //恢复高压开关状态 - power_data.high_voltage_switch = (uint8_t)getParam("high_sw"); - power_data.old_high_voltage_switch = power_data.high_voltage_switch; - //恢复急停开关状态 - power_data.emergency_stop = (uint8_t)getParam("stop_sw"); - power_data.old_emergency_stop = power_data.emergency_stop; // 订阅输入信号 subscribe(&un_sw_sample, powerInput); // 急停开关、高压开关 diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_power.h b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_power.h index adb981a..eb190d3 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_power.h +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_power.h @@ -33,13 +33,11 @@ typedef struct { Timer timer1; Timer timer_pre_charge; // 预充定时器 PowerState last_state; // 上一次状态 - uint8_t emergency_stop_switch; // 急停开关 + uint8_t emergency_stop_switch; // 急停开关 uint8_t high_voltage_switch; // 高压开关 - uint8_t old_high_voltage_switch; // 上一次高压开关 uint8_t remote_power_switch; // 遥控器电源开关 uint8_t remote_emergency_stop; // 遥控器急停开关 uint8_t emergency_stop; // 急停状态 - uint8_t old_emergency_stop; // 上一次急停开关 uint8_t pre_charge_finish; // 预充完成标志位 uint8_t old_state; // 上一次状态 uint8_t neutral_cnt; diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_request.c b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_request.c index 85a25b3..a1c9d1c 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_request.c +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_request.c @@ -22,10 +22,10 @@ static void processRequestframe(uint16_t id) switch (id)//注意是高位在前，低位在后 { case 0x2000://状态帧 - un_vehicle_Info_output.bit_data.frame_header = 0xCCAA;//帧头 - un_vehicle_Info_output.bit_data.frame_type = 0x2000;//帧类型 - un_vehicle_Info_output.bit_data.frame_length = 0x2900;//帧长 - un_vehicle_Info_output.bit_data.accumulated = VehicleStaACC0++;//累加值 + un_vehicle_Info_output.bit_data.frame_header = 0xCCAA;//帧头 + un_vehicle_Info_output.bit_data.frame_type = 0x2000;//帧类型 + un_vehicle_Info_output.bit_data.frame_length = 0x2900;//帧长 + un_vehicle_Info_output.bit_data.accumulated = VehicleStaACC0++;//累加值 TempAcc = 0; for (i = 0; i < 40; i++)//累加前40个字节 @@ -251,14 +251,8 @@ static void requestInput(void *signal_id) un_vehicle_Info_output.bit_data.speed = ((request16_temp << 8) | (request16_temp >> 8));//当前速度 request16_temp = (uint16_t)(int16_t)(diff_data.curvature * 10000.0); - un_vehicle_Info_output.bit_data.curvature = ((request16_temp << 8) | (request16_temp >> 8));//当前曲率 + un_vehicle_Info_output.bit_data.curvature = ((request16_temp << 8) | (request16_temp >> 8));//当前曲率 - request16_temp = (uint16_t)(int16_t)(diff_data.left_motor_speed *6); - un_vehicle_Info_output.bit_data.set_left_speed = ((request16_temp << 8) | (request16_temp >> 8));//当前速度 - - request16_temp = (uint16_t)(int16_t)(diff_data.right_motor_speed *6); - un_vehicle_Info_output.bit_data.set_right_speed = ((request16_temp << 8) | (request16_temp >> 8));//当前曲率 - } else if(signal_id == &un_auto_computer_input) { @@ -280,19 +274,18 @@ static void requestInput(void *signal_id) } else if(signal_id == &un_motor_input1) { - un_motor_status_output.bit_data.left_wheel_speed = SWAP_ENDIAN_16( (uint16_t)((int16_t)(un_motor_input1.bit_data.MotCon_1Signal4) + 30000) ); - -// un_motor_status_output.bit_data.left_torque = ((un_motor_input1.bit_data.torque << 8) | (un_motor_input1.bit_data.torque >> 8));//左侧扭矩 -// un_motor_status_output.bit_data.left_voltage = ((un_motor_input1.bit_data.bus_voltage << 8) | (un_motor_input1.bit_data.bus_voltage >> 8));//左侧电压 -// un_motor_status_output.bit_data.left_fault_code = un_motor_input1.bit_data.fault_code;//左侧故障码 + un_motor_status_output.bit_data.left_wheel_speed = ((un_motor_input1.bit_data.speed << 8) | (un_motor_input1.bit_data.speed >> 8));//左侧轮速 + un_motor_status_output.bit_data.left_torque = ((un_motor_input1.bit_data.torque << 8) | (un_motor_input1.bit_data.torque >> 8));//左侧扭矩 + un_motor_status_output.bit_data.left_voltage = ((un_motor_input1.bit_data.bus_voltage << 8) | (un_motor_input1.bit_data.bus_voltage >> 8));//左侧电压 + un_motor_status_output.bit_data.left_fault_code = un_motor_input1.bit_data.fault_code;//左侧故障码 } else if(signal_id == &un_motor_input2) { - un_motor_status_output.bit_data.right_wheel_speed = SWAP_ENDIAN_16 ( (uint16_t)((int16_t)(un_motor_input1.bit_data.MotCon_1Signal3) + 30000) );//左侧轮速 -// un_motor_status_output.bit_data.right_torque = ((un_motor_input2.bit_data.torque << 8) | (un_motor_input2.bit_data.torque >> 8));//右侧扭矩 -// un_motor_status_output.bit_data.right_fault_code = un_motor_input2.bit_data.fault_code;//右侧故障码 -// un_motor_status_output.bit_data.right_voltage = ((un_motor_input2.bit_data.bus_voltage << 8) | (un_motor_input2.bit_data.bus_voltage >> 8));//右侧电压 + un_motor_status_output.bit_data.right_wheel_speed = ((un_motor_input2.bit_data.speed << 8) | (un_motor_input2.bit_data.speed >> 8));//右侧轮速 + un_motor_status_output.bit_data.right_torque = ((un_motor_input2.bit_data.torque << 8) | (un_motor_input2.bit_data.torque >> 8));//右侧扭矩 + un_motor_status_output.bit_data.right_fault_code = un_motor_input2.bit_data.fault_code;//右侧故障码 + un_motor_status_output.bit_data.right_voltage = ((un_motor_input2.bit_data.bus_voltage << 8) | (un_motor_input2.bit_data.bus_voltage >> 8));//右侧电压 } else if(signal_id == &un_remote_control_input) diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_request.h b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_request.h index 13ac8b7..0f51034 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_request.h +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_request.h @@ -11,12 +11,6 @@ extern "C" -#define SWAP_ENDIAN_16(x) ((((x) & 0xFF) << 8) | (((x) >> 8) & 0xFF)) -#define SWAP_ENDIAN_32(x) (((x) << 24) | (((x) & 0xFF00) << 8) | (((x) >> 8) & 0xFF00) | ((x) >> 24)) - - - - //typedef enum //{ // MODE_MANUAL, // 手动模式 diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/interface.c b/boards/e3_176_ref/app_demo/eth-xip/sf/interface.c index ed3ea37..062b883 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/interface.c +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/interface.c @@ -41,16 +41,13 @@ UnUltrasonicOutput un_ultrasonic_output1 ;//超声波传感 -StrTxCanFrame un_sdo_output1 ;//电机1输出 -StrTxCanFrame un_sdo_output2 ;//电机2输出 -StrTxCanFrame un_sdo_output3 ;//电机3输出 -StrTxCanFrame un_sdo_output4 ;//电机3速度输出 - -StrTxCanFrame un_sdo_output5 ;//电机使能输出 - -StrRxCanFrame un_pitch_intput ;//电机输入 -StrRxCanFrame un_right_intput ;//电机输入 -StrRxCanFrame un_turn_intput ;//电机输入 +//UnSdoOutput un_sdo_output1 ;//设定转向电机位置 +//UnSdoOutput un_sdo_output2 ;//设定转向电机速度 +//UnSdoOutput un_sdo_output3 ;//使能向下一个点 +//UnSdoOutput un_sdo_output4 ;//设定电机使能 +//UnSdoOutput un_sdo_output5 ;//设置电机模式 +//UnSdoOutput un_sdo_output6 ;//接收数据 +//UnSdoOutput un_sdo_output7 ;//发送使能数据 //IO口 @@ -62,13 +59,7 @@ UnAutoComputerInput un_auto_computer_input ;//自主计算机 UnManualComputerInput un_manual_computer_input ;//自主计算机手动数据 UnRequestFrame un_request_frame ;//请求帧 -UnComputerTurnableInput un_computer_turnable_Input ;//转台以太网输入 - - UnComputerOutput un_computer_output ;//输出给自主计算机 - - - //输出给上位机 UnVehicleInfoOutput un_vehicle_Info_output ;// 车辆信息，输出给上位机 @@ -82,24 +73,9 @@ UnAutoControlOutput un_auto_control_output ;// 自动控制数 -UnCanDebugOutput un_can_debug_output;//调试输出 -// 限制值在最小值和最大值之间 -float constrain(float value, float min_val, float max_val) -{ - if (value < min_val) - { - return min_val; - } - else if (value > max_val) - { - return max_val; - } - else - { - return value; - } -} + + diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/interface.h b/boards/e3_176_ref/app_demo/eth-xip/sf/interface.h index 6e474b4..f459821 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/interface.h +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/interface.h @@ -25,14 +25,14 @@ // 接收电机控制器输入 typedef struct _StrMotorInput { -//-----接收数据0x589或者0x189---------------------------------------------- - uint8_t MotCon_1Signal1 ; - uint8_t MotCon_1Signal2 ; - uint16_t MotCon_1Signal3 ; - uint16_t MotCon_1Signal4 ; - uint16_t MotCon_1Signal5 ; + //-----接收数据0x101或者0x201---------------------------------------------- + unsigned int speed : 16; // 转速 偏移量 -30000 + unsigned int bus_voltage : 16; // 母线电压 系数 0.1 偏移量 -3000 + unsigned int torque : 16; // 扭矩 系数 0.01 偏移量 -300 实际物理量＝数据×系数＋偏移量 + unsigned int fault_code : 8; // 故障码 + unsigned int heartbeat : 8; // 心跳 -} StrMotorInput; +} StrMotorInput; typedef union _UnMotorInput { @@ -42,32 +42,6 @@ typedef union _UnMotorInput -// 接收电机控制器输入 -typedef struct _StrCanDebugOutput -{ - uint8_t speed; - uint8_t desired_speed; - uint8_t curvature; - uint8_t desired_curvature; - uint16_t set_left_out; - uint16_t set_right_out; -} StrCanDebugOutput; - -typedef union _UnCanDebugOutput -{ - StrCanDebugOutput bit_data; // 使用定义的结构体变量名 - uint8_t arr[sizeof(StrCanDebugOutput)]; // 通过结构体类型确定大小 -} UnCanDebugOutput; - - - - - - - - - - typedef struct _StrMotorTempInput { //-----接收数据0x103或者0x104---------------------------------------------- @@ -191,32 +165,12 @@ typedef struct _StrManualComputerInput typedef union _UnManualComputerInput { StrManualComputerInput bit_data; // 使用定义的结构体变量名 - uint8_t arr[sizeof(StrManualComputerInput)]; // 通过结构体类型确定大小 + uint8_t arr[sizeof(StrManualComputerInput)]; // 通过结构体类型确定大小 +// unsigned int arr[sizeof(StrManualComputerInput) / sizeof(unsigned int)]; // 通过结构体类型确定大小 } UnManualComputerInput; -// 接收转台指令输入 -typedef struct _StrComputerTurnableInput -{ - // 多字节数据，高位在前，低位在后 - unsigned int frame_header : 16; // 帧头 固定值0xFFCC - unsigned int frame_type : 16; // 帧类型 固定值0x0001 - unsigned int frame_length : 8; // 帧长 固定值0x19 - unsigned int heartbeat : 8; // 心跳 按帧累加 - // --- 坐标数据部分 --- - int32_t position_x; // X轴坐标 - int32_t position_y; // Y轴坐标 - int32_t position_z; // Z轴坐标 - - unsigned int crc : 8; // CRC 按字节累加之和，溢出取低8位 -} StrComputerTurnableInput; - -typedef union _UnComputerTurnableInput -{ - StrComputerTurnableInput bit_data; // 使用定义的结构体变量名 - uint8_t arr[sizeof(StrComputerTurnableInput)]; // 通过结构体类型确定大小 -} UnComputerTurnableInput; @@ -258,11 +212,6 @@ typedef struct _StrRemoteControlInput unsigned int reserve3 : 1; // 保留 unsigned int enable : 8; // 使能 - - uint16_t y_axis; - uint16_t x_axis; - uint16_t reserve4; - uint16_t reserve5; } StrRemoteControlInput; typedef union _UnRemoteControlInput @@ -340,19 +289,18 @@ typedef union _UnSwSample // 输出到电机控制器 typedef struct _StrMotorOutput { -//-----发送数据0x609或者0x209---------------------------------------------- - uint8_t MotCon_1Signal1 ; - uint8_t MotCon_1Signal2 ; - uint16_t MotCon_1Signal3 ; - uint16_t MotCon_1Signal4 ; - uint16_t MotCon_1Signal5 ; -//-----接收数据0x589或者0x189---------------------------------------------- - uint8_t MotCon_1Signal6 ; - uint8_t MotCon_1Signal7 ; - uint16_t MotCon_1Signal8 ; - uint16_t MotCon_1Signal9 ; - uint16_t MotCon_1Signal10 ; - +//-----发送数据0x201或者0x202---------------------------------------------- + unsigned int mode : 8; // 模式 0x1恒速模式，0x2恒扭模式，其他无效 + unsigned int gear : 8; // 档位 0x0空挡模式，0x1前进挡，0x2倒退档，其他无效 + unsigned int set_torque : 16; // 给定扭矩 系数 0.01 偏移量 -300 实际物理量＝数据×系数＋偏移量 + unsigned int set_rotation_speed : 16; // 给定转速 偏移量 -30000 + unsigned int fault_code : 8; // 故障码 + unsigned int heartbeat : 8; // 心跳 +//-----发送数据0x401或者0x402---------------------------------------------- + unsigned int feed_power : 16; // 馈电功率 单位为 W 最大为10KW + unsigned int discharge_power : 16; // 放电功率 单位为 W 最大为15kW + unsigned int reserve1 : 16; // 保留 + unsigned int reserve2 : 16; // 保留 } StrMotorOutput; typedef union _UnMotorOutput @@ -385,172 +333,26 @@ typedef union _UnGatherOutput -// CAN ID 解析（联合体形式，支持位域和32位直接访问） -typedef union _UnCanIdInfo + + + +// canoe协议输出 +typedef struct _StrSdoOutput { - uint32_t raw; // 32位整型，直接读写整个CAN ID - struct - { - uint32_t motor_id : 8; // 0-7 bit (电机ID) - uint32_t data : 16; // 8-23 bit (数据字段) - uint32_t mode : 5; // 24-28 bit (模式) - uint32_t res : 3; // 29-31 bit (保留位) - } bits; +//-----发送数据0x601---------------------------------------------- + unsigned int cmd : 8; // 命令 + unsigned int object_index : 16; // 索引 + unsigned int sub_index : 8; // 从索引 + unsigned int data : 32; // 数据 +} StrSdoOutput; -} UnCanIdInfo; - - -// 输出can数据 -typedef struct _StrTxCanOutput +typedef union _UnSdoOutput { - uint16_t index; // 索引（类似寄存器地址） - uint16_t object_index; // 子索引（通常为0x0000） - uint32_t data; // 数据字段 -} StrTxCanOutput; + StrSdoOutput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrSdoOutput)]; // 通过结构体类型确定大小 +} UnSdoOutput; -// CAN数据区联合体 (8字节，严格遵循图片协议，大端序数据) -typedef union _UnTxCanData { - StrTxCanOutput bit_data; // 结构化访问 - uint8_t arr[sizeof(StrTxCanOutput)]; // 字节数组形式（用于原始数据读写） -} UnTxCanData; - - -// 接收CAN帧结构体 -typedef struct _StrTxCanFrame -{ - UnCanIdInfo tx_can_id; // 接收到的29位CAN ID - UnTxCanData tx_can_data; // 接收到的8字节数据区 -} StrTxCanFrame; - - - - - - - - - - -//// CAN ID 解析（联合体形式，支持位域和32位直接访问） -//typedef union _UnCanIdInfo -//{ -// struct -// { -// uint32_t motor_id : 8; // 0-7 bit (电机ID) -// uint32_t data : 16; // 8-23 bit (数据字段) -// uint32_t mode : 5; // 24-28 bit (模式) -// uint32_t res : 3; // 29-31 bit (保留位) -// } bits; -// uint32_t raw; // 32位整型，直接读写整个CAN ID -//} UnCanIdInfo; -// -//// CANoe协议输出（主结构体） -//typedef struct _StrSdoOutput -//{ -// uint16_t index; // 索引（类似寄存器地址） -// uint16_t object_index; // 子索引（通常为0x0000） -// uint32_t data; // 数据字段 -// UnCanIdInfo rx_can_id; // CAN ID（联合体形式） -//} StrSdoOutput; -// -//// 主联合体（支持结构体和字节数组访问） -//typedef union _UnSdoOutput -//{ -// StrSdoIntput bit_data; // 结构化访问 -// uint8_t arr[sizeof(StrSdoIntput)]; // 字节数组形式（用于原始数据读写） -//} UnSdoIntput; - - - - -// CAN ID 解析联合体 (29位扩展帧，严格遵循图片协议，小端序适配) -typedef union _UnRxCanIdInfo { - uint32_t raw; // 完整的32位值 - - struct { - // 注意：小端序下，位域布局从低位到高位（Bit0到Bit31） - // 编译器通常从低位开始分配位域 - - // 主机CAN_ID (Bit7~Bit0) - 8位 - 最低字节 - uint32_t host_id : 8; // Bit7~0: 主机CAN_ID - - // 电机状态与故障信息域 (Bit23~Bit8) - 16位 - uint32_t motor_can_id : 8; // Bit15~8: 当前电机CAN ID - uint32_t undervoltage : 1; // Bit16: 欠压故障 (0无1有) - uint32_t overcurrent : 1; // Bit17: 过流 (0无1有) - uint32_t overtemperature : 1; // Bit18: 过温 (0无1有) - uint32_t mag_encoder_fault : 1; // Bit19: 磁编码故障 (0无1有) - uint32_t hall_fault : 1; // Bit20: HALL编码故障 (0无1有) - uint32_t uncalibrated : 1; // Bit21: 未标定 (0无1有) - uint32_t mode_state : 2; // Bit23~22: 模式状态 (0:Reset,1:Cali,2:Motor) - - // 协议标识 (Bit28~Bit24) - 5位 - uint32_t protocol_id : 5; // Bit28~24: 协议标识（图中为2） - - // 保留位 (Bit31~29) - 3位（图片中未使用） - uint32_t reserved : 3; // Bit31~29: 保留位，应设置为0 - } bits; -} UnRxCanIdInfo; - -// 输出can数据 -typedef struct _StrRxCanOutput -{ - uint16_t current_angle; // Byte0~1: 当前角度 [0~65535]对应(-4π~4π) - uint16_t current_velocity; // Byte2~3: 当前角速度 [0~65535]对应(-15rad/s~15rad/s) - uint16_t current_torque; // Byte4~5: 当前力矩 [0~65535]对应(-120Nm~120Nm) - uint16_t temperature; // Byte6~7: 当前温度: Temp(摄氏度)*10 -} StrRxCanOutput; - -// CAN数据区联合体 (8字节，严格遵循图片协议，大端序数据) -typedef union _UnRxCanData -{ - StrRxCanOutput bit_data; // 结构化访问 - uint8_t arr[sizeof(StrRxCanOutput)]; // 字节数组形式（用于原始数据读写） -} UnRxCanData; - -// 接收CAN帧结构体 -typedef struct _StrRxCanFrame -{ - UnRxCanIdInfo rx_can_id; // 接收到的29位CAN ID - UnRxCanData rx_can_data; // 接收到的8字节数据区 -} StrRxCanFrame; - - - - - - - - -//// ID 解析输出 -//typedef struct _StrCanIdInfo -//{ -////-----发送数据0x601---------------------------------------------- -// uint32_t motor_id:8; -// uint32_t data:16; -// uint32_t mode:5; -// uint32_t res:3; -//} StrCanIdInfo; -// -// -// -//// canoe协议输出 -//typedef struct _StrSdoOutput -//{ -////-----发送数据0x601---------------------------------------------- -// uint16_t index; //索引，类似寄存器地址 -// uint16_t object_index; // 从索引 为0x0000 -// uint32_t data; // 数据 -// StrCanIdInfo rx_can_id; -//} StrSdoOutput; -// -//typedef union _UnSdoOutput -//{ -// StrSdoOutput bit_data; // 使用定义的结构体变量名 -// uint8_t arr[sizeof(StrSdoOutput)]; // 通过结构体类型确定大小 -//} UnSdoOutput; - // 超声波数据发送 typedef struct _StrUltrasonicOutput @@ -1097,17 +899,6 @@ extern UnTempModuleInput un_temp_module_input;//温度采集模块 extern UnAutoComputerInput un_auto_computer_input;//自主计算机自动数据 extern UnManualComputerInput un_manual_computer_input;//自主计算机手动数据 -extern StrTxCanFrame un_sdo_output1 ;//电机1输出 -extern StrTxCanFrame un_sdo_output2 ;//电机2输出 -extern StrTxCanFrame un_sdo_output3 ;//电机3输出 -extern StrTxCanFrame un_sdo_output4 ;//电机3速度输出 -extern StrTxCanFrame un_sdo_output5 ;//电机使能输出 - -extern StrRxCanFrame un_pitch_intput ;//电机输入 -extern StrRxCanFrame un_right_intput ;//电机输入 -extern StrRxCanFrame un_turn_intput ;//电机输入 - - extern UnUltrasonicInput un_ultrasonic_input1;//超声波传感器输入1 extern UnUltrasonicOutput un_ultrasonic_output1;//超声波传感器输出 @@ -1141,8 +932,6 @@ extern UnRequestFrame un_request_frame; //请求帧 extern UnComputerOutput un_computer_output; //输出给自主计算机 -extern UnComputerTurnableInput un_computer_turnable_Input ;//转台以太网输入 - //输出给上位机 extern UnVehicleInfoOutput un_vehicle_Info_output; // 车辆信息，输出给上位机 extern UnMotorStatusOutput un_motor_status_output; // 电机状态信息，输出给上位机 @@ -1151,19 +940,18 @@ extern UnAnalogSignalOutput un_analog_signal_output; // 模拟信号输 extern UnRemoteControlOutput un_remote_control_output;// 遥控器数据输出，给上位机 extern UnManualControlOutput un_manual_control_output;// 手动控制数据，返回给请求者 extern UnAutoControlOutput un_auto_control_output; // 自动控制数据输出，返回给请求者 +extern UnSdoOutput un_sdo_output ;//转向电机输出 //变量 extern uint8_t test_app[26]; -extern UnCanDebugOutput un_can_debug_output;//调试输出 + //函数 void canSendAll(void *signal_id); void ethernetSendAll(void *signal_id); -float constrain(float value, float min_val, float max_val); - #pragma pack() diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/interface_boot.c b/boards/e3_176_ref/app_demo/eth-xip/sf/interface_boot.c index a4b30e5..4bbf0f2 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/interface_boot.c +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/interface_boot.c @@ -51,10 +51,6 @@ void bootmian(void *signal_id) ssdk_printf(SSDK_INFO, "go to boot!\r\n"); wrbyte_24c02(Update_Flg_E2adr,CAN_BOOTLOADER_UPGRADE); //д־ // udelay(3000);//ʱ3ms - wrbyte_24c02(BOOT_DES_IP, ethernet_parameter.download_ip[0]); - wrbyte_24c02(BOOT_DES_IP+1, ethernet_parameter.download_ip[1]); - wrbyte_24c02(BOOT_DES_IP+2, ethernet_parameter.download_ip[2]); - wrbyte_24c02(BOOT_DES_IP+3, ethernet_parameter.download_ip[3]); // sdrv_rstgen_global_reset(&rstctl_glb); for(;;) @@ -84,7 +80,7 @@ void bootmian(void *signal_id) } } - timerStart(&boot_timer_interface, 100,1); + timerStart(&boot_timer_interface, 100,0); // printf("bootAPP spend time:%d\n",getCurrentTime() - time_boot);//app˶೤ʱ } @@ -114,7 +110,7 @@ void bootInterfaceInit(void) // Ķʱźţڶʱɼ subscribe(&boot_timer_interface, bootmian); - timerStart(&boot_timer_interface, 100,1); //100ms + timerStart(&boot_timer_interface, 100,0); //100ms feedWatchdog();//ι,ʼιһ diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/interface_can.c b/boards/e3_176_ref/app_demo/eth-xip/sf/interface_can.c index 44fdebb..4c2efef 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/interface_can.c +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/interface_can.c @@ -9,7 +9,7 @@ #include "app/app_differential_drive.h" #include "app/app_param_manage.h" #include "app/app_ultrasonic.h" -#include "app/app_turntable.h" + uint32_t OTA_CANTxID = 0x02;//ĬϷIDΪ0x02 @@ -312,15 +312,15 @@ void flexcan_Receive_callback_1(flexcan_handle_t *handle, //ź publishMessage(&un_motor_input1, 1); } -// else if( MOTOR_INPUT_ID_3 == (buf->id) ) -// { -// for(i = 0; i < (buf->length); i++) -// { -// un_motor_temp1.arr[i] = buf->dataBuffer[i]; -// } -// //ź -// publishMessage(&un_motor_temp1, 1);//ź -// } + else if( MOTOR_INPUT_ID_3 == (buf->id) ) + { + for(i = 0; i < (buf->length); i++) + { + un_motor_temp1.arr[i] = buf->dataBuffer[i]; + } + //ź + publishMessage(&un_motor_temp1, 1);//ź + } else{} break; @@ -376,20 +376,20 @@ void flexcan_Receive_callback_2(flexcan_handle_t *handle, can_fault_info.bit_data.motor2_count ++; for(i = 0; i < (buf->length); i++) { - un_motor_input2.arr[i] = buf->dataBuffer[i]; + un_motor_input2.arr[i] = buf->dataBuffer[i]; } //ź publishMessage(&un_motor_input2, 1); } -// else if( MOTOR_INPUT_ID_4 == (buf->id) ) -// { -// for(i = 0; i < (buf->length); i++) -// { -// un_motor_temp2.arr[i] = buf->dataBuffer[i]; -// } -// //ź -// publishMessage(&un_motor_temp2, 1);//ź -// } + else if( MOTOR_INPUT_ID_4 == (buf->id) ) + { + for(i = 0; i < (buf->length); i++) + { + un_motor_temp2.arr[i] = buf->dataBuffer[i]; + } + //ź + publishMessage(&un_motor_temp2, 1);//ź + } else{} break; @@ -533,8 +533,6 @@ void flexcan_Receive_callback_5(flexcan_handle_t *handle, flexcan_status_e status, uint32_t result, void *userData) { - flexcan_frame_t *buf = (flexcan_frame_t *)userData; - uint8_t i = 0; //-------------------------------------------------------------- switch (status) { @@ -542,33 +540,7 @@ void flexcan_Receive_callback_5(flexcan_handle_t *handle, break; case FLEXCAN_RX_FIFO_IDLE: - if( PITCH_MOTOR_RxCANID == ( (buf->id) & MOTOR_RxCAN_Mask) ) - { - un_pitch_intput.rx_can_id.raw = (buf->id); - for(i = 0; i < (buf->length); i++) - { - un_pitch_intput.rx_can_data.arr[i] = buf->dataBuffer[i]; - } - } - else if( RIGHT_MOTOR_RxCANID == ( (buf->id) & MOTOR_RxCAN_Mask) ) - { - un_right_intput.rx_can_id.raw = (buf->id); - for(i = 0; i < (buf->length); i++) - { - un_right_intput.rx_can_data.arr[i] = buf->dataBuffer[i]; - } - - } - else if( TURN_MOTOR_RxCANID == ( (buf->id) & MOTOR_RxCAN_Mask) ) - { - un_turn_intput.rx_can_id.raw = (buf->id); - for(i = 0; i < (buf->length); i++) - { - un_turn_intput.rx_can_data.arr[i] = buf->dataBuffer[i]; - } - - } - else{} + break; case FLEXCAN_TX_IDLE: @@ -612,7 +584,6 @@ void flexcan_Receive_callback_6(flexcan_handle_t *handle, break; case FLEXCAN_RX_FIFO_IDLE: - if(OTA_CANRxID == (buf->id))//IDΪ1 boot { boot_can_flag = true; @@ -634,8 +605,7 @@ void flexcan_Receive_callback_6(flexcan_handle_t *handle, } publishMessage(&un_ultrasonic_input1, 1); } - - else{} + else{} break; case FLEXCAN_TX_IDLE: @@ -694,13 +664,6 @@ void flexcan_Receive_callback_7(flexcan_handle_t *handle, // printf("ori_remote_stop: %d\n", un_remote_control_input.bit_data.switch_b); } - else if(REMOTE_ID_1 == (buf->id)) - { - for(i = 0; i < (buf->length); i++) - { - un_remote_control_input.arr[i+8] = buf->dataBuffer[i]; - } - } else { } @@ -1051,35 +1014,24 @@ void canTimerProcess(void *signal_id) } -static void processSdoOutput1(void *signal_id) -{ - (void)signal_id; // DZΪʹã - CAN_Send_Msg(&can_handle_4, un_sdo_output1.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output1.tx_can_data.arr[0], 8, 15);// -} - -static void processSdoOutput2(void *signal_id) -{ - (void)signal_id; // DZΪʹã - CAN_Send_Msg(&can_handle_4, un_sdo_output2.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output2.tx_can_data.arr[0], 8, 16); -} - -static void processSdoOutput3(void *signal_id) -{ - (void)signal_id; // DZΪʹã - CAN_Send_Msg(&can_handle_4, un_sdo_output3.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output3.tx_can_data.arr[0], 8, 17); -} - -static void processSdoOutput4(void *signal_id) -{ - (void)signal_id; // DZΪʹã - CAN_Send_Msg(&can_handle_4, un_sdo_output4.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output4.tx_can_data.arr[0], 8, 18); -} - -static void processSdoOutput5(void *signal_id) -{ - (void)signal_id; // DZΪʹã - CAN_Send_Msg(&can_handle_4, un_sdo_output5.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output5.tx_can_data.arr[0], 8, 19); -} +//static void processSdoOutput1(void *signal_id) +//{ +// (void)signal_id; // DZΪʹã +// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output1, 8, 15);// +//} +// +//static void processSdoOutput2(void *signal_id) +//{ +// (void)signal_id; // DZΪʹã +// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output2, 8, 16); +//} +// +//static void processSdoOutput3(void *signal_id) +//{ +// (void)signal_id; // DZΪʹã +// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output3, 8, 17); +//} +// //static void processSdoOutput4(void *signal_id) //{ // (void)signal_id; // DZΪʹã @@ -1104,30 +1056,26 @@ static void processSdoOutput5(void *signal_id) static void processMotorOutput1(void *signal_id) { (void)signal_id; // DZΪʹã - un_motor_output1.bit_data.MotCon_1Signal1 = 0x5A; - un_motor_output1.bit_data.MotCon_1Signal2 = 0x02; - CAN_Send_Msg(&can_handle_1, 0x609, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1, 8, 15);//1Ťغת + CAN_Send_Msg(&can_handle_1, 0x201, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1, 8, 15);//1Ťغת } static void processMotorOutput2(void *signal_id) { (void)signal_id; // DZΪʹã - un_motor_output2.bit_data.MotCon_1Signal1 = 0x5A; - un_motor_output2.bit_data.MotCon_1Signal2 = 0x02; - CAN_Send_Msg(&can_handle_2, 0x60A, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2, 8, 15);//2Ťغת + CAN_Send_Msg(&can_handle_2, 0x202, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2, 8, 15);//2Ťغת } static void processMotorOutput3(void *signal_id) { - (void)signal_id; // DZΪʹã - - CAN_Send_Msg(&can_handle_1, 0x209, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1.arr[8], 8, 16);//1 + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_1, 0x401, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1.arr[8], 8, 16);//1 +// CAN_Send_Msg(&can_handle_6, 0x12000023, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_remote_control_input, 8, 15);// ãң } static void processMotorOutput4(void *signal_id) { (void)signal_id; // DZΪʹã - CAN_Send_Msg(&can_handle_2, 0x20A, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2.arr[8], 8, 16);//2 + CAN_Send_Msg(&can_handle_2, 0x402, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2.arr[8], 8, 16);//2 } static void processKgfOutput1(void *signal_id) @@ -1175,7 +1123,7 @@ static void processWheelSpeedOutput(void *signal_id) un_wheel_wpeed_output.bit_data.right_front = (uint16_t)((int16_t)(CANPressSpeedTemp));//ת*1.8*60*100/1000/ ת/ӡkm/h һȦ1.8,Ҫ100ϵΪ0.01תΪ޷ͣ 20240629 ٱȲҪѾ un_wheel_wpeed_output.bit_data.right_rear = (uint16_t)((int16_t)(CANPressSpeedTemp));//תΪintתΪ޷ - CAN_Send_Msg(&can_handle_5, 0x98, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_wheel_wpeed_output, 8, 15);// + CAN_Send_Msg(&can_handle_4, 0x98, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_wheel_wpeed_output, 8, 15);// } static void processHBridgeOutput(void *signal_id) @@ -1195,7 +1143,7 @@ static void processUnGatherOutput(void *signal_id) static void processUltrasonicOutput(void *signal_id) { (void)signal_id; // DZΪʹã - CAN_Send_Msg(&can_handle_6, ULTRASONIC_ID_1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_ultrasonic_output1, 8, 18);// + CAN_Send_Msg(&can_handle_6, ULTRASONIC_ID_1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_ultrasonic_output1, 8, 15);// } //static void processHBridgeOutput2(void *signal_id) @@ -1219,62 +1167,50 @@ void canSendAll(void *signal_id) { static uint16_t wheel_speed_cnt = 0; // static uint8_t kgf_cnt = 0; - static uint16_t bms_cnt1 = 0; - static uint16_t bms_cnt2 = 0; - static uint16_t motor_speed_cnt = 0; +// static uint8_t bms_cnt1 = 0; +// static uint8_t bms_cnt2 = 0; +// static uint16_t motor_speed_cnt = 0; static uint16_t motor_power_cnt = 0; // static uint8_t h_bridge_cnt = 0; uint8_t CanData[8] = {0,0,0,0,0,0,0,0}; //------------------------------------------------------------------------- (void)signal_id; // DZΪʹã - bms_cnt1 ++; - if(bms_cnt1 >= 1000)//1s - { - bms_cnt1 = 0; - - CanData[0] = 0x5A; - - CAN_Send_Msg(&can_handle_3, 0x100, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 15);//BMS - } - - bms_cnt2 ++; - if(bms_cnt2 >= 1100)//1s - { - bms_cnt2 = 0; - - CanData[0] = 0x5A; - - CAN_Send_Msg(&can_handle_3, 0x101, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 16);//BMS - } +// bms_cnt1 ++; +// if(bms_cnt1 >= 1000)//1s +// { +// bms_cnt1 = 0; +// +// CanData[0] = 0x5A; +// +// CAN_Send_Msg(&can_handle_3, 0x100, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 15);//BMS +// } +// +// bms_cnt2 ++; +// if(bms_cnt2 >= 9)//1s +// { +// bms_cnt2 = 0; +// +// CanData[0] = 0x5A; +// +// CAN_Send_Msg(&can_handle_3, 0x101, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 16);//BMS +// } - motor_speed_cnt ++; - if(motor_speed_cnt >= 1000)//1sһ - { - motor_speed_cnt = 0; - - CanData[0] = 0xA4; - CanData[1] = 0x02; - CanData[2] = 0; - CanData[3] = 0xFF; - CanData[4] = 0; - CanData[5] = 0xFF; - - CAN_Send_Msg(&can_handle_1, 0x609, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData,CanData, 8, 17);//1 - CAN_Send_Msg(&can_handle_2, 0x60A, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData,CanData, 8, 17);//1 - } +// motor_speed_cnt ++; +// if(motor_speed_cnt >= 1)//1msһ +// { +// motor_speed_cnt = 0; +// //дߵת +// processMotorOutput1(CanData); +//// //дұߵת +// processMotorOutput2(CanData); +// } motor_power_cnt ++; - if(motor_power_cnt >= 10)//ȡת 20ms + if(motor_power_cnt >= 1000)//д 1s { motor_power_cnt = 0; - - un_motor_output1.bit_data.MotCon_1Signal6 = 0x84; - un_motor_output1.bit_data.MotCon_1Signal7 = 0x02; - un_motor_output2.bit_data.MotCon_1Signal6 = 0x84; - un_motor_output2.bit_data.MotCon_1Signal7 = 0x02; - processMotorOutput3(CanData); processMotorOutput4(CanData); } @@ -1290,7 +1226,7 @@ void canSendAll(void *signal_id) // //ٷ wheel_speed_cnt ++; - if(wheel_speed_cnt >= 100) + if(wheel_speed_cnt >= 500) { wheel_speed_cnt = 0; @@ -1302,7 +1238,7 @@ void canSendAll(void *signal_id) // CAN_Send_Msg(&can_handle_3, 0x123, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS // CAN_Send_Msg(&can_handle_4, 0x124, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS // CAN_Send_Msg(&can_handle_5, 0x125, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS -// CAN_Send_Msg(&can_handle_6, 0x126, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, &un_can_debug_output.arr[0], 8, 19);//BMS +// CAN_Send_Msg(&can_handle_6, 0x126, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS // CAN_Send_Msg(&can_handle_7, 0x127, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS } // @@ -1388,12 +1324,12 @@ void canInterfaceInit(void) -// un_motor_output1.bit_data.set_torque = 30000;//ƫʼ -// un_motor_output1.bit_data.set_rotation_speed = 30000; -// un_motor_output1.bit_data.mode = 0; -// un_motor_output2.bit_data.set_torque = 30000; -// un_motor_output2.bit_data.set_rotation_speed = 30000; -// un_motor_output2.bit_data.mode = 0; + un_motor_output1.bit_data.set_torque = 30000;//ƫʼ + un_motor_output1.bit_data.set_rotation_speed = 30000; + un_motor_output1.bit_data.mode = 0; + un_motor_output2.bit_data.set_torque = 30000; + un_motor_output2.bit_data.set_rotation_speed = 30000; + un_motor_output2.bit_data.mode = 0; can_fault_info.bit_data.navigator_state = 1; memset(&un_inf_can_kgf_output1, 0, sizeof(UnInfCanKGFOutput)); @@ -1419,19 +1355,16 @@ void canInterfaceInit(void) subscribe(&un_h_bridge_output, processHBridgeOutput); subscribe(&un_gather_output, processUnGatherOutput); subscribe(&un_sw_sample, canInterfaceInput); // ͣءѹ ѱ־ - subscribe(&un_ultrasonic_output1, processUltrasonicOutput); // + subscribe(&un_ultrasonic_output1, processUltrasonicOutput); // + - // subscribe(&un_h_bridge_output2, processHBridgeOutput2); // subscribe(&un_lifter_output, processLifterOutput); - subscribe(&un_sdo_output1, processSdoOutput1); - subscribe(&un_sdo_output2, processSdoOutput2); - subscribe(&un_sdo_output3, processSdoOutput3); - subscribe(&un_sdo_output4, processSdoOutput4); - subscribe(&un_sdo_output5, processSdoOutput5); - - +// subscribe(&un_sdo_output1, processSdoOutput1); +// subscribe(&un_sdo_output2, processSdoOutput2); +// subscribe(&un_sdo_output3, processSdoOutput3); +// subscribe(&un_sdo_output4, processSdoOutput4); // subscribe(&un_sdo_output5, processSdoOutput5); // subscribe(&un_sdo_output7, processSdoOutput7); diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/interface_can.h b/boards/e3_176_ref/app_demo/eth-xip/sf/interface_can.h index 577c9a8..aa2e1cd 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/interface_can.h +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/interface_can.h @@ -21,8 +21,8 @@ #define TX_MB_INDEX (USED_MB_FOR_FIFO) -#define MOTOR_INPUT_ID_1 0x189 -#define MOTOR_INPUT_ID_2 0x18A +#define MOTOR_INPUT_ID_1 0x101 +#define MOTOR_INPUT_ID_2 0x102 #define MOTOR_INPUT_ID_3 0x103 #define MOTOR_INPUT_ID_4 0x104 @@ -35,9 +35,9 @@ //#define MOTOR_INPUT_ID_4 0x10F94708//Һ // // -//#define MOTOR_INPUT_ID_1 0x10F81708//ǰ λ -//#define MOTOR_INPUT_ID_2 0x10F82708//ǰ -//#define MOTOR_INPUT_ID_3 0x10F83708// +#define MOTOR_INPUT_ID_5 0x10F81708//ǰ λ +#define MOTOR_INPUT_ID_6 0x10F82708//ǰ +#define MOTOR_INPUT_ID_7 0x10F83708// #define MOTOR_INPUT_ID_8 0x10F84708//Һ @@ -46,7 +46,6 @@ #define BMS_INPUT_ID1 0x100 #define BMS_INPUT_ID2 0x101 #define REMOTE_ID 0x12000023 -#define REMOTE_ID_1 0x12000024 #define TEMP_MODULE_INPUT_ID_1 0x15000003 diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/interface_ethernet.c b/boards/e3_176_ref/app_demo/eth-xip/sf/interface_ethernet.c index 00ffbf3..a2eaa7c 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/interface_ethernet.c +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/interface_ethernet.c @@ -207,16 +207,7 @@ void udp_Callback_1(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_ad uint16_t i = 0; uint16_t len = 0; uint16_t udp_temp = 0; -//------------------------------------------------------------------------------ -// uint8_t ip_addr[4] = {0,0,0,0}; -// -// ip_addr[0] = addr->addr & 0xff; /* IADDR4 */ -// ip_addr[1] = (addr->addr >> 8) & 0xff; /* IADDR3 */ -// ip_addr[2] = (addr->addr >> 16) & 0xff; /* IADDR2 */ -// ip_addr[3] = (addr->addr >> 24) & 0xff; /* IADDR1 */ - - - +//------------------------------------------------------------------------------ if( (0xFF == buf[0] ) && ( 0xBB == buf[1] ) )//ֶңԶң { @@ -240,13 +231,19 @@ void udp_Callback_1(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_ad un_manual_computer_input.bit_data.set_speed = udp_temp; udp_temp = ((un_manual_computer_input.bit_data.set_curvature << 8) | (un_manual_computer_input.bit_data.set_curvature >> 8));//趨 - un_manual_computer_input.bit_data.set_curvature = udp_temp; - - publishMessage(&un_manual_computer_input, 1); - -// p->len = len; -// printf("Manualrecive len:%d, speed: %d , cur: %d\n",len,un_manual_computer_input.bit_data.set_speed,un_manual_computer_input.bit_data.set_curvature); + un_manual_computer_input.bit_data.set_curvature = udp_temp; + publishMessage(&un_manual_computer_input, 1); + + + +// printf("un_manual_computer_input.arr (hex):\n"); +// for ( i = 0; i < 12; i++) { +// printf("arr[%d] = 0x%08X\n", i, un_manual_computer_input.arr[i]); +// } + +// p->len = len; +// printf("Manualrecive len:%d\n",len); // udp_sendto(upcb, p, addr, port); } else if( (0xFF == buf[0] ) && (0xCC == buf[1] ) )// Զ @@ -290,35 +287,9 @@ void udp_Callback_1(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_ad // printf("Autorecive len:%d\n",len); // udp_sendto(upcb, p, addr, port); } - else if( (0xFF == buf[0] ) && ( 0x12 == buf[1] ) ) + else { - - if( (p->len) >= sizeof(un_computer_turnable_Input) )//ȡȳȡС - { - len = sizeof(un_computer_turnable_Input); - } - else - { - len = p->len; - } - - - for(i = 0; i < len; i++) - { - un_computer_turnable_Input.arr[i] = buf[i]; -// printf("%d", un_computer_turnable_Input.arr[i]); // 16ƴӡʺ϶ݣ - } - publishMessage(&un_computer_turnable_Input, 1); - -// // ӡPITCHCAN -// printf("un_computer_turnable_Input - ID: 0x%08X, Data: ", buf->id); -// for(i = 0; i < 8; i++) { -// printf("%02X ", buf->dataBuffer[i]); -// } -// printf("\n"); - - } - else{} + } pbuf_free(p); } @@ -357,21 +328,8 @@ void udp_Callback_2(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_ad void udp_Callback_4(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) { uint8_t *buf = (uint8_t *)(p->payload); - uint8_t ip_addr[4] = {0,0,0,0}; //------------------------------------------------------------------------------------------------ - //20250822 ӱˢдλIP - ip_addr[0] = addr->addr & 0xff; /* IADDR4 */ - ip_addr[1] = (addr->addr >> 8) & 0xff; /* IADDR3 */ - ip_addr[2] = (addr->addr >> 16) & 0xff; /* IADDR2 */ - ip_addr[3] = (addr->addr >> 24) & 0xff; /* IADDR1 */ - - ethernet_parameter.download_ip[0] = ip_addr[0]; - ethernet_parameter.download_ip[1] = ip_addr[1]; - ethernet_parameter.download_ip[2] = ip_addr[2]; - ethernet_parameter.download_ip[3] = ip_addr[3]; - - boot_eth_flag = true; FrameHeader = ( (buf[0] << 8) | (buf[1]) ); printf("FrameHeader %d\n",FrameHeader); @@ -481,7 +439,6 @@ static void manualControlOutput(void *signal_id) (void)signal_id; // DZΪʹã UdpSendToData(UDPCB_2,(uint8_t *)(&un_manual_control_output),sizeof(un_manual_control_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); - } static void autoControlOutput(void *signal_id) @@ -582,26 +539,12 @@ void udp_Callback_3(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_ad static unsigned int received_size = 0; uint8_t *buf = (uint8_t *)(p->payload); - uint8_t ip_addr[4] = {0,0,0,0}; -//------------------------------------------------------------------------------------------------ - - //20250822 ӱˢдλIP - ip_addr[0] = addr->addr & 0xff; /* IADDR4 */ - ip_addr[1] = (addr->addr >> 8) & 0xff; /* IADDR3 */ - ip_addr[2] = (addr->addr >> 16) & 0xff; /* IADDR2 */ - ip_addr[3] = (addr->addr >> 24) & 0xff; /* IADDR1 */ - - // Ƿµ֡ͷ if ((0x80 == buf[1]) && (0xFF == buf[0])) { // ýչ received_size = 0; printf("⵽µ֡ͷ,ýչ\n"); - -printf("IPַ: %d.%d.%d.%d\n", ip_addr[0], ip_addr[1], ip_addr[2], ip_addr[3]); - - } // Ƿ㹻Ŀռ洢 diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/main.c b/boards/e3_176_ref/app_demo/eth-xip/sf/main.c index f70f56d..8799f1b 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/main.c +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/main.c @@ -31,7 +31,6 @@ #include #include "app/app_request.h" #include "app/app_ultrasonic.h" -#include "app/app_turntable.h" void testAppInit(void); @@ -112,7 +111,7 @@ int main(void) //发送重启后的第一帧给上位机 CAN_Send_Msg(&can_handle_6,OTA_CANTxID, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, BOOT_Arr,2, TX_MB_INDEX);//app 帧 - printf("CAN_Send_Msg can_handle_6 OK %d\n",getCurrentTime()); + printf("CAN_Send_Msg can_handle_6 OK %d\n",getCurrentTime());\ //打印版本号 @@ -130,11 +129,11 @@ int main(void) requestAppInit(); canInterfaceInit(); bootInterfaceInit(); - turnableInit(); // ultrasonicAppInit(); printf("All init OK ------ %d\n",getCurrentTime()); + sdrv_gpio_set_pin_output_level(GPIO_B9, 1); //测量时间 for (;;) { // 处理信号 diff --git a/drivers/source/eth/sdrv_eth.c b/drivers/source/eth/sdrv_eth.c index 6a2d7db..3abecb6 100644 --- a/drivers/source/eth/sdrv_eth.c +++ b/drivers/source/eth/sdrv_eth.c @@ -585,9 +585,7 @@ static int dwc_eth_irq_handler(uint32_t irq, void *arg) if (!status) { return -1; } - //20250426 ӹж -// irq_state_t state; -// state = arch_irq_save();3 + irq_state_t state = enter_critical_section();//20250426 жϱ if (status & (1 << 17)) {