diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..630e90e0 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,6 @@ +{ + "files.associations": { + "app_config.h": "c", + "interface.h": "c" + } +} \ No newline at end of file diff --git a/app/app_brake.c b/app/app_brake.c index eb882c4e..b820fb82 100644 --- a/app/app_brake.c +++ b/app/app_brake.c @@ -97,7 +97,7 @@ static void brakeTimerProcess(void *signal_id) brake_data.brake_motor_state = 2; } brakeOutput(NULL); - timerStart(&brake_data.brake_apply_timer, (uint32_t)(getParam("brk_on")), 0); + timerStart(&brake_data.brake_apply_timer, (uint32_t)(getParam("brk_on")), 1); } break; @@ -115,7 +115,7 @@ static void brakeTimerProcess(void *signal_id) brake_data.brake_motor_state = 1; } brakeOutput(NULL); - timerStart(&brake_data.brake_release_timer, (uint32_t)(getParam("brk_off")), 0); + timerStart(&brake_data.brake_release_timer, (uint32_t)(getParam("brk_off")), 1); } break; @@ -144,15 +144,15 @@ 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); - - } +// // 如果刹车位置有变化，存入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/app/app_differential_drive.c b/app/app_differential_drive.c index b3cb677e..e74ae36e 100644 --- a/app/app_differential_drive.c +++ b/app/app_differential_drive.c @@ -11,7 +11,12 @@ #include "app_brake.h" #include "app_power.h" +#include "drive_rs04.h" + Timer diff_app_timer; +Timer diff_app_timer2; + +Timer diff_app_timer3; // 定义全局变量 DiffData diff_data; @@ -114,111 +119,39 @@ void handleVehicleState(MotorState *ctx, float speed, float torque) // 设置电机输出 void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint16_t discharge_power) { - float abs_left_front_speed = 0; float abs_right_front_speed = 0; float abs_left_rear_speed = 0; float abs_right_rear_speed = 0; + + static float previous_time2 = 0.0f; - // 档位 - abs_left_front_speed = calculateTorqueOutput(diff_data.motor_state[0], out_torq[0]); //根据挡位增加转矩方向 - abs_right_front_speed = calculateTorqueOutput(diff_data.motor_state[1], out_torq[1]); - abs_left_rear_speed = calculateTorqueOutput(diff_data.motor_state[2], out_torq[2]); - abs_right_rear_speed = calculateTorqueOutput(diff_data.motor_state[3], out_torq[3]); - - un_motor_output1.bit_data.gear = diff_data.motor_state[0]; - un_motor_output2.bit_data.gear = diff_data.motor_state[1]; - - if(STATE_FORWARD == diff_data.motor_state[2])//把后两台电机反相 - { - un_motor_output3.bit_data.gear = STATE_BACKWARD; - } - else if(STATE_BACKWARD == diff_data.motor_state[2]) - { - un_motor_output3.bit_data.gear = STATE_FORWARD; - } - else - { - un_motor_output3.bit_data.gear = STATE_INIT; - } + float time1 = (float)getCurrentTime(); + float dt = (time1 - previous_time2) / PERIOD_TICK; + previous_time2 = time1; + - if(STATE_FORWARD == diff_data.motor_state[3]) - { - un_motor_output4.bit_data.gear = STATE_BACKWARD; - } - else if(STATE_BACKWARD == diff_data.motor_state[3]) - { - un_motor_output4.bit_data.gear = STATE_FORWARD; - } - else - { - un_motor_output4.bit_data.gear = STATE_INIT; - } - - - // 设置左右电机期望转速 -// un_motor_output1.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_left_speed) + 30000); // 20240921 增加偏移量 30000 -// un_motor_output2.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_right_speed) + 30000); // 20240921 增加偏移量 30000 - - // 设置模式为扭矩模式 - un_motor_output1.bit_data.mode = MOTOR_MODE; - un_motor_output2.bit_data.mode = MOTOR_MODE; - un_motor_output3.bit_data.mode = MOTOR_MODE; - un_motor_output4.bit_data.mode = MOTOR_MODE; - - // 设置扭矩 - un_motor_output1.bit_data.set_torque = (uint16_t)( (int16_t)abs_left_front_speed ); - un_motor_output2.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_front_speed ); - un_motor_output3.bit_data.set_torque = (uint16_t)( (int16_t)abs_left_rear_speed ); - un_motor_output4.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_rear_speed ); + abs_left_front_speed = dynamic_current_limit(&diff_data.out_torq_last[0],-diff_data.out_torq[0],dt); + abs_right_front_speed = dynamic_current_limit(&diff_data.out_torq_last[1],diff_data.out_torq[1],dt); + abs_left_rear_speed = dynamic_current_limit(&diff_data.out_torq_last[2],-diff_data.out_torq[2],dt); + abs_right_rear_speed = dynamic_current_limit(&diff_data.out_torq_last[3],diff_data.out_torq[3],dt); - //设定转速 - un_motor_output1.bit_data.set_rotation_speed = 30000; - un_motor_output2.bit_data.set_rotation_speed = 30000; - un_motor_output3.bit_data.set_rotation_speed = 30000; - un_motor_output4.bit_data.set_rotation_speed = 30000; - - // 设置馈电功率 - un_motor_output1.bit_data.feed_power = feed_power; - un_motor_output2.bit_data.feed_power = feed_power; - un_motor_output3.bit_data.feed_power = feed_power; - un_motor_output4.bit_data.feed_power = feed_power; - - - // 设置放电功率 - un_motor_output1.bit_data.discharge_power = discharge_power; - un_motor_output2.bit_data.discharge_power = discharge_power; - un_motor_output3.bit_data.discharge_power = discharge_power; - un_motor_output4.bit_data.discharge_power = discharge_power; - - + setMotorWrite(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1, IQ_REF_INDEX,abs_left_front_speed); + setMotorWrite(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2, IQ_REF_INDEX,abs_right_front_speed); + setMotorWrite(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3, IQ_REF_INDEX,abs_left_rear_speed); + setMotorWrite(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4, IQ_REF_INDEX,abs_right_rear_speed); } -// 限制值在最小值和最大值之间 -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) { // 获取轮子周长 - float wheel_circumference = (float)getParam("whl_dia") * M_PI; + float wheel_circumference = diff_data.wheel_radius * M_PI; // 获取减速比 - float gear_ratio = (float)getParam("gRatio"); + float gear_ratio = diff_data.gear_ratio; if (fabsf(gear_ratio) < EPSILON) { return 0; // 避免除以0的情况 @@ -240,7 +173,7 @@ uint8_t calculateCurrentSpeedYawRate(void) // 计算速度差 float speed_diff = left_speed_mps - right_speed_mps; // 计算角速度 - float wheel_base = (float)getParam("whl_bas"); + float wheel_base = diff_data.wheel_base; if (fabsf(wheel_base) < EPSILON) { return 0; // 避免除以0的情况 @@ -277,11 +210,11 @@ float calculateDeceleration(float speed, float previous_speed, float dt) float calculateMaxSpeed() { // 获取最大电机转速 (RPM) - float max_rpm = (float)getParam("max_rpm"); + float max_rpm = diff_data.max_motor_rpm ; // 获取轮子周长 - float wheel_circumference = (float)getParam("whl_dia") * M_PI; + float wheel_circumference = diff_data.wheel_radius * M_PI; // 获取减速比 - float gear_ratio = (float)getParam("gRatio"); + float gear_ratio = diff_data.gear_ratio; if (fabsf(gear_ratio) < EPSILON) { return 0; // 避免除以0的情况 @@ -289,16 +222,16 @@ float calculateMaxSpeed() // 将最大电机转速 (RPM) 转换为线速度 (m/s)，考虑减速比 float max_speed = (max_rpm * wheel_circumference) / (60.0f * gear_ratio); return max_speed; -} +} // 计算最大加速度 float calculateMaxAcceleration(void) { // 获取车辆参数 - float max_motor_torque = (float)getParam("maxTorq"); // 最大电机扭矩 - float vehicle_mass = (float)getParam("VehMass"); // 车辆质量 - float wheel_radius = (float)getParam("whl_dia") / 2.0f; // 轮子半径 - float gear_ratio = (float)getParam("gRatio"); // 减速比 + float max_motor_torque = diff_data.max_Torq; // 最大电机扭矩 + float vehicle_mass = diff_data.vehicle_mass; // 车辆质量 + float wheel_radius = diff_data.wheel_radius / 2.0f; // 轮子半径 + float gear_ratio = diff_data.gear_ratio; // 减速比 if (fabsf(wheel_radius) < EPSILON || fabsf(vehicle_mass) < EPSILON ) { return 0; // 避免除以0的情况 @@ -375,7 +308,6 @@ void distributeTorque(float rpm1, float rpm2, float total_torque, float* torque1 if ( fabs(*torque2) < min_torque) { *torque2 = copysignf(min_torque, *torque2); } - } @@ -473,10 +405,10 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max motor_speed[1] = right_speed_mps; motor_speed[3] = right_speed_mps; - handleVehicleState(&diff_data.motor_state[0], diff_data.left_front_motor_speed, motor_speed[0]); //通过扭矩以及速度来判断挡位 - handleVehicleState(&diff_data.motor_state[1], diff_data.right_front_motor_speed, motor_speed[1]); - handleVehicleState(&diff_data.motor_state[2], diff_data.left_rear_motor_speed, motor_speed[2]); - handleVehicleState(&diff_data.motor_state[3], diff_data.right_rear_motor_speed, motor_speed[3]); +// handleVehicleState(&diff_data.motor_state[0], diff_data.left_front_motor_speed, motor_speed[0]); //通过扭矩以及速度来判断挡位 +// handleVehicleState(&diff_data.motor_state[1], diff_data.right_front_motor_speed, motor_speed[1]); +// handleVehicleState(&diff_data.motor_state[2], diff_data.left_rear_motor_speed, motor_speed[2]); +// handleVehicleState(&diff_data.motor_state[3], diff_data.right_rear_motor_speed, motor_speed[3]); // distributeTorque(diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed,2*left_speed_mps,&motor_speed[0],&motor_speed[2],diff_data.max_Torq,diff_data.min_Torq); @@ -486,32 +418,32 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max // 限制线速度和偏航率 linear_velocity_x = constrain(linear_velocity_x, -max_speed, max_speed); - float max_yaw_rate = max_speed / ((float)getParam("whl_bas") / 2.0f); + float max_yaw_rate = max_speed / diff_data.wheel_base / 2.0f); yaw_rate = constrain(yaw_rate, -max_yaw_rate, max_yaw_rate); // 计算旋转速度 - float rotational_velocity = ((float)getParam("whl_bas") / 2.0f) * yaw_rate; + float rotational_velocity = (diff_data.wheel_base / 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 wheel_circumference = (float)getParam("whl_dia") * M_PI; + float wheel_circumference = diff_data.wheel_radius * M_PI; // 将车辆左右线速度转换为轮子转速 (RPM) float left_wheel_rpm = (left_speed_mps * 60.0f) / wheel_circumference; float right_wheel_rpm = (right_speed_mps * 60.0f) / wheel_circumference; // 获取减速比 - float gear_ratio = (float)getParam("gRatio"); + float gear_ratio = diff_data.gear_ratio; // 将轮子转速转换为电机转速，考虑减速比 float left_motor_rpm = left_wheel_rpm * gear_ratio; float right_motor_rpm = right_wheel_rpm * gear_ratio; // 限制电机的最大和最小转速 - float max_motor_rpm = (float)getParam("max_rpm"); + float max_motor_rpm = diff_data.max_motor_rpm; left_motor_rpm = constrain(left_motor_rpm, -max_motor_rpm, max_motor_rpm); right_motor_rpm = constrain(right_motor_rpm, -max_motor_rpm, max_motor_rpm); // 当电机转速小于50转时，设置为0 @@ -621,7 +553,7 @@ static void diffProcess(void *signal_id) output_yaw_rate = 0; } - // 使用差速车辆动力学模型计算左右电机的期望速度 + // 使用差速车辆动力学模型计算左右电机的期望速度 float out_torque[4] = {0,0,0,0}; // 使用差速车辆动力学模型计算左右电机的期望速度 computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, out_torque); @@ -671,7 +603,6 @@ static void diffProcess(void *signal_id) 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); @@ -682,25 +613,29 @@ static void diffProcess(void *signal_id) out_torque[1] = constrain(out_torque[1], -diff_data.max_Torq, diff_data.max_Torq); out_torque[2] = constrain(out_torque[2], -diff_data.max_Torq, diff_data.max_Torq); out_torque[3] = constrain(out_torque[3], -diff_data.max_Torq, diff_data.max_Torq); - -// printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral); - - - + // 设置电机输出 setMotorOutput(&diff_data.out_torq[0], diff_data.max_Torq,// - (uint16_t)getParam("feedPwr"), - (uint16_t)getParam("dispPwr")); + diff_data.feedPower, + diff_data.dispPower); // 发布左右电机期望转速,电源在工作状态才能发送 - if (power_data.current_state == POWER_WORKING) - { +// if (power_data.current_state == POWER_WORKING) +// { publishMessage(&un_motor_output1, 1); - publishMessage(&un_motor_output2, 1); - publishMessage(&un_motor_output3, 1); - publishMessage(&un_motor_output4, 1); - - } + publishMessage(&un_motor_output2, 1); + +// } +// else +// { +// motorDisable(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1); +// motorDisable(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2); +// motorDisable(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3); +// motorDisable(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4); +// +// publishMessage(&un_motor_output1, 1); +// publishMessage(&un_motor_output2, 1); +// } un_can_debug_output.bit_data.speed = (uint8_t)(int8_t)(diff_data.speed*10); @@ -802,40 +737,33 @@ static void diffInput(void *signal_id) 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); } - else if ( (signal_id == &un_motor_input1) || (signal_id == &un_motor_input3) )// 处理第一个电机速度信号（左电机） - { - diff_data.left_front_motor_speed = (float)((int16_t)(un_motor_input1.bit_data.speed - 30000));//20240921 增加偏移量 - diff_data.left_rear_motor_speed = (float)((int16_t)(un_motor_input3.bit_data.speed - 30000));//20240921 增加偏移量 - diff_data.left_rear_motor_speed = - diff_data.left_rear_motor_speed;//20250708 增加反相 - - if(fabs(diff_data.left_rear_motor_speed) > fabs(diff_data.left_front_motor_speed))//取速度较小的轮速 - { - motor_speed_temp = diff_data.left_front_motor_speed; - } - else - { - motor_speed_temp = diff_data.left_rear_motor_speed; - } - diff_data.left_motor_speed = motor_speed_temp; - } - else if( (signal_id == &un_motor_input2) || (signal_id == &un_motor_input4) )// 处理第二个电机速度信号（右电机） - { - diff_data.right_front_motor_speed = (float)((int16_t)(un_motor_input2.bit_data.speed - 30000)); // 20250502 1号控制器增加反相 - diff_data.right_rear_motor_speed = (float)((int16_t)(un_motor_input4.bit_data.speed - 30000)); - - diff_data.right_rear_motor_speed = - diff_data.right_rear_motor_speed;//20250708 增加反相 - - if(fabs(diff_data.right_front_motor_speed) > fabs(diff_data.right_rear_motor_speed))//取速度较小的轮速 - { - motor_speed_temp = diff_data.right_rear_motor_speed; - } - else - { - motor_speed_temp = diff_data.right_front_motor_speed; - } + else{} + + //速度输入处理 20251005 修改为定时处理 + diff_data.left_front_motor_speed = -convertPhysical( SWAP_ENDIAN_16(un_motor_input1.rx_can_data.bit_data.current_velocity),-RS04_ANGULAR_VELOCITY_MAX,RS04_ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE ) * ANGULAR_VELOCITY_TO_RPM;//速度单位转换，rad/s转换为转/分钟 + diff_data.right_front_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_motor_input2.rx_can_data.bit_data.current_velocity), -RS04_ANGULAR_VELOCITY_MAX,RS04_ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE ) * ANGULAR_VELOCITY_TO_RPM;//速度单位转换，rad/s转换为转/分钟 + diff_data.left_rear_motor_speed = -convertPhysical( SWAP_ENDIAN_16(un_motor_input3.rx_can_data.bit_data.current_velocity),-RS04_ANGULAR_VELOCITY_MAX,RS04_ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE ) * ANGULAR_VELOCITY_TO_RPM;//速度单位转换，rad/s转换为转/分钟 + diff_data.right_rear_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_motor_input4.rx_can_data.bit_data.current_velocity), -RS04_ANGULAR_VELOCITY_MAX,RS04_ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE ) * ANGULAR_VELOCITY_TO_RPM;//速度单位转换，rad/s转换为转/分钟 - diff_data.right_motor_speed = motor_speed_temp; + if(fabs(diff_data.left_rear_motor_speed) > fabs(diff_data.left_front_motor_speed))//取速度较小的轮速 + { + motor_speed_temp = diff_data.left_front_motor_speed; } + else + { + motor_speed_temp = diff_data.left_rear_motor_speed; + } + diff_data.left_motor_speed = motor_speed_temp;//左侧轮速 //motor_speed_temp + + if(fabs(diff_data.right_front_motor_speed) > fabs(diff_data.right_rear_motor_speed))//取速度较小的轮速 + { + motor_speed_temp = diff_data.right_rear_motor_speed; + } + else + { + motor_speed_temp = diff_data.right_front_motor_speed; + } + diff_data.right_motor_speed = motor_speed_temp;//右侧轮速 // 急停开关 diff_data.emergency_stop_state = (uint8_t)(diff_data.emergency_stop_switch == app_close() || diff_data.remote_emergency_stop == app_close()); @@ -853,6 +781,90 @@ static void diffInput(void *signal_id) diff_data.desired_curvature = 0.0; } + switch(diff_data.motor_init_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据 + { + case 0: +// timerStart(&diff_app_timer3, 1000, 0); // 启动定时器，1s + diff_data.motor_init_state = 1; + break; + + case 1: + if(diff_data.diff_cnt >= 10)//发送5次 + { + diff_data.diff_cnt = 0; + diff_data.motor_init_state = 2; + } + else + { + diff_data.diff_cnt ++; + diff_data.motor_init_state = 1; + } + break; + + case 2://模式设置 + if(diff_data.diff_cnt >= 5)//发送5次 + { + diff_data.diff_cnt = 0; + diff_data.motor_init_state = 3; + } + else + { + diff_data.diff_cnt ++; + diff_data.motor_init_state = 2; + + setMotorMode(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1, CURRENT_MODE); + setMotorMode(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2, CURRENT_MODE); + setMotorMode(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3, CURRENT_MODE); + setMotorMode(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4, CURRENT_MODE); + + publishMessage(&un_motor_output1, 1); + publishMessage(&un_motor_output2, 1); + +// publishMessage(&un_motor_output3, 1); +// publishMessage(&un_motor_output4, 1); + } + break; + +//------------------------------------------------------------------------------ + case 3: + if(diff_data.diff_cnt >= 5)//发送5次 + { + diff_data.diff_cnt = 0; + diff_data.motor_init_state = 4; + } + else + { + diff_data.diff_cnt ++; + diff_data.motor_init_state = 3; + + motorEnable(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1); + motorEnable(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2); + motorEnable(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3); + motorEnable(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4); + + publishMessage(&un_motor_output1, 1); + publishMessage(&un_motor_output2, 1); +// publishMessage(&un_motor_output3, 1); +// publishMessage(&un_motor_output4, 1); + } + break; + + case 4: + diff_data.motor_init_state = 4; + diffProcess(&diff_data);//计算左右电机期望转速 + break; + + default:break; + } + + + + + + + + + // if (diff_data.emergency_stop_state == 1)//刹车 20241017 增加的扭矩限制 // { // diff_data.max_Torq = 5;//20240403修改。刹车就是5N @@ -865,20 +877,23 @@ static void diffInput(void *signal_id) // { // diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩 // } - if((power_data.current_state == POWER_WORKING))//电机上电才运行 - { - diffProcess(&diff_data);//计算左右电机期望转速 - } - else - { - resetPidIntegral(&speed_pid); - resetPidIntegral(&yaw_rate_pid); - diff_data.motor_state[0] = STATE_INIT; - diff_data.motor_state[1] = STATE_INIT; - diff_data.motor_state[2] = STATE_INIT; - diff_data.motor_state[3] = STATE_INIT; - } + +// if((power_data.current_state == POWER_WORKING))//电机上电才运行 +// { +// diff_data.motor_init_state = 0; +// } +// else +// { +// resetPidIntegral(&speed_pid); +// resetPidIntegral(&yaw_rate_pid); +// +// diff_data.out_torq[0] = STATE_INIT; +// diff_data.out_torq[1] = STATE_INIT; +// diff_data.out_torq[2] = STATE_INIT; +// diff_data.out_torq[3] = STATE_INIT; +// } + timerStart(&diff_app_timer2,1,1);//1ms调用一次 } @@ -887,20 +902,84 @@ 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")); - // 档位 - un_motor_output1.bit_data.gear = 0; // 0表示空挡 - un_motor_output2.bit_data.gear = 0; - un_motor_output3.bit_data.gear = 0; // 0表示空挡 - un_motor_output4.bit_data.gear = 0; - - publishMessage(&un_motor_output1, 1); - publishMessage(&un_motor_output2, 1); - publishMessage(&un_motor_output3, 1); - publishMessage(&un_motor_output4, 1); +// setMotorOutput(out_torq, (uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr")); +//------------------------------------------------------------------------------------------------------------------------------- + switch(diff_data.motor_init_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据 + { + case 0: +// timerStart(&diff_app_timer3, 1000, 0); // 启动定时器，1s + diff_data.motor_init_state = 1; + break; + + case 1: + if(diff_data.diff_cnt >= 10)//发送5次 + { + diff_data.diff_cnt = 0; + diff_data.motor_init_state = 2; + } + else + { + diff_data.diff_cnt ++; + diff_data.motor_init_state = 1; + } + break; + + case 2://模式设置 + if(diff_data.diff_cnt >= 5)//发送5次 + { + diff_data.diff_cnt = 0; + diff_data.motor_init_state = 3; + } + else + { + diff_data.diff_cnt ++; + diff_data.motor_init_state = 2; + + setMotorMode(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1, CURRENT_MODE); + setMotorMode(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2, CURRENT_MODE); + setMotorMode(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3, CURRENT_MODE); + setMotorMode(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4, CURRENT_MODE); + + publishMessage(&un_motor_output1, 1); + publishMessage(&un_motor_output2, 1); + +// publishMessage(&un_motor_output3, 1); +// publishMessage(&un_motor_output4, 1); + } + break; + +//------------------------------------------------------------------------------ + case 3: + if(diff_data.diff_cnt >= 5)//发送5次 + { + diff_data.diff_cnt = 0; + diff_data.motor_init_state = 4; + } + else + { + diff_data.diff_cnt ++; + diff_data.motor_init_state = 3; + + motorEnable(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1); + motorEnable(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2); + motorEnable(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3); + motorEnable(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4); + + publishMessage(&un_motor_output1, 1); + publishMessage(&un_motor_output2, 1); +// publishMessage(&un_motor_output3, 1); +// publishMessage(&un_motor_output4, 1); + } + break; + + case 4: + diff_data.motor_init_state = 4; + publishMessage(&un_motor_output1, 1);//一直使能防止 ，防止can报故障 + publishMessage(&un_motor_output2, 1); + break; + + default:break; + } } @@ -915,7 +994,7 @@ void diffParametersInit(void *signal_id) getParam("Ospd_ki"), getParam("Ospd_kd"), getParam("Ospd_il"), - getParam("Ospd_ol") + getParam("Ospd_ol") ); setPidParameters(&yaw_rate_pid, @@ -963,9 +1042,44 @@ void diffParametersInit(void *signal_id) Dec_front_speed_pid.output_limit ); + + if(0 == un_motor_input1.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能 + { + motorEnable(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output5); + publishMessage(&un_motor_output5, 1); + } + + if(0 == un_motor_input2.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能 + { + motorEnable(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output6); + publishMessage(&un_motor_output6, 1); + } + + if(0 == un_motor_input3.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能 + { + motorEnable(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output7); + publishMessage(&un_motor_output7, 1); + } + + if(0 == un_motor_input4.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能 + { + motorEnable(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output8); + publishMessage(&un_motor_output8, 1); + } + + diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩 diff_data.max_Torq = (float)getParam("maxTorq"); + diff_data.feedPower = (uint16_t)getParam("feedPwr"); + diff_data.dispPower = (uint16_t)getParam("dispPwr"); + diff_data.vehicle_mass = (float)getParam("VehMass"); + diff_data.wheel_radius = (float)getParam("whl_dia"); + diff_data.gear_ratio = (float)getParam("gRatio"); + diff_data.wheel_base = (float)getParam("whl_bas"); + diff_data.max_motor_rpm = (float)getParam("max_rpm"); + + if(0 == (float)getParam("diff_sp"))//20250711 防止参数为0，影响计算。 { diff_data.diff_dead_zone = 2; @@ -974,9 +1088,7 @@ void diffParametersInit(void *signal_id) { diff_data.diff_dead_zone = (float)getParam("diff_sp");//参数读取设定最大扭矩 } - - - + printf("desired_speed: %f, desired_yaw_rate: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate); printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate); // printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate); @@ -992,25 +1104,30 @@ void diffParametersInit(void *signal_id) deffspeed = deffspeed * 0.01f; deffcurvature = deffcurvature * 0.0001f; - printf("remote_speed: %f, remote_yaw_rate: %f\n", deffspeed, deffcurvature); +// printf("remote_speed: %f, remote_yaw_rate: %f\n", deffspeed, deffcurvature); - printf(" car state = %d,%d,%d,%d\n", diff_data.motor_state[0],diff_data.motor_state[1],diff_data.motor_state[2],diff_data.motor_state[3]); +// printf(" motor_init_state = %d\n", diff_data.motor_init_state); timerStart(&diff_app_timer,1000,1);//1s调用一次 } + + // 差速初始化函数 void diffAppInit(void) { // 初始化 diff_data memset(&diff_data, 0, sizeof(DiffData)); + +// timerInit(&brake_data.brake_timer); + + // 订阅相关信号 subscribe(&un_sw_sample, diffInput); // 急停开关、高压开关 - subscribe(&un_motor_input1, diffInput); - subscribe(&un_motor_input2, diffInput); - - +// subscribe(&un_motor_input1, diffInput); +// subscribe(&un_motor_input2, diffInput); + subscribe(&un_auto_computer_input, diffInput); subscribe(&un_manual_computer_input, diffInput); subscribe(&un_remote_control_input, diffInput); @@ -1063,6 +1180,9 @@ void diffAppInit(void) subscribe(&diff_app_timer, diffParametersInit); timerStart(&diff_app_timer,1000,1);//1s调用一次 + + subscribe(&diff_app_timer2, diffInput); + timerStart(&diff_app_timer2,1,1);//1ms调用一次 printf("diffControl: diffAppInit OK \n"); } diff --git a/app/app_differential_drive.h b/app/app_differential_drive.h index ad816075..450163ae 100644 --- a/app/app_differential_drive.h +++ b/app/app_differential_drive.h @@ -19,13 +19,35 @@ extern "C" #define SPEED_MODE 0x01 #define TORQUE_MODE 0x02 +#include + + +#define ANGULAR_VELOCITY_TO_RPM 30.0 / PI + #define TORQUE_HYSTERESIS_THRESHOLD 0.3f - #define MOTOR_MODE TORQUE_MODE + + + +//轮毂电机 +#define FRONT_LEFT_MOTOR_CANID 0x7F +#define FRONT_RIGHT_MOTOR_CANID 0x7F +#define REAR_LEFT_MOTOR_CANID 0x7F +#define REAR_RIGHT_MOTOR_CANID 0x7F + + +#define FRONT_LEFT_MOTOR_RxCANID (0x20000FD + (FRONT_LEFT_MOTOR_CANID << 8)) // 0x2007CFD +#define FRONT_RIGHT_MOTOR_RxCANID (0x20000FD + (FRONT_RIGHT_MOTOR_CANID << 8)) // 0x2007DFD +#define REAR_LEFT_MOTOR_RxCANID (0x20000FD + (REAR_LEFT_MOTOR_CANID << 8)) // 0x2007EFD +#define REAR_RIGHT_MOTOR_RxCANID (0x20000FD + (REAR_RIGHT_MOTOR_CANID << 8)) // 0x2007FFD + + + + #define ALPHA 0.1f // 滤波系数α∈[0.01,0.3]，0.2对应截止频率约10Hz（假设采样周期10ms） #define LOWPASS_FILTER(speed, prev) (ALPHA * (speed) + (1 - ALPHA) * (prev)) @@ -46,6 +68,8 @@ typedef enum typedef struct DiffData { + uint16_t diff_cnt; + uint8_t motor_init_state; // 电机状态 ControlMode mode ; // 控制模式 MotorState motor_state[4]; //当前车辆状态 float desired_speed; // 期望速度 @@ -71,6 +95,11 @@ typedef struct DiffData float out_left_motor_speed; // 输出左电机速度 float out_right_motor_speed; // 输出右电机速度 float out_torq[4]; //4个电机扭矩 + float out_torq_last[4]; //4个电机扭矩 + + uint16_t feedPower; //馈电功率 + uint16_t dispPower; //放电功率 + float max_Torq; // 最大扭矩限制 float min_Torq; // 最小扭矩限制 @@ -79,7 +108,14 @@ typedef struct DiffData float left_diff_touue; // 左侧扭矩差 float right_diff_touue; // 右侧扭矩差 - float diff_dead_zone; // 差速速度死区 + float diff_dead_zone; // 差速速度死区 + + float vehicle_mass; // 车重 + float wheel_radius; // 轮胎直径 + float gear_ratio; // 减速比 + float wheel_base; // 轴距 + float max_motor_rpm; // 电机最大速度 + } DiffData; diff --git a/app/app_frm_signal.h b/app/app_frm_signal.h index f598bfa0..cac6043e 100644 --- a/app/app_frm_signal.h +++ b/app/app_frm_signal.h @@ -10,7 +10,7 @@ extern "C" #include "app_dependence.h" #define MAX_SIGNALS 500u // 每个优先级的最大信号数量 -#define MAX_SUBSCRIBERS 50u // 不同信号的订阅者数量 +#define MAX_SUBSCRIBERS 100u // 不同信号的订阅者数量 #define MAX_CALLBACKS 25u // 每个信号最多支持多少订阅者 #define PRIORITY_LEVELS 2u // 优先级层次 diff --git a/app/app_light.c b/app/app_light.c index 14084c4b..8c7eb8ca 100644 --- a/app/app_light.c +++ b/app/app_light.c @@ -110,41 +110,22 @@ static void lightOutput(void *signal_id) switch (i) {//正常所有灯光熄灭 case LIGHT_HEAD://头灯，前面4个灯 - un_inf_can_kgf_output1.bit_data.KGF05 = state_value; - un_inf_can_kgf_output1.bit_data.KGF06 = state_value; - un_inf_can_kgf_output1.bit_data.KGF10 = state_value; - un_inf_can_kgf_output1.bit_data.KGF11 = state_value; - + un_inf_can_kgf_output1.bit_data.KGF13 = state_value; break; case LIGHT_TAIL://尾灯，后面4个灯 - un_inf_can_kgf_output2.bit_data.KGF12 = state_value; - un_inf_can_kgf_output2.bit_data.KGF13 = state_value; - un_inf_can_kgf_output2.bit_data.KGF14 = state_value; - un_inf_can_kgf_output2.bit_data.KGF15 = state_value; + un_inf_can_kgf_output1.bit_data.KGF14 = state_value; break; case LIGHT_LEFT_TURN://左转向，左边4个灯 - un_inf_can_kgf_output1.bit_data.KGF06 = state_value; - un_inf_can_kgf_output1.bit_data.KGF11 = state_value; - un_inf_can_kgf_output2.bit_data.KGF12 = state_value; - un_inf_can_kgf_output2.bit_data.KGF13 = state_value; + un_inf_can_kgf_output1.bit_data.KGF13 = state_value; break; case LIGHT_RIGHT_TURN://右转向灯，右边4个灯 - un_inf_can_kgf_output1.bit_data.KGF05 = state_value; - un_inf_can_kgf_output1.bit_data.KGF10 = state_value; - un_inf_can_kgf_output2.bit_data.KGF14 = state_value; - un_inf_can_kgf_output2.bit_data.KGF15 = state_value; + un_inf_can_kgf_output1.bit_data.KGF14 = state_value; break; case LIGHT_BRAKE://刹车灯，四个黄灯 - un_inf_can_kgf_output1.bit_data.KGF10 = state_value; - un_inf_can_kgf_output1.bit_data.KGF11 = state_value; - un_inf_can_kgf_output2.bit_data.KGF13 = state_value; - un_inf_can_kgf_output2.bit_data.KGF15 = state_value; + un_inf_can_kgf_output1.bit_data.KGF13 = state_value; break; case LIGHT_ALARM://报警灯，四个红灯 - un_inf_can_kgf_output1.bit_data.KGF05 = state_value; - un_inf_can_kgf_output1.bit_data.KGF06 = state_value; - un_inf_can_kgf_output2.bit_data.KGF12 = state_value; - un_inf_can_kgf_output2.bit_data.KGF14 = state_value; + un_inf_can_kgf_output1.bit_data.KGF14 = state_value; break; } } diff --git a/app/app_param_manage.h b/app/app_param_manage.h index 1c71921b..8a1438d3 100644 --- a/app/app_param_manage.h +++ b/app/app_param_manage.h @@ -41,8 +41,8 @@ extern "C" X(pwr_btn) \ X(sleepTm) \ X(wakeTm) \ - X(Ospd_kp) \ - X(Ospd_ki) \ + X(minYpos) \ + X(maxYpos) \ X(Ospd_kd) \ X(Ospd_il) \ X(Ospd_ol) \ @@ -59,6 +59,10 @@ extern "C" X(mot_il) \ X(mot_ol) \ X(diff_sp) \ + X(turn_sp) \ + X(pit_sp) \ + X(xy_pos) \ + X(z_pos) \ X(test) // 定义一个包含所有参数名称的结构体 diff --git a/app/app_power.c b/app/app_power.c index 5a158c96..59200699 100644 --- a/app/app_power.c +++ b/app/app_power.c @@ -34,73 +34,73 @@ typedef struct { 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 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; +// } +//} // 输出处理函数 @@ -113,99 +113,111 @@ 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.KGF07 = setPowerOff(); // 高压继电器 - un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 - un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF11 = 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.KGF03 = setPowerOn(); // 遥控器 - un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + un_inf_can_kgf_output1.bit_data.KGF01 = setPowerOff(); // E3 + un_inf_can_kgf_output1.bit_data.KGF02 = setPowerOn(); // 导航仪 + un_inf_can_kgf_output1.bit_data.KGF03 = 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.KGF09 = 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.KGF14 = setPowerOn(); // 后左右红灯 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 网络摄像头 + un_inf_can_kgf_output1.bit_data.KGF16 = 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_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF11 = 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.KGF03 = setPowerOn(); // 遥控器 - un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + un_inf_can_kgf_output1.bit_data.KGF01 = setPowerOff(); // E3 + un_inf_can_kgf_output1.bit_data.KGF02 = setPowerOn(); // 导航仪 + un_inf_can_kgf_output1.bit_data.KGF03 = 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.KGF09 = 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.KGF14 = setPowerOn(); // 后左右红灯 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 网络摄像头 + un_inf_can_kgf_output1.bit_data.KGF16 = 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_output2.bit_data.KGF10 = setPowerOff(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOff(); // 低压继电器 - 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.KGF03 = setPowerOn(); // 遥控器 - un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + un_inf_can_kgf_output1.bit_data.KGF01 = setPowerOff(); // E3 + un_inf_can_kgf_output1.bit_data.KGF02 = setPowerOn(); // 导航仪 + un_inf_can_kgf_output1.bit_data.KGF03 = 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.KGF09 = 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.KGF14 = setPowerOn(); // 后左右红灯 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 网络摄像头 + un_inf_can_kgf_output1.bit_data.KGF16 = 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_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF11 = 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.KGF03 = setPowerOn(); // 遥控器 - un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + un_inf_can_kgf_output1.bit_data.KGF01 = setPowerOff(); // E3 + un_inf_can_kgf_output1.bit_data.KGF02 = setPowerOn(); // 导航仪 + un_inf_can_kgf_output1.bit_data.KGF03 = 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.KGF09 = 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.KGF14 = setPowerOn(); // 后左右红灯 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 网络摄像头 + un_inf_can_kgf_output1.bit_data.KGF16 = 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_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF11 = 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.KGF03 = setPowerOn(); // 遥控器 - un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + un_inf_can_kgf_output1.bit_data.KGF01 = setPowerOff(); // E3 + un_inf_can_kgf_output1.bit_data.KGF02 = setPowerOn(); // 导航仪 + un_inf_can_kgf_output1.bit_data.KGF03 = 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.KGF09 = 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.KGF14 = setPowerOn(); // 后左右红灯 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 网络摄像头 + un_inf_can_kgf_output1.bit_data.KGF16 = 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_output2.bit_data.KGF10 = setPowerOff(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOff(); // 低压继电器 - 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.KGF03 = setPowerOn(); // 遥控器 - un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + un_inf_can_kgf_output1.bit_data.KGF01 = setPowerOff(); // E3 + un_inf_can_kgf_output1.bit_data.KGF02 = setPowerOn(); // 导航仪 + un_inf_can_kgf_output1.bit_data.KGF03 = 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.KGF09 = 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.KGF14 = setPowerOn(); // 后左右红灯 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 网络摄像头 + un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 遥控器 break; default: @@ -213,7 +225,7 @@ static void powerOutput(void *signal_id) } publishMessage(&power_data, 1); publishMessage(&un_inf_can_kgf_output1, 1); - publishMessage(&un_inf_can_kgf_output2, 1); +// publishMessage(&un_inf_can_kgf_output2, 1); } static void wakeupProcess(void *signal_id) @@ -246,13 +258,13 @@ static void powerTimerProcess(void *signal_id) (void)signal_id; // 调用电源按钮处理函数 - handlePowerButton(); +// 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; - } +// 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) @@ -274,7 +286,7 @@ static void powerTimerProcess(void *signal_id) 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; // 预充完成 @@ -282,25 +294,27 @@ static void powerTimerProcess(void *signal_id) 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"); - } + + power_data.current_state = POWER_WORKING; // 休眠 20251005 修改不需要休眠，直接上电 +// 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()) // 急停开关闭合 +// if (power_data.high_voltage_switch == app_open()) // 高压开关断开 +// { +// power_data.current_state = POWER_SLEEP; // 休眠 +// printf("Power: Transitioning from STANDBY to SLEEP state\n"); +// + if (power_data.emergency_stop == app_close()) // 急停开关闭合 { power_data.current_state = POWER_EMERGENCY; // 急停 printf("Power: Transitioning from WORKING to EMERGENCY state\n"); @@ -310,17 +324,17 @@ static void powerTimerProcess(void *signal_id) } 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()) // 急停断开 +// 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()) // 遥控器电源开关断开 //20251005 修改不需要电源开关 +// { +// power_data.current_state = POWER_STANDBY; // 待机 +// printf("Power: Transitioning from EMERGENCY to STANDBY state\n"); +// } + 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); // 启动预充定时器 @@ -362,10 +376,13 @@ static void powerInput(void *signal_id) memcpy(&old_data, &power_data, sizeof(PowerSystem)); // 填充数据 + power_data.emergency_stop_switch = 0;//急停开关 20251005 修改四轮四转车无急停开关以及采集模块 高压 + power_data.high_voltage_switch = 1;//高压开关 + 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;//高压开关 +// 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) )// 遥控器断线，不更新数据 { @@ -408,7 +425,22 @@ void powerAppInit(void) 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; + power_data.old_emergency_stop = power_data.emergency_stop; + + un_inf_can_kgf_output1.bit_data.KGF01 = setPowerOff(); // E3 初始化上电默认打开 + un_inf_can_kgf_output1.bit_data.KGF02 = setPowerOn(); // 导航仪 + un_inf_can_kgf_output1.bit_data.KGF03 = 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.KGF09 = 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.KGF14 = setPowerOn(); // 后左右红灯 + un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 网络摄像头 + un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 遥控器 // 订阅输入信号 subscribe(&un_sw_sample, powerInput); // 急停开关、高压开关 diff --git a/app/app_turntable.c b/app/app_turntable.c new file mode 100644 index 00000000..f0c697a2 --- /dev/null +++ b/app/app_turntable.c @@ -0,0 +1,403 @@ +#include "app_config.h" +#include "app_dependence.h" +#include "interface.h" +#include "app_turntable.h" +#include "app_pid.h" +#include "app_param_manage.h" + +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" + +#include "drive_rs04.h" +#include "sdrv_vic.h" +#include + +PID_t turnable_speed_pid; +PID_t turnable_position_pid; + +TurnableData turnable_data = {0}; + + +/** + * @brief 将笛卡尔坐标 (x,y,z) 转换为球坐标 (r,θ,φ) + * + * @param x X轴坐标值（单位：米） + * @param y Y轴坐标值（单位：米） + * @param z Z轴坐标值（单位：米） + * @param[out] out 输出球坐标结构体指针 + * @return bool 转换是否成功： + * - true: 转换成功 + * - false: 输入无效（包含NaN/INF或out为NULL） + * + * @note 特殊输入处理： + * 1. 如果输入包含 NaN 或无穷大，返回 false + * 2. 当 r < 1e-10 时视为原点，设置 out=(0, 0, 0) + * 3. 在Z轴附近 (|z/r| ≈ 1) 时自动截断到 [-1,1] 保证数值稳定性 + * + * @warning 使用要求： + * - 必须检查返回值，不能直接使用out内容 + * - out指针必须指向有效内存 + * + * @example 正确用法： + * SphericalCoordinate sph; + * if (cartesianToSpherical(1.0f, 0.0f, 0.0f, &sph)) { + * // 使用sph... + * } + */ +uint8_t cartesianToSpherical(float x, float y, float z, SphericalCoordinate* out) +{ + // 参数有效性检查（防御性编程） + if (!out || !isfinite(x) || !isfinite(y) || !isfinite(z)) { + return 0; + } + + // 计算径向距离 r = √(x² + y² + z²) + out->r = sqrtf(x * x + y * y + z * z); + + // 原点判定（使用容差避免浮点误差） + if (out->r < 1e-10f) { + out->theta = 0.0f; + out->phi = 0.0f; + return 1; + } + + // 极角 θ = acos(z/r) 的数值稳定性处理 + float z_over_r = z / out->r; + if (z_over_r > 1.0f) z_over_r = 1.0f; // 处理上溢出 + if (z_over_r < -1.0f) z_over_r = -1.0f; // 处理下溢出 + out->theta = acosf(z_over_r); + + // 方位角 φ = atan2(y, x) + out->phi = atan2f(y, x); + + return 1; +} + + + + +// 计算CRC8校验（多项式 x^8 + 1，简单的累加和） +uint8_t encoder_calculate_crc(const uint8_t* data, uint8_t length) +{ + uint8_t crc = 0x00; + for (int i = 0; i < length; i++) { + crc += data[i]; + } + return crc; +} + + + + + + + + + + + + + + + +static void setTurnableMotorOutput() +{ + + turnable_data.out_pitch_motor_ampere = constrain(turnable_data.out_pitch_motor_ampere, -RS02_ANGULAR_VELOCITY_MAX, RS02_ANGULAR_VELOCITY_MAX); + turnable_data.out_left_motor_ampere = constrain(turnable_data.out_left_motor_ampere, -RS02_ANGULAR_VELOCITY_MAX, RS02_ANGULAR_VELOCITY_MAX); + + + setMotorWrite(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1, LIMIT_SPEED_INDEX,turnable_data.out_pitch_motor_ampere); + setMotorWrite(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output4, LOC_REF_INDEX,turnable_data.desired_pitch_position); + + setMotorWrite(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2, SPD_REF,turnable_data.out_left_motor_ampere); + + un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(turnable_data.out_left_motor_ampere_limit * 100); + un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(turnable_data.out_right_motor_ampere_limit*100); + + publishMessage(&un_sdo_output1, 1); + publishMessage(&un_sdo_output2, 1); + publishMessage(&un_sdo_output4, 1); +} + + + + + +// 转台 +static void turnableProcess(void *signal_id) +{ +// if((turnable_data.current_state == POWER_WORKING))//高压上电才运行 +// { + switch(turnable_data.turnable_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据 + { + case 0: + timerStart(&turnable_data.turnable_timer, 1000, 1); // 启动定时器，1s + turnable_data.turnable_state = 1; + break; + + case 1: + if (!turnable_data.turnable_timer.active)// 1s定时 + { + turnable_data.turnable_state = 2; + } + else + { + turnable_data.turnable_state = 1; + } + break; + + case 2://模式设置 + if(turnable_data.turnable_cnt >= 5)//发送5次 + { + turnable_data.turnable_cnt = 0; + turnable_data.turnable_state = 3; + } + else + { + turnable_data.turnable_cnt ++; + turnable_data.turnable_state = 2; + + setMotorMode(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1, POSITION_MODE_CSP); + setMotorMode(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2, VELOCITY_MODE); + + setMotorWrite(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output5, LIMIT_CUR,5); //设置最大电流为5A + + publishMessage(&un_sdo_output1, 1); + publishMessage(&un_sdo_output2, 1); + publishMessage(&un_sdo_output5, 1); + + } + break; + +//------------------------------------------------------------------------------ + case 3: + if(turnable_data.turnable_cnt >= 5)//发送5次 + { + turnable_data.turnable_cnt = 0; + turnable_data.turnable_state = 4; + } + else + { + turnable_data.turnable_cnt ++; + turnable_data.turnable_state = 3; + + motorEnable(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1); + motorEnable(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2); + + publishMessage(&un_sdo_output1, 1); + publishMessage(&un_sdo_output2, 1); + } + break; + + case 4: + turnable_data.turnable_cnt = 0; + turnable_data.turnable_state = 4; + + setTurnableMotorOutput();//输出函数 + break; + + default:break; + } + +} + + + + +void turnableParametersInit(void *signal_id) +{ + (void)signal_id; // 标记变量为已使用，避免编译器警告 + + if(0 == un_right_intput.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能 + { + motorEnable(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output3); + publishMessage(&un_sdo_output3, 1); + } + + if(0 == un_pitch_intput.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能 + { + motorEnable(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output3); + publishMessage(&un_sdo_output3, 1); + } + + + turnable_data.desired_horizontal_speed = getParam("turn_sp"); + turnable_data.desired_pitch_speed = getParam("pit_sp"); + + turnable_data.min_pitch_postion = getParam("minYpos"); //俯仰位置最小限制值 + turnable_data.max_pitch_postion = getParam("maxYpos"); //俯仰位置最大限制值 + + printf( "turnable left A %f\n",turnable_data.out_left_motor_ampere); + printf( "turnable right A %f\n",turnable_data.out_right_motor_ampere); + printf( "turnable pitch A %f\n",turnable_data.out_pitch_motor_ampere); +// printf( "desired speed %f\n",turnable_data.desired_speed); + printf( "speed %f\n",turnable_data.speed); + printf( "turnable state %d\n",turnable_data.turnable_state); + + timerStart(&turnable_data.turnable_timer1,1000,1);//100ms调用一次 +} + + + + +static void dataRequest(void *signal_id) +{ + str_magnetic_encoder.magnetic_data = ENCODER_HEADER; + + publishMessage(&str_magnetic_encoder, 1); + timerStart(&turnable_data.turnable_timer3,100,1);//100ms调用一次 +} + + + + + + + + +// 差速输入处理函数 +static void turnableInput(void *signal_id) +{ + if(signal_id == &power_data)//电机上电 + { + turnable_data.current_state = power_data.current_state; + } + else if(signal_id == &un_computer_turnable_Input) + { + turnable_data.position_x = (float)( SWAP_ENDIAN_32(un_computer_turnable_Input.bit_data.position_x) ); + turnable_data.position_y = (float)( SWAP_ENDIAN_32(un_computer_turnable_Input.bit_data.position_y) ); + turnable_data.position_z = (float)( SWAP_ENDIAN_32(un_computer_turnable_Input.bit_data.position_z) ); + } + else if(signal_id == &un_encoder_data_input) + { + if( un_encoder_data_input.arr[sizeof(un_encoder_data_input)-1] == encoder_calculate_crc(&un_encoder_data_input.arr[0], sizeof(un_encoder_data_input)-1) )//CRC校验 + { + turnable_data.horizontal_position = (float)(un_encoder_data_input.bit_data.abs_value)*2.0f*PI/ENCODER_MAX_COUNTS;//将数据转换为实际角度 + } + } + else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据 + { +// diff_data.remote_emergency_stop = !(uint8_t)un_remote_control_input.bit_data.switch_b; +// diff_data.mode = un_remote_control_input.bit_data.switch_c = + + float x_axis_temp = (float)(un_remote_control_input.bit_data.x_axis) - REMOTE_ZERO; + if( ( x_axis_temp > 50 ) || ( x_axis_temp < -50 ) ) + { + turnable_data.out_left_motor_ampere = 0.02*(x_axis_temp);//计算电流 + turnable_data.out_right_motor_ampere = turnable_data.out_left_motor_ampere; + } + else + { + turnable_data.out_left_motor_ampere = 0;//计算电流 + turnable_data.out_right_motor_ampere = turnable_data.out_left_motor_ampere; + } + + x_axis_temp = (float)(un_remote_control_input.bit_data.y_axis) - REMOTE_ZERO; + if(x_axis_temp > 50) //根据Y轴数据来定义 + { + turnable_data.out_pitch_motor_ampere = 0.01*fabs(x_axis_temp); + turnable_data.desired_pitch_position = turnable_data.max_pitch_postion; + } + else if(x_axis_temp < -50) + { + turnable_data.out_pitch_motor_ampere = 0.01*fabs(x_axis_temp); + turnable_data.desired_pitch_position = turnable_data.min_pitch_postion; + } + else + { + turnable_data.out_pitch_motor_ampere = 0; + } + } + else if(signal_id == &un_pitch_intput) + { + turnable_data.pitch_position = convertPhysical( SWAP_ENDIAN_16(un_right_intput.rx_can_data.bit_data.current_angle),-RS02_ANGULAR_VELOCITY_MAX,RS02_ANGULAR_VELOCITY_MAX,MOTOR_ANGLE_DEADZONE ); + } + else{} + + turnable_data.right_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_right_intput.rx_can_data.bit_data.current_velocity),-RS02_ANGULAR_VELOCITY_MAX,RS02_ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE ); + turnable_data.speed = (turnable_data.right_motor_speed + turnable_data.left_motor_speed)/2.0f; + + + if ( (power_data.current_state == POWER_STANDBY) || (power_data.current_state == POWER_SLEEP) )//这几种状态可以转转台 + { + turnable_data.turnable_state = 0;//清空状态。保证每次上电都初始化 + } + else + { + turnableProcess(signal_id);//处理映射 + } + +// timerStart(&turnable_data.turnable_timer2,100,1);//100ms调用一次 +} + + +void turnableInit() +{ + // 初始化速度 PID 控制器 + initializePid(&turnable_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); + +// // 设置速度 PID 控制器的参数 +// setPidParameters(&turnable_speed_pid, +// getParam("spd_kp"), +// getParam("spd_ki"), +// getParam("spd_kd"), +// getParam("spd_il"), +// getParam("spd_ol") +// ); + + //目标参数初始化 + str_magnetic_encoder.ip[0] = 192; + str_magnetic_encoder.ip[1] = 168; + str_magnetic_encoder.ip[2] = 17; + str_magnetic_encoder.ip[3] = 33; + str_magnetic_encoder.port = 2011; + + subscribe(&un_remote_control_input, turnableInput); + subscribe(&un_computer_turnable_Input, turnableInput); + subscribe(&un_pitch_intput, turnableInput); + + timerInit(&turnable_data.turnable_timer); + timerInit(&turnable_data.turnable_timer1); + +// timerInit(&turnable_data.turnable_timer2); +// subscribe(&turnable_data.turnable_timer2, turnableInput); +// +// timerStart(&turnable_data.turnable_timer2,100,1);//100ms调用一次 + + subscribe(&turnable_data.turnable_timer1, turnableParametersInit); + timerStart(&turnable_data.turnable_timer1,1000,1);//100ms调用一次 + + timerInit(&turnable_data.turnable_timer3); + subscribe(&turnable_data.turnable_timer3, dataRequest); + timerStart(&turnable_data.turnable_timer3,100,1);//100ms调用一次 + + subscribe(&un_encoder_data_input, turnableInput); + + + turnable_data.turnable_state = 0; + un_right_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE; + un_right_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE; + un_right_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE; + + + un_pitch_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE; + un_pitch_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE; + un_pitch_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE; + + printf( "turnable: initial OK %d\n",getCurrentTime()); +} + + + + + + + + + + + + + diff --git a/app/app_turntable.h b/app/app_turntable.h new file mode 100644 index 00000000..a9590b6b --- /dev/null +++ b/app/app_turntable.h @@ -0,0 +1,104 @@ +#ifndef TURNTAABLE_H +#define TURNTAABLE_H + + +#include "app_power.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//编码器协议头 + +#define ENCODER_HEADER 0x1A +#define ENCODER_MAX_COUNTS 0x1FFFFF// 最大位21位 + +#define ENCODER_PORT 2011 + + +//转台电机 +#define PITCH_MOTOR_CANID 0x7D +#define RIGHT_MOTOR_CANID 0x7E +#define TURN_MOTOR_CANID 0x7F + + +#define PITCH_MOTOR_RxCANID (0x20000FD + (PITCH_MOTOR_CANID << 8)) // 0x2007DFD +#define RIGHT_MOTOR_RxCANID (0x20000FD + (RIGHT_MOTOR_CANID << 8)) // 0x2007EFD +#define TURN_MOTOR_RxCANID (0x20000FD + (TURN_MOTOR_CANID << 8)) // 0x2007FFD + + + +// 力矩量程定义 (对应Byte4~5: 当前力矩) +#define TORQUE_MIN -120.0f // 最小力矩: -120 Nm +#define TORQUE_MAX 120.0f // 最大力矩: 120 Nm + +#define ZERO_VAULE 0x0080 // 32768,需要高位在前 + +#define REMOTE_ZERO 1022 + + + +typedef struct { + float r; // 径向距离 + float theta; // 极角(与Z轴的夹角，弧度制，范围[0, π]) + float phi; // 方位角(XY平面内与X轴的夹角，弧度制，范围[-π, π]) +} SphericalCoordinate; + + + +typedef struct TurnableData +{ + uint8_t turnable_state; + PowerState current_state; // 当前电源状态 + + float position_x; //转台相对位置x + float position_y; //转台相对位置y + float position_z; //转台相对位置z + + float desired_pitch_speed; // 期望俯仰位置 + float desired_horizontal_speed; // 期望水平位置 + float desired_pitch_position; // 期望俯仰位置 + float desired_horizontal_position; // 期望水平位置 + + float left_motor_speed; // 当前左电机速度 + float right_motor_speed; // 当前右电机速度 + float speed; // 当前转盘速度 + float pitch_position; // 当前俯仰位置 + float horizontal_position; // 当前水平位置 + + float max_speed; // 最大速度 + float out_left_motor_ampere; // 输出左电机电流 + float out_right_motor_ampere; // 输出右电机电流 + float out_pitch_motor_ampere; // 输出右电机电流 + + float out_left_motor_ampere_last; // 输出左电机电流 + float out_right_motor_ampere_last; // 输出右电机电流 + float out_pitch_motor_ampere_last; // 输出右电机电流 + + float out_left_motor_ampere_limit; // 输出左电机电流限制值 + float out_right_motor_ampere_limit; // 输出右电机电流限制值 + float out_pitch_motor_ampere_limit; // 输出右电机电流限制值 + + Timer turnable_timer; // 定时器 + Timer turnable_timer1; // 定时器 + Timer turnable_timer2; // 定时器 + Timer turnable_timer3; // 定时器 + + uint8_t turnable_cnt; + + float max_ampere; // 最大电流限制 + + float min_pitch_postion; // 位置信息 + float max_pitch_postion; // 位置信息 +} TurnableData; + + + +void turnableInit(); + + +#ifdef __cplusplus +} +#endif + +#endif // TURNTAABLE_H diff --git a/app_brake.c b/app_brake.c deleted file mode 100644 index a7b42c1b..00000000 --- a/app_brake.c +++ /dev/null @@ -1,127 +0,0 @@ -#include "app_config.h" - -//定义并初始化 -BrakeSystem brake_system = { - .brake_timer = {0, 0, 0, 0, 0}, // 初始化 Timer 结构体 - .brake_command = 0, - .brake_motor_state = 0, - .brake_command_in_progress = 0, - .emergency_stop_switch = 0, - .remote_emergency_stop = 0, - .remote_fault = 0, - .can_bus_fault = 0, - .ethernet_fault = 0, - .mode_signal = 0 -}; - - -// 处理所有输入信号的函数 -static void processInputSignals(void *signal_id) -{ - (void)signal_id; // 标记变量为已使用，避免编译器警告 - - // 如果当前有刹车命令在执行，忽略新的输入信号 - if (brake_system.brake_command_in_progress) - { - return; - } - - if (brake_system.emergency_stop_switch || brake_system.remote_emergency_stop || - (brake_system.mode_signal == 0 && brake_system.remote_fault) || // 手动模式下的遥控器故障 - (brake_system.mode_signal == 1 && (brake_system.can_bus_fault || brake_system.ethernet_fault))) // 自动模式下的CAN总线或以太网通信故障 - { - brake_system.brake_command = 1; // 发送刹车指令 - publishMessage(&brake_system.brake_command, 1); - brake_system.brake_command_in_progress = 1; // 标记刹车命令正在执行 - } - else - { - brake_system.brake_command = 2; // 发送释放刹车指令 - publishMessage(&brake_system.brake_command, 1); - brake_system.brake_command_in_progress = 1; // 标记释放命令正在执行 - } -} - -// 刹车信号处理函数 -static void brakeProcess(void *signal_id) -{ - (void)signal_id; // 标记变量为已使用，避免编译器警告 - int signal = *(int *)signal_id; - - if (signal == 1) // 刹车 - { - brake_system.brake_motor_state = 1; - publishMessage(&brake_system.brake_motor_state, 1); // 状态改变时发送刹车电机状态信号 - timerInit(&brake_system.brake_timer, 1500); // 控制刹车电机前进,保持1.5秒 - } - else if (signal == 2) // 释放刹车 - { - brake_system.brake_motor_state = 2; - publishMessage(&brake_system.brake_motor_state, 1); // 状态改变时发送刹车电机状态信号 - timerInit(&brake_system.brake_timer, 800); // 控制刹车电机后退,保持0.8秒 - } - - timerStart(&brake_system.brake_timer); -} - -// 定时器信号处理函数 -static void brakeTimerProcess(void *signal_id) -{ - (void)signal_id; // 标记变量为已使用，避免编译器警告 - - brake_system.brake_motor_state = 0; - brake_system.brake_command_in_progress = 0; // 刹车命令执行完成，重置状态 - publishMessage(&brake_system.brake_motor_state, 1); // 状态改变时发送刹车电机状态信号 -} - -// 输出处理函数 -static void outputProcess(void *signal_id) -{ - (void)signal_id; // 标记变量为已使用，避免编译器警告 - //根据电机状态，填充发送数据结构，发送信号 - switch (brake_system.brake_motor_state) { - case 1: // 电机前进状态 - UnHBridgeOutput.brake_motor1 = 0; - UnHBridgeOutput.brake_motor2 = 1; - UnHBridgeOutput.sleep_01 = 0; - break; - - case 2: // 电机后退状态 - UnHBridgeOutput.brake_motor1 = 1; - UnHBridgeOutput.brake_motor2 = 0; - UnHBridgeOutput.sleep_01 = 0; - break; - default: - UnHBridgeOutput.brake_motor1 = 0; - UnHBridgeOutput.brake_motor2 = 0; - UnHBridgeOutput.sleep_01 = 0; - break; - } - publishMessage(&UnHBridgeOutput, 1); -} - -// APP模块的初始化 -void brakeAppInit(void) -{ - // 初始化定时器，使用 brake_timer 的地址作为信号ID - timerInit(&brake_system.brake_timer, 0); - - // 订阅刹车命令 - subscribe(&brake_system.brake_command, brakeProcess); - - // 订阅定时器信号，用于停止电机 - subscribe(&brake_system.brake_timer, brakeTimerProcess); - - // 订阅电机状态 - subscribe(&brake_system.brake_motor_state, outputProcess); - - // 订阅输入信号，处理刹车逻辑 - subscribe(&UnSwSample, processInputSignals); - subscribe(&UnRemoteControlInput, processInputSignals); - subscribe(&brake_system.remote_fault, processInputSignals); - subscribe(&brake_system.can_bus_fault, processInputSignals); - subscribe(&brake_system.ethernet_fault, processInputSignals); - subscribe(&brake_system.mode_signal, processInputSignals); - - -} diff --git a/app_brake.h b/app_brake.h deleted file mode 100644 index 34f2250b..00000000 --- a/app_brake.h +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef APP_BRAKE_H -#define APP_BRAKE_H - -#ifdef __cplusplus -extern "C" { -#endif - - -#include "app_config.h" - - -typedef struct { - Timer brake_timer; // 定时器结构体 - int brake_command; // 刹车命令变量：1表示刹车，2表示释放 - int brake_motor_state; // 刹车电机状态变量：0停止，1前进，2后退 - int brake_command_in_progress; // 刹车命令执行状态：0表示空闲，1表示正在执行 - //输入 - int emergency_stop_switch; // 急停开关 - int remote_emergency_stop; // 遥控器急停开关 - int remote_fault; // 遥控器故障 - int can_bus_fault; // CAN总线故障 - int ethernet_fault; // 以太网通信故障 - int mode_signal; // 模式信号：0表示手动模式，1表示自动模式 -} BrakeSystem; - -// 在头文件中声明外部变量 -extern BrakeSystem brake_system; - - -#ifdef __cplusplus -} -#endif - -#endif // APP_BRAKE_H diff --git a/app_turntable.c b/app_turntable.c deleted file mode 100644 index d4c42366..00000000 --- a/app_turntable.c +++ /dev/null @@ -1,460 +0,0 @@ -#include "app_config.h" -#include "app_dependence.h" -#include "interface.h" -#include "app_turntable.h" -#include "app_pid.h" -#include "app_param_manage.h" - -#include "app_frm_monitor.h" -#include "app_frm_signal.h" -#include "app_frm_timer.h" - -#include "sdrv_vic.h" - -PID_t turnable_speed_pid; -PID_t turnable_position_pid; - -TurnableData turnable_data = {0}; - - - - - -/** - * @brief 带死区的原始数据到物理量转换函数（简单版） - * @param raw_value 原始16位无符号整数值 [0, 65535] - * @param min 物理量最小值（如 -10.0） - * @param max 物理量最大值（如 +10.0） - * @param deadzone 死区范围（物理量单位，如 1.0 表示 ±1.0 内为死区） - * @return 转换后的物理量值（若在死区内返回0，否则返回实际值） - */ -static float convertPhysical(uint16_t raw_value, float min, float max, float deadzone) -{ - // 1. 计算实际物理量值 - float physical_value = min + ((float)raw_value / 65535.0f) * (max - min); - - // 2. 判断是否在死区内（绝对值 ≤ deadzone） - if (fabs(physical_value) <= deadzone) - { - return 0.0f; // 死区内返回0 - } - else - { - return physical_value; // 死区外返回实际值 - } -} - - - -/** - * @brief 将浮点数转换为uint32_t（按小端序存储） - * @param num 输入的浮点数 - * @return 转换后的uint32_t值（直接内存拷贝结果） - * @note 此函数通过内存直接拷贝实现转换，不进行数值计算，结果受平台字节序影响 - */ -uint32_t floatToUint32(float num) -{ - uint32_t result; - // 将浮点数的内存数据直接拷贝到uint32_t变量 - memcpy(&result, &num, sizeof(num)); - return result; -} - - -/** - * @brief 电机失能函数（停止电机运行） - * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255) - * @param master_id 主控制器ID (用于标识发送方) - * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据 - * @return 0: 成功, -1: 参数无效 - * @note 此函数会修改unsdodata指向的结构体内容，调用后需及时发送CAN报文 - */ -int8_t motorDisable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata) -{ - /* 参数有效性检查 */ - if (unsdodata == NULL) { - return -1; - } - - /* 设置CAN报文ID域 */ - unsdodata->tx_can_id.bits.mode = 3; /* 通信模式3：电机失能 */ - unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */ - unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */ - unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */ - - /* 清零数据域 */ - unsdodata->tx_can_data.bit_data.data = 0; - unsdodata->tx_can_data.bit_data.index = 0; - unsdodata->tx_can_data.bit_data.object_index = 0; - - return 0; -} - -/** - * @brief 电机使能函数（启动电机运行） - * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255) - * @param master_id 主控制器ID (用于标识发送方) - * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据 - * @return 0: 成功, -1: 参数无效 - * @note 通信模式4：电机使能 - */ -int8_t motorEnable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata) -{ - /* 参数有效性检查 */ - if (unsdodata == NULL) { - return -1; - } - - /* 设置CAN报文ID域 */ - unsdodata->tx_can_id.bits.mode = 3; /* 通信模式4：电机使能 */ - unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */ - unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */ - unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */ - - /* 清零数据域 */ - unsdodata->tx_can_data.bit_data.data = 0; - unsdodata->tx_can_data.bit_data.index = 0; - unsdodata->tx_can_data.bit_data.object_index = 0; - - return 0; -} - -/** - * @brief 设置电机运行模式 - * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255) - * @param master_id 主控制器ID (用于标识发送方) - * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据 - * @param mode 要设置的模式值 (具体含义需参考电机协议文档) - * @return 0: 成功, -1: 参数无效 - * @note RUM_MODE应为预定义的宏，表示运行模式索引 - */ -int8_t setMotorMode(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint8_t mode) -{ - /* 参数有效性检查 */ - if (unsdodata == NULL) { - return -1; - } - - /* 设置CAN报文ID域 */ - unsdodata->tx_can_id.bits.mode = 0x12; /* 通信模式0x12：参数写入 */ - unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */ - unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */ - unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */ - - /* 设置数据域 */ - unsdodata->tx_can_data.bit_data.index = RUM_MODE; /* 运行模式索引 */ - unsdodata->tx_can_data.bit_data.object_index = 0; /* 子索引通常为0 */ - unsdodata->tx_can_data.bit_data.data = mode; /* 模式值 */ - - return 0; -} - -/** - * @brief 写入电机参数 - * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255) - * @param master_id 主控制器ID (用于标识发送方) - * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据 - * @param index 要写入的参数索引 (具体含义需参考电机协议文档) - * @param ref 要写入的参数值 (浮点数，会自动转换为uint32_t) - * @return 0: 成功, -1: 参数无效 - * @note 使用floatToUint32函数转换浮点参数 - */ -int8_t setMotorWrite(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint16_t index, float ref) -{ - /* 参数有效性检查 */ - if (unsdodata == NULL) { - return -1; - } - - /* 设置CAN报文ID域 */ - unsdodata->tx_can_id.bits.mode = 0x12; /* 通信模式0x12：参数写入 */ - unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */ - unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */ - unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */ - - /* 设置数据域 */ - unsdodata->tx_can_data.bit_data.index = index; /* 参数索引 */ - unsdodata->tx_can_data.bit_data.object_index = 0; /* 子索引通常为0 */ - unsdodata->tx_can_data.bit_data.data = floatToUint32(ref); /* 转换并写入参数值 */ - - return 0; -} - - -/** - * @brief 动态斜率限制（支持变时间间隔） - * @param last_command 上一次的电流指令值 - * @param target_current 本次目标电流指令 - * @param delta_time 距离上一次调用的时间间隔 (s) - * @return 限制后的安全电流指令 - */ -float dynamic_current_limit(float *last_command, float target_current, float delta_time) -{ - // 计算期望的变化量 - float desired_change = target_current - *last_command; - - // 计算两种限制 - float step_limit = MAX_STEP; - float time_limit = MAX_DI_DT * delta_time; // 动态计算时间限制 - - // 选择更严格的限制 - float max_allowed_change = (step_limit < time_limit) ? step_limit : time_limit; - - // 应用限制并返回新指令 - float actual_change = constrain(desired_change, -max_allowed_change, max_allowed_change); - *last_command = *last_command + actual_change;//更新过去值 - - return *last_command + actual_change; -} - -static void setTurnableMotorOutput() -{ - static float previous_time2 = 0.0f; - - float time1 = (float)getCurrentTime(); - float dt = (time1 - previous_time2) / PERIOD_TICK; - previous_time2 = time1; - - turnable_data.out_pitch_motor_ampere_limit = dynamic_current_limit(&turnable_data.out_pitch_motor_ampere_last,turnable_data.out_pitch_motor_ampere,dt); - turnable_data.out_right_motor_ampere_limit = dynamic_current_limit(&turnable_data.out_right_motor_ampere_last,turnable_data.out_right_motor_ampere,dt); - turnable_data.out_left_motor_ampere_limit = dynamic_current_limit(&turnable_data.out_left_motor_ampere_last ,turnable_data.out_left_motor_ampere ,dt); - - - - - setMotorWrite(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1, IQ_REF_INDEX,turnable_data.out_pitch_motor_ampere_limit); - setMotorWrite(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2, IQ_REF_INDEX,turnable_data.out_right_motor_ampere_limit); - setMotorWrite(MASTER_CANID, TURN_MOTOR_CANID, &un_sdo_output3, IQ_REF_INDEX,turnable_data.out_left_motor_ampere_limit); - - - un_can_debug_output.bit_data.speed = (uint8_t)(int8_t)(turnable_data.speed*10); - un_can_debug_output.bit_data.desired_speed = (uint8_t)(int8_t)(turnable_data.desired_speed*10); - -// un_can_debug_output.bit_data.curvature = (uint8_t)(int8_t)(diff_data.yaw_rate*10); -// un_can_debug_output.bit_data.desired_curvature = (uint8_t)(int8_t)(diff_data.desired_yaw_rate*10); - - un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(turnable_data.out_left_motor_ampere_limit * 100); - un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(turnable_data.out_right_motor_ampere_limit*100); - - - - - - publishMessage(&un_sdo_output1, 1); - publishMessage(&un_sdo_output2, 1); - publishMessage(&un_sdo_output3, 1); -} - - - - - -// 转台 -static void turnableProcess(void *signal_id) -{ - - static float previous_time1 = 0.0f; - - float time1 = (float)getCurrentTime(); - float dt = (time1 - previous_time1) / PERIOD_TICK; - previous_time1 = time1; -// if((turnable_data.current_state == POWER_WORKING))//高压上电才运行 -// { - switch(turnable_data.turnable_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据 - { - case 0: - timerStart(&turnable_data.turnable_timer, 1000, 0); // 启动定时器，1s - turnable_data.turnable_state = 1; - break; - - case 1: - if (!turnable_data.turnable_timer.active)// 1s定时 - { - turnable_data.turnable_state = 2; - } - else - { - turnable_data.turnable_state = 1; - } - break; - - case 2://模式设置 - if(turnable_data.turnable_cnt >= 5)//发送5次 - { - turnable_data.turnable_cnt = 0; - turnable_data.turnable_state = 3; - } - else - { - turnable_data.turnable_cnt ++; - turnable_data.turnable_state = 2; - - setMotorMode(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1, POSITION_MODE_CSP); - setMotorMode(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2, CURRENT_MODE); - setMotorMode(MASTER_CANID, TURN_MOTOR_CANID, &un_sdo_output3, CURRENT_MODE); - - publishMessage(&un_sdo_output1, 1); - publishMessage(&un_sdo_output2, 1); - publishMessage(&un_sdo_output3, 1); - - } - break; - -//------------------------------------------------------------------------------ - case 3: - if(turnable_data.turnable_cnt >= 5)//发送5次 - { - turnable_data.turnable_cnt = 0; - turnable_data.turnable_state = 4; - } - else - { - turnable_data.turnable_cnt ++; - turnable_data.turnable_state = 3; - - motorEnable(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1); - motorEnable(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2); - motorEnable(MASTER_CANID, TURN_MOTOR_CANID, &un_sdo_output3); - - publishMessage(&un_sdo_output1, 1); - publishMessage(&un_sdo_output2, 1); - publishMessage(&un_sdo_output3, 1); - } - break; - - case 4: - turnable_data.out_left_motor_ampere = calculatePidOutput(&turnable_speed_pid, turnable_data.desired_speed, turnable_data.speed, 0.0f, dt); - - turnable_data.turnable_cnt = 0; - turnable_data.turnable_state = 4; - - setTurnableMotorOutput();//输出函数 - break; - - default:break; - } -// } -// else -// { -// turnable_data.turnable_cnt ++; -// turnable_data.turnable_state = 0; -// } -} - - - - -void turnableParametersInit(void *signal_id) -{ - (void)signal_id; // 标记变量为已使用，避免编译器警告 - - setPidParameters(&turnable_speed_pid, - getParam("spd_kp"), - getParam("spd_ki"), - getParam("spd_kd"), - getParam("spd_il"), - getParam("spd_ol") - ); - - printf( "turnable left A %f\n",turnable_data.out_left_motor_ampere); - printf( "turnable right A %f\n",turnable_data.out_right_motor_ampere); - printf( "turnable pitch A %f\n",turnable_data.out_pitch_motor_ampere); - printf( "desired speed %f\n",turnable_data.desired_speed); - printf( "speed %f\n",turnable_data.speed); - printf( "turnable state %d\n",turnable_data.turnable_state); - - timerStart(&turnable_data.turnable_timer1,1000,1);//100ms调用一次 - - -} - - - - - - - - - - - - - -// 差速输入处理函数 -static void turnableInput(void *signal_id) -{ - if(signal_id == &power_data)//电机上电 - { - turnable_data.current_state = power_data.current_state; - } - else if(signal_id == &un_computer_turnable_Input) - { - turnable_data.desired_speed = (float)( SWAP_ENDIAN_32(un_computer_turnable_Input.bit_data.position_x) ); - - } - else{} - - turnable_data.right_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_right_intput.rx_can_data.bit_data.current_velocity),ANGULAR_VELOCITY_MIN,ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE ); - turnable_data.left_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_turn_intput.rx_can_data.bit_data.current_velocity) ,ANGULAR_VELOCITY_MIN,ANGULAR_VELOCITY_MAX, MOTOR_VELOCITY_DEADZONE ); - turnable_data.speed = (turnable_data.right_motor_speed + turnable_data.left_motor_speed)/2.0f; - - turnableProcess(signal_id);//处理映射 - timerStart(&turnable_data.turnable_timer2,100,1);//100ms调用一次 -} - - -void turnableInit() -{ - // 初始化速度 PID 控制器 - initializePid(&turnable_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); - - // 设置速度 PID 控制器的参数 - setPidParameters(&turnable_speed_pid, - getParam("spd_kp"), - getParam("spd_ki"), - getParam("spd_kd"), - getParam("spd_il"), - getParam("spd_ol") - ); - - subscribe(&un_computer_turnable_Input, turnableInput); - - timerInit(&turnable_data.turnable_timer); - timerInit(&turnable_data.turnable_timer1); - - timerInit(&turnable_data.turnable_timer2); - subscribe(&turnable_data.turnable_timer2, turnableInput); - - timerStart(&turnable_data.turnable_timer2,100,1);//100ms调用一次 - - subscribe(&turnable_data.turnable_timer1, turnableParametersInit); - timerStart(&turnable_data.turnable_timer1,1000,1);//100ms调用一次 - - turnable_data.turnable_state = 0; - un_right_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE; - un_right_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE; - un_right_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE; - - un_turn_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE; - un_turn_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE; - un_turn_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE; - - un_pitch_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE; - un_pitch_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE; - un_pitch_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE; - - printf( "turnable: initial OK %d\n",getCurrentTime()); -} - - - - - - - - - - - - - diff --git a/app_turntable.h b/app_turntable.h deleted file mode 100644 index 5a2093b0..00000000 --- a/app_turntable.h +++ /dev/null @@ -1,119 +0,0 @@ -#ifndef TURNTAABLE_H -#define TURNTAABLE_H - - -#include "app_power.h" - -#ifdef __cplusplus -extern "C" { -#endif - -#define MASTER_CANID 0xFD - -#define PITCH_MOTOR_CANID 0x7D -#define RIGHT_MOTOR_CANID 0x7E -#define TURN_MOTOR_CANID 0x7F - -#define PITCH_MOTOR_RxCANID (0x20000FD + (PITCH_MOTOR_CANID << 8)) // 0x2007DFD -#define RIGHT_MOTOR_RxCANID (0x20000FD + (RIGHT_MOTOR_CANID << 8)) // 0x2007EFD -#define TURN_MOTOR_RxCANID (0x20000FD + (TURN_MOTOR_CANID << 8)) // 0x2007FFD - -#define MOTOR_RxCAN_Mask 0x1F00FFFF //CAN Ƴλ - -#define LIMIT_SPEED_INDEX 0x7017//CSPٶ -#define LOC_REF_INDEX 0x7016//CSPλ -#define IQ_REF_INDEX 0x7006//ģʽ - -#define RUM_MODE 0x7005//modeģʽ - -#define OPERATION_MODE 0 // ˿ģʽ -#define POSITION_MODE_PP 1 // λģʽ (PP - Profile Position) -#define VELOCITY_MODE 2 // ٶģʽ -#define CURRENT_MODE 3 // ģʽ -#define POSITION_MODE_CSP 5 // λģʽ (CSP - Cyclic Synchronous Position) - - -// ĵ仯 (A) -#define MAX_STEP 1.0f // 仯 (A)5Aʼ -#define MAX_DI_DT 1000.0f // 仯 (A/s)5000A/sʼ - - -// ѧ -#define PI 3.14159f // ߾Ȧֵ - -// Ƕ̶ (ӦByte0~1: ǰǶ) -#define ANGLE_RANGE_MIN (-4.0f * PI) // СǶ: -4 -#define ANGLE_RANGE_MAX (4.0f * PI) // Ƕ: 4 - -// ٶ̶ (ӦByte2~3: ǰٶ) -#define ANGULAR_VELOCITY_MIN -15.0f // Сٶ: -15 rad/s -#define ANGULAR_VELOCITY_MAX 15.0f // ٶ: 15 rad/s - - -#define MOTOR_VELOCITY_DEADZONE 1.0f// : 120 Nm - -// ̶ (ӦByte4~5: ǰ) -#define TORQUE_MIN -120.0f // С: -120 Nm -#define TORQUE_MAX 120.0f // : 120 Nm - -#define ZERO_VAULE 0x0080 // 32768,Ҫλǰ - - - - -#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 struct TurnableData -{ - uint8_t turnable_state; - PowerState current_state; // ǰԴ״̬ - - float position_x; //ת̨λx - float position_y; //ת̨λy - float position_z; //ת̨λz - - float desired_speed; // ת̨ٶ - float desired_pitch_position; // λ - float desired_horizontal_position; // ˮƽλ - - float left_motor_speed; // ǰٶ - float right_motor_speed; // ǰҵٶ - float speed; // ǰתٶ - float pitch_position; // ǰλ - float horizontal_position; // ǰˮƽλ - - float max_speed; // ٶ - float out_left_motor_ampere; // - float out_right_motor_ampere; // ҵ - float out_pitch_motor_ampere; // ҵ - - float out_left_motor_ampere_last; // - float out_right_motor_ampere_last; // ҵ - float out_pitch_motor_ampere_last; // ҵ - - float out_left_motor_ampere_limit; // ֵ - float out_right_motor_ampere_limit; // ҵֵ - float out_pitch_motor_ampere_limit; // ҵֵ - - - Timer turnable_timer; // ʱ - Timer turnable_timer1; // ʱ - Timer turnable_timer2; // ʱ - uint8_t turnable_cnt; - - float max_ampere; // -} TurnableData; - - - -void turnableInit(); - - -#ifdef __cplusplus -} -#endif - -#endif // TURNTAABLE_H diff --git a/drive_rs04.c b/drive_rs04.c new file mode 100644 index 00000000..410221d1 --- /dev/null +++ b/drive_rs04.c @@ -0,0 +1,212 @@ +#include "interface.h" +#include "drive_rs04.h" + +// 限制值在最小值和最大值之间 +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; + } +} + + + +/** + * @brief 带死区的原始数据到物理量转换函数（简单版） + * @param raw_value 原始16位无符号整数值 [0, 65535] + * @param min 物理量最小值（如 -10.0） + * @param max 物理量最大值（如 +10.0） + * @param deadzone 死区范围（物理量单位，如 1.0 表示 ±1.0 内为死区） + * @return 转换后的物理量值（若在死区内返回0，否则返回实际值） + */ +float convertPhysical(uint16_t raw_value, float min, float max, float deadzone) +{ + // 1. 计算实际物理量值 + float physical_value = min + ((float)raw_value / 65535.0f) * (max - min); + + // 2. 判断是否在死区内（绝对值 ≤ deadzone） + if (fabs(physical_value) <= deadzone) + { + return 0.0f; // 死区内返回0 + } + else + { + return physical_value; // 死区外返回实际值 + } +} + + +/** + * @brief 将浮点数转换为uint32_t（按小端序存储） + * @param num 输入的浮点数 + * @return 转换后的uint32_t值（直接内存拷贝结果） + * @note 此函数通过内存直接拷贝实现转换，不进行数值计算，结果受平台字节序影响 + */ +uint32_t floatToUint32(float num) +{ + uint32_t result; + // 将浮点数的内存数据直接拷贝到uint32_t变量 + memcpy(&result, &num, sizeof(num)); + return result; +} + + +/** + * @brief 电机失能函数（停止电机运行） + * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255) + * @param master_id 主控制器ID (用于标识发送方) + * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据 + * @return 0: 成功, -1: 参数无效 + * @note 此函数会修改unsdodata指向的结构体内容，调用后需及时发送CAN报文 + */ +int8_t motorDisable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata) +{ + /* 参数有效性检查 */ + if (unsdodata == NULL) { + return -1; + } + + /* 设置CAN报文ID域 */ + unsdodata->tx_can_id.bits.mode = 3; /* 通信模式3：电机失能 */ + unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */ + unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */ + unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */ + + /* 清零数据域 */ + unsdodata->tx_can_data.bit_data.data = 0; + unsdodata->tx_can_data.bit_data.index = 0; + unsdodata->tx_can_data.bit_data.object_index = 0; + + return 0; +} + +/** + * @brief 电机使能函数（启动电机运行） + * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255) + * @param master_id 主控制器ID (用于标识发送方) + * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据 + * @return 0: 成功, -1: 参数无效 + * @note 通信模式4：电机使能 + */ +int8_t motorEnable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata) +{ + /* 参数有效性检查 */ + if (unsdodata == NULL) { + return -1; + } + + /* 设置CAN报文ID域 */ + unsdodata->tx_can_id.bits.mode = 3; /* 通信模式4：电机使能 */ + unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */ + unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */ + unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */ + + /* 清零数据域 */ + unsdodata->tx_can_data.bit_data.data = 0; + unsdodata->tx_can_data.bit_data.index = 0; + unsdodata->tx_can_data.bit_data.object_index = 0; + + return 0; +} + +/** + * @brief 设置电机运行模式 + * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255) + * @param master_id 主控制器ID (用于标识发送方) + * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据 + * @param mode 要设置的模式值 (具体含义需参考电机协议文档) + * @return 0: 成功, -1: 参数无效 + * @note RUM_MODE应为预定义的宏，表示运行模式索引 + */ +int8_t setMotorMode(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint8_t mode) +{ + /* 参数有效性检查 */ + if (unsdodata == NULL) { + return -1; + } + + /* 设置CAN报文ID域 */ + unsdodata->tx_can_id.bits.mode = 0x12; /* 通信模式0x12：参数写入 */ + unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */ + unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */ + unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */ + + /* 设置数据域 */ + unsdodata->tx_can_data.bit_data.index = RUM_MODE; /* 运行模式索引 */ + unsdodata->tx_can_data.bit_data.object_index = 0; /* 子索引通常为0 */ + unsdodata->tx_can_data.bit_data.data = mode; /* 模式值 */ + + return 0; +} + +/** + * @brief 写入电机参数 + * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255) + * @param master_id 主控制器ID (用于标识发送方) + * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据 + * @param index 要写入的参数索引 (具体含义需参考电机协议文档) + * @param ref 要写入的参数值 (浮点数，会自动转换为uint32_t) + * @return 0: 成功, -1: 参数无效 + * @note 使用floatToUint32函数转换浮点参数 + */ +int8_t setMotorWrite(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint16_t index, float ref) +{ + /* 参数有效性检查 */ + if (unsdodata == NULL) { + return -1; + } + + /* 设置CAN报文ID域 */ + unsdodata->tx_can_id.bits.mode = 0x12; /* 通信模式0x12：参数写入 */ + unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */ + unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */ + unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */ + + /* 设置数据域 */ + unsdodata->tx_can_data.bit_data.index = index; /* 参数索引 */ + unsdodata->tx_can_data.bit_data.object_index = 0; /* 子索引通常为0 */ + unsdodata->tx_can_data.bit_data.data = floatToUint32(ref); /* 转换并写入参数值 */ + + return 0; +} + + +/** + * @brief 动态斜率限制（支持变时间间隔） + * @param last_command 上一次的电流指令值 + * @param target_current 本次目标电流指令 + * @param delta_time 距离上一次调用的时间间隔 (s) + * @return 限制后的安全电流指令 + */ +float dynamic_current_limit(float *last_command, float target_current, float delta_time) +{ + // 计算期望的变化量 + float desired_change = target_current - *last_command; + + // 计算两种限制 + float step_limit = MAX_STEP; + float time_limit = MAX_DI_DT * delta_time; // 动态计算时间限制 + + // 选择更严格的限制 + float max_allowed_change = (step_limit < time_limit) ? step_limit : time_limit; + + // 应用限制并返回新指令 + float actual_change = constrain(desired_change, -max_allowed_change, max_allowed_change); + *last_command = *last_command + actual_change;//更新过去值 + + return *last_command; +} + + + + + diff --git a/drive_rs04.h b/drive_rs04.h new file mode 100644 index 00000000..7d851a38 --- /dev/null +++ b/drive_rs04.h @@ -0,0 +1,69 @@ +#ifndef _DRIVE_RS04_H_ +#define _DRIVE_RS04_H_ + + +#define MASTER_CANID 0xFD + +#define PI 3.1415926 + +#define MOTOR_RxCAN_Mask 0x1F00FFFF //CAN Ƴλ + +// ĵ仯 (A) +#define MAX_STEP 1.0f // 仯 (A)5Aʼ +#define MAX_DI_DT 1000.0f // 仯 (A/s)5000A/sʼ + + +#define LIMIT_SPEED_INDEX 0x7017//CSPٶ +#define LOC_REF_INDEX 0x7016//CSPλ +#define IQ_REF_INDEX 0x7006//ģʽ +#define SPD_REF 0x700A//ٶģʽ ٶֵ + +#define LIMIT_CUR 0X7018//ٶλģʽ + + + +#define RUM_MODE 0x7005//modeģʽ + +#define OPERATION_MODE 0 // ˿ģʽ +#define POSITION_MODE_PP 1 // λģʽ (PP - Profile Position) +#define VELOCITY_MODE 2 // ٶģʽ +#define CURRENT_MODE 3 // ģʽ +#define POSITION_MODE_CSP 5 // λģʽ (CSP - Cyclic Synchronous Position) + +// Ƕ̶ (ӦByte0~1: ǰǶ) +#define ANGLE_RANGE_MIN (-4.0f * PI) // СǶ: -4 +#define ANGLE_RANGE_MAX (4.0f * PI) // Ƕ: 4 + +// ٶ̶ (ӦByte2~3: ǰٶ) +#define RS02_ANGULAR_VELOCITY_MAX 20.0f // RS02ͺٶ: 20 rad/s +#define RS04_ANGULAR_VELOCITY_MAX 15.0f // RS04ͺٶ: 15 rad/s + + + + + +#define MOTOR_VELOCITY_DEADZONE 0.1f// +#define MOTOR_ANGLE_DEADZONE 0.01f// + +#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)) + + + +float constrain(float value, float min_val, float max_val); +float convertPhysical(uint16_t raw_value, float min, float max, float deadzone); +uint32_t floatToUint32(float num); +int8_t motorDisable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata); +int8_t motorEnable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata); +int8_t setMotorMode(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint8_t mode); +int8_t setMotorWrite(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint16_t index, float ref); +float dynamic_current_limit(float *last_command, float target_current, float delta_time); + + + + +#ifdef __cplusplus +} +#endif + +#endif // _DRIVE_RS04_H_ \ No newline at end of file diff --git a/interface.c b/interface.c index 6ff4f7f2..25a0a754 100644 --- a/interface.c +++ b/interface.c @@ -2,10 +2,10 @@ #include "interface_config.h" -UnMotorInput un_motor_input1 ;//电机控制器1 左前侧 -UnMotorInput un_motor_input2 ;//电机控制器2 右前侧 -UnMotorInput un_motor_input3 ;//电机控制器1 左后侧 -UnMotorInput un_motor_input4 ;//电机控制器2 右后侧 +StrRxCanFrame un_motor_input1 ;//电机控制器1 左前侧 +StrRxCanFrame un_motor_input2 ;//电机控制器2 右前侧 +StrRxCanFrame un_motor_input3 ;//电机控制器1 左后侧 +StrRxCanFrame un_motor_input4 ;//电机控制器2 右后侧 UnMotorTempInput un_motor_temp1 ;//电机控制器1 左前侧 @@ -18,10 +18,16 @@ UnMotorTempInput un_motor_temp4 ;//电机控制器4 UnBmsInput un_bms_input ;//BMS接收数据 UnTempModuleInput un_temp_module_input ;//温度采集模块 -UnMotorOutput un_motor_output1 ;//电机输出 -UnMotorOutput un_motor_output2 ;//电机输出 -UnMotorOutput un_motor_output3 ;//电机输出 -UnMotorOutput un_motor_output4 ;//电机输出 +StrTxCanFrame un_motor_output1 ;//电机输出 +StrTxCanFrame un_motor_output2 ;//电机输出 +StrTxCanFrame un_motor_output3 ;//电机输出 +StrTxCanFrame un_motor_output4 ;//电机输出 + +StrTxCanFrame un_motor_output5 ;//电机输出 +StrTxCanFrame un_motor_output6 ;//电机输出 +StrTxCanFrame un_motor_output7 ;//电机输出 +StrTxCanFrame un_motor_output8 ;//电机输出 + UnInfCanKGFOutput un_inf_can_kgf_output1 ;//kgf输出 UnInfCanKGFOutput un_inf_can_kgf_output2 ; @@ -41,6 +47,21 @@ UnUltrasonicInput un_ultrasonic_input1 ;//超声波传感 UnUltrasonicOutput un_ultrasonic_output1 ;//超声波传感器输出 +StrTxCanFrame un_sdo_output1 ;//电机1输出 +StrTxCanFrame un_sdo_output2 ;//电机2输出 +StrTxCanFrame un_sdo_output3 ;//电机3输出 +StrTxCanFrame un_sdo_output4 ;//电机4输出 +StrTxCanFrame un_sdo_output5 ;//电机5输出 + + + + +StrRxCanFrame un_pitch_intput ;//电机输入 +StrRxCanFrame un_right_intput ;//电机输入 + + + + //IO口 UnSwSample un_sw_sample ;//采集 @@ -51,6 +72,12 @@ UnManualComputerInput un_manual_computer_input ;//自主计算机 UnRequestFrame un_request_frame ;//请求帧 UnComputerOutput un_computer_output ;//输出给自主计算机 + +UnComputerTurnableInput un_computer_turnable_Input ;//转台以太网输入 + +UnEncoderData un_encoder_data_input ; + +StrMagneticEncoder str_magnetic_encoder ;//编码器请求 //输出给上位机 UnVehicleInfoOutput un_vehicle_Info_output ;// 车辆信息，输出给上位机 diff --git a/interface.h b/interface.h index d9e18a00..6a9b64db 100644 --- a/interface.h +++ b/interface.h @@ -59,14 +59,6 @@ typedef union _UnCanDebugOutput } UnCanDebugOutput; - - - - - - - - typedef struct _StrMotorTempInput { //-----接收数据0x103或者0x104---------------------------------------------- @@ -86,31 +78,15 @@ typedef union _UnMotorTempInput uint8_t arr[sizeof(StrMotorInput)]; // 通过结构体类型确定大小 } UnMotorTempInput; - - - - - - - - - - // 接收BMS输入 typedef struct _StrBmsInput { -//----接收0x100---------------------------------- - // 多字节数据，高位在前，低位在后 - unsigned int bus_voltage : 16; // 母线电压 单位为10mV - unsigned int bus_current : 16; // 母线电流 单位为10mA - unsigned int remainder_capacity : 16; // 剩余容量 单位为10mAh 充电为正，放电为负 - unsigned int crc1 : 16; //crc -//----接收0x101---------------------------------- - // 多字节数据，高位在前，低位在后 - unsigned int full_capacity : 16; // 充满容量 单位为10mAh - unsigned int Discharge_times : 16; // 放电循环次数 单位为1次 - unsigned int soc : 16; // soc 1% - unsigned int crc2 : 16; //crc + //-----接收BMS数据---------------------------------------------- + uint16_t bus_voltage; // 总电压 系数 0.1V/bit + uint16_t bus_current; // 电流 系数 0.1A/bit 偏移量 -30000 + uint16_t soc; // SOC 系数 0.1%/bit + uint8_t Life; // 生命周期 范围 0~255 + uint8_t Reserve; // 保留位 } StrBmsInput; typedef union _UnBmsInput @@ -169,9 +145,6 @@ typedef union _UnAutoComputerInput } UnAutoComputerInput; - - - // 接收自主计算机手动输入 typedef struct _StrManualComputerInput @@ -193,14 +166,6 @@ typedef union _UnManualComputerInput unsigned int arr[sizeof(StrManualComputerInput) / sizeof(unsigned int)]; // 通过结构体类型确定大小 } UnManualComputerInput; - - - - - - - - // 接收请求帧 typedef struct _StrRequestFrame { @@ -236,6 +201,11 @@ 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 @@ -244,6 +214,14 @@ typedef union _UnRemoteControlInput uint8_t arr[sizeof(StrRemoteControlInput)]; // 通过结构体类型确定大小 } UnRemoteControlInput; +// 磁编 +typedef struct _StrMagneticEncoder +{ + uint8_t ip[4]; // IPv4 + uint16_t port; // 端口 + uint8_t magnetic_data; // 协议类型（1A） +} StrMagneticEncoder; + //-----IO口--------------------------------------------------------------- // 从IO口输入 @@ -664,6 +642,103 @@ typedef union _UnrLifterOutput } UnLifterOutput; +// CAN ID 解析（联合体形式，支持位域和32位直接访问） +typedef union _UnCanIdInfo +{ + 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; + +} UnCanIdInfo; + + +// 输出can数据 +typedef struct _StrTxCanOutput +{ + uint16_t index; // 索引（类似寄存器地址） + uint16_t object_index; // 子索引（通常为0x0000） + uint32_t data; // 数据字段 +} StrTxCanOutput; + + +// 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 解析联合体 (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; + + + + + + //-----以太网------------------------------------------------------------- // 输出给自主计算机 typedef struct _StrComputerOutput @@ -895,6 +970,57 @@ typedef union _UnAutoControlOutput } UnAutoControlOutput; +// 接收转台指令输入 +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; + + + +// 编码器数据帧结构体（针对0x1A命令的响应） +typedef struct _StrEncoderData { + //-------------------------------------------------- + // 编码器数据帧 - 对应 0x1A 命令的响应格式（10字节） + unsigned int cf : 8; // 命令识别符 固定值0x1A + unsigned int sf : 8; // 编码器状态 1byte + unsigned int abs_value : 24; // 角度数据 3byte LSB（重命名：abs_value） + unsigned int enid : 8; // 编码器ID 1byte + unsigned int abm : 24; // 圈数数据 3byte LSB + unsigned int almc : 8; // 编码器故障 1byte + unsigned int crc : 8; // CRC校验 多项式x^8+1 +} StrEncoderData; + +// 联合体定义，便于字节数组访问 +typedef union _UnEncoderData { + StrEncoderData bit_data; // 位域方式访问 + unsigned char arr[sizeof(StrEncoderData)]; // 字节数组方式访问 +} UnEncoderData; + + + + + + + + @@ -914,10 +1040,11 @@ typedef union _UnVoltageSignalOutput //外部数据结构声明 -extern UnMotorInput un_motor_input1 ;//电机控制器1 左侧 -extern UnMotorInput un_motor_input2 ;//电机控制器2 右侧 -extern UnMotorInput un_motor_input3 ;//电机控制器1 左后侧 -extern UnMotorInput un_motor_input4 ;//电机控制器2 右后侧 +extern StrRxCanFrame un_motor_input1 ;//电机控制器1 左侧 +extern StrRxCanFrame un_motor_input2 ;//电机控制器2 右侧 +extern StrRxCanFrame un_motor_input3 ;//电机控制器1 左后侧 +extern StrRxCanFrame un_motor_input4 ;//电机控制器2 右后侧 + extern UnBmsInput un_bms_input ;//BMS接收数据 extern UnTempModuleInput un_temp_module_input;//温度采集模块 extern UnAutoComputerInput un_auto_computer_input;//自主计算机自动数据 @@ -929,10 +1056,18 @@ extern UnUltrasonicOutput un_ultrasonic_output1;//超声波传感器输出 -extern UnMotorOutput un_motor_output1; //电机输出 -extern UnMotorOutput un_motor_output2; //电机输出 -extern UnMotorOutput un_motor_output3; //电机输出 -extern UnMotorOutput un_motor_output4; //电机输出 +extern StrTxCanFrame un_motor_output1; //电机输出 +extern StrTxCanFrame un_motor_output2; //电机输出 +extern StrTxCanFrame un_motor_output3; //电机输出 +extern StrTxCanFrame un_motor_output4; //电机输出 + + +extern StrTxCanFrame un_motor_output5 ;//电机输出 +extern StrTxCanFrame un_motor_output6 ;//电机输出 +extern StrTxCanFrame un_motor_output7 ;//电机输出 +extern StrTxCanFrame un_motor_output8 ;//电机输出 + + extern UnMotorTempInput un_motor_temp1; //电机控制器1 温度 extern UnMotorTempInput un_motor_temp2; //电机控制器2 温度 extern UnMotorTempInput un_motor_temp3; //电机控制器3 左后侧 @@ -958,6 +1093,11 @@ extern UnSwSample un_sw_sample; extern UnRequestFrame un_request_frame; //请求帧 extern UnComputerOutput un_computer_output; //输出给自主计算机 +extern UnComputerTurnableInput un_computer_turnable_Input ;//转台以太网输入 + +extern UnEncoderData un_encoder_data_input ; + +extern StrMagneticEncoder str_magnetic_encoder ;//编码器请求 //输出给上位机 extern UnVehicleInfoOutput un_vehicle_Info_output; // 车辆信息，输出给上位机 @@ -969,6 +1109,20 @@ extern UnManualControlOutput un_manual_control_output;// 手动控制数 extern UnAutoControlOutput un_auto_control_output; // 自动控制数据输出，返回给请求者 extern UnSdoOutput un_sdo_output ;//转向电机输出 +extern StrTxCanFrame un_sdo_output1 ;//电机1输出 +extern StrTxCanFrame un_sdo_output2 ;//电机2输出 +extern StrTxCanFrame un_sdo_output3 ;//电机3输出 +extern StrTxCanFrame un_sdo_output4 ;//电机4输出 +extern StrTxCanFrame un_sdo_output5 ;//电机5输出 + +extern StrTxCanFrame un_motor_output5 ;//电机1输出 +extern StrTxCanFrame un_motor_output6 ;//电机2输出 +extern StrTxCanFrame un_motor_output7 ;//电机3输出 +extern StrTxCanFrame un_motor_output8 ;//电机4输出 + +extern StrRxCanFrame un_pitch_intput ;//电机输入 +extern StrRxCanFrame un_right_intput ;//电机输入 + //变量 extern uint8_t test_app[26]; diff --git a/interface_boot.c b/interface_boot.c index 4bbf0f27..59b5f4ef 100644 --- a/interface_boot.c +++ b/interface_boot.c @@ -80,7 +80,7 @@ void bootmian(void *signal_id) } } - timerStart(&boot_timer_interface, 100,0); + timerStart(&boot_timer_interface, 100,1); // printf("bootAPP spend time:%d\n",getCurrentTime() - time_boot);//app˶೤ʱ } @@ -110,7 +110,7 @@ void bootInterfaceInit(void) // Ķʱźţڶʱɼ subscribe(&boot_timer_interface, bootmian); - timerStart(&boot_timer_interface, 100,0); //100ms + timerStart(&boot_timer_interface, 100,1); //100ms feedWatchdog();//ι,ʼιһ diff --git a/interface_can.c b/interface_can.c index 39b82fc0..08029fdd 100644 --- a/interface_can.c +++ b/interface_can.c @@ -3,14 +3,16 @@ #include "debug.h" #include "interface_config.h" #include "app/app_config.h" -#define TX_PADDING_VAL (0xA5) #include "sdrv_vic.h" #include "intrinsics.h" #include "app/app_differential_drive.h" #include "app/app_param_manage.h" #include "app/app_ultrasonic.h" +#include "app/app_turntable.h" +#include "drive_rs04.h" +#define TX_PADDING_VAL (0xA5) uint32_t OTA_CANTxID = 0x02;//ĬϷIDΪ0x02 uint32_t OTA_CANRxID = 0x01;//ĬϽIDΪ0x01 @@ -296,45 +298,15 @@ void flexcan_Receive_callback_1(flexcan_handle_t *handle, break; case FLEXCAN_RX_FIFO_IDLE: - if(LEFT_FRONT_MOTOR_INPUT1 == (buf->id)) - { - can_fault_info.bit_data.motor1_count ++; - for(i = 0; i < (buf->length); i++) - { - un_motor_input1.arr[i] = buf->dataBuffer[i]; - } - //ź - publishMessage(&un_motor_input1, 1); - } - else if( LEFT_FRONT_MOTOR_INPUT2 == (buf->id) ) + if( FRONT_LEFT_MOTOR_RxCANID == ( (buf->id) & MOTOR_RxCAN_Mask) ) { + un_motor_input1.rx_can_id.raw = (buf->id); for(i = 0; i < (buf->length); i++) { - un_motor_temp1.arr[i] = buf->dataBuffer[i]; - } - //ź - publishMessage(&un_motor_temp1, 1);//ź - } - else if( LEFT_REAR_MOTOR2_INPUT1 == (buf->id) )// - { - can_fault_info.bit_data.motor3_count ++; - for(i = 0; i < (buf->length); i++) - { - un_motor_input3.arr[i] = buf->dataBuffer[i]; - } - //ź -// publishMessage(&un_motor_input3, 1); //޸Ϊ - } - else if( LEFT_REAR_MOTOR2_INPUT2 == (buf->id) ) - { - for(i = 0; i < (buf->length); i++) - { - un_motor_temp3.arr[i] = buf->dataBuffer[i]; - } - //ź - publishMessage(&un_motor_temp3, 1);//ź - } - else{} + un_motor_input1.rx_can_data.arr[i] = buf->dataBuffer[i]; + } + + } break; case FLEXCAN_TX_IDLE: @@ -380,45 +352,16 @@ void flexcan_Receive_callback_2(flexcan_handle_t *handle, break; case FLEXCAN_RX_FIFO_IDLE: - if(RIGHT_FRONT_MOTOR_INPUT1 == (buf->id)) - { - can_fault_info.bit_data.motor2_count ++; - for(i = 0; i < (buf->length); i++) - { - un_motor_input2.arr[i] = buf->dataBuffer[i]; - } - //ź - publishMessage(&un_motor_input2, 1); - } - else if( RIGHT_FRONT_MOTOR_INPUT2 == (buf->id) ) + if( FRONT_RIGHT_MOTOR_RxCANID == ( (buf->id) & MOTOR_RxCAN_Mask) ) { + un_motor_input2.rx_can_id.raw = (buf->id); for(i = 0; i < (buf->length); i++) { - un_motor_temp2.arr[i] = buf->dataBuffer[i]; - } - //ź - publishMessage(&un_motor_temp2, 1);//ź - } - else if( RIGHT_REAR_MOTOR_INPUT1 == (buf->id) )// - { - can_fault_info.bit_data.motor4_count ++; - for(i = 0; i < (buf->length); i++) - { - un_motor_input4.arr[i] = buf->dataBuffer[i]; - } - //ź - publishMessage(&un_motor_input4, 1); - } - else if( RIGHT_REAR_MOTOR_INPUT2 == (buf->id) ) - { - for(i = 0; i < (buf->length); i++) - { - un_motor_temp4.arr[i] = buf->dataBuffer[i]; - } - //ź - publishMessage(&un_motor_temp4, 1);//ź - } - else{} + un_motor_input2.rx_can_data.arr[i] = buf->dataBuffer[i]; + } + + } + else{} break; case FLEXCAN_TX_IDLE: @@ -464,26 +407,17 @@ void flexcan_Receive_callback_3(flexcan_handle_t *handle, break; case FLEXCAN_RX_FIFO_IDLE: - - if(BMS_INPUT_ID1 == (buf->id))//BMS - { - can_fault_info.bit_data.bms_count ++; + + if( REAR_LEFT_MOTOR_RxCANID == ( (buf->id) & MOTOR_RxCAN_Mask) ) + { + un_motor_input3.rx_can_id.raw = (buf->id); for(i = 0; i < (buf->length); i++) { - un_bms_input.arr[i] = buf->dataBuffer[i]; - } - publishMessage(&un_bms_input, 1); - } - else if(BMS_INPUT_ID2 == (buf->id))//BMS - { - for(i = 0; i < (buf->length); i++) - { - un_bms_input.arr[i+8] = buf->dataBuffer[i]; - } - //ź - publishMessage(&un_bms_input, 1); - } - else{} + un_motor_input3.rx_can_data.arr[i] = buf->dataBuffer[i]; + } + + } + else{} break; @@ -520,13 +454,25 @@ void flexcan_Receive_callback_4(flexcan_handle_t *handle, void *userData) { //-------------------------------------------------------------- + flexcan_frame_t *buf = (flexcan_frame_t *)userData; + uint8_t i = 0; + switch (status) { case FLEXCAN_RX_IDLE: break; case FLEXCAN_RX_FIFO_IDLE: - + if( REAR_RIGHT_MOTOR_RxCANID == ( (buf->id) & MOTOR_RxCAN_Mask) ) + { + un_motor_input4.rx_can_id.raw = (buf->id); + for(i = 0; i < (buf->length); i++) + { + un_motor_input4.rx_can_data.arr[i] = buf->dataBuffer[i]; + } + } + else{} + break; case FLEXCAN_TX_IDLE: @@ -561,6 +507,8 @@ 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) { @@ -568,7 +516,12 @@ void flexcan_Receive_callback_5(flexcan_handle_t *handle, break; case FLEXCAN_RX_FIFO_IDLE: - + for(i = 0; i < (buf->length); i++) + { + un_bms_input.arr[i] = buf->dataBuffer[i]; + } + //ź + publishMessage(&un_bms_input, 1); break; case FLEXCAN_TX_IDLE: @@ -633,6 +586,24 @@ void flexcan_Receive_callback_6(flexcan_handle_t *handle, } publishMessage(&un_ultrasonic_input1, 1); } + 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]; + } +// publishMessage(&un_right_intput, 1); + } + else 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]; + } +// publishMessage(&un_pitch_intput, 1); + } else{} break; @@ -689,9 +660,14 @@ void flexcan_Receive_callback_7(flexcan_handle_t *handle, } //ź publishMessage(&un_remote_control_input, 1); - -// 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 { } @@ -898,11 +874,11 @@ void initialization_All_Flexcan(void) { sdrv_gpio_set_pin_output_level(GPIO_H12, 0); //CANʹ - initialization_Flexcan(CAN_INDEX_0, &can_0_config, &can_0_fifo_config, flexcan_Receive_callback_0);//250k + initialization_Flexcan(CAN_INDEX_0, &can_0_config, &can_0_fifo_config, flexcan_Receive_callback_0);//1M - initialization_Flexcan(CAN_INDEX_1, &can_1_config, &can_1_fifo_config, flexcan_Receive_callback_1);//250k + initialization_Flexcan(CAN_INDEX_1, &can_1_config, &can_1_fifo_config, flexcan_Receive_callback_1);//1M - initialization_Flexcan(CAN_INDEX_2, &can_2_config, &can_2_fifo_config, flexcan_Receive_callback_2);//250k + initialization_Flexcan(CAN_INDEX_2, &can_2_config, &can_2_fifo_config, flexcan_Receive_callback_2);//1M initialization_Flexcan(CAN_INDEX_3, &can_3_config, &can_3_fifo_config, flexcan_Receive_callback_3);//1M @@ -1041,85 +1017,112 @@ void canTimerProcess(void *signal_id) } } - -//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Ϊʹã -// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output4, 8, 18); -//} -// -//static void processSdoOutput5(void *signal_id) -//{ -// (void)signal_id; // DZΪʹã -// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output5, 8, 19); -//} -// -// -//static void processSdoOutput7(void *signal_id) -//{ -// (void)signal_id; // DZΪʹã -// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output7, 8, 20); -//} - - - static void processMotorOutput1(void *signal_id) { (void)signal_id; // DZΪʹã - CAN_Send_Msg(&can_handle_1, LEFT_FRONT_MOTOR_OUTPUT1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1, 8, 15);//1Ťغת - CAN_Send_Msg(&can_handle_1, LEFT_REAR_MOTOR_OUTPUT1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output3, 8, 16);//1Ťغת + CAN_Send_Msg(&can_handle_1, un_motor_output1.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1.tx_can_data.arr[0], 8, 15);//1Ťغת + CAN_Send_Msg(&can_handle_3, un_motor_output3.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output3.tx_can_data.arr[0], 8, 16);//1Ťغת } static void processMotorOutput2(void *signal_id) { (void)signal_id; // DZΪʹã - CAN_Send_Msg(&can_handle_2, RIGHT_FRONT_MOTOR_OUTPUT1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2, 8, 15);//2Ťغת - CAN_Send_Msg(&can_handle_2, RIGHT_REAR_MOTOR_OUTPUT1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output4, 8, 16);//2Ťغת + CAN_Send_Msg(&can_handle_2, un_motor_output2.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2.tx_can_data.arr[0], 8, 15);//2Ťغת + CAN_Send_Msg(&can_handle_4, un_motor_output4.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output4.tx_can_data.arr[0], 8, 16);//2Ťغת } -static void processMotorOutput3(void *signal_id) + +static void processSdoOutput1(void *signal_id) { - (void)signal_id; // DZΪʹã - - CAN_Send_Msg(&can_handle_1, LEFT_FRONT_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1.arr[8], 8, 17);//1 - CAN_Send_Msg(&can_handle_1, LEFT_REAR_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output3.arr[8], 8, 18);//1 - + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_6, un_sdo_output1.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output1.tx_can_data.arr[0], 8, 15);//1Ťغת + CAN_Send_Msg(&can_handle_6, un_sdo_output4.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output4.tx_can_data.arr[0], 8, 16);//1Ťغת } -static void processMotorOutput4(void *signal_id) + + +static void processmotorOutput5(void *signal_id) { - (void)signal_id; // DZΪʹã - CAN_Send_Msg(&can_handle_2, RIGHT_FRONT_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2.arr[8], 8, 17);//2 - CAN_Send_Msg(&can_handle_2, RIGHT_REAR_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output4.arr[8], 8, 18);//2 + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_6, un_sdo_output5.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output5.tx_can_data.arr[0], 8, 19);//2Ťغת } +static void processSdoOutput2(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_6, un_sdo_output2.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output2.tx_can_data.arr[0], 8, 17);//2Ťغת +} + +static void processSdoOutput3(void *signal_id) +{ + (void)signal_id; // DZΪʹã + + switch(un_sdo_output3.tx_can_id.raw) + { + case RIGHT_MOTOR_CANID: + CAN_Send_Msg(&can_handle_6, un_sdo_output3.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output3.tx_can_data.arr[0], 8, 18);//2Ťغת + break; + + case PITCH_MOTOR_CANID: + CAN_Send_Msg(&can_handle_6, un_sdo_output3.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output3.tx_can_data.arr[0], 8, 18);//2Ťغת + break; + default:; + } +} + +static void processSdoOutput5(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_1, un_motor_output5.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output5.tx_can_data.arr[0], 8, 17);//2Ťغת +} + +static void processSdoOutput6(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_2, un_motor_output6.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output6.tx_can_data.arr[0], 8, 17);//2Ťغת +} + + +static void processSdoOutput7(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_3, un_motor_output7.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output7.tx_can_data.arr[0], 8, 17);//2Ťغת +} + +static void processSdoOutput8(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_4, un_motor_output8.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output8.tx_can_data.arr[0], 8, 17);//2Ťغת +} + + + + +//static void processMotorOutput3(void *signal_id) +//{ +// (void)signal_id; // DZΪʹã +// +//// CAN_Send_Msg(&can_handle_1, LEFT_FRONT_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1.arr[8], 8, 17);//1 +//// CAN_Send_Msg(&can_handle_1, LEFT_REAR_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output3.arr[8], 8, 18);//1 +// +//} +// +//static void processMotorOutput4(void *signal_id) +//{ +// (void)signal_id; // DZΪʹã +//// CAN_Send_Msg(&can_handle_2, RIGHT_FRONT_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2.arr[8], 8, 17);//2 +//// CAN_Send_Msg(&can_handle_2, RIGHT_REAR_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output4.arr[8], 8, 18);//2 +//} + static void processKgfOutput1(void *signal_id) { (void)signal_id; // DZΪʹã - if(1 == ecu_online)//20250318 ޸Ϊ߲ܷͣɼģE3ͬʱ - { +// if(1 == ecu_online)//20250318 ޸Ϊ߲ܷͣɼģE3ͬʱ +// { un_inf_can_kgf_output1.bit_data.can_rx5 = 0xE3; CAN_Send_Msg(&can_handle_0, 0x11000002, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_inf_can_kgf_output1, 8, 15);//KGF1 - } +// } } static void processKgfOutput2(void *signal_id) @@ -1133,16 +1136,6 @@ static void processKgfOutput2(void *signal_id) } -//static void processKgfOutput2(void *signal_id) -//{ -// (void)signal_id; // DZΪʹã -// if(1 == ecu_online)//20250318 ޸Ϊ߲ܷͣɼģE3ͬʱ -// { -// un_inf_can_kgf_output2.bit_data.can_rx5 = 0xE3; -// CAN_Send_Msg(&can_handle_0, 0x14000002, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_inf_can_kgf_output2, 8, 16);//KGF2 -// } -// -//} @@ -1161,7 +1154,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_4, 0x98, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_wheel_wpeed_output, 8, 15);// + CAN_Send_Msg(&can_handle_7, 0x98, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_wheel_wpeed_output, 8, 15);// } static void processHBridgeOutput(void *signal_id) @@ -1205,11 +1198,11 @@ static void processUltrasonicOutput(void *signal_id) void canSendAll(void *signal_id) { static uint16_t wheel_speed_cnt = 0; -// static uint8_t kgf_cnt = 0; + static uint16_t kgf_cnt = 0; static uint16_t bms_cnt1 = 0; - static uint16_t bms_cnt2 = 0; +// static uint16_t bms_cnt2 = 0; // static uint16_t motor_speed_cnt = 0; - static uint16_t motor_power_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}; //------------------------------------------------------------------------- @@ -1222,27 +1215,18 @@ void canSendAll(void *signal_id) CanData[0] = 0x5A; - CAN_Send_Msg(&can_handle_3, 0x100, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 15);//BMS + CAN_Send_Msg(&can_handle_5, BMS_REQUEST_ID, 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 - } - motor_power_cnt ++; - if(motor_power_cnt >= 1000)//1sһ - { - motor_power_cnt = 0; - processMotorOutput3(CanData); - processMotorOutput4(CanData); - } +// motor_power_cnt ++; +// if(motor_power_cnt >= 1000)//1sһ +// { +// motor_power_cnt = 0; +// processMotorOutput3(CanData); +// processMotorOutput4(CanData); +// } // motor_power_cnt ++; // if(motor_power_cnt >= 10)//ȡת 20ms @@ -1258,14 +1242,14 @@ void canSendAll(void *signal_id) // processMotorOutput4(CanData); // } -// kgf_cnt ++; -// if(kgf_cnt >= 5)//20210312 CANͨѶģ 20210710޸Ϊ50ms -// { -// kgf_cnt = 0; -// -// processKgfOutput1(CanData); + kgf_cnt ++; + if(kgf_cnt >= 1000)//20210312 CANͨѶģ 20210710޸Ϊ50ms + { + kgf_cnt = 0; + + processKgfOutput1(CanData); // processKgfOutput2(CanData); -// } + } // //ٷ wheel_speed_cnt ++; @@ -1275,14 +1259,14 @@ void canSendAll(void *signal_id) processWheelSpeedOutput(CanData); - CAN_Send_Msg(&can_handle_0, 0x120, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS - CAN_Send_Msg(&can_handle_1, 0x121, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS - CAN_Send_Msg(&can_handle_2, 0x122, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS - 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, 16);//BMS - CAN_Send_Msg(&can_handle_7, 0x127, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS +// CAN_Send_Msg(&can_handle_0, 0x120, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS +// CAN_Send_Msg(&can_handle_1, 0x121, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS +// CAN_Send_Msg(&can_handle_2, 0x122, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS +// 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, 16);//BMS +// CAN_Send_Msg(&can_handle_7, 0x127, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS } // @@ -1394,6 +1378,19 @@ void canInterfaceInit(void) subscribe(&un_motor_output1, processMotorOutput1); subscribe(&un_motor_output2, processMotorOutput2); + subscribe(&un_sdo_output1, processSdoOutput1); + subscribe(&un_sdo_output2, processSdoOutput2); + subscribe(&un_sdo_output3, processSdoOutput3); + subscribe(&un_sdo_output5, processmotorOutput5); + + subscribe(&un_motor_output5, processSdoOutput5); + subscribe(&un_motor_output6, processSdoOutput6); + subscribe(&un_motor_output7, processSdoOutput7); + subscribe(&un_motor_output8, processSdoOutput8); + + + + subscribe(&un_inf_can_kgf_output1, processKgfOutput1); subscribe(&un_inf_can_kgf_output2, processKgfOutput2); subscribe(&un_wheel_wpeed_output, processWheelSpeedOutput); @@ -1404,8 +1401,8 @@ void canInterfaceInit(void) - - + + diff --git a/interface_can.h b/interface_can.h index 4e5c237d..98169055 100644 --- a/interface_can.h +++ b/interface_can.h @@ -66,9 +66,11 @@ -#define BMS_INPUT_ID1 0x100 -#define BMS_INPUT_ID2 0x101 +#define BMS_INPUT_ID1 0x4028001 +#define BMS_REQUEST_ID 0x400FF80 + #define REMOTE_ID 0x12000023 +#define REMOTE_ID_1 0x12000024 #define TEMP_MODULE_INPUT_ID_1 0x15000003 diff --git a/interface_ethernet.c b/interface_ethernet.c index 76df6e1c..a1373d53 100644 --- a/interface_ethernet.c +++ b/interface_ethernet.c @@ -1,660 +1,683 @@ -#include -#include -#include "debug.h" -#include -#include -#include "interface_ethernet.h" -#include "eth_cfg.h" -#include "lwip/timeouts.h" -#include "sdrv_eth.h" -#include "app/app_param_manage.h" -#include "app/app_differential_drive.h" - - -// -#define MAX_BUFFER_SIZE (sizeof(((RequestContext *)0)->param_request->arr)) -#define MAX_PACKET_SIZE 1024 // СΪ1K - -Timer ethernet_timer_interface; -Timer ethernet_timer_interface1; - -struct udp_pcb *udpcb_1; -struct udp_pcb *udpcb_2; -struct udp_pcb *udpcb_3; -struct udp_pcb *udpcb_4; -struct udp_pcb *udpcb_5; - -bool eth_pool_rx = false; - - -UnEthernetFault ethernet_fault_Info = { - .bit_data.auto_count = 0, - .bit_data.manual_count = 0, - .bit_data.auto_state = 0, - .bit_data.manual_state = 0, -}; - - - -StrEthernetParameter ethernet_parameter = { - .local_ip = {192,168,17,20}, - .mask = {255,255,255,0}, - .computer_ip = {192,168,17,3}, - .local_communication_port = 8011, - .Local_upper_port = 8000, - .Local_download_port = 7811, - .upper_ip = {192,168,17,3}, - .download_ip = {192,168,17,183}, - .target_upper_port = 8000, - .target_download_port = 7811, - .target_communication_port = 8011, -}; - - - -static bool g_eth1_rx_pkt; - - - -static void eth_dma_rx_int_cb(struct net_driver_s *dev) -{ - if (dev == &g_eth1_dev) - g_eth1_rx_pkt = true; - -// printf("eth_dma_rx_int_cb\n"); -} - - - - -static void lwip_Ethernet(void *signal_id) -{ -// uint32_t time_boot = getCurrentTime();//¼ǰʱ - (void)signal_id; // DZΪʹã - // ETH֡ݸLWIPЭջ - // ʹܽжϣΪISRdisableжϣˣ - // жϽձ־(g_eth1_rx_pkt)Ĵв - // µĽжϱӦԣﲻԭ⡣ - if ((!eth_pool_rx && g_eth1_rx_pkt) || eth_pool_rx) - { - g_eth1_rx_pkt = false; - dwc_eth_rx(&g_eth1_dev, eth_pool_rx); - } - - // LWIPЭջӿڣLWIPʱ - sys_check_timeouts(); - - timerStart(ðernet_timer_interface1, 10,1); //10msһ - - // printf("lwipInterface spend time:%d\n",getCurrentTime() - time_boot);//app˶೤ʱ -} - - -//err_t -//udp_bind(struct udp_pcb *pcb, const ip_addr_t *ipaddr, u16_t port) - -int8_t UdpSendToData(uint8_t udpcbID, uint8_t *buf, uint16_t len, uint8_t *sip, uint16_t port) -{ - struct pbuf *DesPuff = NULL; - ip4_addr_t Desaddr; - int8_t SendState = 0; -//---------------------------------------------------------------------------------------------------- - IP4_ADDR(&Desaddr,sip[0],sip[1],sip[2],sip[3]);//ֵַת - - DesPuff = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_POOL); /* ڴ */ - - pbuf_take(DesPuff, buf, len); /* g_lwip_demo_sendbufеݴpbufṹ */ - - if(UDPCB_1 == udpcbID) - { - SendState = udp_sendto(udpcb_1, DesPuff, &Desaddr, port); - } - else if(UDPCB_2 == udpcbID) - { - SendState = udp_sendto(udpcb_2, DesPuff, &Desaddr, port); - } - else if(UDPCB_3 == udpcbID) - { - SendState = udp_sendto(udpcb_3, DesPuff, &Desaddr, port); - } - else if(UDPCB_3 == udpcbID) - { - SendState = udp_sendto(udpcb_3, DesPuff, &Desaddr, port); - } - else if(UDPCB_4 == udpcbID) - { - SendState = udp_sendto(udpcb_4, DesPuff, &Desaddr, port); - } - else if(UDPCB_5 == udpcbID) - { - SendState = udp_sendto(udpcb_5, DesPuff, &Desaddr, port); - } - else - { - } - pbuf_free(DesPuff); /* ͷڴ */ - - return SendState; -} - -// udpcbID UDP -//recv ص -//port 󶨶˿ -void UDP_Echo_Init(uint8_t udpcbID, udp_recv_fn recv, uint16_t port) -{ -//------------------------------------------ - if(UDPCB_1 == udpcbID) - { - /* ½һƿ*/ - udpcb_1 = udp_new(); - ASSERT(udpcb_1); - /* 󶨶˿ں */ - udp_bind(udpcb_1, IP_ADDR_ANY, port); - /* עݻص */ - udp_recv(udpcb_1, recv, (void *)udpcbID); - } - else if(UDPCB_2 == udpcbID) - { - /* ½һƿ*/ - udpcb_2 = udp_new(); - ASSERT(udpcb_2); - /* 󶨶˿ں */ - udp_bind(udpcb_2, IP_ADDR_ANY, port); - /* עݻص */ - udp_recv(udpcb_2, recv, (void *)udpcbID); - } - else if(UDPCB_3 == udpcbID) - { - /* ½һƿ*/ - udpcb_3 = udp_new(); - ASSERT(udpcb_3); - /* 󶨶˿ں */ - udp_bind(udpcb_3, IP_ADDR_ANY, port); - /* עݻص */ - udp_recv(udpcb_3, recv, (void *)udpcbID); - } - else if(UDPCB_4 == udpcbID) - { - /* ½һƿ*/ - udpcb_4 = udp_new(); - ASSERT(udpcb_4); - /* 󶨶˿ں */ - udp_bind(udpcb_4, IP_ADDR_ANY, port); - /* עݻص */ - udp_recv(udpcb_4, recv, (void *)udpcbID); - } - else if(UDPCB_5 == udpcbID) - { - /* ½һƿ*/ - udpcb_5 = udp_new(); - ASSERT(udpcb_5); - /* 󶨶˿ں */ - udp_bind(udpcb_5, IP_ADDR_ANY, port); - /* עݻص */ - udp_recv(udpcb_5, recv, (void *)udpcbID); - } - else - { - } - -} - - - -void udp_Callback_1(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) -{ - uint8_t *buf = (uint8_t *)(p->payload); - uint16_t i = 0; - uint16_t len = 0; - uint16_t udp_temp = 0; -//------------------------------------------------------------------------------ - - if( (0xFF == buf[0] ) && ( 0xBB == buf[1] ) )//ֶңԶң - { - ethernet_fault_Info.bit_data.manual_count ++; - - if( (p->len) >= sizeof(UnManualComputerInput) )//ȡȳȡС - { - len = sizeof(UnManualComputerInput); - } - else - { - len = p->len; - } - - for(i = 0; i < len; i++) - { - un_manual_computer_input.arr[i] = buf[i]; - } - publishMessage(&un_manual_computer_input, 1); - -// p->len = len; -// printf("Manualrecive len:%d\n",len); -// udp_sendto(upcb, p, addr, port); - } - else if( (0xFF == buf[0] ) && (0xCC == buf[1] ) )// Զ - { - ethernet_fault_Info.bit_data.auto_count ++; - - if( (p->len) >= sizeof(UnAutoComputerInput) )//ȡȳȡС - { - len = sizeof(UnAutoComputerInput); - } - else - { - len = p->len; - } - - -// unsigned int frame_header : 16; // ֡ͷ ̶ֵ0xFFCC -// unsigned int frame_type : 16; // ֡ ̶ֵ0x0001 -// unsigned int frame_length : 8; // ֡ ̶ֵ0x19 -// unsigned int heartbeat : 8; // ֡ۼ -// unsigned int set_speed : 16; // 趨ٶ ϵ0.01ΪǰΪ λm/s -// unsigned int set_curvature : 16; // 趨 ϵ0.0001ΪתΪת -// unsigned int latitude : 32; // γ ϵ10^-7431234567ʾ43.1234567 -// unsigned int longitude : 32; // ϵ10^-7431234567ʾ43.1234567 -// unsigned int altitude : 32; // ߶ λmm -// unsigned int heading : 16; // 35999ʾ359.99 -// unsigned int crc : 8; // CRC ֽۼ֮ͣȡ8λ - for(i = 0; i < len; i++) - { - un_auto_computer_input.arr[i] = buf[i]; - } - - udp_temp = ((un_auto_computer_input.bit_data.set_speed << 8) | (un_auto_computer_input.bit_data.set_speed >> 8));//趨ٶ 20240928 ޸ĸߵλ - un_auto_computer_input.bit_data.set_speed = udp_temp; - - udp_temp = ((un_auto_computer_input.bit_data.set_curvature << 8) | (un_auto_computer_input.bit_data.set_curvature >> 8));//趨 - un_auto_computer_input.bit_data.set_curvature = udp_temp; - publishMessage(&un_auto_computer_input, 1); - -// p->len = len; -// printf("Autorecive len:%d\n",len); -// udp_sendto(upcb, p, addr, port); - } - else - { - } - - pbuf_free(p); -} - - - -void udp_Callback_2(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}; - -//------------------------------------------------------------------------------------------------ - - 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.target_upper_port = port;//յĶ˿ںIP´ηͶӦIPͶ˿ - - ethernet_parameter.upper_ip[0] = ip_addr[0]; - ethernet_parameter.upper_ip[1] = ip_addr[1]; - ethernet_parameter.upper_ip[2] = ip_addr[2]; - ethernet_parameter.upper_ip[3] = ip_addr[3]; - - if( (0x80 == buf[0]) && (0x00 == buf[1]) ) - { - memcpy((uint8_t *)&(un_request_frame.arr[0]),buf, sizeof(UnRequestFrame)); - publishMessage(&un_request_frame, 1); - } - - pbuf_free(p); -} - - -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); -//------------------------------------------------------------------------------------------------ - - boot_eth_flag = true; - FrameHeader = ( (buf[0] << 8) | (buf[1]) ); - printf("FrameHeader %d\n",FrameHeader); - - pbuf_free(p); -} - - -void udp_Callback_5(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) -{ - uint8_t *buf = (uint8_t *)(p->payload); -//------------------------------------------------------------------------------------------------ - - if(FEEDDOG_HEADER == ( (buf[0] << 8) | (buf[1]) )) - { - WDTReFresh_flag = true;//ι־ - printf("Feed dog flag received %d\n",getCurrentTime()); - } - - - pbuf_free(p); -} - - -//100ms һ -void ethernetTimerProcess(void) -{ - static uint8_t ethernet_timer = 0; - static uint8_t ethernet_temp[4] = {0,0,0,0};//мжֵ - uint8_t temp = 0; - - ethernet_timer ++; - if(ethernet_timer >= 5)//500msжһ - { - ethernet_timer = 0; - -//ж------------------------------------------------------------ - if(ethernet_fault_Info.bit_data.auto_count == ethernet_temp[0])//һʾ - { - ethernet_fault_Info.bit_data.auto_state = FAULT; - } - else - { - ethernet_fault_Info.bit_data.auto_state = NORMAL; - ethernet_temp[0] = ethernet_fault_Info.bit_data.auto_count;//ݸ - } - - if(ethernet_fault_Info.bit_data.auto_state != ethernet_temp[1]) - { - ethernet_temp[1] = ethernet_fault_Info.bit_data.auto_state; - temp ++; - } - - -//ֶж------------------------------------------------------------ - if(ethernet_fault_Info.bit_data.manual_count == ethernet_temp[2])//һʾ - { - ethernet_fault_Info.bit_data.manual_state = FAULT; - } - else - { - ethernet_fault_Info.bit_data.manual_state = NORMAL; - ethernet_temp[2] = ethernet_fault_Info.bit_data.manual_count;//ݸ - } - - if(ethernet_fault_Info.bit_data.manual_state != ethernet_temp[3]) - { - ethernet_temp[3] = ethernet_fault_Info.bit_data.manual_state; - temp ++; - } - } - - publishMessage(ðernet_fault_Info, 1);// ״̬仯 ź -} - - -static void vehicleInfOoutput(void *signal_id) -{ - (void)signal_id; // DZΪʹã - - UdpSendToData(UDPCB_2,(uint8_t *)(&un_vehicle_Info_output), sizeof(un_vehicle_Info_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); -} - -static void motorStatusOutput(void *signal_id) -{ - (void)signal_id; // DZΪʹã - - UdpSendToData(UDPCB_2,(uint8_t *)(&un_motor_status_output), sizeof(un_motor_status_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); -} - -static void pidOutput(void *signal_id) -{ - (void)signal_id; // DZΪʹã - - UdpSendToData(UDPCB_2,(uint8_t *)(&un_pid_output), sizeof(un_pid_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); -} - -static void remoteControlOutput(void *signal_id) -{ - (void)signal_id; // DZΪʹã - - UdpSendToData(UDPCB_2,(uint8_t *)(&un_remote_control_output), sizeof(un_remote_control_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); -} - -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) -{ - (void)signal_id; // DZΪʹã - - UdpSendToData(UDPCB_2,(uint8_t *)(&un_auto_control_output), sizeof(un_auto_control_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); -} - - - - - - -//̫ -void ethernetSendAll(void *signal_id) -{ - static uint8_t computer_output_cnt = 0; - uint32_t crc_temp = 0; - uint8_t i = 0; - uint16_t Rg_Tmp = 0; - float Rg_FloatTmp = 0; - static uint8_t eth_cnt = 0; - static uint8_t app_output_cnt = 0; - uint8_t BOOT_Arr[2] = {0x01,0x02};//ϵ緢app֡λ -//------------------------------------------------------------------------- - //̫50ms 20210811޸Ϊ100ms - //20210827޸Ϊλǰλٺһ - computer_output_cnt ++; - if(computer_output_cnt >= 1) - { - computer_output_cnt = 0; - //ֽۼӺͣעⲻź 20210826޸İֽۼ - -// printf("left_motor_speed = %f\n",diff_data.left_motor_speed); -// printf("right_motor_speed = %f\n",diff_data.right_motor_speed); - - un_computer_output.bit_data.frame_header = 0xCCFF; - un_computer_output.bit_data.frame_type = 0x1100; - un_computer_output.bit_data.frame_length = 0x0B; - un_computer_output.bit_data.accumulated = eth_cnt ++; - - Rg_FloatTmp = (diff_data.left_motor_speed*(float)getParam("whl_dia") * M_PI*60)/1000.0/(float)getParam("gRatio");// ת/ӡkm/h һȦ1.8 - Rg_FloatTmp = Rg_FloatTmp*100;//,Ҫ100 ,ϵ0.01 - Rg_Tmp = (uint16_t)((int16_t)(Rg_FloatTmp));//תΪ޷); //20231012޸ٷ 20231226-3ų޸ķ - un_computer_output.bit_data.speed = ((Rg_Tmp << 8) | (Rg_Tmp >> 8));//λǰλֺ - - Rg_FloatTmp = (diff_data.right_motor_speed*(float)getParam("whl_dia") * M_PI*60)/1000.0/(float)getParam("gRatio");// ת/ӡkm/h һȦ1.8 - Rg_FloatTmp = Rg_FloatTmp*100;//,Ҫ100 ,ϵ0.01 - Rg_Tmp = (uint16_t)((int16_t)(Rg_FloatTmp));//תΪ޷); //20231012޸ٷ 20231226-3ų޸ķ - un_computer_output.bit_data.curvature = ((Rg_Tmp << 8) | (Rg_Tmp >> 8));//λǰλֺ - - crc_temp = 0; - for(i = 0; i<10 ;i++)//ȹ̶ - { - crc_temp = crc_temp + (un_computer_output.arr[i]); - } - un_computer_output.bit_data.crc = (uint8_t)crc_temp; - - UdpSendToData(UDPCB_1,(uint8_t *)(&un_computer_output.arr[0]), 11, (uint8_t *)ðernet_parameter.computer_ip[0], COMMUNICATION_PORT); - UdpSendToData(UDPCB_4,(uint8_t *)(&un_computer_output.arr[0]), 11, (uint8_t *)ðernet_parameter.computer_ip[0], COMMUNICATION_PORT); - } - - - if(app_output_cnt < 10)//20250109 100ms 10֡app̫֡ûгʼɾͷ͵Ķ֡ - { - app_output_cnt ++; - UdpSendToData(UDPCB_4, BOOT_Arr, 2, (uint8_t *)ðernet_parameter.download_ip[0], BOOT_TX_PORT); - } -} - -// ̫ʱźŴ -static void ethernetInterfaceTimerProcess(void *signal_id) -{ - uint32_t time_boot = getCurrentTime();//¼ǰʱ - (void)signal_id; // DZΪʹã - - ethernetSendAll(signal_id); - - ethernetTimerProcess();//ж - - timerStart(ðernet_timer_interface, 100,1); //100msһ - -// printf("ethernetInterface spend time:%d\n",getCurrentTime() - time_boot);//app˶೤ʱ -} - - - - - - - - -static unsigned char receive_buffer[MAX_BUFFER_SIZE]; -//ص -void udp_Callback_3(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) -{ - static unsigned int received_size = 0; - uint8_t *buf = (uint8_t *)(p->payload); - - // Ƿµ֡ͷ - if ((0x80 == buf[1]) && (0xFF == buf[0])) - { - // ýչ - received_size = 0; - printf("⵽µ֡ͷ,ýչ\n"); - } - - // Ƿ㹻Ŀռ洢 - if (received_size + p->len <= MAX_BUFFER_SIZE) - { - printf("p->len %d :\n" , p->len); - - memcpy(receive_buffer + received_size, buf, p->len); - received_size += p->len; - - // Ƿյ - if (received_size >= sizeof(request_context.param_request->arr)) - { - printf("received_size : %d :\n" , received_size); - - for(u16_t i = 0; i< sizeof(request_context.param_request->arr); i++) - { - request_context.param_request->arr[i] = receive_buffer[i]; - } - - memcpy(request_context.param_request->arr, receive_buffer, sizeof(request_context.param_request->arr)); - request_context.sender_ip = (unsigned int)addr->addr; - request_context.sender_port = port; - - // ӡ request_context.param_request->arr -// printf("յ param_request->arr :\n"); -// for (size_t i = 0; i < sizeof(request_context.param_request->arr); ++i) -// { -// printf("%02X ", request_context.param_request->arr[i]); -// if ((i + 1) % 1024 == 0 || i == sizeof(request_context.param_request->arr) - 1) -// { -// printf("\n"); -// feedWatchdog(); -// } -// } - - publishMessage(&request_context, 1); - - printf("received_size%d \n" , received_size); - // ýջ - received_size = 0; - printf("յ,\n"); - } - } - else - { - // ,ý - received_size = 0; - printf("ջ,ݶ\n"); - } - - unsigned int tmp_count = pbuf_free(p); - printf("pbuf_freeѾͷţtmp_count :%d \n" , tmp_count); - p = NULL; -} - -void OnParamSend(void *data) -{ - uint8_t ip_addr[4] = {0,0,0,0}; - RequestContext *signal = (RequestContext *)(data); - - ip_addr[0] = signal->sender_ip & 0xff; /* IADDR4 */ - ip_addr[1] = (signal->sender_ip >> 8) & 0xff; /* IADDR3 */ - ip_addr[2] = (signal->sender_ip >> 16) & 0xff; /* IADDR2 */ - ip_addr[3] = (signal->sender_ip >> 24) & 0xff; /* IADDR1 */ - - uint32_t total_size = sizeof(signal->param_request->arr); - uint32_t sent_size = 0; - - - while (sent_size < total_size) - { - uint32_t remaining = total_size - sent_size; - uint32_t packet_size = (remaining > MAX_PACKET_SIZE) ? MAX_PACKET_SIZE : remaining; - - UdpSendToData(UDPCB_3, - (uint8_t *)(signal->param_request->arr) + sent_size, - packet_size, - ip_addr, - signal->sender_port); - - sent_size += packet_size; - // Сӳ,ʧ - udelay(1000); - } - printf("ݷɣ%d \n", sent_size); -} - - - -// APPģijʼ -void ethernetInterfaceInit(void) -{ -//----------------------------------- - if (!eth_pool_rx) { - // ʹDMA RxжϡTxδʹжϣΪETHʺܸߣ - // Txжϴʱٶȡ - dwc_eth_enable_dma_rx_int(&g_eth1_dev, eth_dma_rx_int_cb); - } - - timerInit(ðernet_timer_interface1); - timerInit(ðernet_timer_interface); - - // Ķʱźţڶʱɼ - subscribe(ðernet_timer_interface, ethernetInterfaceTimerProcess); - subscribe(ðernet_timer_interface1, lwip_Ethernet); - - - subscribe(&un_vehicle_Info_output, vehicleInfOoutput); - subscribe(&un_motor_status_output, motorStatusOutput); - subscribe(&un_pid_output, pidOutput); - subscribe(&un_remote_control_output, remoteControlOutput); - subscribe(&un_manual_control_output, manualControlOutput); - subscribe(&un_auto_control_output, autoControlOutput); - - // Ϣʹþ̬ԱΪص - subscribe(&request_send, OnParamSend); - - - timerStart(ðernet_timer_interface, 100,1); //100msһ - timerStart(ðernet_timer_interface1, 10,1); //10msһ - - printf( "ethernetInterface: initial OK %d\n",getCurrentTime()); -} - +#include +#include +#include "debug.h" +#include +#include +#include "interface_ethernet.h" +#include "eth_cfg.h" +#include "lwip/timeouts.h" +#include "sdrv_eth.h" +#include "app/app_param_manage.h" +#include "app/app_differential_drive.h" +#include "app/app_turntable.h" + +// +#define MAX_BUFFER_SIZE (sizeof(((RequestContext *)0)->param_request->arr)) +#define MAX_PACKET_SIZE 1024 // СΪ1K + +Timer ethernet_timer_interface; +Timer ethernet_timer_interface1; + +struct udp_pcb *udpcb_1; +struct udp_pcb *udpcb_2; +struct udp_pcb *udpcb_3; +struct udp_pcb *udpcb_4; +struct udp_pcb *udpcb_5; + +bool eth_pool_rx = false; + + +UnEthernetFault ethernet_fault_Info = { + .bit_data.auto_count = 0, + .bit_data.manual_count = 0, + .bit_data.auto_state = 0, + .bit_data.manual_state = 0, +}; + + + +StrEthernetParameter ethernet_parameter = { + .local_ip = {192,168,17,20}, + .mask = {255,255,255,0}, + .computer_ip = {192,168,17,3}, + .local_communication_port = 8011, + .Local_upper_port = 8000, + .Local_download_port = 7811, + .upper_ip = {192,168,17,3}, + .download_ip = {192,168,17,183}, + .target_upper_port = 8000, + .target_download_port = 7811, + .target_communication_port = 8011, +}; + + + +static bool g_eth1_rx_pkt; + + + +static void eth_dma_rx_int_cb(struct net_driver_s *dev) +{ + if (dev == &g_eth1_dev) + g_eth1_rx_pkt = true; + +// printf("eth_dma_rx_int_cb\n"); +} + + + + +static void lwip_Ethernet(void *signal_id) +{ +// uint32_t time_boot = getCurrentTime();//¼ǰʱ + (void)signal_id; // DZΪʹã + // ETH֡ݸLWIPЭջ + // ʹܽжϣΪISRdisableжϣˣ + // жϽձ־(g_eth1_rx_pkt)Ĵв + // µĽжϱӦԣﲻԭ⡣ + if ((!eth_pool_rx && g_eth1_rx_pkt) || eth_pool_rx) + { + g_eth1_rx_pkt = false; + dwc_eth_rx(&g_eth1_dev, eth_pool_rx); + } + + // LWIPЭջӿڣLWIPʱ + sys_check_timeouts(); + + timerStart(ðernet_timer_interface1, 10,1); //10msһ + + // printf("lwipInterface spend time:%d\n",getCurrentTime() - time_boot);//app˶೤ʱ +} + + +//err_t +//udp_bind(struct udp_pcb *pcb, const ip_addr_t *ipaddr, u16_t port) + +int8_t UdpSendToData(uint8_t udpcbID, uint8_t *buf, uint16_t len, uint8_t *sip, uint16_t port) +{ + struct pbuf *DesPuff = NULL; + ip4_addr_t Desaddr; + int8_t SendState = 0; +//---------------------------------------------------------------------------------------------------- + IP4_ADDR(&Desaddr,sip[0],sip[1],sip[2],sip[3]);//ֵַת + + DesPuff = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_POOL); /* ڴ */ + + pbuf_take(DesPuff, buf, len); /* g_lwip_demo_sendbufеݴpbufṹ */ + + if(UDPCB_1 == udpcbID) + { + SendState = udp_sendto(udpcb_1, DesPuff, &Desaddr, port); + } + else if(UDPCB_2 == udpcbID) + { + SendState = udp_sendto(udpcb_2, DesPuff, &Desaddr, port); + } + else if(UDPCB_3 == udpcbID) + { + SendState = udp_sendto(udpcb_3, DesPuff, &Desaddr, port); + } + else if(UDPCB_3 == udpcbID) + { + SendState = udp_sendto(udpcb_3, DesPuff, &Desaddr, port); + } + else if(UDPCB_4 == udpcbID) + { + SendState = udp_sendto(udpcb_4, DesPuff, &Desaddr, port); + } + else if(UDPCB_5 == udpcbID) + { + SendState = udp_sendto(udpcb_5, DesPuff, &Desaddr, port); + } + else + { + } + pbuf_free(DesPuff); /* ͷڴ */ + + return SendState; +} + +// udpcbID UDP +//recv ص +//port 󶨶˿ +void UDP_Echo_Init(uint8_t udpcbID, udp_recv_fn recv, uint16_t port) +{ +//------------------------------------------ + if(UDPCB_1 == udpcbID) + { + /* ½һƿ*/ + udpcb_1 = udp_new(); + ASSERT(udpcb_1); + /* 󶨶˿ں */ + udp_bind(udpcb_1, IP_ADDR_ANY, port); + /* עݻص */ + udp_recv(udpcb_1, recv, (void *)udpcbID); + } + else if(UDPCB_2 == udpcbID) + { + /* ½һƿ*/ + udpcb_2 = udp_new(); + ASSERT(udpcb_2); + /* 󶨶˿ں */ + udp_bind(udpcb_2, IP_ADDR_ANY, port); + /* עݻص */ + udp_recv(udpcb_2, recv, (void *)udpcbID); + } + else if(UDPCB_3 == udpcbID) + { + /* ½һƿ*/ + udpcb_3 = udp_new(); + ASSERT(udpcb_3); + /* 󶨶˿ں */ + udp_bind(udpcb_3, IP_ADDR_ANY, port); + /* עݻص */ + udp_recv(udpcb_3, recv, (void *)udpcbID); + } + else if(UDPCB_4 == udpcbID) + { + /* ½һƿ*/ + udpcb_4 = udp_new(); + ASSERT(udpcb_4); + /* 󶨶˿ں */ + udp_bind(udpcb_4, IP_ADDR_ANY, port); + /* עݻص */ + udp_recv(udpcb_4, recv, (void *)udpcbID); + } + else if(UDPCB_5 == udpcbID) + { + /* ½һƿ*/ + udpcb_5 = udp_new(); + ASSERT(udpcb_5); + /* 󶨶˿ں */ + udp_bind(udpcb_5, IP_ADDR_ANY, port); + /* עݻص */ + udp_recv(udpcb_5, recv, (void *)udpcbID); + } + else + { + } + +} + + + +void udp_Callback_1(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) +{ + uint8_t *buf = (uint8_t *)(p->payload); + uint16_t i = 0; + uint16_t len = 0; + uint16_t udp_temp = 0; +//------------------------------------------------------------------------------ + + if( (0xFF == buf[0] ) && ( 0xBB == buf[1] ) )//ֶңԶң + { + ethernet_fault_Info.bit_data.manual_count ++; + + if( (p->len) >= sizeof(UnManualComputerInput) )//ȡȳȡС + { + len = sizeof(UnManualComputerInput); + } + else + { + len = p->len; + } + + for(i = 0; i < len; i++) + { + un_manual_computer_input.arr[i] = buf[i]; + } + publishMessage(&un_manual_computer_input, 1); + +// p->len = len; +// printf("Manualrecive len:%d\n",len); +// udp_sendto(upcb, p, addr, port); + } + else if( (0xFF == buf[0] ) && (0xCC == buf[1] ) )// Զ + { + ethernet_fault_Info.bit_data.auto_count ++; + + if( (p->len) >= sizeof(UnAutoComputerInput) )//ȡȳȡС + { + len = sizeof(UnAutoComputerInput); + } + else + { + len = p->len; + } + + +// unsigned int frame_header : 16; // ֡ͷ ̶ֵ0xFFCC +// unsigned int frame_type : 16; // ֡ ̶ֵ0x0001 +// unsigned int frame_length : 8; // ֡ ̶ֵ0x19 +// unsigned int heartbeat : 8; // ֡ۼ +// unsigned int set_speed : 16; // 趨ٶ ϵ0.01ΪǰΪ λm/s +// unsigned int set_curvature : 16; // 趨 ϵ0.0001ΪתΪת +// unsigned int latitude : 32; // γ ϵ10^-7431234567ʾ43.1234567 +// unsigned int longitude : 32; // ϵ10^-7431234567ʾ43.1234567 +// unsigned int altitude : 32; // ߶ λmm +// unsigned int heading : 16; // 35999ʾ359.99 +// unsigned int crc : 8; // CRC ֽۼ֮ͣȡ8λ + for(i = 0; i < len; i++) + { + un_auto_computer_input.arr[i] = buf[i]; + } + + udp_temp = ((un_auto_computer_input.bit_data.set_speed << 8) | (un_auto_computer_input.bit_data.set_speed >> 8));//趨ٶ 20240928 ޸ĸߵλ + un_auto_computer_input.bit_data.set_speed = udp_temp; + + udp_temp = ((un_auto_computer_input.bit_data.set_curvature << 8) | (un_auto_computer_input.bit_data.set_curvature >> 8));//趨 + un_auto_computer_input.bit_data.set_curvature = udp_temp; + publishMessage(&un_auto_computer_input, 1); + +// p->len = len; +// printf("Autorecive len:%d\n",len); +// udp_sendto(upcb, p, addr, port); + } + else if(ENCODER_HEADER == buf[0])// + { + if( (p->len) >= sizeof(un_encoder_data_input) )//ȡȳȡС + { + len = sizeof(un_encoder_data_input); + } + else + { + len = p->len; + } + + for(i = 0; i < len; i++) + { + un_encoder_data_input.arr[i] = buf[i]; + } + publishMessage(&un_encoder_data_input, 1); + } + + else + { + } + + pbuf_free(p); +} + + + +void udp_Callback_2(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}; + +//------------------------------------------------------------------------------------------------ + + 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.target_upper_port = port;//յĶ˿ںIP´ηͶӦIPͶ˿ + + ethernet_parameter.upper_ip[0] = ip_addr[0]; + ethernet_parameter.upper_ip[1] = ip_addr[1]; + ethernet_parameter.upper_ip[2] = ip_addr[2]; + ethernet_parameter.upper_ip[3] = ip_addr[3]; + + if( (0x80 == buf[0]) && (0x00 == buf[1]) ) + { + memcpy((uint8_t *)&(un_request_frame.arr[0]),buf, sizeof(UnRequestFrame)); + publishMessage(&un_request_frame, 1); + } + + pbuf_free(p); +} + + +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); +//------------------------------------------------------------------------------------------------ + + boot_eth_flag = true; + FrameHeader = ( (buf[0] << 8) | (buf[1]) ); + printf("FrameHeader %d\n",FrameHeader); + + pbuf_free(p); +} + + +void udp_Callback_5(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) +{ + uint8_t *buf = (uint8_t *)(p->payload); +//------------------------------------------------------------------------------------------------ + + if(FEEDDOG_HEADER == ( (buf[0] << 8) | (buf[1]) )) + { + WDTReFresh_flag = true;//ι־ + printf("Feed dog flag received %d\n",getCurrentTime()); + } + + + pbuf_free(p); +} + + +//100ms һ +void ethernetTimerProcess(void) +{ + static uint8_t ethernet_timer = 0; + static uint8_t ethernet_temp[4] = {0,0,0,0};//мжֵ + uint8_t temp = 0; + + ethernet_timer ++; + if(ethernet_timer >= 5)//500msжһ + { + ethernet_timer = 0; + +//ж------------------------------------------------------------ + if(ethernet_fault_Info.bit_data.auto_count == ethernet_temp[0])//һʾ + { + ethernet_fault_Info.bit_data.auto_state = FAULT; + } + else + { + ethernet_fault_Info.bit_data.auto_state = NORMAL; + ethernet_temp[0] = ethernet_fault_Info.bit_data.auto_count;//ݸ + } + + if(ethernet_fault_Info.bit_data.auto_state != ethernet_temp[1]) + { + ethernet_temp[1] = ethernet_fault_Info.bit_data.auto_state; + temp ++; + } + + +//ֶж------------------------------------------------------------ + if(ethernet_fault_Info.bit_data.manual_count == ethernet_temp[2])//һʾ + { + ethernet_fault_Info.bit_data.manual_state = FAULT; + } + else + { + ethernet_fault_Info.bit_data.manual_state = NORMAL; + ethernet_temp[2] = ethernet_fault_Info.bit_data.manual_count;//ݸ + } + + if(ethernet_fault_Info.bit_data.manual_state != ethernet_temp[3]) + { + ethernet_temp[3] = ethernet_fault_Info.bit_data.manual_state; + temp ++; + } + } + + publishMessage(ðernet_fault_Info, 1);// ״̬仯 ź +} + + +static void vehicleInfOoutput(void *signal_id) +{ + (void)signal_id; // DZΪʹã + + UdpSendToData(UDPCB_2,(uint8_t *)(&un_vehicle_Info_output), sizeof(un_vehicle_Info_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); +} + +static void motorStatusOutput(void *signal_id) +{ + (void)signal_id; // DZΪʹã + + UdpSendToData(UDPCB_2,(uint8_t *)(&un_motor_status_output), sizeof(un_motor_status_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); +} + +static void pidOutput(void *signal_id) +{ + (void)signal_id; // DZΪʹã + + UdpSendToData(UDPCB_2,(uint8_t *)(&un_pid_output), sizeof(un_pid_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); +} + +static void remoteControlOutput(void *signal_id) +{ + (void)signal_id; // DZΪʹã + + UdpSendToData(UDPCB_2,(uint8_t *)(&un_remote_control_output), sizeof(un_remote_control_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); +} + +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) +{ + (void)signal_id; // DZΪʹã + + UdpSendToData(UDPCB_2,(uint8_t *)(&un_auto_control_output), sizeof(un_auto_control_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); +} + +static void magneticEncoderOutput(void *signal_id) +{ + (void)signal_id; // DZΪʹã + + UdpSendToData(UDPCB_2,(uint8_t *)(&str_magnetic_encoder), 1, (uint8_t *)&str_magnetic_encoder.ip[0], str_magnetic_encoder.port); +} + +//̫ +void ethernetSendAll(void *signal_id) +{ + static uint8_t computer_output_cnt = 0; + uint32_t crc_temp = 0; + uint8_t i = 0; + uint16_t Rg_Tmp = 0; + float Rg_FloatTmp = 0; + static uint8_t eth_cnt = 0; + static uint8_t app_output_cnt = 0; + uint8_t BOOT_Arr[2] = {0x01,0x02};//ϵ緢app֡λ +//------------------------------------------------------------------------- + //̫50ms 20210811޸Ϊ100ms + //20210827޸Ϊλǰλٺһ + computer_output_cnt ++; + if(computer_output_cnt >= 1) + { + computer_output_cnt = 0; + //ֽۼӺͣעⲻź 20210826޸İֽۼ + +// printf("left_motor_speed = %f\n",diff_data.left_motor_speed); +// printf("right_motor_speed = %f\n",diff_data.right_motor_speed); + + un_computer_output.bit_data.frame_header = 0xCCFF; + un_computer_output.bit_data.frame_type = 0x1100; + un_computer_output.bit_data.frame_length = 0x0B; + un_computer_output.bit_data.accumulated = eth_cnt ++; + + Rg_FloatTmp = (diff_data.left_motor_speed*(float)getParam("whl_dia") * M_PI*60)/1000.0/(float)getParam("gRatio");// ת/ӡkm/h һȦ1.8 + Rg_FloatTmp = Rg_FloatTmp*100;//,Ҫ100 ,ϵ0.01 + Rg_Tmp = (uint16_t)((int16_t)(Rg_FloatTmp));//תΪ޷); //20231012޸ٷ 20231226-3ų޸ķ + un_computer_output.bit_data.speed = ((Rg_Tmp << 8) | (Rg_Tmp >> 8));//λǰλֺ + + Rg_FloatTmp = (diff_data.right_motor_speed*(float)getParam("whl_dia") * M_PI*60)/1000.0/(float)getParam("gRatio");// ת/ӡkm/h һȦ1.8 + Rg_FloatTmp = Rg_FloatTmp*100;//,Ҫ100 ,ϵ0.01 + Rg_Tmp = (uint16_t)((int16_t)(Rg_FloatTmp));//תΪ޷); //20231012޸ٷ 20231226-3ų޸ķ + un_computer_output.bit_data.curvature = ((Rg_Tmp << 8) | (Rg_Tmp >> 8));//λǰλֺ + + crc_temp = 0; + for(i = 0; i<10 ;i++)//ȹ̶ + { + crc_temp = crc_temp + (un_computer_output.arr[i]); + } + un_computer_output.bit_data.crc = (uint8_t)crc_temp; + + UdpSendToData(UDPCB_1,(uint8_t *)(&un_computer_output.arr[0]), 11, (uint8_t *)ðernet_parameter.computer_ip[0], COMMUNICATION_PORT); + UdpSendToData(UDPCB_4,(uint8_t *)(&un_computer_output.arr[0]), 11, (uint8_t *)ðernet_parameter.computer_ip[0], COMMUNICATION_PORT); + } + + + if(app_output_cnt < 10)//20250109 100ms 10֡app̫֡ûгʼɾͷ͵Ķ֡ + { + app_output_cnt ++; + UdpSendToData(UDPCB_4, BOOT_Arr, 2, (uint8_t *)ðernet_parameter.download_ip[0], BOOT_TX_PORT); + } +} + +// ̫ʱźŴ +static void ethernetInterfaceTimerProcess(void *signal_id) +{ + uint32_t time_boot = getCurrentTime();//¼ǰʱ + (void)signal_id; // DZΪʹã + + ethernetSendAll(signal_id); + + ethernetTimerProcess();//ж + + timerStart(ðernet_timer_interface, 100,1); //100msһ + +// printf("ethernetInterface spend time:%d\n",getCurrentTime() - time_boot);//app˶೤ʱ +} + + + + + + + + +static unsigned char receive_buffer[MAX_BUFFER_SIZE]; +//ص +void udp_Callback_3(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) +{ + static unsigned int received_size = 0; + uint8_t *buf = (uint8_t *)(p->payload); + + // Ƿµ֡ͷ + if ((0x80 == buf[1]) && (0xFF == buf[0])) + { + // ýչ + received_size = 0; + printf("⵽µ֡ͷ,ýչ\n"); + } + + // Ƿ㹻Ŀռ洢 + if (received_size + p->len <= MAX_BUFFER_SIZE) + { + printf("p->len %d :\n" , p->len); + + memcpy(receive_buffer + received_size, buf, p->len); + received_size += p->len; + + // Ƿյ + if (received_size >= sizeof(request_context.param_request->arr)) + { + printf("received_size : %d :\n" , received_size); + + for(u16_t i = 0; i< sizeof(request_context.param_request->arr); i++) + { + request_context.param_request->arr[i] = receive_buffer[i]; + } + + memcpy(request_context.param_request->arr, receive_buffer, sizeof(request_context.param_request->arr)); + request_context.sender_ip = (unsigned int)addr->addr; + request_context.sender_port = port; + + // ӡ request_context.param_request->arr +// printf("յ param_request->arr :\n"); +// for (size_t i = 0; i < sizeof(request_context.param_request->arr); ++i) +// { +// printf("%02X ", request_context.param_request->arr[i]); +// if ((i + 1) % 1024 == 0 || i == sizeof(request_context.param_request->arr) - 1) +// { +// printf("\n"); +// feedWatchdog(); +// } +// } + + publishMessage(&request_context, 1); + + printf("received_size%d \n" , received_size); + // ýջ + received_size = 0; + printf("յ,\n"); + } + } + else + { + // ,ý + received_size = 0; + printf("ջ,ݶ\n"); + } + + unsigned int tmp_count = pbuf_free(p); + printf("pbuf_freeѾͷţtmp_count :%d \n" , tmp_count); + p = NULL; +} + +void OnParamSend(void *data) +{ + uint8_t ip_addr[4] = {0,0,0,0}; + RequestContext *signal = (RequestContext *)(data); + + ip_addr[0] = signal->sender_ip & 0xff; /* IADDR4 */ + ip_addr[1] = (signal->sender_ip >> 8) & 0xff; /* IADDR3 */ + ip_addr[2] = (signal->sender_ip >> 16) & 0xff; /* IADDR2 */ + ip_addr[3] = (signal->sender_ip >> 24) & 0xff; /* IADDR1 */ + + uint32_t total_size = sizeof(signal->param_request->arr); + uint32_t sent_size = 0; + + + while (sent_size < total_size) + { + uint32_t remaining = total_size - sent_size; + uint32_t packet_size = (remaining > MAX_PACKET_SIZE) ? MAX_PACKET_SIZE : remaining; + + UdpSendToData(UDPCB_3, + (uint8_t *)(signal->param_request->arr) + sent_size, + packet_size, + ip_addr, + signal->sender_port); + + sent_size += packet_size; + // Сӳ,ʧ + udelay(1000); + } + printf("ݷɣ%d \n", sent_size); +} + + + +// APPģijʼ +void ethernetInterfaceInit(void) +{ +//----------------------------------- + if (!eth_pool_rx) { + // ʹDMA RxжϡTxδʹжϣΪETHʺܸߣ + // Txжϴʱٶȡ + dwc_eth_enable_dma_rx_int(&g_eth1_dev, eth_dma_rx_int_cb); + } + + timerInit(ðernet_timer_interface1); + timerInit(ðernet_timer_interface); + + // Ķʱźţڶʱɼ + subscribe(ðernet_timer_interface, ethernetInterfaceTimerProcess); + subscribe(ðernet_timer_interface1, lwip_Ethernet); + + + subscribe(&un_vehicle_Info_output, vehicleInfOoutput); + subscribe(&un_motor_status_output, motorStatusOutput); + subscribe(&un_pid_output, pidOutput); + subscribe(&un_remote_control_output, remoteControlOutput); + subscribe(&un_manual_control_output, manualControlOutput); + subscribe(&un_auto_control_output, autoControlOutput); + + subscribe(&str_magnetic_encoder, magneticEncoderOutput); + + + // Ϣʹþ̬ԱΪص + subscribe(&request_send, OnParamSend); + + + timerStart(ðernet_timer_interface, 100,1); //100msһ + timerStart(ðernet_timer_interface1, 10,1); //10msһ + + printf( "ethernetInterface: initial OK %d\n",getCurrentTime()); +} + \ No newline at end of file diff --git a/main.c b/main.c index 2d192de1..5cd37a61 100644 --- a/main.c +++ b/main.c @@ -31,6 +31,8 @@ #include #include "app/app_request.h" #include "app/app_ultrasonic.h" +#include "app/app_turntable.h" + void testAppInit(void); @@ -112,8 +114,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("version: V1.75 \n"); // 初始化框架 放在最前面，解决电机can发送信号累积不处理的问题。 @@ -121,16 +122,16 @@ int main(void) testAppInit(); paramAppInit(); diffAppInit(); - brakeAppInit(); +// brakeAppInit(); powerAppInit(); //电源管理 - tempAppInit(); //温度 +// tempAppInit(); //温度 lightAppInit(); //灯光 ethernetInterfaceInit(); //以太网先初始化 requestAppInit(); canInterfaceInit(); bootInterfaceInit(); // ultrasonicAppInit(); - + turnableInit(); printf("All init OK ------ %d\n",getCurrentTime()); for (;;)