commit 7730610c15c67d30c6f7cc3069d7581fadcd3558 Author: liumin Date: Tue Aug 26 10:53:01 2025 +0800 第一次提交 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..af5eb65 --- /dev/null +++ b/.gitignore @@ -0,0 +1,10 @@ +# 忽略整个目录及其所有子文件和子目录 +/configs/ +/IAR/ +/IAR_Norflash/ +/SES/ +/configs/** +/IAR/** +/IAR_Norflash/** +/SES/** +*.zip \ No newline at end of file diff --git a/MFC2AF5DEDB.tmp b/MFC2AF5DEDB.tmp new file mode 100644 index 0000000..f2942d8 --- /dev/null +++ b/MFC2AF5DEDB.tmp @@ -0,0 +1,178 @@ +#ifndef ETH_DEMO_H +#define ETH_DEMO_H + +#include "app_config.h" + + + + +//volatile uint8_t RgCanToGPSCnt = 0;//导航仪CAN计数器 +//volatile uint8_t RgCanToGPSCntLast = 0;//导航仪CAN计数器过去值 +//volatile uint8_t RgCanGPSFault = 0;//导航仪CAN故障 + +//volatile uint8_t RgCanMoterFault = 0;//电机控制器1CAN故障 +// +//volatile uint8_t RgENTFaultContral = 0;//控制器遥控以太网故障 +//volatile uint8_t RgConEnetVaulelast = 0;//遥控以太网过去数据 +// +//volatile uint8_t RgENTFaultOwn = 0;//控制器自主以太网故障 +//volatile uint8_t RgOwnEnetVaulelast = 0;//自主以太网过去数据 +// +//volatile uint8_t RgENTFaultRemoteControlRoom = 0;//控制器遥控室故障 +//volatile uint8_t RgRemoteControlRoomENTVaulelast = 0;//控制器遥控室过去数据 +// +//volatile uint16_t RgCanRC6GSFault = 0;//汽车遥控CAN接收故障 +//volatile uint16_t RgCanRC6GSCnt = 0;//汽车遥控CAN接收计数器 +//volatile uint16_t RgCanRC6GSCntLast = 0;//汽车遥控接收计数器过去值 +// +// +//uint16_t RgCanToBatteryCnt = 0;//BMS CAN接收计数器 +//uint16_t RgETHToPortableCnt = 0;//以太网遥控器接受计数器 +//uint16_t RgETHToOwnComputerCnt = 0;//以太网接受自主计算器 +//uint16_t RgETHToRemoteControlCnt = 0;//以太网监控室接收计数器 +// +//uint16_t RgCanToMotorCnt1 = 0;//CAN接收计数器 +//uint16_t RgCanToMotorCntLast1 = 0;//CAN接收计数器过去值 +// +//uint16_t RgCanToMotorCnt2 = 0;//CAN接收计数器 +//uint16_t RgCanToMotorCntLast2 = 0;//CAN接收计数器过去值 +// +//uint8_t RgEthernetOwnCnt = 0;//自主接收故障定时器 +//uint8_t RgEthernetTelecontrolCnt = 0;//手柄遥控故障定时器 + + + +typedef struct +{ + unsigned char gps_state; // GPS状态 | 0故障 1正常 + unsigned char motor1_state; // 电机1状态 | 0故障 1正常 + unsigned char motor2_state; // 电机2状态 | 0故障 1正常 + unsigned char remote_state; // 遥控状态 | 0故障 1正常 + unsigned char bms_state; // BMS状态 | 0故障 1正常 + unsigned char auto_state; // 自动状态 | 0故障 1正常 + unsigned char manual_state; // 手动状态 | 0故障 1正常 + + unsigned char gps_change; // GPS变化 | 0未发生变化 1变化 + unsigned char motor1_change; // 电机1变化 | 0未发生变化 1变化 + unsigned char motor2_change; // 电机2变化 | 0未发生变化 1变化 + unsigned char remote_change; // 遥控变化 | 0未发生变化 1变化 + unsigned char bms_change; // BMS变化 | 0未发生变化 1变化 + unsigned char auto_change; // 自动变化 | 0未发生变化 1变化 + unsigned char manual_change; // 手动变化 | 0未发生变化 1变化 + +} FaultInfo; + + +typedef struct +{ + uint8_t auto_count; //自动数据计数器 + uint8_t manual_count; //手动数据计数器 + uint8_t auto_state; // 自动状态 | 0故障 1正常 + uint8_t manual_state; // 手动状态 | 0故障 1正常 + + + uint8_t auto_count_old; //自动数据计数器 + uint8_t manual_count_old; //手动数据计数器 + uint8_t auto_state_old; // 自动状态 | 0故障 1正常 + uint8_t manual_state_old; // 手动状态 | 0故障 1正常 + uint8_t auto_state; // 自动状态 | 0故障 1正常 + uint8_t manual_state; // 手动状态 | 0故障 1正常 + +} StrFaultInformation; + + + + +typedef struct +{ + uint16_t navigator_counter; //导航仪计数器 + uint16_t motor1_counter; //电机控制器1计数器 + uint16_t motor2_counter; //电机控制器2计数器 + uint16_t remote_counter; //遥控器计数器 + uint16_t bms_cnt; //BMS计数器 + uint16_t auto_cnt; //自动数据计数器 + uint16_t manual_cnt; //手动数据计数器 + + uint16_t can_gps_cnt_old ; //导航仪计数器过去值 + uint16_t can_motor1_cnt_last; //电机控制器1计数器过去值 + uint16_t can_motor2_cnt_last; //电机控制器2计数器过去值 + uint16_t can_remote_control_cnt_last; //遥控器计数器过去值 + uint16_t can_bms_cnt_last; //bms计数器过去值 + uint16_t ethernet_auto_computer_cnt_last; //自动数据计数器过去值 + uint16_t ethernet_manual_computer_cnt_last;//手动数据计数器过去值 + + bool can_gps_state_last; //导航仪故障状态过去值 0故障 1正常 + bool can_motor1_state_last; //电机控制器1状态故障过去值 0故障 1正常 + bool can_motor2_state_last; //电机控制器2状态故障过去值 + bool can_bms_state_last; //bms故障状态过去值 + bool can_remote_control_state_last; //遥控器故障状态过去值 + bool ethernet_auto_computer_state_last; //自动数据故障状态过去值 + bool ethernet_manual_computer_state_last; //手动数据故障状态过去值 + +} StrFaultTemp; + + + + + + + + +typedef struct +{ + uint8_t local_ip[4]; + uint8_t mask[4]; + uint16_t Local_upper_port; + uint16_t Local_download_port; + uint16_t local_communication_port; + uint8_t computer_ip[4]; + uint8_t upper_ip[4]; + uint8_t download_ip[4]; + uint16_t target_upper_port; + uint16_t target_download_port; + uint16_t target_communication_port; + +} StrEthernetParameter; + + + + + + + + + + + + + + + + + + + + +#define UDPCB_1 1//通讯端口 +#define UDPCB_2 2//上位机端口 +#define UDPCB_3 3//下载端口 + + +#define COMMUNICATION_PORT 8011 +#define UPPER_PORT 8000 +#define DOWNLOAR_PORT 7811 + + +//外部函数 +void UDP_Echo_Init(uint8_t udpcbID, udp_recv_fn recv, uint16_t port); +void UdpSendToData(uint8_t udpcbID, uint8_t *buf, uint16_t len, uint8_t *sip, uint16_t port); + +void udp_Callback_1(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port); +void udp_Callback_2(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port); +//外部变量 +extern struct udp_pcb *udpcb_1; + +extern StrEthernetParameter ethernet_parameter; +extern StrFaultInformation str_faultInformation; + +#endif /*ETH_DEMO_H */ \ No newline at end of file diff --git a/app/app_base.c b/app/app_base.c new file mode 100644 index 0000000..c5925ae --- /dev/null +++ b/app/app_base.c @@ -0,0 +1,159 @@ +#include "app_config.h" +#include "app_dependence.h" +#include "interface.h" + +#include "app_base.h" + + +// 使用内联函数 +static inline uint8_t setBaseOn(void) { return 1; } +static inline uint8_t setBaseOff(void) { return 0; } + +BaseSystem base_data; + +// 输出处理函数 +static void baseOutput(void *signal_id) +{ + (void)signal_id; + // 根据电机状态，填充发送数据结构，发送信号 + switch (base_data.base_command) + { + case 1: // 上升状态 + un_lifter_output.bit_data.cmd = BASE_UP; + un_h_bridge_output1.bit_data.channel_01 = setBaseOn(); + un_h_bridge_output1.bit_data.channel_02 = setBaseOff(); + un_h_bridge_output2.bit_data.channel_01 = setBaseOn(); + un_h_bridge_output2.bit_data.channel_02 = setBaseOff(); +// printf("base: up\n"); + break; + + case 2: // 下降状态 + un_lifter_output.bit_data.cmd = BASE_DOWN; + un_h_bridge_output1.bit_data.channel_01 = setBaseOff(); + un_h_bridge_output1.bit_data.channel_02 = setBaseOn(); + un_h_bridge_output2.bit_data.channel_01 = setBaseOff(); + un_h_bridge_output2.bit_data.channel_02 = setBaseOn(); +// printf("base: down\n"); + break; + + default://停止 + un_lifter_output.bit_data.cmd = BASE_STOP; + un_h_bridge_output1.bit_data.channel_01 = setBaseOff(); + un_h_bridge_output1.bit_data.channel_02 = setBaseOff(); + un_h_bridge_output2.bit_data.channel_01 = setBaseOff(); + un_h_bridge_output2.bit_data.channel_02 = setBaseOff(); +// printf("base: stop\n"); + break; + } + publishMessage(&un_h_bridge_output1, 1); + publishMessage(&un_h_bridge_output2, 1); + publishMessage(&un_lifter_output, 1); +} + + + + + +//处理开关以及模式状态。还需要保存状态防止重启的问题。 +static void baseTimerProcess(void *signal_id) +{ + (void)signal_id; + + if( (base_data.emergency_stop_switch) || (base_data.remote_emergency_stop) )//急停只要有一个有效就停止 + { + base_data.base_command = BASE_STOP; + } + else if(base_data.mode)//自动模式 一直上升就自动 + { + if(base_data.input_data > REMOTE_DEADBAND)//上升 + { + base_data.base_command = BASE_UP; + } + else if(base_data.input_data < -REMOTE_DEADBAND)//下降 + { + base_data.base_command = BASE_DOWN; + } + else//不停止 + { + } + + } + else//手动模式- + { + if(base_data.input_data > REMOTE_DEADBAND)//上升 + { + base_data.base_command = BASE_UP; + } + else if(base_data.input_data < -REMOTE_DEADBAND)//下降 + { + base_data.base_command = BASE_DOWN; + } + else + { + base_data.base_command = BASE_STOP; + } + } + baseOutput(signal_id); +} + + + + + + + + + + + + +// 处理所有输入信号的函数 +static void baseInput(void *signal_id) +{ +// BaseSystem old_data = base_data; // 定义并初始化old_data + // 填充数据 + if (signal_id == &un_sw_sample) + { + base_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch; + base_data.Left_switch = (uint8_t)un_sw_sample.bit_data.Left_switch; + base_data.right_switch = (uint8_t)un_sw_sample.bit_data.right_switch; + } + else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据 + { + base_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1; + base_data.mode = ((uint8_t)un_remote_control_input.bit_data.switch_c == 1) ? 1 : 0;// + + } + else if( (signal_id == &un_remote_control_input1) && (1 == un_remote_control_input.bit_data.enable) ) + { + base_data.input_data = (int16_t)(un_remote_control_input1.bit_data.speed) - REMOTE_ZERO; +// printf("un_remote_control_input1: %d\n", base_data.input_data); + } + else if (signal_id == &can_fault_info) + { +// base_data.remote_fault = !can_fault_info.bit_data.remote_state; +// base_data.can_bus_fault = !can_fault_info.bit_data.motor1_state || !can_fault_info.bit_data.motor2_state || !can_fault_info.bit_data.navigator_state; + } + else if (signal_id == ðernet_fault_Info) + { +// base_data.ethernet_fault = !ethernet_fault_Info.bit_data.auto_state && !ethernet_fault_Info.bit_data.manual_state; + } + baseTimerProcess(signal_id); +} + +// 修改APP模块的初始化函数 +void baseAppInit(void) +{ + // 初始化 + memset(&base_data, 0, sizeof(BaseSystem)); + + // 初始化定时器 + timerInit(&base_data.base_timer); + + // 订阅输入信号,处理刹车逻辑 + subscribe(&un_sw_sample, baseInput); + subscribe(&un_remote_control_input, baseInput); + subscribe(&un_remote_control_input1, baseInput); + + printf("app_base: initial OK \n"); +} \ No newline at end of file diff --git a/app/app_base.h b/app/app_base.h new file mode 100644 index 0000000..f56ea98 --- /dev/null +++ b/app/app_base.h @@ -0,0 +1,55 @@ +#ifndef BASE_H +#define BASE_H + +#ifdef __cplusplus +extern "C" { +#endif + + +// ɲ״̬״̬ +typedef enum { + BASE_STOP, + BASE_UP, + BASE_DOWN, +} baseState; + + + + +typedef struct BaseSystem +{ + uint32_t start_time; + Timer base_timer; // ʱṹ + uint8_t base_command; // վ0ֹͣ,1,2½ + uint8_t base_state; // վ״̬1ʾ,2ʾ½У3̶λã4½̶λ + uint8_t mode; // ңģʽ02ֶ1Զ + uint8_t Left_switch; // λ + uint8_t right_switch; // Ҳλ + uint8_t emergency_stop_switch; // ͣأ¼ͣ½ͣ + uint8_t remote_emergency_stop; // Զ̼ͣأ¼ͣ½ͣ + int16_t input_data; // ңݣ½ +} BaseSystem; + + + +#define REMOTE_ZERO 1022 + + +#define REMOTE_DEADBAND 50 + + + + +void baseAppInit(void); + + + + + + + +#ifdef __cplusplus +} +#endif + +#endif // BASE_H diff --git a/app/app_brake.c b/app/app_brake.c new file mode 100644 index 0000000..cdfd2fb --- /dev/null +++ b/app/app_brake.c @@ -0,0 +1,236 @@ +#include "app_config.h" +#include "interface.h" +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" +#include "app_param_manage.h" + +#include "app_power.h" + +#include "app_brake.h" +#include "app_differential_drive.h" + +// 使用内联函数 +static inline uint8_t setBrakeOn(void) { return 1; } +static inline uint8_t setBrakeOff(void){ return 0; } + +BrakeSystem brake_data; + +// 输出处理函数 +static void brakeOutput(void *signal_id) +{ + (void)signal_id; + // 根据电机状态，填充发送数据结构，发送信号 + switch (brake_data.brake_motor_state) + { + case 1: // 电机前进状态 + un_h_bridge_output.bit_data.channel_01 = setBrakeOn(); + un_h_bridge_output.bit_data.channel_04 = setBrakeOn(); + un_h_bridge_output.bit_data.channel_02 = setBrakeOff(); + un_h_bridge_output.bit_data.channel_03 = setBrakeOff(); + un_h_bridge_output.bit_data.sleep_01 = setBrakeOn(); + un_h_bridge_output.bit_data.sleep_02 = setBrakeOn(); // 正转 + printf("Brake: Motor forward\n"); + break; + + case 2: // 电机后退状态 + un_h_bridge_output.bit_data.channel_01 = setBrakeOff(); + un_h_bridge_output.bit_data.channel_04 = setBrakeOff(); + un_h_bridge_output.bit_data.channel_02 = setBrakeOn(); + un_h_bridge_output.bit_data.channel_03 = setBrakeOn(); + un_h_bridge_output.bit_data.sleep_01 = setBrakeOn(); + un_h_bridge_output.bit_data.sleep_02 = setBrakeOn(); // 反转 + printf("Brake: Motor reverse\n"); + break; + + default: + un_h_bridge_output.bit_data.channel_01 = setBrakeOff(); + un_h_bridge_output.bit_data.channel_04 = setBrakeOff(); + un_h_bridge_output.bit_data.channel_02 = setBrakeOff(); + un_h_bridge_output.bit_data.channel_03 = setBrakeOff(); + un_h_bridge_output.bit_data.sleep_01 = setBrakeOff(); + un_h_bridge_output.bit_data.sleep_02 = setBrakeOff(); // 关闭 + printf("Brake: Motor off\n"); + break; + } + publishMessage(&un_h_bridge_output, 1); +} + +// 判断是否需要刹车 +static uint8_t shouldApplyBrake() +{ + return (brake_data.emergency_stop_switch || + brake_data.remote_emergency_stop || + (brake_data.mode_signal == 0 && brake_data.remote_fault) || + (brake_data.mode_signal == 1 && brake_data.can_bus_fault));//20241021 修改不计算以太网故障 +// (brake_data.mode_signal == 1 && (brake_data.can_bus_fault || brake_data.ethernet_fault))); +} + +// 判断是否需要释放刹车 +static uint8_t shouldReleaseBrake() +{ + return (!brake_data.emergency_stop_switch && + !brake_data.remote_emergency_stop && + ((brake_data.mode_signal == 0 && !brake_data.remote_fault) || + (brake_data.mode_signal == 1 && !brake_data.can_bus_fault)));//20241021 修改不计算以太网故障 +// (brake_data.mode_signal == 1 && (!brake_data.can_bus_fault && !brake_data.ethernet_fault)))); +} + +// 修改刹车定时器处理函数 +static void brakeTimerProcess(void *signal_id) +{ + (void)signal_id; + + switch (brake_data.state) + { + case BRAKE_STATE_IDLE: + if (shouldApplyBrake()) + { + brake_data.state = BRAKE_STATE_APPLYING_BRAKE; + if( 0 == brake_data.brake_direction) + { + brake_data.brake_motor_state = 1; + } + else + { + brake_data.brake_motor_state = 2; + } + + brakeOutput(NULL); + timerStart(&brake_data.brake_apply_timer, (uint32_t)(getParam("brk_on")), 0); + } + break; + + case BRAKE_STATE_BRAKE_ON: + if (shouldReleaseBrake() && power_data.current_state == POWER_WORKING) + { + brake_data.state = BRAKE_STATE_RELEASING_BRAKE; + + if( 0 == brake_data.brake_direction) + { + brake_data.brake_motor_state = 2; + } + else + { + brake_data.brake_motor_state = 1; + } + brakeOutput(NULL); + timerStart(&brake_data.brake_release_timer, (uint32_t)(getParam("brk_off")), 0); + } + break; + + case BRAKE_STATE_APPLYING_BRAKE: + if (!brake_data.brake_apply_timer.active) + { + brake_data.state = BRAKE_STATE_BRAKE_ON; + brake_data.brake_motor_state = 0; + brakeOutput(NULL); + brake_data.brake_position = 1; // 刹车位置：1表示刹车 + } + break; + + case BRAKE_STATE_RELEASING_BRAKE: + if (!brake_data.brake_release_timer.active) + { + brake_data.state = BRAKE_STATE_IDLE; + brake_data.brake_motor_state = 0; + brakeOutput(NULL); + brake_data.brake_position = 0; // 刹车位置：0表示未刹车 + } + break; + + default: + printf("ERROR: Unknown state\n"); + brake_data.state = BRAKE_STATE_IDLE; + break; + } + + // 如果刹车位置有变化，存入EEPROM + if (brake_data.brake_position != brake_data.old_brake_position) + { + setParam("brk_pos", (float)brake_data.brake_position); + brake_data.old_brake_position = brake_data.brake_position; + } + + timerStart(&brake_data.brake_timer, 100, 1); // 周期调用 +} + + +// 处理所有输入信号的函数 +static void brakeInput(void *signal_id) +{ +// BrakeSystem old_data = brake_data; // 定义并初始化old_data + + // 填充数据 + if (signal_id == &un_sw_sample) + { + brake_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch; + } + else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据 + { + brake_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1; + brake_data.mode_signal = ((uint8_t)un_remote_control_input.bit_data.switch_c == 1) ? 1 : 0; + } + else if (signal_id == &can_fault_info) + { + brake_data.remote_fault = !can_fault_info.bit_data.remote_state; + brake_data.can_bus_fault = !can_fault_info.bit_data.motor1_state || !can_fault_info.bit_data.motor2_state || !can_fault_info.bit_data.navigator_state; + } + else if (signal_id == ðernet_fault_Info) + { + brake_data.ethernet_fault = !ethernet_fault_Info.bit_data.auto_state && !ethernet_fault_Info.bit_data.manual_state; + } + +} + +void paramUpdate(void *signal_id) +{ + brake_data.brake_direction = (uint8_t)getParam("brk_rev"); + timerStart(&brake_data.brake_param_timer, 1000, 1); +} + + +// 修改APP模块的初始化函数 +void brakeAppInit(void) +{ + // 初始化 + memset(&brake_data, 0, sizeof(BrakeSystem)); + brake_data.state = BRAKE_STATE_IDLE; + // 初始化时恢复刹车位置 + brake_data.brake_position = (uint8_t)getParam("brk_pos"); + brake_data.old_brake_position = brake_data.brake_position; + // 根据刹车位置恢复刹车状态 + if (brake_data.brake_position == 1) + { + brake_data.state = BRAKE_STATE_BRAKE_ON; + } + else + { + brake_data.state = BRAKE_STATE_IDLE; + } + + // 初始化定时器 + timerInit(&brake_data.brake_timer); + timerInit(&brake_data.brake_apply_timer); + timerInit(&brake_data.brake_release_timer); + timerInit(&brake_data.brake_param_timer); + + + // 订阅输入信号,处理刹车逻辑 + subscribe(&un_sw_sample, brakeInput); + subscribe(&un_remote_control_input, brakeInput); + subscribe(&can_fault_info, brakeInput); + subscribe(ðernet_fault_Info, brakeInput); + + + // 订阅定时器信号,用于状态机的定时处理 + subscribe(&brake_data.brake_timer, brakeTimerProcess); + subscribe(&brake_data.brake_apply_timer, brakeTimerProcess); + subscribe(&brake_data.brake_release_timer, brakeTimerProcess); + subscribe(&brake_data.brake_param_timer, paramUpdate); + // 启动定时器，定期调用 brakeTimerProcess + timerStart(&brake_data.brake_timer, 500, 1); + timerStart(&brake_data.brake_param_timer, 1000, 1); + + printf("app_brake: initial OK \n"); +} diff --git a/app/app_brake.h b/app/app_brake.h new file mode 100644 index 0000000..c0fd2d4 --- /dev/null +++ b/app/app_brake.h @@ -0,0 +1,57 @@ +#ifndef APP_BRAKE_H +#define APP_BRAKE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "app_config.h" +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" + + +// 定义刹车状态机状态 +typedef enum { + BRAKE_STATE_IDLE, + BRAKE_STATE_APPLYING_BRAKE, + BRAKE_STATE_RELEASING_BRAKE, + BRAKE_STATE_BRAKE_ON, +} BrakeState; + +typedef struct BrakeSystem +{ + uint32_t start_time; + Timer brake_timer; // 定时器结构体 + uint8_t brake_command; // 刹车命令变量：1表示刹车,2表示释放 + uint8_t brake_motor_state; // 刹车电机状态变量：0停止,1前进,2后退 + uint8_t brake_command_in_progress; // 刹车命令执行状态：0表示空闲,1表示正在执行 + uint8_t brake_direction; + + uint8_t emergency_stop_switch; // 急停开关 + uint8_t remote_emergency_stop; // 遥控器急停开关 + uint8_t remote_fault; // 遥控器故障 + uint8_t can_bus_fault; // CAN总线故障 + uint8_t ethernet_fault; // 以太网通信故障 + uint8_t mode_signal; // 模式信号：0表示手动模式,1表示自动模式 + BrakeState state; // 刹车状态机 + uint8_t brake_position; // 刹车位置：0表示未刹车,1表示刹车 + uint8_t old_brake_position; // 旧的刹车位置 + Timer brake_apply_timer; // 刹车定时器 + Timer brake_release_timer; // 释放刹车定时器 + Timer brake_param_timer; //参数更新时间 + +} BrakeSystem; + + +// 声明外部变量 +extern BrakeSystem brake_data; + + +void brakeAppInit(void); + +#ifdef __cplusplus +} +#endif + +#endif // APP_BRAKE_H diff --git a/app/app_config.h b/app/app_config.h new file mode 100644 index 0000000..91fa977 --- /dev/null +++ b/app/app_config.h @@ -0,0 +1,39 @@ +#ifndef APP_CONFIG_H +#define APP_CONFIG_H + + +#ifdef __cplusplus +extern "C" { +#endif + +// 标准库，每个模块中也会包含 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//外部依赖放在这里，方便分层管理，实际代码中也有这个文件，但是没有依赖 +//移植的时候，存放依赖的文件不用移植 +#include "app_dependence.h" +// 接口，全局变量都放在这里 +#include "interface.h" + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +static inline uint8_t app_close(void) { return 1; } +static inline uint8_t app_open(void) { return 0; } + + +#ifdef __cplusplus +} +#endif + + +#endif // APP_CONFIG_H diff --git a/app/app_dependence.cpp b/app/app_dependence.cpp new file mode 100644 index 0000000..7ed6e2a --- /dev/null +++ b/app/app_dependence.cpp @@ -0,0 +1,43 @@ +#include "app_dependence.h" +#include + +// 添加Qt相关头文件 +#include + +UnCanFault can_fault_info; +UnEthernetFault ethernet_fault_Info; + +unsigned int wrbyte_24c02(unsigned int addr, unsigned char data) +{ + (void)addr; + (void)data; + return 0; +} + +unsigned char rdbyte_24c02(unsigned int addr) +{ + (void)addr; + return 0; +} + +void feedWatchdog(void) +{ +} + +void udelay(uint32_t t ) +{ + (void)t; +} + +extern "C" { + uint32_t getCurrentTime(void) + { + // 获取当前时间 + QDateTime current = QDateTime::currentDateTime(); + // 转换为毫秒级时间戳 + uint32_t timestamp = static_cast(current.toMSecsSinceEpoch()); + + return timestamp; + } +} + diff --git a/app/app_dependence.h b/app/app_dependence.h new file mode 100644 index 0000000..47cd146 --- /dev/null +++ b/app/app_dependence.h @@ -0,0 +1,90 @@ +#ifndef TEST_H +#define TEST_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "irq.h" +#include "udelay\udelay.h" + +#define PERIOD_TICK 1000000.0f // 1000000us = 1s + + + + +#ifdef __cplusplus +extern "C" { +#endif + +//unsigned int wrbyte_24c02(unsigned int addr, unsigned char data); +//unsigned char rdbyte_24c02(unsigned int addr); +//void feedWatchdog(void); +//uint32_t getCurrentTime(void); +//void udelay(uint32_t t ); +// +//typedef unsigned int irq_state_t; + +//static inline irq_state_t arch_irq_save(void) +//{ +// unsigned int cpsr = 0; +// return cpsr; +//} +// +//static inline void arch_irq_restore(irq_state_t flags) +//{ +// flags += 1; +//} + +//// 故障信息 +//typedef struct _StrCanFault +//{ +// uint8_t navigator_count; // 导航仪计数器 +// uint8_t motor1_count; // 左电机计数器 +// uint8_t motor2_count; // 右电机计数器 +// uint8_t bms_count; // bms计数器 +// uint8_t temperature_count; // 温度计数器 +// uint8_t remote_count; // 遥控计数器 +// +// uint8_t navigator_state; // 导航仪故障状态 +// uint8_t motor1_state; // 电机1故障状态 +// uint8_t motor2_state; // 电机2故障状态 +// uint8_t bms_state; // bms状态 +// uint8_t temperature_state; // 温度计数器 +// uint8_t remote_state; // 遥控状态 +//} StrCanFault; + +//typedef union _UnCanFault +//{ +// StrCanFault bit_data; // 使用定义的结构体类型 +// uint8_t arr[sizeof(StrCanFault)]; // 通过结构体确定数组大小 +//} UnCanFault; + +//extern UnCanFault can_fault_info; + +//typedef struct _StrEthernetFault +//{ +// uint8_t auto_count; // 自动数据计数器 +// uint8_t manual_count; // 手动数据计数器 +// uint8_t auto_state; // 自动状态 | 0正常 1故障 +// uint8_t manual_state; // 手动状态 | 0正常 1故障 +//} StrEthernetFault; +// +//typedef union _UnEthernetFault +//{ +// StrEthernetFault bit_data; // 使用定义的结构体类型 +// uint8_t arr[sizeof(StrEthernetFault)]; // 通过结构体大小确定数组大小 +//} UnEthernetFault; + +//extern UnEthernetFault ethernet_fault_Info; + +#ifdef __cplusplus +} +#endif + +#endif // TEST_H diff --git a/app/app_differential_drive (conflicted copy 2025-07-03 101418).c b/app/app_differential_drive (conflicted copy 2025-07-03 101418).c new file mode 100644 index 0000000..620e84f --- /dev/null +++ b/app/app_differential_drive (conflicted copy 2025-07-03 101418).c @@ -0,0 +1,1103 @@ +#include "app_config.h" +#include "interface.h" + +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" +#include "app_param_manage.h" +#include "app_pid.h" + +#include "app_differential_drive.h" +#include "app_brake.h" +#include "app_power.h" + +Timer diff_app_timer; +Timer diff_dir_timer; + + +// 定义全局变量 +DiffData diff_data; + +PID_t speed_pid; +PID_t yaw_rate_pid; + + +PID_t Acc_front_speed_pid; +PID_t Dec_front_speed_pid; + + + +// 设置电机输出 +void setMotorOutput(float *out_speed, uint16_t max_torque, uint16_t feed_power, uint16_t discharge_power) +{ + float abs_speed[4] = {0.0,0.0,0.0,0.0}; +//-------------------------------------------------------------- + if(out_speed[2] > 0) + { + un_motor_output1.bit_data.gear = 1; + } + else if(out_speed[2] < 0) + { + un_motor_output1.bit_data.gear = 2; + } + else{} + + if(out_speed[0] > 0) + { + un_motor_output3.bit_data.gear = 1; + } + else if(out_speed[0] < 0) + { + un_motor_output3.bit_data.gear = 2; + } + else{} + + if(out_speed[1] > 0) + { + un_motor_output2.bit_data.gear = 1; + } + else if(out_speed[1] < 0) + { + un_motor_output2.bit_data.gear = 2; + } + else{} + + + if(out_speed[3] > 0) + { + un_motor_output4.bit_data.gear = 1; + } + else if(out_speed[3] < 0) + { + un_motor_output4.bit_data.gear = 2; + } + else{} + + +// if(diff_data.out_motor_dir != 0)//如果为0那就是空挡，那就保留之前的挡位否则以输入的速度为挡位 +// { +// un_motor_output1.bit_data.gear = diff_data.out_motor_dir; +// un_motor_output2.bit_data.gear = diff_data.out_motor_dir; +// un_motor_output3.bit_data.gear = diff_data.out_motor_dir; +// un_motor_output4.bit_data.gear = diff_data.out_motor_dir; +// } +// + +// un_motor_output1.bit_data.gear = (diff_data.out_motor_dir <= 0) ? 1 : 2; // 1 表示前进，2 表示后退 +// un_motor_output3.bit_data.gear = (diff_data.out_motor_dir <= 0) ? 1 : 2; // 1 表示前进，2 表示后退 + +// un_motor_output2.bit_data.gear = (diff_data.out_motor_dir <= 0) ? 1 : 2; +// un_motor_output4.bit_data.gear = (diff_data.out_motor_dir <= 0) ? 1 : 2; + + + for(uint8_t i = 0; i<4; i++) + { +// if(out_speed[i] < 0) //给负扭矩就发0，刹车 +// { +// out_speed[i] = 0; +// } + abs_speed[i] = fabs(out_speed[i]);//要扩大10倍 扭矩比例是10 + } + + un_motor_output1.bit_data.control_data1 = ( (uint16_t)roundf(abs_speed[2]) ); // 20240921 增加偏移量 30000//限定值为256 + un_motor_output1.bit_data.control_data2 = ( (uint16_t)roundf(abs_speed[2]) ); // 20240921 增加偏移量 30000 + un_motor_output3.bit_data.control_data1 = ( (uint16_t)roundf(abs_speed[0]) ); // 20240921 增加偏移量 30000//限定值为256 + un_motor_output3.bit_data.control_data2 = ( (uint16_t)roundf(abs_speed[0]) ); // 20240921 增加偏移量 30000 + + un_motor_output2.bit_data.control_data1 = ( (uint16_t)roundf(abs_speed[1]) ); // 20240921 增加偏移量 30000 + un_motor_output2.bit_data.control_data2 = ( (uint16_t)roundf(abs_speed[1]) ); // 20240921 增加偏移量 30000 + un_motor_output4.bit_data.control_data1 = ( (uint16_t)roundf(abs_speed[3]) ); // 20240921 增加偏移量 30000 + un_motor_output4.bit_data.control_data2 = ( (uint16_t)roundf(abs_speed[3]) ); // 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.motor_direction = 1;//电机2和3是反相的 +// un_motor_output2.bit_data.motor_direction = 1; + un_motor_output1.bit_data.motor_direction = 1;//电机2和3是反相的 + un_motor_output2.bit_data.motor_direction = 0; + un_motor_output3.bit_data.motor_direction = 1; + un_motor_output4.bit_data.motor_direction = 0; + + + // can挡位一直有效 + un_motor_output1.bit_data.can_gear = 1; + un_motor_output2.bit_data.can_gear = 1; + un_motor_output3.bit_data.can_gear = 1; + un_motor_output4.bit_data.can_gear = 1; + + un_motor_output1.bit_data.can_break = 1; + un_motor_output2.bit_data.can_break = 1; + un_motor_output3.bit_data.can_break = 1; + un_motor_output4.bit_data.can_break = 1; +} + +// 限制值在最小值和最大值之间 +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 gear_ratio = (float)getParam("gRatio"); + if (fabsf(gear_ratio) < EPSILON) + { + return 0; // 避免除以0的情况 + } + // 将电机转速 (RPM) 转换为线速度 (m/s)，考虑减速比 + float left_speed_mps = (diff_data.left_motor_speed * wheel_circumference) / (60.0f * gear_ratio); + float right_speed_mps = (diff_data.right_motor_speed * wheel_circumference) / (60.0f * gear_ratio); + // 计算当前速度 + diff_data.speed = (left_speed_mps + right_speed_mps) / 2.0f; + // 计算速度差 + float speed_diff = left_speed_mps - right_speed_mps; + // 计算角速度 + float wheel_base = (float)getParam("whl_bas"); + if (fabsf(wheel_base) < EPSILON) + { + return 0; // 避免除以0的情况 + } + diff_data.yaw_rate = speed_diff / wheel_base; + + return 0; +} + + +// 计算加速度 +float calculateAcceleration(float speed, float previous_speed, float dt) +{ + if (fabs(dt) < EPSILON) + { + return 0; // 避免除以0的情况 + } + float acceleration = (speed - previous_speed) / dt; + return acceleration; +} + +// 计算减速度 +float calculateDeceleration(float speed, float previous_speed, float dt) +{ + if (fabs(dt) < EPSILON) + { + return 0; // 避免除以0的情况 + } + float deceleration = calculateAcceleration(previous_speed, speed, dt); // 减速度就是负的加速度 + return deceleration; +} + +// 计算最大速度 +float calculateMaxSpeed() +{ + // 获取最大电机转速 (RPM) + float max_rpm = (float)getParam("max_rpm"); + // 获取轮子周长 + float wheel_circumference = (float)getParam("whl_dia") * M_PI; + // 获取减速比 + float gear_ratio = (float)getParam("gRatio"); + if (fabsf(gear_ratio) < EPSILON) + { + return 0; // 避免除以0的情况 + } + // 将最大电机转速 (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"); // 减速比 + if (fabsf(wheel_radius) < EPSILON || fabsf(vehicle_mass) < EPSILON ) + { + return 0; // 避免除以0的情况 + } + // 减速比计算扭矩 + float effective_torque = max_motor_torque * gear_ratio; + // 计算最大加速度 + float max_acceleration = (effective_torque / wheel_radius) / vehicle_mass; + return max_acceleration; +} + +// 计算当前状态，包括当前速度、角速度、加速度、减速度、最大速度 +void calculateCurrentState(float dt) +{ + static float previous_speed = 0.0f; + // 更新当前速度和当前角速度 + calculateCurrentSpeedYawRate(); + // 更新加速度、减速度等，根据需要计算 + diff_data.acceleration = calculateAcceleration(diff_data.speed, previous_speed, dt); + diff_data.deceleration = calculateDeceleration(diff_data.speed, previous_speed, dt); + diff_data.max_speed = calculateMaxSpeed(); + previous_speed = diff_data.speed; +} + + +/** + * @brief 根据轮速差动态调整电机扭矩（带非负限制） + * @param speed_left 左轮速度（单位：rpm或自定义） + * @param speed_right 右轮速度（单位：rpm或自定义） + * @param torque_left 左轮扭矩指针（单位：Nm或自定义） + * @param torque_right 右轮扭矩指针（单位：Nm或自定义） + * @param threshold 触发调整的速差阈值（单位同轮速） + * @param k 扭矩调整系数（无量纲，建议0 threshold) { + // 计算需要减少的扭矩量（速差超出阈值部分×系数） + float torque_reduction = (speed_diff - threshold) * k; + + if (speed_left > speed_right) { + // 左轮过快时减少左扭矩，并限制最小值为0 + *torque_left = fmaxf(*torque_left - torque_reduction, 0.0f); + } else { + // 右轮过快时减少右扭矩，并限制最小值为0 + *torque_right = fmaxf(*torque_right - torque_reduction, 0.0f); + } + } +} + +// 计算左右电机速度 +void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max_speed, float *motor_speed) +{ + // 防止速度过低导致不必要的计算 + if (fabs(max_speed) < EPSILON) + { + motor_speed[0] = 0.0f; + motor_speed[1] = 0.0f; + motor_speed[2] = 0.0f; + motor_speed[3] = 0.0f; + return; + } + +#if THROTTLE_PID_MODE + float max_torque = (float)getParam("maxTorq"); + + linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque); + yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque); + + float left_speed_mps = linear_velocity_x + yaw_rate; + float right_speed_mps = linear_velocity_x - yaw_rate; + +//扭矩分配 + if(max_torque < left_speed_mps) + { + right_speed_mps = right_speed_mps - (left_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差 + left_speed_mps = max_torque; + } + else if(-max_torque > left_speed_mps) + { + right_speed_mps = right_speed_mps - (left_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差 + left_speed_mps = -max_torque; + } + else if(max_torque < right_speed_mps) + { + left_speed_mps = left_speed_mps - (right_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差 + right_speed_mps = max_torque; + } + else if(-max_torque > right_speed_mps) + { + left_speed_mps = left_speed_mps - (right_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差 + right_speed_mps = -max_torque; + } + else{} + + // 返回计算结果 + motor_speed[0] = left_speed_mps; + motor_speed[2] = left_speed_mps; + + motor_speed[1] = right_speed_mps; + motor_speed[3] = right_speed_mps; + +// adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],50, 0.2); +// adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],50, 0.2); + +#else + // 限制线速度和偏航率 + linear_velocity_x = constrain(linear_velocity_x, -max_speed, max_speed); + float max_yaw_rate = max_speed / ((float)getParam("whl_bas") / 2.0f); + yaw_rate = constrain(yaw_rate, -max_yaw_rate, max_yaw_rate); + + // 计算旋转速度 + float rotational_velocity = ((float)getParam("whl_bas") / 2.0f) * yaw_rate; + + // 计算车辆左右线速度 (m/s) + float left_speed_mps = linear_velocity_x + rotational_velocity; + float right_speed_mps = linear_velocity_x - rotational_velocity; + + // 计算轮子周长 + float wheel_circumference = (float)getParam("whl_dia") * M_PI; + + // 将车辆左右线速度转换为轮子转速 (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 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"); + left_motor_rpm = constrain(left_motor_rpm, -max_motor_rpm, max_motor_rpm); + right_motor_rpm = constrain(right_motor_rpm, -max_motor_rpm, max_motor_rpm); + + // 当电机转速小于50转时，设置为0 + if (fabsf(left_motor_rpm) < 50)//速度慢所以设置位10转 + { + left_motor_rpm = 0; + } + if (fabsf(right_motor_rpm) < 50)//速度慢所以设置位10转 + { + right_motor_rpm = 0; + } + + // 返回计算结果 + *left_motor_speed = left_motor_rpm; + *right_motor_speed = right_motor_rpm; +#endif + +} + +// 映射遥控器速度，分为死区、低速区和高速区。 +float mapRemoteControlSpeed( + float input_speed, + float deadzone_limit, + float input_max, + float output_max, + float input_slow, + float output_slow +) +{ + float output_speed = 0.0f; + // 获取输入速度的绝对值 + float abs_input = fabsf(input_speed); + + if (abs_input < deadzone_limit + EPSILON) + { + output_speed = 0.0f;// 死区 + } + else if (abs_input < input_slow + EPSILON)// 低速区 + { + output_speed = (abs_input - deadzone_limit) * output_slow / (input_slow - deadzone_limit); + } + else if (abs_input <= input_max + EPSILON)// 高速区 + { + output_speed = output_slow + (abs_input - input_slow) * (output_max - output_slow) / (input_max - input_slow); + } + else // 超出范围 + { + output_speed = output_max; + } + // 根据原始输入速度的符号恢复方向 + if (input_speed < 0) + { + output_speed = -output_speed; + } + return output_speed; +} + + +// 差速处理函数 +static void diffProcess(void *signal_id) +{ + (void)signal_id; + static float previous_time1 = 0.0f; + + float time1 = (float)getCurrentTime(); + float dt = (time1 - previous_time1) / PERIOD_TICK; + previous_time1 = time1; + + // 计算当前状态，包括当前速度、角速度、加速度、减速度、最大速度 + calculateCurrentState(dt); + + // 当速度小于1时，设定为原地转向 + if (fabsf(diff_data.desired_speed) <= 1.0f) + { + diff_data.desired_yaw_rate = diff_data.desired_curvature * 1.0f; // 原地转向时，设定一个固定的偏航率 + } + else + { + diff_data.desired_yaw_rate = diff_data.desired_curvature * diff_data.desired_speed; + } + + + if( ( (diff_data.desired_speed > 0) && (diff_data.speed > 0) ) || ( (diff_data.desired_speed < 0) && (diff_data.speed < 0) ) )//同相 + { + if( fabs(diff_data.desired_speed) > fabs(diff_data.speed) )//期望大于当前速度，表示加速，所以用大PID + { + setPidParameters(&speed_pid, + Acc_front_speed_pid.kp, + Acc_front_speed_pid.ki, + Acc_front_speed_pid.kd, + Acc_front_speed_pid.integral_limit, + Acc_front_speed_pid.output_limit + ); + } + else + { + setPidParameters(&speed_pid, + Dec_front_speed_pid.kp, + Dec_front_speed_pid.ki, + Dec_front_speed_pid.kd, + Dec_front_speed_pid.integral_limit, + Dec_front_speed_pid.output_limit + ); + + } + }//反相就先小PID减速 + else + { + setPidParameters(&speed_pid, + Dec_front_speed_pid.kp, + Dec_front_speed_pid.ki, + Dec_front_speed_pid.kd, + Dec_front_speed_pid.integral_limit, + Dec_front_speed_pid.output_limit + ); + } + + + +// float positive_speed = fabs(diff_data.desired_speed);//取绝对值 20250701 扭矩控制不需要考虑正负 +// +// if(diff_data.desired_speed < 0)//判断方向 小于0就反转，大于0电机正转 +// { +// diff_data.out_motor_dir = 2; +// } +// else if(diff_data.desired_speed > 0) +// { +// diff_data.out_motor_dir = 1; +// } +// else +// { +// diff_data.out_motor_dir = 0; +// } + + // 使用 PID 控制器计算输出速度和曲率 + float output_speed = calculatePidOutput(&speed_pid, diff_data.desired_speed, diff_data.speed, 0.0f, dt); + + float output_yaw_rate = calculatePidOutput(&yaw_rate_pid, diff_data.desired_yaw_rate, diff_data.yaw_rate, 0.0f, dt); + + // 计算最大加速度，用函数计算 + float max_acceleration = calculateMaxAcceleration(); + +// if( ( (output_speed > 0) && (diff_data.speed < 0) ) || ( (output_speed < 0) && (diff_data.speed > 0) ) )//方向相反不给反方向扭矩，直接清0，给反方向扭矩转速有问题 +// { +// if( ( (diff_data.desired_speed > 0) && (diff_data.speed < 0) ) || ( (diff_data.desired_speed < 0) && (diff_data.speed > 0) ) )//期望和当前速度相反的话，那直接运行反方向扭矩，解决下坡不能减速的问题。 +// { +// +// } +// else +// { +// output_speed = 0; +// resetPidIntegral(&speed_pid); +// } +// } +// +// if( (0 == output_speed) && (diff_data.desired_speed != 0) )//速度超过解决发0直接减速的问题 +// { +// output_speed = 1; +// } + + + + // 限制输出速度在当前速度和最大加速度计算出来的速度之间 + // output_speed = constrain(output_speed, diff_data.speed - max_acceleration * dt, diff_data.speed + max_acceleration * dt); + +// printf("output_speed: %f, output_yaw: %f\n", output_speed, output_yaw_rate); + +// if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) && (diff_data.left_motor_speed < 50.0f) && (diff_data.right_motor_speed > -50.0f))//手柄回中，速度小的时候清0 +// { +// resetPidIntegral(&speed_pid); +// resetPidIntegral(&yaw_rate_pid); +// output_speed = 0; +// output_yaw_rate = 0; +// } +// + + if( (0 == diff_data.desired_speed) && ( fabs(diff_data.left_motor_speed) < 10 ) && ( fabs(diff_data.right_motor_speed) < 10) )//手柄回中，速度小的时候清0 + { + resetPidIntegral(&speed_pid); + resetPidIntegral(&yaw_rate_pid); + output_speed = 0; + output_yaw_rate = 0; + } + + // 使用差速车辆动力学模型计算左右电机的期望速度 + computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, &diff_data.out_motor_speed[0]); + +// diff_data.out_motor_speed[0] = calculatePidOutput(&Left_front_speed_pid, diff_data.out_left_motor_speed, diff_data.left_front_motor_speed, 0.0f, dt); +// diff_data.out_motor_speed[1] = calculatePidOutput(&right_front_speed_pid, diff_data.out_right_motor_speed, diff_data.right_front_motor_speed, 0.0f, dt); +// diff_data.out_motor_speed[2] = calculatePidOutput(&Left_Rear_speed_pid, diff_data.out_left_motor_speed, diff_data.left_rear_motor_speed, 0.0f, dt); +// diff_data.out_motor_speed[3] = calculatePidOutput(&right_Rear_speed_pid, diff_data.out_right_motor_speed, diff_data.right_rear_motor_speed, 0.0f, dt); + +// diff_data.out_motor_speed[0] = diff_data.out_left_motor_speed; +// diff_data.out_motor_speed[2] = diff_data.out_left_motor_speed; +// +// diff_data.out_motor_speed[1] = diff_data.out_right_motor_speed; +// diff_data.out_motor_speed[3] = diff_data.out_right_motor_speed; + + // 设置电机输出 + setMotorOutput(&diff_data.out_motor_speed[0], + diff_data.max_Torq, + (uint16_t)getParam("feedPwr"), + (uint16_t)getParam("dispPwr")); + // 发布左右电机期望转速,电源在工作状态才能发送 + if (power_data.current_state == POWER_WORKING) + { + publishMessage(&un_motor_output1, 1); + publishMessage(&un_motor_output2, 1); + } + publishMessage(&diff_data, 1); +} + + +/** + * @brief 电机状态处理函数 + * @param ctx 电机状态上下文指针，包含当前状态、转速和输出扭矩数据 + * @param original_dir 电机原始方向 + * @note 该函数实现基于转速和扭矩方向的状态转换逻辑： + * - 初始态：转速为0时根据扭矩方向切换前进/后退状态 + * - 前进/后退态：转速归零时返回初始态 + * - 默认处理：异常状态自动复位到初始态 + * @warning 需确保ctx->out_motor_speed[0]已正确初始化（正数表前进，负数表后退） + */ +void handleMotorState(DiffData *ctx,uint8_t original_dir) +{ + switch (ctx->state) + { + case STATE_INIT://初始 + { + if (ctx->speed == 0.0f) + { + if (ctx->out_motor_speed[0] > 0.0f) // 修改为数组索引 + { + ctx->state = STATE_FORWARD; // 扭矩为正 → 前进 + ctx->motor_dir = FORWARD; + } + else if (ctx->out_motor_speed[0] < 0.0f) // 修改为数组索引 + { + ctx->state = STATE_BACKWARD; // 扭矩为负 → 后退 + ctx->motor_dir = BACKWARD; + } + else + { + ctx->state = STATE_INIT; + } + } + else//停下来以后，还出现速度，说明溜坡或者外力接入，直接用实际电机方向 + { + if(1 == original_dir) + { + ctx->motor_dir = FORWARD; + ctx->state = STATE_FORWARD; // 保持前进 + } + else + { + ctx->motor_dir = BACKWARD; + ctx->state = STATE_BACKWARD; // 保持后退 + } + } + + break; + } + case STATE_FORWARD://前进 + { + if ( (ctx->speed == 0.0f) && (ctx->out_motor_speed[0] == 0.0f) ) + { + ctx->state = STATE_INIT; + } + else if( (ctx->speed == 0.0f) && (ctx->out_motor_speed[0] < 0.0f) )//直接给反相扭矩减速 + { + ctx->motor_dir = BACKWARD; + ctx->state = STATE_BACKWARD; // 保持后退 + } + else + { + ctx->motor_dir = FORWARD; + ctx->state = STATE_FORWARD; // 保持前进 + } + break; + } + +// case DEC_FORWARD://前进减速 +// { +// if(ctx->speed > 0)//停下来以后，还出现速度，说明溜坡或者外力接入，直接用实际电机方向 +// { +// ctx->motor_dir = BACKWARD; +// ctx->state = STATE_BACKWARD; // 保持后退 +// } +// else +// { +// ctx->state = STATE_INIT; +// } +// break; +// } +// + case STATE_BACKWARD://后退 + { + if ( (ctx->speed == 0.0f) && (ctx->out_motor_speed[0] == 0.0f) ) + { + ctx->state = STATE_INIT; + } + else if( (ctx->speed == 0.0f) && (ctx->out_motor_speed[0] > 0.0f) )//直接给反相扭矩减速 + { + ctx->motor_dir = FORWARD; + ctx->state = STATE_FORWARD; // 保持前进 + } + else + { + ctx->motor_dir = BACKWARD; + ctx->state = STATE_BACKWARD; // 保持后退 + } + break; + } + + // 异常处理 + default: + { + ctx->state = STATE_INIT; + break; + } + } +} + + +/** + * @brief 速度方向校准函数（需每100ms调用一次） + */ +void calibrateSpeedDirection(DiffData *ctx) +{ + switch (ctx->dir_state) + { + //------------------------------------------- + // 初始状态：检测速度与扭矩方向是否冲突 + //------------------------------------------- + case STATE_CALIB_INIT: + { + if (ctx->speed * ctx->out_motor_speed[0] < 0) + { + // 方向冲突，保存当前值并进入判断状态 + ctx->speed_last = ctx->speed; + ctx->torq_last = ctx->out_motor_speed[0]; + ctx->counter = 0; + ctx->dir_state = STATE_CALIB_JUDGE; + } + break; + } + + //------------------------------------------- + // 判断状态：持续300ms验证方向异常 + //------------------------------------------- + case STATE_CALIB_JUDGE: + { + bool is_same_dir = (ctx->speed * ctx->speed_last > 0) && + (ctx->out_motor_speed[0] * ctx->torq_last > 0); + bool is_value_grow = (fabs(ctx->speed) > fabs(ctx->speed_last)) && + (fabs(ctx->out_motor_speed[0]) > fabs(ctx->torq_last)); + + if (is_same_dir && is_value_grow) + { + ctx->counter ++; + ctx->dir_state = STATE_CALIB_JUDGE; //保持当前状态 + if (ctx->counter >= 3) + { + // 300ms验证通过，强制反向以及切换状态 + if(BACKWARD == ctx->motor_dir) + { + ctx->motor_dir = FORWARD; + ctx->state = STATE_FORWARD; + } + else + { + ctx->motor_dir = BACKWARD; + ctx->state = STATE_BACKWARD; + } + ctx->dir_state = STATE_CALIB_INIT; + } + ctx->speed_last = ctx->speed;//判断完之后，继续保存 + ctx->torq_last = ctx->out_motor_speed[0]; + } + else + { + // 验证失败回初始态 + ctx->dir_state = STATE_CALIB_INIT; + } + break; + } + } +} + +void diff100ms(void *signal_id) +{ +// calibrateSpeedDirection(&diff_data); + timerStart(&diff_dir_timer,100,1);//100ms调用一次 +} + + +// 差速输入处理函数 +static void diffInput(void *signal_id) +{ + if (signal_id == &un_sw_sample) + { + diff_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch; + } + else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据 + { + 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 == 1 ? MODE_AUTO : MODE_MANUAL; + + if (diff_data.mode == MODE_MANUAL) + { + diff_data.desired_speed = (float)((int16_t)(un_remote_control_input.bit_data.speed)); + diff_data.desired_curvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature)); + // 单位转换 + diff_data.desired_speed = diff_data.desired_speed * 0.01f; + diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f; + // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出 + diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5); + diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0.01, 2, 2, 1, 0.5); + diff_data.desired_curvature = diff_data.desired_curvature; + } + } + else if ( (signal_id == &un_manual_computer_input) && (diff_data.mode == MODE_AUTO) ) + { + diff_data.desired_speed = (float)((int16_t)(un_manual_computer_input.bit_data.set_speed)); + diff_data.desired_curvature = (float)((int16_t)(un_manual_computer_input.bit_data.set_curvature)); + // 单位转换 + diff_data.desired_speed = diff_data.desired_speed * 0.01f; + diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f; + // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出 + diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0, 2, 10, 1, 5); + diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1); + } + else if ( (signal_id == &un_auto_computer_input) && (diff_data.mode == MODE_AUTO) ) + { + diff_data.desired_speed = (float)((int16_t)(un_auto_computer_input.bit_data.set_speed)); + diff_data.desired_curvature = (float)((int16_t)(un_auto_computer_input.bit_data.set_curvature)); + // 单位转换 + diff_data.desired_speed = diff_data.desired_speed * 0.01f; + diff_data.desired_curvature = - diff_data.desired_curvature * 0.0001f;// 20241016 增加转弯反相 + // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出 + diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0, 5, 10, 2.5, 5); + diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1); + } + else if( (signal_id == &un_motor_input1) || (signal_id == &un_motor_input3) )// 处理第一个电机速度信号（左侧电机） + { + float speed_temp = fabs ( (float) ( (int16_t)(un_motor_input1.bit_data.speed) ) ); + + if(1 == diff_data.motor_dir)//1为负数 20250620 修改为判断电机转动方向， 挡位不可靠 + { + diff_data.left_rear_motor_speed = -speed_temp; + } + else + { + diff_data.left_rear_motor_speed = speed_temp; + } + + + if(fabs(diff_data.left_rear_motor_speed) < SWITCH_SPEED) + { + diff_data.left_rear_motor_speed = 0; + } + + + speed_temp = fabs ( (float) ( (int16_t)(un_motor_input3.bit_data.speed) ) ); + if(1 == diff_data.motor_dir)//1为负数 + { + diff_data.left_front_motor_speed = -speed_temp; + } + else + { + diff_data.left_front_motor_speed = speed_temp; + } + + if(fabs(diff_data.left_front_motor_speed) < SWITCH_SPEED)//死区 + { + diff_data.left_front_motor_speed = 0; + } + + if(fabs(diff_data.left_front_motor_speed) < fabs(diff_data.left_rear_motor_speed) )//取最小值,避免打滑，拉高转速 + { + diff_data.left_motor_speed = diff_data.left_front_motor_speed; + } + else + { + diff_data.left_motor_speed = diff_data.left_rear_motor_speed; + } + +// printf("motor1: %f, motor3: %f\n", diff_temp[0], diff_temp[1]); +// diff_data.left_motor_speed = 0;// 左侧两个电机所以取平均值 + + } + else if( (signal_id == &un_motor_input2) || (signal_id == &un_motor_input4) )// 处理第二个电机速度信号（右侧电机） + { + + float speed_temp = fabs ( (float) ( (int16_t)( un_motor_input2.bit_data.speed) ) ); + if(1 == diff_data.motor_dir)//1为负数 + { + diff_data.right_front_motor_speed = - speed_temp; + } + else + { + diff_data.right_front_motor_speed = speed_temp; + } +// + if(fabs(diff_data.right_front_motor_speed) < SWITCH_SPEED)//死区 + { + diff_data.right_front_motor_speed = 0; + } + + + speed_temp = fabs( (float) ( (int16_t)(un_motor_input4.bit_data.speed) ) ); + + if(1 == diff_data.motor_dir)//1为负数 + { + diff_data.right_rear_motor_speed = - speed_temp; + } + else + { + diff_data.right_rear_motor_speed = speed_temp; + } + + if(fabs(diff_data.right_rear_motor_speed) < SWITCH_SPEED)//死区 + { + diff_data.right_rear_motor_speed = 0; + } + + if( fabs(diff_data.right_front_motor_speed) < fabs(diff_data.right_rear_motor_speed) )//取最小值,避免打滑，拉高转速 + { + diff_data.right_motor_speed = diff_data.right_front_motor_speed; + } + else + { + diff_data.right_motor_speed = diff_data.right_rear_motor_speed; + } + +// diff_data.right_motor_speed = 0;// 左侧两个电机所以取平均值 + + } + else{} + +// printf("rightspeed: %f, leftspeed: %f\n", diff_data.right_motor_speed, diff_data.left_motor_speed); + // 急停开关 + diff_data.emergency_stop_state = (uint8_t)(diff_data.emergency_stop_switch == app_close() || diff_data.remote_emergency_stop == app_close()); + + // 如果急停被激活,强制设定速度为0,急停包括车上急停开关和遥控器急停开关 + if (diff_data.emergency_stop_state == 1) + { + diff_data.desired_speed = 0.0; + diff_data.desired_curvature = 0.0; + } + // 遥控器断线，而且是在手动模式，期望值清0 + if ( (diff_data.mode == MODE_MANUAL) && (0 == un_remote_control_input.bit_data.enable) ) + { + diff_data.desired_speed = 0.0; + diff_data.desired_curvature = 0.0; + } + +// if (diff_data.emergency_stop_state == 1)//刹车 20241017 增加的扭矩限制 +// { +// diff_data.max_Torq = 5;//20240403修改。刹车就是5N +// } +// else if ((0 == diff_data.desired_speed) && (0 == diff_data.desired_curvature) && ((diff_data.left_motor_speed > -100) && (diff_data.left_motor_speed < 100)) && (((diff_data.right_motor_speed > -100) && (diff_data.right_motor_speed < 100))))//20240330只有当手柄回中，然后当前已经停止的状态才设置为最小停车扭矩 +// { +// diff_data.max_Torq = 0;//停车 就为0 +// } +// else +// { + +// } + + diff_data.speed_inter = (uint16_t)(int16_t)speed_pid.integral; + diffProcess(&diff_data);//计算左右电机期望转速 + + + printf("dir_state: %d\n", diff_data.state); + handleMotorState(&diff_data,un_motor_input3.bit_data.rotation_dir);//方向判断函数 20250630增加 + + publishMessage(&diff_data, 1); +} + + + +// 预充完成处理函数 +void preChargeFinish(void *signal_id) +{ + (void)signal_id; + + for(uint8_t i = 0; i<4; i++) + { + diff_data.out_motor_speed[i] = 0; + } + + setMotorOutput(&diff_data.out_motor_speed[0],(uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr")); + // 档位 + un_motor_output1.bit_data.gear = 0; // 0表示空挡 + un_motor_output2.bit_data.gear = 0; + publishMessage(&un_motor_output1, 1); + publishMessage(&un_motor_output2, 1); +} + + +void diffParametersInit(void *signal_id) +{ + (void)signal_id; // 标记变量为已使用，避免编译器警告 +// setPidParameters(&speed_pid, +// getParam("spd_kp"), +// getParam("spd_ki"), +// getParam("spd_kd"), +// getParam("spd_il"), +// getParam("spd_ol") +// ); + + // 设置曲率 PID 控制器的参数 + setPidParameters(&yaw_rate_pid, + getParam("crv_kp"), + getParam("crv_ki"), + getParam("crv_kd"), + getParam("crv_il"), + getParam("crv_ol") + ); + + // 设置曲率 PID 控制器的参数 + setPidParameters(&Dec_front_speed_pid, + getParam("mot_kp"), + getParam("mot_ki"), + getParam("mot_kd"), + getParam("mot_il"), + getParam("mot_ol") + ); + + // 设置曲率 PID 控制器的参数 + setPidParameters(&Acc_front_speed_pid, + getParam("spd_kp"), + getParam("spd_ki"), + getParam("spd_kd"), + getParam("spd_il"), + getParam("spd_ol") + ); + + diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩 + printf("PID_P: %f, PID_I: %f\n", speed_pid.kp, speed_pid.ki); + + printf("left_front: %f, right_front: %f, left_rear: %f, right_rear: %f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed,diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed); + + printf("left_front_tou: %f, right_front_tou: %f, left_rear_tou: %f, right_rear_tou: %f\n", diff_data.out_motor_speed[0], diff_data.out_motor_speed[2],diff_data.out_motor_speed[2], diff_data.out_motor_speed[3]); + + + timerStart(&diff_app_timer,1000,1);//1s调用一次 +} + + +// 差速初始化函数 +void diffAppInit(void) +{ + // 初始化 diff_data + memset(&diff_data, 0, sizeof(DiffData)); + + // 订阅相关信号 + subscribe(&un_sw_sample, 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); + subscribe(&power_data.pre_charge_finish, preChargeFinish); + + // 初始化速度 PID 控制器 + initializePid(&speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); + // 设置速度 PID 控制器的参数 + setPidParameters(&speed_pid, + getParam("spd_kp"), + getParam("spd_ki"), + getParam("spd_kd"), + getParam("spd_il"), + getParam("spd_ol") + ); + + + + // 初始化曲率 PID 控制器 + initializePid(&yaw_rate_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); + // 设置曲率 PID 控制器的参数 + setPidParameters(&yaw_rate_pid, + getParam("crv_kp"), + getParam("crv_ki"), + getParam("crv_kd"), + getParam("crv_il"), + getParam("crv_ol") + ); + + + // 初始化减速 PID 控制器 + initializePid(&Dec_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); + // 设置曲率 PID 控制器的参数 + setPidParameters(&Dec_front_speed_pid, + getParam("mot_kp"), + getParam("mot_ki"), + getParam("mot_kd"), + getParam("mot_il"), + getParam("mot_ol") + ); + + // 初始化加速 PID 控制器 + initializePid(&Acc_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); + // 设置曲率 PID 控制器的参数 + setPidParameters(&Acc_front_speed_pid, + getParam("spd_kp"), + getParam("spd_ki"), + getParam("spd_kd"), + getParam("spd_il"), + getParam("spd_ol") + ); + + + subscribe(&diff_app_timer, diffParametersInit); + timerStart(&diff_app_timer,1000,1);//1s调用一次 + + + subscribe(&diff_dir_timer, diff100ms); + timerStart(&diff_dir_timer,100,1);//100ms调用一次 + + + printf("diffControl: diffAppInit OK \n"); +} diff --git a/app/app_differential_drive.c b/app/app_differential_drive.c new file mode 100644 index 0000000..da0c296 --- /dev/null +++ b/app/app_differential_drive.c @@ -0,0 +1,1364 @@ +#include "app_config.h" +#include "interface.h" + +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" +#include "app_param_manage.h" +#include "app_pid.h" + +#include "app_differential_drive.h" +#include "app_brake.h" +#include "app_power.h" + +Timer diff_app_timer; +Timer diff_dir_timer; + + +// 定义全局变量 +DiffData diff_data; + +PID_t speed_pid; +PID_t yaw_rate_pid; + + +PID_t Acc_front_speed_pid; +PID_t Dec_front_speed_pid; + + + +// 设置电机输出 +void setMotorOutput(float *out_speed, uint16_t max_torque, uint16_t feed_power, uint16_t discharge_power) +{ + float abs_speed[4] = {0.0,0.0,0.0,0.0}; +//-------------------------------------------------------------- + if(out_speed[2] > 0) + { + un_motor_output1.bit_data.gear = 1; + } + else if(out_speed[2] < 0) + { + un_motor_output1.bit_data.gear = 2; + } + else{} + + if(out_speed[0] > 0) + { + un_motor_output3.bit_data.gear = 1; + } + else if(out_speed[0] < 0) + { + un_motor_output3.bit_data.gear = 2; + } + else{} + + if(out_speed[1] > 0) + { + un_motor_output2.bit_data.gear = 1; + } + else if(out_speed[1] < 0) + { + un_motor_output2.bit_data.gear = 2; + } + else{} + + + if(out_speed[3] > 0) + { + un_motor_output4.bit_data.gear = 1; + } + else if(out_speed[3] < 0) + { + un_motor_output4.bit_data.gear = 2; + } + else{} + + +// if(diff_data.out_motor_dir != 0)//如果为0那就是空挡，那就保留之前的挡位否则以输入的速度为挡位 +// { +// un_motor_output1.bit_data.gear = diff_data.out_motor_dir; +// un_motor_output2.bit_data.gear = diff_data.out_motor_dir; +// un_motor_output3.bit_data.gear = diff_data.out_motor_dir; +// un_motor_output4.bit_data.gear = diff_data.out_motor_dir; +// } +// + +// un_motor_output1.bit_data.gear = (diff_data.out_motor_dir <= 0) ? 1 : 2; // 1 表示前进，2 表示后退 +// un_motor_output3.bit_data.gear = (diff_data.out_motor_dir <= 0) ? 1 : 2; // 1 表示前进，2 表示后退 + +// un_motor_output2.bit_data.gear = (diff_data.out_motor_dir <= 0) ? 1 : 2; +// un_motor_output4.bit_data.gear = (diff_data.out_motor_dir <= 0) ? 1 : 2; + + + for(uint8_t i = 0; i<4; i++) + { +// if(out_speed[i] < 0) //给负扭矩就发0，刹车 +// { +// out_speed[i] = 0; +// } + abs_speed[i] = fabs(out_speed[i]);//要扩大10倍 扭矩比例是10 + } + + un_motor_output1.bit_data.control_data1 = ( (uint16_t)roundf(abs_speed[2]) ); // 20240921 增加偏移量 30000//限定值为256 + un_motor_output1.bit_data.control_data2 = ( (uint16_t)roundf(abs_speed[2]) ); // 20240921 增加偏移量 30000 + un_motor_output3.bit_data.control_data1 = ( (uint16_t)roundf(abs_speed[0]) ); // 20240921 增加偏移量 30000//限定值为256 + un_motor_output3.bit_data.control_data2 = ( (uint16_t)roundf(abs_speed[0]) ); // 20240921 增加偏移量 30000 + + un_motor_output2.bit_data.control_data1 = ( (uint16_t)roundf(abs_speed[1]) ); // 20240921 增加偏移量 30000 + un_motor_output2.bit_data.control_data2 = ( (uint16_t)roundf(abs_speed[1]) ); // 20240921 增加偏移量 30000 + un_motor_output4.bit_data.control_data1 = ( (uint16_t)roundf(abs_speed[3]) ); // 20240921 增加偏移量 30000 + un_motor_output4.bit_data.control_data2 = ( (uint16_t)roundf(abs_speed[3]) ); // 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.motor_direction = 1;//电机2和3是反相的 + un_motor_output2.bit_data.motor_direction = 1; +// un_motor_output1.bit_data.motor_direction = 1;//电机2和3是反相的 +// un_motor_output2.bit_data.motor_direction = 0; + un_motor_output3.bit_data.motor_direction = 0; + un_motor_output4.bit_data.motor_direction = 0; + + + // can挡位一直有效 + un_motor_output1.bit_data.can_gear = 1; + un_motor_output2.bit_data.can_gear = 1; + un_motor_output3.bit_data.can_gear = 1; + un_motor_output4.bit_data.can_gear = 1; + + un_motor_output1.bit_data.can_break = 1; + un_motor_output2.bit_data.can_break = 1; + un_motor_output3.bit_data.can_break = 1; + un_motor_output4.bit_data.can_break = 1; +} + +// 限制值在最小值和最大值之间 +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 gear_ratio = (float)getParam("gRatio"); + if (fabsf(gear_ratio) < EPSILON) + { + return 0; // 避免除以0的情况 + } + // 将电机转速 (RPM) 转换为线速度 (m/s)，考虑减速比 + float left_speed_mps = (diff_data.left_motor_speed * wheel_circumference) / (60.0f * gear_ratio); + float right_speed_mps = (diff_data.right_motor_speed * wheel_circumference) / (60.0f * gear_ratio); + // 计算当前速度 + diff_data.speed = (left_speed_mps + right_speed_mps) / 2.0f; + // 计算速度差 + float speed_diff = left_speed_mps - right_speed_mps; + // 计算角速度 + float wheel_base = (float)getParam("whl_bas"); + if (fabsf(wheel_base) < EPSILON) + { + return 0; // 避免除以0的情况 + } + diff_data.yaw_rate = speed_diff / wheel_base; + + return 0; +} + + +// 计算加速度 +float calculateAcceleration(float speed, float previous_speed, float dt) +{ + if (fabs(dt) < EPSILON) + { + return 0; // 避免除以0的情况 + } + float acceleration = (speed - previous_speed) / dt; + return acceleration; +} + +// 计算减速度 +float calculateDeceleration(float speed, float previous_speed, float dt) +{ + if (fabs(dt) < EPSILON) + { + return 0; // 避免除以0的情况 + } + float deceleration = calculateAcceleration(previous_speed, speed, dt); // 减速度就是负的加速度 + return deceleration; +} + +// 计算最大速度 +float calculateMaxSpeed() +{ + // 获取最大电机转速 (RPM) + float max_rpm = (float)getParam("max_rpm"); + // 获取轮子周长 + float wheel_circumference = (float)getParam("whl_dia") * M_PI; + // 获取减速比 + float gear_ratio = (float)getParam("gRatio"); + if (fabsf(gear_ratio) < EPSILON) + { + return 0; // 避免除以0的情况 + } + // 将最大电机转速 (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"); // 减速比 + if (fabsf(wheel_radius) < EPSILON || fabsf(vehicle_mass) < EPSILON ) + { + return 0; // 避免除以0的情况 + } + // 减速比计算扭矩 + float effective_torque = max_motor_torque * gear_ratio; + // 计算最大加速度 + float max_acceleration = (effective_torque / wheel_radius) / vehicle_mass; + return max_acceleration; +} + +// 计算当前状态，包括当前速度、角速度、加速度、减速度、最大速度 +void calculateCurrentState(float dt) +{ + static float previous_speed = 0.0f; + // 更新当前速度和当前角速度 + calculateCurrentSpeedYawRate(); + // 更新加速度、减速度等，根据需要计算 + diff_data.acceleration = calculateAcceleration(diff_data.speed, previous_speed, dt); + diff_data.deceleration = calculateDeceleration(diff_data.speed, previous_speed, dt); + diff_data.max_speed = calculateMaxSpeed(); + previous_speed = diff_data.speed; +} + + +/** + * @brief 根据轮速差动态调整电机扭矩（带非负限制） + * @param speed_left 左轮速度（单位：rpm或自定义） + * @param speed_right 右轮速度（单位：rpm或自定义） + * @param torque_left 左轮扭矩指针（单位：Nm或自定义） + * @param torque_right 右轮扭矩指针（单位：Nm或自定义） + * @param threshold 触发调整的速差阈值（单位同轮速） + * @param k 扭矩调整系数（无量纲，建议0 threshold) { + // 计算需要减少的扭矩量（速差超出阈值部分×系数） + float torque_reduction = (speed_diff - threshold) * k; + + if (speed_left > speed_right) { + // 左轮过快时减少左扭矩，并限制最小值为0 + *torque_left = fmaxf(*torque_left - torque_reduction, 0.0f); + } else { + // 右轮过快时减少右扭矩，并限制最小值为0 + *torque_right = fmaxf(*torque_right - torque_reduction, 0.0f); + } + } +} + +// 计算左右电机速度 +void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max_speed, float *motor_speed) +{ + // 防止速度过低导致不必要的计算 + if (fabs(max_speed) < EPSILON) + { + motor_speed[0] = 0.0f; + motor_speed[1] = 0.0f; + motor_speed[2] = 0.0f; + motor_speed[3] = 0.0f; + return; + } + +#if THROTTLE_PID_MODE + float max_torque = (float)getParam("maxTorq"); + + linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque); + yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque); + + float left_speed_mps = linear_velocity_x + yaw_rate; + float right_speed_mps = linear_velocity_x - yaw_rate; + +//扭矩分配 + if(max_torque < left_speed_mps) + { + right_speed_mps = right_speed_mps - (left_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差 + left_speed_mps = max_torque; + } + else if(-max_torque > left_speed_mps) + { + right_speed_mps = right_speed_mps - (left_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差 + left_speed_mps = -max_torque; + } + else if(max_torque < right_speed_mps) + { + left_speed_mps = left_speed_mps - (right_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差 + right_speed_mps = max_torque; + } + else if(-max_torque > right_speed_mps) + { + left_speed_mps = left_speed_mps - (right_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差 + right_speed_mps = -max_torque; + } + else{} + + // 返回计算结果 + motor_speed[0] = left_speed_mps; + motor_speed[2] = left_speed_mps; + + motor_speed[1] = right_speed_mps; + motor_speed[3] = right_speed_mps; + + + + +// adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],50, 0.2); +// adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],50, 0.2); + +#else + // 限制线速度和偏航率 + linear_velocity_x = constrain(linear_velocity_x, -max_speed, max_speed); + float max_yaw_rate = max_speed / ((float)getParam("whl_bas") / 2.0f); + yaw_rate = constrain(yaw_rate, -max_yaw_rate, max_yaw_rate); + + // 计算旋转速度 + float rotational_velocity = ((float)getParam("whl_bas") / 2.0f) * yaw_rate; + + // 计算车辆左右线速度 (m/s) + float left_speed_mps = linear_velocity_x + rotational_velocity; + float right_speed_mps = linear_velocity_x - rotational_velocity; + + // 计算轮子周长 + float wheel_circumference = (float)getParam("whl_dia") * M_PI; + + // 将车辆左右线速度转换为轮子转速 (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 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"); + left_motor_rpm = constrain(left_motor_rpm, -max_motor_rpm, max_motor_rpm); + right_motor_rpm = constrain(right_motor_rpm, -max_motor_rpm, max_motor_rpm); + + // 当电机转速小于50转时，设置为0 + if (fabsf(left_motor_rpm) < 50)//速度慢所以设置位10转 + { + left_motor_rpm = 0; + } + if (fabsf(right_motor_rpm) < 50)//速度慢所以设置位10转 + { + right_motor_rpm = 0; + } + + // 返回计算结果 + *left_motor_speed = left_motor_rpm; + *right_motor_speed = right_motor_rpm; +#endif + +} + +// 映射遥控器速度，分为死区、低速区和高速区。 +float mapRemoteControlSpeed( + float input_speed, + float deadzone_limit, + float input_max, + float output_max, + float input_slow, + float output_slow +) +{ + float output_speed = 0.0f; + // 获取输入速度的绝对值 + float abs_input = fabsf(input_speed); + + if (abs_input < deadzone_limit + EPSILON) + { + output_speed = 0.0f;// 死区 + } + else if (abs_input < input_slow + EPSILON)// 低速区 + { + output_speed = (abs_input - deadzone_limit) * output_slow / (input_slow - deadzone_limit); + } + else if (abs_input <= input_max + EPSILON)// 高速区 + { + output_speed = output_slow + (abs_input - input_slow) * (output_max - output_slow) / (input_max - input_slow); + } + else // 超出范围 + { + output_speed = output_max; + } + // 根据原始输入速度的符号恢复方向 + if (input_speed < 0) + { + output_speed = -output_speed; + } + return output_speed; +} + + +/** + * @brief 状态机处理函数（修改后版本） + */ +void handleVehicleState(DiffData *ctx,float *torque) +{ + static uint8_t handle_cnt = 0; + switch (ctx->state) + { + //------------------------------------------- + // 初始状态：根据期望速度方向跳转 + //------------------------------------------- + case STATE_INIT: + { + if (ctx->desired_speed < 0.0f) + { + ctx->state = STATE_BACKWARD; + } + else + { + ctx->state = STATE_FORWARD; + } + break; + } + + //------------------------------------------- + // 前进状态：处理反向指令（新增else分支） + //------------------------------------------- + case STATE_FORWARD: + { + if ((ctx->desired_speed < 0.0f) && (ctx->speed == 0.0f)) + { + ctx->state = STATE_BACKWARD; // 零速时允许切换方向 + } + else if ((ctx->desired_speed < 0.0f) && (ctx->speed != 0.0f)) + { + ctx->state = DEC_FORWARD; // 显式保持当前状态 + *torque = 1;//减速的话先发1，然后再发0 + handle_cnt = 0; + } + else + { + ctx->state = STATE_FORWARD; // 新增：其他情况保持前进状态 + } + break; + } + //------------------------------------------- + // 前进减速状态： + //------------------------------------------- + case DEC_FORWARD: + { + if (handle_cnt >= 10) + { + *torque = 0;//减速的话先发1，然后再发0 + ctx->state = DEC_FORWARD2; // 零速时允许切换方向 + handle_cnt = 0; + } + else + { + handle_cnt ++; + ctx->state = DEC_FORWARD; // 新增：其他情况保持前进状态 + } + break; + } + //------------------------------------------- + // 前进溜车状态： + //------------------------------------------- + case DEC_FORWARD2: + { + if( (ctx->speed == 0.0f) && (ctx->desired_speed < 0.0f) )//后退 + { + ctx->state = STATE_BACKWARD; // 零速时允许切换方向 + } + else if ( (ctx->speed == 0.0f) && (ctx->desired_speed > 0.0f) ) //前进 + { + ctx->state = STATE_FORWARD; // 显式保持当前状态 + } + else + { + ctx->state = DEC_FORWARD2; // 新增：其他情况保持前进状态 + *torque = 0;//减速的话先发1，然后再发0 + ctx->desired_speed = 0; + } + break; + } + + + //------------------------------------------- + // 倒车状态：处理正向指令（新增else分支） + //------------------------------------------- + case STATE_BACKWARD: + { + if ((ctx->desired_speed > 0.0f) && (ctx->speed == 0.0f)) + { + ctx->state = STATE_FORWARD; // 零速时允许切换方向 + } + else if ((ctx->desired_speed > 0.0f) && (ctx->speed != 0.0f)) + { + ctx->state = DEC_BACKWARD; // 显式保持当前状态 + *torque = -1;//倒挡减速的话先发-1，然后再发0 + handle_cnt = 0; + } + else + { + ctx->state = STATE_BACKWARD; // 新增：其他情况保持倒车状态 + } + break; + } + + //------------------------------------------- + // 后退减速状态： + //------------------------------------------- + case DEC_BACKWARD: + { + if (handle_cnt >= 10) + { + *torque = 0;//减速的话先发1，然后再发0 + ctx->state = DEC_BACKWARD2; // 零速时允许切换方向 + handle_cnt = 0; + } + else + { + handle_cnt ++; + ctx->state = DEC_BACKWARD; // 新增：其他情况保持前进状态 + } + break; + } + //------------------------------------------- + // 后退减速状态： + //------------------------------------------- + case DEC_BACKWARD2: + { + if( (ctx->speed == 0.0f) && (ctx->desired_speed < 0.0f) )//后退 + { + ctx->state = STATE_BACKWARD; // 零速时允许切换方向 + } + else if ( (ctx->speed == 0.0f) && (ctx->desired_speed > 0.0f) ) //前进 + { + ctx->state = STATE_FORWARD; // 显式保持当前状态 + } + else + { + ctx->state = DEC_BACKWARD2; // 新增：其他情况保持前进状态 + *torque = 0;//减速的话先发1，然后再发0 + ctx->desired_speed = 0; + } + break; + } + } +} + + + +// 差速处理函数 +static void diffProcess(void *signal_id) +{ + (void)signal_id; + static float previous_time1 = 0.0f; + + float time1 = (float)getCurrentTime(); + float dt = (time1 - previous_time1) / PERIOD_TICK; + previous_time1 = time1; + + // 计算当前状态，包括当前速度、角速度、加速度、减速度、最大速度 + calculateCurrentState(dt); + + // 当速度小于1时，设定为原地转向 + if (fabsf(diff_data.desired_speed) <= 1.0f) + { + diff_data.desired_yaw_rate = diff_data.desired_curvature * 1.0f; // 原地转向时，设定一个固定的偏航率 + } + else + { + diff_data.desired_yaw_rate = diff_data.desired_curvature * diff_data.desired_speed; + } + + +// if( ( (diff_data.desired_speed > 0) && (diff_data.speed > 0) ) || ( (diff_data.desired_speed < 0) && (diff_data.speed < 0) ) )//同相 +// { +// if( fabs(diff_data.desired_speed) > fabs(diff_data.speed) )//期望大于当前速度，表示加速，所以用大PID +// { +// setPidParameters(&speed_pid, +// Acc_front_speed_pid.kp, +// Acc_front_speed_pid.ki, +// Acc_front_speed_pid.kd, +// Acc_front_speed_pid.integral_limit, +// Acc_front_speed_pid.output_limit +// ); +// } +// else +// { +// setPidParameters(&speed_pid, +// Dec_front_speed_pid.kp, +// Dec_front_speed_pid.ki, +// Dec_front_speed_pid.kd, +// Dec_front_speed_pid.integral_limit, +// Dec_front_speed_pid.output_limit +// ); +// +// } +// }//反相就先小PID减速 +// else +// { +// setPidParameters(&speed_pid, +// Dec_front_speed_pid.kp, +// Dec_front_speed_pid.ki, +// Dec_front_speed_pid.kd, +// Dec_front_speed_pid.integral_limit, +// Dec_front_speed_pid.output_limit +// ); +// } + + + +// float positive_speed = fabs(diff_data.desired_speed);//取绝对值 20250701 扭矩控制不需要考虑正负 +// +// if(diff_data.desired_speed < 0)//判断方向 小于0就反转，大于0电机正转 +// { +// diff_data.out_motor_dir = 2; +// } +// else if(diff_data.desired_speed > 0) +// { +// diff_data.out_motor_dir = 1; +// } +// else +// { +// diff_data.out_motor_dir = 0; +// } + + // 使用 PID 控制器计算输出速度和曲率 + float output_speed = calculatePidOutput(&speed_pid, diff_data.desired_speed, diff_data.speed, 0.0f, dt); + + float output_yaw_rate = calculatePidOutput(&yaw_rate_pid, diff_data.desired_yaw_rate, diff_data.yaw_rate, 0.0f, dt); + + // 计算最大加速度，用函数计算 + float max_acceleration = calculateMaxAcceleration(); + +// if( ( (output_speed > 0) && (diff_data.speed < 0) ) || ( (output_speed < 0) && (diff_data.speed > 0) ) )//方向相反不给反方向扭矩，直接清0，给反方向扭矩转速有问题 +// { +// if( ( (diff_data.desired_speed > 0) && (diff_data.speed < 0) ) || ( (diff_data.desired_speed < 0) && (diff_data.speed > 0) ) )//期望和当前速度相反的话，那直接运行反方向扭矩，解决下坡不能减速的问题。 +// { +// +// } +// else +// { +// output_speed = 0; +// resetPidIntegral(&speed_pid); +// } +// } +// + handleVehicleState(&diff_data,&output_speed); //20250704 换挡函数 速度为0才能换挡,溜车需要先 + + + + if( ( output_speed < 0) && (diff_data.desired_speed > 0) && (diff_data.speed > 0) )//解决同向导致电机减速，震荡的问题。同相减速，发1。 + { + output_speed = 1; + } + else if( ( output_speed > 0) && (diff_data.desired_speed < 0) && (diff_data.speed < 0) ) + { + output_speed = -1; + } + else + { + } + + + // 限制输出速度在当前速度和最大加速度计算出来的速度之间 + // output_speed = constrain(output_speed, diff_data.speed - max_acceleration * dt, diff_data.speed + max_acceleration * dt); + +// printf("output_speed: %f, output_yaw: %f\n", output_speed, output_yaw_rate); + +// if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) && (diff_data.left_motor_speed < 50.0f) && (diff_data.right_motor_speed > -50.0f))//手柄回中，速度小的时候清0 +// { +// resetPidIntegral(&speed_pid); +// resetPidIntegral(&yaw_rate_pid); +// output_speed = 0; +// output_yaw_rate = 0; +// } +// + + if( (0 == diff_data.desired_speed) )//手柄回中，速度小的时候清0 + { + resetPidIntegral(&speed_pid); + resetPidIntegral(&yaw_rate_pid); + output_speed = 0; + output_yaw_rate = 0; + } + + // 使用差速车辆动力学模型计算左右电机的期望速度 + float out_torque[4] = {0,0,0,0}; + computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, out_torque); +// computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, &diff_data.out_motor_speed[0]); + + + if( ( (diff_data.left_front_motor_speed / diff_data.left_rear_motor_speed) >= diff_data.diff_dead_zone ) || ( (diff_data.left_front_motor_speed / diff_data.left_rear_motor_speed) <= (1/diff_data.diff_dead_zone) ) )//如果超过2倍，或者小于2倍 + { + diff_data.left_speed_diff = diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed; + diff_data.left_diff_touue = calculatePidOutput(&Acc_front_speed_pid, 0.0f, diff_data.left_speed_diff, 0.0f, dt); //左侧转速差PID + } + else + { + diff_data.left_speed_diff = 0; + Acc_front_speed_pid.integral = 0; + diff_data.left_diff_touue = 0; + } + + + if( ( (diff_data.right_front_motor_speed / diff_data.right_rear_motor_speed) >= diff_data.diff_dead_zone ) || ( (diff_data.right_front_motor_speed / diff_data.right_rear_motor_speed) <= (1/diff_data.diff_dead_zone) ) )//如果超过2倍，或者小于2倍 + { + diff_data.right_speed_diff = diff_data.right_front_motor_speed - diff_data.right_rear_motor_speed; + diff_data.right_diff_touue = calculatePidOutput(&Dec_front_speed_pid, 0.0f, diff_data.right_speed_diff, 0.0f, dt); //左侧转速差PID + } + else + { + diff_data.right_speed_diff = 0; + Dec_front_speed_pid.integral = 0; + diff_data.right_diff_touue = 0; + } + + if(out_torque[0] > 0) + { + diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, -2*out_torque[0], 2*out_torque[0]); + } + else + { + diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, 2*out_torque[0], -2*out_torque[0]); + } + + + if(out_torque[1] > 0) + { + diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, -2*out_torque[1], 2*out_torque[1]); + } + else + { + diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, 2*out_torque[1], -2*out_torque[1]); + } + + + diff_data.out_motor_speed[0] = (2*out_torque[0] + diff_data.left_diff_touue)/2.0f;//因为每一个电机都是相同的扭矩，所以扭矩和为2倍。 + diff_data.out_motor_speed[2] = (2*out_torque[0] - diff_data.left_diff_touue)/2.0f; + + diff_data.out_motor_speed[1] = (2*out_torque[1] + diff_data.right_diff_touue)/2.0f; + diff_data.out_motor_speed[3] = (2*out_torque[1] - diff_data.right_diff_touue)/2.0f; + +// diff_data.out_motor_speed[0] = calculatePidOutput(&Left_front_speed_pid, diff_data.out_left_motor_speed, diff_data.left_front_motor_speed, 0.0f, dt); +// diff_data.out_motor_speed[1] = calculatePidOutput(&right_front_speed_pid, diff_data.out_right_motor_speed, diff_data.right_front_motor_speed, 0.0f, dt); +// diff_data.out_motor_speed[2] = calculatePidOutput(&Left_Rear_speed_pid, diff_data.out_left_motor_speed, diff_data.left_rear_motor_speed, 0.0f, dt); +// diff_data.out_motor_speed[3] = calculatePidOutput(&right_Rear_speed_pid, diff_data.out_right_motor_speed, diff_data.right_rear_motor_speed, 0.0f, dt); + +// diff_data.out_motor_speed[0] = diff_data.out_left_motor_speed; +// diff_data.out_motor_speed[2] = diff_data.out_left_motor_speed; +// +// diff_data.out_motor_speed[1] = diff_data.out_right_motor_speed; +// diff_data.out_motor_speed[3] = diff_data.out_right_motor_speed; + + // 设置电机输出 + setMotorOutput(&diff_data.out_motor_speed[0], + diff_data.max_Torq, + (uint16_t)getParam("feedPwr"), + (uint16_t)getParam("dispPwr")); + // 发布左右电机期望转速,电源在工作状态才能发送 + if (power_data.current_state == POWER_WORKING) + { + publishMessage(&un_motor_output1, 1); + publishMessage(&un_motor_output2, 1); + } + publishMessage(&diff_data, 1); +} + + +///** +// * @brief 电机状态处理函数 +// * @param ctx 电机状态上下文指针，包含当前状态、转速和输出扭矩数据 +// * @param original_dir 电机原始方向 +// * @note 该函数实现基于转速和扭矩方向的状态转换逻辑： +// * - 初始态：转速为0时根据扭矩方向切换前进/后退状态 +// * - 前进/后退态：转速归零时返回初始态 +// * - 默认处理：异常状态自动复位到初始态 +// * @warning 需确保ctx->out_motor_speed[0]已正确初始化（正数表前进，负数表后退） +// */ +//void handleMotorState(DiffData *ctx,uint8_t original_dir) +//{ +// switch (ctx->state) +// { +// case STATE_INIT://初始 +// { +// if (ctx->speed == 0.0f) +// { +// if (ctx->out_motor_speed[0] > 0.0f) // 修改为数组索引 +// { +// ctx->state = STATE_FORWARD; // 扭矩为正 → 前进 +// ctx->motor_dir = FORWARD; +// } +// else if (ctx->out_motor_speed[0] < 0.0f) // 修改为数组索引 +// { +// ctx->state = STATE_BACKWARD; // 扭矩为负 → 后退 +// ctx->motor_dir = BACKWARD; +// } +// else +// { +// ctx->state = STATE_INIT; +// } +// } +// else//停下来以后，还出现速度，说明溜坡或者外力接入，直接用实际电机方向 +// { +// if(1 == original_dir) +// { +// ctx->motor_dir = FORWARD; +// ctx->state = STATE_FORWARD; // 保持前进 +// } +// else +// { +// ctx->motor_dir = BACKWARD; +// ctx->state = STATE_BACKWARD; // 保持后退 +// } +// } +// +// break; +// } +// case STATE_FORWARD://前进 +// { +// if ( (ctx->speed == 0.0f) && (ctx->out_motor_speed[0] == 0.0f) ) +// { +// ctx->state = STATE_INIT; +// } +// else if( (ctx->speed == 0.0f) && (ctx->out_motor_speed[0] < 0.0f) )//直接给反相扭矩减速 +// { +// ctx->motor_dir = BACKWARD; +// ctx->state = STATE_BACKWARD; // 保持后退 +// } +// else +// { +// ctx->motor_dir = FORWARD; +// ctx->state = STATE_FORWARD; // 保持前进 +// } +// break; +// } +// +//// case DEC_FORWARD://前进减速 +//// { +//// if(ctx->speed > 0)//停下来以后，还出现速度，说明溜坡或者外力接入，直接用实际电机方向 +//// { +//// ctx->motor_dir = BACKWARD; +//// ctx->state = STATE_BACKWARD; // 保持后退 +//// } +//// else +//// { +//// ctx->state = STATE_INIT; +//// } +//// break; +//// } +//// +// case STATE_BACKWARD://后退 +// { +// if ( (ctx->speed == 0.0f) && (ctx->out_motor_speed[0] == 0.0f) ) +// { +// ctx->state = STATE_INIT; +// } +// else if( (ctx->speed == 0.0f) && (ctx->out_motor_speed[0] > 0.0f) )//直接给反相扭矩减速 +// { +// ctx->motor_dir = FORWARD; +// ctx->state = STATE_FORWARD; // 保持前进 +// } +// else +// { +// ctx->motor_dir = BACKWARD; +// ctx->state = STATE_BACKWARD; // 保持后退 +// } +// break; +// } +// +// // 异常处理 +// default: +// { +// ctx->state = STATE_INIT; +// break; +// } +// } +//} + + + +//void diff100ms(void *signal_id) +//{ +//// calibrateSpeedDirection(&diff_data); +// timerStart(&diff_dir_timer,100,1);//100ms调用一次 +//} + + +// 差速输入处理函数 +static void diffInput(void *signal_id) +{ + if (signal_id == &un_sw_sample) + { + diff_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch; + } + else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据 + { + 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 == 1 ? MODE_AUTO : MODE_MANUAL; + + if (diff_data.mode == MODE_MANUAL) + { + diff_data.desired_speed = (float)((int16_t)(un_remote_control_input.bit_data.speed)); + diff_data.desired_curvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature)); + // 单位转换 + diff_data.desired_speed = diff_data.desired_speed * 0.01f; + diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f; + // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出 + diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5); + diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0.01, 2, 2, 1, 0.5); + diff_data.desired_curvature = diff_data.desired_curvature; + } + } + else if ( (signal_id == &un_manual_computer_input) && (diff_data.mode == MODE_AUTO) ) + { + diff_data.desired_speed = (float)((int16_t)(un_manual_computer_input.bit_data.set_speed)); + diff_data.desired_curvature = (float)((int16_t)(un_manual_computer_input.bit_data.set_curvature)); + // 单位转换 + diff_data.desired_speed = diff_data.desired_speed * 0.01f; + diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f; + // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出 + diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0, 2, 10, 1, 5); + diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1); + } + else if ( (signal_id == &un_auto_computer_input) && (diff_data.mode == MODE_AUTO) ) + { + diff_data.desired_speed = (float)((int16_t)(un_auto_computer_input.bit_data.set_speed)); + diff_data.desired_curvature = (float)((int16_t)(un_auto_computer_input.bit_data.set_curvature)); + // 单位转换 + diff_data.desired_speed = diff_data.desired_speed * 0.01f; + diff_data.desired_curvature = - diff_data.desired_curvature * 0.0001f;// 20241016 增加转弯反相 + // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出 + diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0, 5, 10, 2.5, 5); + diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1); + } + else if( (signal_id == &un_motor_input1) || (signal_id == &un_motor_input3) )// 处理第一个电机速度信号（左侧电机） + { + float speed_temp = (float) ( (int16_t)(un_motor_input1.bit_data.speed) ) ; + if(1 == un_motor_input1.bit_data.gear)//判断挡位 + { + if(speed_temp < 0)//转速小于0 + { + diff_data.left_rear_motor_speed = -speed_temp; + } + else + { + diff_data.left_rear_motor_speed = speed_temp; + } + } + else if(2 == un_motor_input1.bit_data.gear) + { + if(speed_temp < 0)//转速小于0 + { + diff_data.left_rear_motor_speed = speed_temp;//两个负号得正 + } + else + { + diff_data.left_rear_motor_speed = -speed_temp; + } + } + else + { + } +// if(1 == diff_data.motor_dir)//1为负数 20250620 修改为判断电机转动方向， 挡位不可靠 +// { +// diff_data.left_rear_motor_speed = -speed_temp; +// } +// else +// { +// diff_data.left_rear_motor_speed = speed_temp; +// } + if(fabs(diff_data.left_rear_motor_speed) < SWITCH_SPEED) + { + diff_data.left_rear_motor_speed = 0; + } + + + speed_temp = (float) ( (int16_t)(un_motor_input3.bit_data.speed) ) ; + + if(1 == un_motor_input3.bit_data.gear)//判断挡位 + { + if(speed_temp < 0)//转速小于0 + { + diff_data.left_front_motor_speed = -speed_temp; + } + else + { + diff_data.left_front_motor_speed = speed_temp; + } + } + else if(2 == un_motor_input3.bit_data.gear) + { + if(speed_temp < 0)//转速小于0 + { + diff_data.left_front_motor_speed = speed_temp;//两个负号得正 + } + else + { + diff_data.left_front_motor_speed = -speed_temp; + } + } + else + { + } + +// if(1 == diff_data.motor_dir)//1为负数 +// { +// diff_data.left_front_motor_speed = -speed_temp; +// } +// else +// { +// diff_data.left_front_motor_speed = speed_temp; +// } + + if(fabs(diff_data.left_front_motor_speed) < SWITCH_SPEED)//死区 + { + diff_data.left_front_motor_speed = 0; + } + + if(fabs(diff_data.left_front_motor_speed) < fabs(diff_data.left_rear_motor_speed) )//取最小值,避免打滑，拉高转速 + { + diff_data.left_motor_speed = diff_data.left_front_motor_speed; + } + else + { + diff_data.left_motor_speed = diff_data.left_rear_motor_speed; + } + +// printf("motor1: %f, motor3: %f\n", diff_temp[0], diff_temp[1]); +// diff_data.left_motor_speed = 0;// 左侧两个电机所以取平均值 + + } + else if( (signal_id == &un_motor_input2) || (signal_id == &un_motor_input4) )// 处理第二个电机速度信号（右侧电机） + { + + float speed_temp = (float) ( (int16_t)( un_motor_input2.bit_data.speed) ); +// if(1 == diff_data.motor_dir)//1为负数 +// { +// diff_data.right_front_motor_speed = - speed_temp; +// } +// else +// { +// diff_data.right_front_motor_speed = speed_temp; +// } + if(1 == un_motor_input2.bit_data.gear)//判断挡位 + { + if(speed_temp < 0)//转速小于0 + { + diff_data.right_front_motor_speed = -speed_temp; + } + else + { + diff_data.right_front_motor_speed = speed_temp; + } + } + else if(2 == un_motor_input2.bit_data.gear) + { + if(speed_temp < 0)//转速小于0 + { + diff_data.right_front_motor_speed = speed_temp;//两个负号得正 + } + else + { + diff_data.right_front_motor_speed = -speed_temp; + } + } + else + { + } + if(fabs(diff_data.right_front_motor_speed) < SWITCH_SPEED)//死区 + { + diff_data.right_front_motor_speed = 0; + } + + + speed_temp = ( (float) ( (int16_t)(un_motor_input4.bit_data.speed) ) ); +// if(1 == diff_data.motor_dir)//1为负数 +// { +// diff_data.right_rear_motor_speed = - speed_temp; +// } +// else +// { +// diff_data.right_rear_motor_speed = speed_temp; +// } + if(1 == un_motor_input4.bit_data.gear)//判断挡位 + { + if(speed_temp < 0)//转速小于0 + { + diff_data.right_rear_motor_speed = -speed_temp; + } + else + { + diff_data.right_rear_motor_speed = speed_temp; + } + } + else if(2 == un_motor_input4.bit_data.gear) + { + if(speed_temp < 0)//转速小于0 + { + diff_data.right_rear_motor_speed = speed_temp;//两个负号得正 + } + else + { + diff_data.right_rear_motor_speed = -speed_temp; + } + } + else + { + } + if(fabs(diff_data.right_rear_motor_speed) < SWITCH_SPEED)//死区 + { + diff_data.right_rear_motor_speed = 0; + } + + if( fabs(diff_data.right_front_motor_speed) < fabs(diff_data.right_rear_motor_speed) )//取最小值,避免打滑，拉高转速 + { + diff_data.right_motor_speed = diff_data.right_front_motor_speed; + } + else + { + diff_data.right_motor_speed = diff_data.right_rear_motor_speed; + } + +// diff_data.right_motor_speed = 0;// 左侧两个电机所以取平均值 + + } + else{} + +// printf("rightspeed: %f, leftspeed: %f\n", diff_data.right_motor_speed, diff_data.left_motor_speed); + // 急停开关 + diff_data.emergency_stop_state = (uint8_t)(diff_data.emergency_stop_switch == app_close() || diff_data.remote_emergency_stop == app_close()); + + // 如果急停被激活,强制设定速度为0,急停包括车上急停开关和遥控器急停开关 + if (diff_data.emergency_stop_state == 1) + { + diff_data.desired_speed = 0.0; + diff_data.desired_curvature = 0.0; + } + // 遥控器断线，而且是在手动模式，期望值清0 + if ( (diff_data.mode == MODE_MANUAL) && (0 == un_remote_control_input.bit_data.enable) ) + { + diff_data.desired_speed = 0.0; + diff_data.desired_curvature = 0.0; + } + +// if (diff_data.emergency_stop_state == 1)//刹车 20241017 增加的扭矩限制 +// { +// diff_data.max_Torq = 5;//20240403修改。刹车就是5N +// } +// else if ((0 == diff_data.desired_speed) && (0 == diff_data.desired_curvature) && ((diff_data.left_motor_speed > -100) && (diff_data.left_motor_speed < 100)) && (((diff_data.right_motor_speed > -100) && (diff_data.right_motor_speed < 100))))//20240330只有当手柄回中，然后当前已经停止的状态才设置为最小停车扭矩 +// { +// diff_data.max_Torq = 0;//停车 就为0 +// } +// else +// { + +// } + + diff_data.speed_inter = (uint16_t)(int16_t)speed_pid.integral; + diffProcess(&diff_data);//计算左右电机期望转速 +// printf("dir_state: %d,speed: %f, tou: %f\n", diff_data.state,diff_data.speed,diff_data.out_motor_speed[0]); + + publishMessage(&diff_data, 1); +} + + + +// 预充完成处理函数 +void preChargeFinish(void *signal_id) +{ + (void)signal_id; + + for(uint8_t i = 0; i<4; i++) + { + diff_data.out_motor_speed[i] = 0; + } + + setMotorOutput(&diff_data.out_motor_speed[0],(uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr")); + // 档位 + un_motor_output1.bit_data.gear = 0; // 0表示空挡 + un_motor_output2.bit_data.gear = 0; + publishMessage(&un_motor_output1, 1); + publishMessage(&un_motor_output2, 1); +} + + +void diffParametersInit(void *signal_id) +{ + (void)signal_id; // 标记变量为已使用，避免编译器警告 + setPidParameters(&speed_pid, + getParam("spd_kp"), + getParam("spd_ki"), + getParam("spd_kd"), + getParam("spd_il"), + getParam("spd_ol") + ); + + // 设置曲率 PID 控制器的参数 + setPidParameters(&yaw_rate_pid, + getParam("crv_kp"), + getParam("crv_ki"), + getParam("crv_kd"), + getParam("crv_il"), + getParam("crv_ol") + ); + + // 设置曲率 PID 控制器的参数 + setPidParameters(&Dec_front_speed_pid, + getParam("mot_kp"), + getParam("mot_ki"), + getParam("mot_kd"), + getParam("mot_il"), + getParam("mot_ol") + ); + + // 设置曲率 PID 控制器的参数 + setPidParameters(&Acc_front_speed_pid, + Dec_front_speed_pid.kp, + Dec_front_speed_pid.ki, + Dec_front_speed_pid.kd, + Dec_front_speed_pid.integral_limit, + Dec_front_speed_pid.output_limit + ); + + diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩 + + if(0 == (float)getParam("diff_sp"))//20250711 防止参数为0，影响计算。 + { + diff_data.diff_dead_zone = 2; + } + else + { + diff_data.diff_dead_zone = (float)getParam("diff_sp");//参数读取设定最大扭矩 + } + + + + printf("PID_P: %f, PID_I: %f\n", speed_pid.kp, speed_pid.ki); + + printf("left_front: %f, right_front: %f, left_rear: %f, right_rear: %f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed,diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed); + + printf("left_front_tou: %f, right_front_tou: %f, left_rear_tou: %f, right_rear_tou: %f\n", diff_data.out_motor_speed[0], diff_data.out_motor_speed[2],diff_data.out_motor_speed[2], diff_data.out_motor_speed[3]); + + + + timerStart(&diff_app_timer,1000,1);//1s调用一次 +} + + +// 差速初始化函数 +void diffAppInit(void) +{ + // 初始化 diff_data + memset(&diff_data, 0, sizeof(DiffData)); + + // 订阅相关信号 + subscribe(&un_sw_sample, 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); + subscribe(&power_data.pre_charge_finish, preChargeFinish); + + // 初始化速度 PID 控制器 + initializePid(&speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); + // 设置速度 PID 控制器的参数 + setPidParameters(&speed_pid, + getParam("spd_kp"), + getParam("spd_ki"), + getParam("spd_kd"), + getParam("spd_il"), + getParam("spd_ol") + ); + + + + // 初始化曲率 PID 控制器 + initializePid(&yaw_rate_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); + // 设置曲率 PID 控制器的参数 + setPidParameters(&yaw_rate_pid, + getParam("crv_kp"), + getParam("crv_ki"), + getParam("crv_kd"), + getParam("crv_il"), + getParam("crv_ol") + ); + + + // 初始化减速 PID 控制器 + initializePid(&Dec_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); + // 设置 PID 控制器的参数 + setPidParameters(&Dec_front_speed_pid, + getParam("mot_kp"), + getParam("mot_ki"), + getParam("mot_kd"), + getParam("mot_il"), + getParam("mot_ol") + ); + + // 初始化加速 PID 控制器 + initializePid(&Acc_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); + // 设置 PID 控制器的参数 + setPidParameters(&Acc_front_speed_pid, + Dec_front_speed_pid.kp, + Dec_front_speed_pid.ki, + Dec_front_speed_pid.kd, + Dec_front_speed_pid.integral_limit, + Dec_front_speed_pid.output_limit + ); + + + subscribe(&diff_app_timer, diffParametersInit); + timerStart(&diff_app_timer,1000,1);//1s调用一次 + + +// subscribe(&diff_dir_timer, diff100ms); +// timerStart(&diff_dir_timer,100,1);//100ms调用一次 + + + printf("diffControl: diffAppInit OK \n"); +} diff --git a/app/app_differential_drive.h b/app/app_differential_drive.h new file mode 100644 index 0000000..4c8bec9 --- /dev/null +++ b/app/app_differential_drive.h @@ -0,0 +1,128 @@ +#ifndef APP_DIFFERENTIAL_DRIVE_H +#define APP_DIFFERENTIAL_DRIVE_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "app_config.h" + +#define SPEED_MODE 0x0C +#define THROTTLE_MODE 0x05 +#define TORQUE_MODE 0x0A + +#define MOTOR_MODE THROTTLE_MODE + +#define SPEED_PID_MODE 0 +#define THROTTLE_PID_MODE 1 + + + + + +#define SWITCH_SPEED 20.0f + + +#define DIFF_SPEED 20.0f + + +typedef enum +{ + STATE_INIT, ///< 初始状态（转速为0且等待扭矩方向判定） + STATE_FORWARD, ///< 正向旋转状态 + STATE_BACKWARD, ///< 反向旋转状态 + DEC_FORWARD, ///< 正向减速状态 + DEC_FORWARD2, ///< 正向溜坡状态 + DEC_BACKWARD, ///< 反向减速状态 + DEC_BACKWARD2 ///< 反向溜坡状态 + +} MotorState; + + +// 状态机内部状态 +typedef enum +{ + STATE_CALIB_INIT, // 初始状态：检测方向一致性 + STATE_CALIB_JUDGE // 判断状态：300ms验证期 +} DirState; + + +typedef enum +{ + FORWARD, ///< 正向旋转状态 + BACKWARD ///< 反向旋转状态 +} MotorDir; + + + +typedef enum +{ + MODE_MANUAL, // 手动模式 + MODE_AUTO // 自动模式 +} ControlMode; + +typedef struct DiffData +{ + ControlMode mode ; // 控制模式 + MotorState state; //当前状态机状态 + MotorDir motor_dir; // 当前车辆方向 + DirState dir_state; + + float desired_speed; // 期望速度 + float desired_curvature; // 期望曲率 + float left_motor_speed; // 当前左电机速度 + float right_motor_speed; // 当前右电机速度 + + float left_front_motor_speed; // 当前左前电机速度 + float right_front_motor_speed; // 当前右前电机速度 + float left_rear_motor_speed; // 当前左后电机速度 + float right_rear_motor_speed; // 当前右后电机速度 + + uint8_t out_motor_dir; // 电机状态 + + float speed_last; // 速度过去值 + float torq_last; // 扭矩过去值 + uint8_t counter; // 方向判断计数器 + + float left_speed_diff; // 左侧转速差 + float right_speed_diff; // 右侧转速差 + + float left_diff_touue; // 左侧扭矩差 + float right_diff_touue; // 右侧扭矩差 + + float diff_dead_zone; // 差速速度死区 + + + float out_motor_speed[4]; + float speed; // 当前车速 + float curvature; // 当前曲率 + float yaw_rate; // 当前角速度 + float desired_yaw_rate; // 期望角速度 + float acceleration; // 当前加速度 + float deceleration; // 当前减速度 + float max_speed; // 最大速度 + float desired_acceleration; // 期望加速度 + float desired_deceleration; // 期望减速度 + uint8_t emergency_stop_switch; // 急停开关 + uint8_t remote_emergency_stop; // 遥控器急停开关 + uint8_t emergency_stop_state; // 急停状态 + float out_left_motor_speed; // 输出左电机速度 + float out_right_motor_speed; // 输出右电机速度 + uint16_t max_Torq; // 最大扭矩限制 + uint16_t speed_inter; + + +} DiffData; + + +// 声明外部变量 +extern DiffData diff_data; + +void diffAppInit(void); + +#ifdef __cplusplus +} +#endif + +#endif // APP_DIFFERENTIAL_DRIVE_H diff --git a/app/app_frm_monitor.c b/app/app_frm_monitor.c new file mode 100644 index 0000000..96f9d12 --- /dev/null +++ b/app/app_frm_monitor.c @@ -0,0 +1,175 @@ +#include "app_config.h" +#include "interface.h" + +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" + +#include +#include +#include +#include + + + +// 全局日志缓冲区实例 +LogBuffer log_buffer; + +// 打印当前信号队列状态 +void printSignalQueueStatus(void) +{ + for (uint32_t i = 0; i < PRIORITY_LEVELS; i++) + { + printf("Priority %u Signal Count: %d\n", i, + getSignalCount(i)); + } +} + +// 打印订阅者信息 +void printSubscriberInfo(void) +{ + printf("Total Subscribers: %u\n", getSubscriberCount()); +} + +// 打印定时器状态 +void printTimerStatus(void) +{ + printf("Current Timer Count: %u / %u\n", + getCurrentTimerCount(), MAX_TIMERS); +} + +// 监控函数 +void monitorSignalSystem(void) +{ + static uint32_t last_print_time = 0; + uint32_t current_print_time = getCurrentTime(); + uint32_t time_interval = current_print_time - last_print_time; + + printf("---------------------------------------------------\n"); + printf("Time since last print: %u us\n", time_interval); + printSignalQueueStatus(); + printSubscriberInfo(); + printTimerStatus(); + printf("---------------------------------------------------\n"); + + last_print_time = current_print_time; +} + +int logBufferWrite(LogBuffer *lb, const char *format, ...) +{ + if (format == NULL || lb == NULL) + { + return -1; + } + + char temp_buffer[256]; // 临时缓冲区，假设单条日志不超过256字节 + va_list args; + va_start(args, format); + int length = vsnprintf(temp_buffer, sizeof(temp_buffer), format, args); + va_end(args); + + if (length <= 0) + { + return -1; + } + + // 进入关键区，保护缓冲区的写操作 + irq_state_t saved_state = enter_critical_section(); + + // 检查缓冲区是否有足够的空间 + if (lb->count + length > LOG_BUFFER_SIZE) + { + // 缓冲区满，无法写入 + exit_critical_section(saved_state); + return -1; + } + + // 写入数据到缓冲区 + for (int i = 0; i < length; i++) + { + lb->buffer[lb->tail] = temp_buffer[i]; + lb->tail = (lb->tail + 1) % LOG_BUFFER_SIZE; + } + lb->count += length; + + exit_critical_section(saved_state); + return 0; +} + +int logBufferRead(LogBuffer *lb, char *data, uint32_t max_length) +{ + if (max_length == 0 || data == NULL || lb == NULL) + { + return -1; + } + + // 进入关键区，保护缓冲区的读操作 + irq_state_t saved_state = enter_critical_section(); + + uint32_t length = lb->count < max_length ? lb->count : max_length; + + // 从缓冲区读取数据 + for (uint32_t i = 0; i < length; i++) + { + data[i] = lb->buffer[lb->head]; + lb->head = (lb->head + 1) % LOG_BUFFER_SIZE; + } + lb->count -= length; + + exit_critical_section(saved_state); + return length; +} + +// 初始化日志缓冲区 +void initLogBuffer(LogBuffer *lb) +{ + lb->head = 0; + lb->tail = 0; + lb->count = 0; + memset(lb->buffer, 0, LOG_BUFFER_SIZE); +} + +// 日志信号处理函数 +void logSignalHandler(void *signal_id) +{ + (void)signal_id; + char log_data[256]; // 假设单次读取不超过256字节 + int read_length; + + // 从缓冲区读取日志信息 + read_length = logBufferRead(&log_buffer, log_data, sizeof(log_data) - 1); + + if (read_length > 0) + { + log_data[read_length] = '\0'; // 确保字符串以NULL结尾 + + // 调用printf进行打印 + printf("%s", log_data); + } +} + +Timer log_timer; + +void logTimerHandler(void *timer_id) +{ + (void)timer_id; + // monitorSignalSystem(); + timerStart(&log_timer, 1000, 1); +} + +void appMonitorInit(void) +{ + // 初始化日志缓冲区 + initLogBuffer(&log_buffer); + // 订阅日志信号 + subscribe(&log_buffer, logSignalHandler); + timerInit(&log_timer); + timerStart(&log_timer, 1000, 1); + subscribe(&log_timer, logTimerHandler); +} + +// // 使用格式化字符串记录日志 +// if (logBufferWrite(&log_buffer, "ISR triggered: IRQ %d\n", irq_number) == 0) { +// // 发送信号，通知主线程有新日志 +// publishMessage(&log_buffer, LOG_SIGNAL_PRIORITY); +// } diff --git a/app/app_frm_monitor.h b/app/app_frm_monitor.h new file mode 100644 index 0000000..00dc8e6 --- /dev/null +++ b/app/app_frm_monitor.h @@ -0,0 +1,41 @@ +#ifndef APP_FRM_MONITOR_H +#define APP_FRM_MONITOR_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#define LOG_BUFFER_SIZE 1024 // 日志缓冲区的大小 +#define LOG_SIGNAL_PRIORITY 1 // 日志信号的优先级 + +typedef struct +{ + char buffer[LOG_BUFFER_SIZE]; + uint32_t head; // 读指针 + uint32_t tail; // 写指针 + uint32_t count; // 缓冲区中的数据量 +} LogBuffer; + +// 全局日志缓冲区实例 +extern LogBuffer log_buffer; + +// 函数声明 +void printSignalQueueStatus(void); +void printSubscriberInfo(void); +void printTimerStatus(void); +void monitorSignalSystem(void); + +// 新增函数接口 +int32_t logBufferWrite(LogBuffer *lb, const char *format, ...); +int32_t logBufferRead(LogBuffer *lb, char *data, uint32_t max_length); +void initLogBuffer(LogBuffer *lb); +void logSignalHandler(void *signal_id); +void appMonitorInit(void); + +#ifdef __cplusplus +} +#endif + +#endif // APP_FRM_MONITOR_H \ No newline at end of file diff --git a/app/app_frm_signal.c b/app/app_frm_signal.c new file mode 100644 index 0000000..bb20e32 --- /dev/null +++ b/app/app_frm_signal.c @@ -0,0 +1,333 @@ +#include "app_config.h" +#include "interface.h" + +#include "app_frm_signal.h" + +typedef struct +{ + void *signals[MAX_SIGNALS]; + uint32_t head; + uint32_t tail; + uint32_t count; +} SignalQueue; + +typedef struct +{ + void *signal_id; + CallbackFunc callbacks[MAX_CALLBACKS]; + uint32_t callback_count; +} Subscriber; + +// 优先级队列 +static SignalQueue priority_queues[PRIORITY_LEVELS]; + +// 订阅者表 +static Subscriber subscriber_table[MAX_SUBSCRIBERS] = {{NULL, {NULL}, 0}}; + +// 初始化队列 +static void initQueue(SignalQueue *q) +{ + q->head = 0; + q->tail = 0; + q->count = 0; +} + +// 将信号请求添加到队列中（按优先级） +static int32_t enqueue(SignalQueue *q, void *signal_id) +{ + if (q == NULL) + { + printf("Error: enqueue received NULL queue pointer\n"); + return -1; + } + if (signal_id == NULL) + { + printf("Error: Cannot enqueue NULL signal_id\n"); + return -1; + } + + irq_state_t saved_state = enter_critical_section(); + + if (q->count >= MAX_SIGNALS) + { + // 队列已满，移除最前面的信号 + q->signals[q->head] = NULL; + q->head = (q->head + 1) % MAX_SIGNALS; + q->count--; + printf("Error: Signal queue is full, remove the first signal\n"); + } + + // 添加新的信号到队列尾部 + q->signals[q->tail] = signal_id; + q->tail = (q->tail + 1) % MAX_SIGNALS; + q->count++; + + exit_critical_section(saved_state); + return 0; +} + + +// 从队列中取出信号请求（按优先级） +static int32_t dequeue(SignalQueue *q, void **signal_id) +{ + if (q == NULL) + { + printf("Error: dequeue received NULL queue pointer\n"); + return -1; + } + if (signal_id == NULL) + { + printf("Error: dequeue received NULL signal pointer\n"); + return -1; + } + + // 仅在修改共享资源时进入临界区 + if (q->count > 0) + { + irq_state_t saved_state = enter_critical_section(); + + *signal_id = q->signals[q->head]; + q->signals[q->head] = NULL; // 清除已取出的信号 + q->head = (q->head + 1) % MAX_SIGNALS; + q->count--; + + exit_critical_section(saved_state); + return 0; + } + // printf("Warning: dequeue attempted to remove signal from an empty queue\n"); + return -1; +} + +// 哈希函数 +static uint32_t hash(void *ptr) +{ + uintptr_t value = (uintptr_t)ptr; + uint32_t hash = 0; + while (value != 0) + { + hash += value & 0xFF; + hash += (hash << 10); + hash ^= (hash >> 6); + value >>= 8; + } + hash += (hash << 3); + hash ^= (hash >> 11); + hash += (hash << 15); + return hash; +} + +// 订阅信号, 给每个信号指定回调函数 +int32_t subscribe(void *signal_id, CallbackFunc callback) +{ + if (signal_id == NULL || callback == NULL) + { + printf("Error: Invalid signal_id or callback\n"); + return -1; + } + + irq_state_t saved_state = enter_critical_section(); + + uint32_t index = hash(signal_id) % MAX_SUBSCRIBERS; + uint32_t original_index = index; + + do + { +// if(signal_id == &un_remote_control_input) +// { +// printf("remote的地址：%p\n", (void*)&un_remote_control_input); // 输出示例：0x7ffd9a3c8b40[5](@ref) +// printf("signal_id的地址：%p\n", (void*)(subscriber_table[index].signal_id)); // 输出示例：0x7ffd9a3c8b40[5](@ref) +// } +// + if (subscriber_table[index].signal_id == NULL || + subscriber_table[index].signal_id == signal_id) + { + if (subscriber_table[index].signal_id == NULL) + { + subscriber_table[index].signal_id = signal_id; + subscriber_table[index].callback_count = 0; + } + + if (subscriber_table[index].callback_count < MAX_CALLBACKS) + { + subscriber_table[index].callbacks[subscriber_table[index].callback_count++] = callback; + + exit_critical_section(saved_state); + return 0; + } + else + { + printf("Error: Maximum callbacks reached for this signal\n"); + exit_critical_section(saved_state); + return -1; + } + } + + index = (index + 1) % MAX_SUBSCRIBERS; + } while (index != original_index); + +// printf("remote addrss：%p\n", (void*)signal_id); // 输出示例：0x7ffd9a3c8b40[5](@ref) +// printf("signal_id addrss：%p\n", (void*)(subscriber_table[index].signal_id)); // 输出示例：0x7ffd9a3c8b40[5](@ref) + + printf("Error: Subscriber table is full\n"); + exit_critical_section(saved_state); + return -1; +} + +// 检查信号是否有订阅者 +static unsigned char hasSubscribers(void *signal_id) +{ + irq_state_t saved_state = enter_critical_section(); + + uint32_t index = hash(signal_id) % MAX_SUBSCRIBERS; + uint32_t original_index = index; + + do + { + if (subscriber_table[index].signal_id == signal_id) + { + unsigned char result = subscriber_table[index].callback_count > 0; + exit_critical_section(saved_state); + return result; + } + if (subscriber_table[index].signal_id == NULL) + { + exit_critical_section(saved_state); + return 0; // 没有订阅者 + } + index = (index + 1) % MAX_SUBSCRIBERS; + } while (index != original_index); + + exit_critical_section(saved_state); + return 0; // 没有订阅者 +} + +// 内部一致性检查 +static void internalConsistencyCheck(void) +{ + for (uint32_t i = 0; i < MAX_SUBSCRIBERS; i++) + { + assert(subscriber_table[i].callback_count <= MAX_CALLBACKS); + } +} + +// 处理队列中的信号, 调用所有匹配的回调函数 +static void processSignals(void) +{ + void *signal_id; + + for (uint32_t priority = 0; priority < PRIORITY_LEVELS; priority++) + { + SignalQueue *q = &priority_queues[priority]; + + while (dequeue(q, &signal_id) == 0) + { + // 进入临界区保护 subscriber_table 的读取 + irq_state_t saved_state = enter_critical_section(); + + uint32_t index = hash(signal_id) % MAX_SUBSCRIBERS; + uint32_t original_index = index; + unsigned char found = 0; + + do + { + if (subscriber_table[index].signal_id == signal_id) + { + CallbackFunc *callbacks = subscriber_table[index].callbacks; + uint32_t callback_count = subscriber_table[index].callback_count; + + // 复制回调函数指针, 避免在临界区外访问共享数据 + CallbackFunc local_callbacks[MAX_CALLBACKS]; + memcpy(local_callbacks, callbacks, sizeof(CallbackFunc) * callback_count); + + exit_critical_section(saved_state); + + // 在临界区外调用回调函数 + for (uint32_t i = 0; i < callback_count; i++) + { + local_callbacks[i](signal_id); + } + found = 1; + break; + } + if (subscriber_table[index].signal_id == NULL) + { + exit_critical_section(saved_state); + break; // 没有订阅者 + } + index = (index + 1) % MAX_SUBSCRIBERS; + } while (index != original_index); + + if (!found) + { + printf("Warning: No subscribers found for signal %p\n", signal_id); + } + } + } + internalConsistencyCheck(); // 处理完所有信号后进行一致性检查 +} + +// 初始化框架 +void initFramework(void) +{ + irq_state_t saved_state = enter_critical_section(); + + for (uint32_t i = 0; i < PRIORITY_LEVELS; i++) + { + initQueue(&priority_queues[i]); + } + memset(subscriber_table, 0, sizeof(subscriber_table)); + + exit_critical_section(saved_state); +} + +// 将信号请求添加到指定优先级的队列中 +int32_t publishMessage(void *signal_id, uint8_t priority) +{ + if ((uint32_t)priority >= PRIORITY_LEVELS) + { + printf("Error: Invalid priority\n"); + return -1; + } + + if (hasSubscribers(signal_id)) + { + return enqueue(&priority_queues[priority], signal_id); + } + else return 1; +} + +// 处理所有优先级的信号 +void processMessages(void) +{ + processSignals(); +} + +// 获取当前队列中的信号数量 +int32_t getSignalCount(uint32_t priority) +{ + if (priority >= PRIORITY_LEVELS) + { + printf("Error: Invalid priority\n"); + return -1; + } + return priority_queues[priority].count; +} + +// 获取订阅者数量 +uint32_t getSubscriberCount(void) +{ + uint32_t count = 0; + + irq_state_t saved_state = enter_critical_section(); + + for (uint32_t i = 0; i < MAX_SUBSCRIBERS; i++) + { + if (subscriber_table[i].signal_id != NULL) + { + count++; + } + } + + exit_critical_section(saved_state); + return count; +} diff --git a/app/app_frm_signal.h b/app/app_frm_signal.h new file mode 100644 index 0000000..5c0ee67 --- /dev/null +++ b/app/app_frm_signal.h @@ -0,0 +1,47 @@ +#ifndef APP_FRM_SIGNAL_H +#define APP_FRM_SIGNAL_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include "app_dependence.h" + +#define MAX_SIGNALS 500u // 每个优先级的最大信号数量 +#define MAX_SUBSCRIBERS 100u // 不同信号的订阅者数量 20250519 解决列表满的问题 +#define MAX_CALLBACKS 25u // 每个信号最多支持多少订阅者 +#define PRIORITY_LEVELS 2u // 优先级层次 + + +// 回调函数类型定义 +typedef void (*CallbackFunc)(void *signal_id); + +// 函数声明 +void initFramework(void); +int32_t subscribe(void *signal_id, CallbackFunc callback); +int32_t publishMessage(void *signal_id, uint8_t priority); +void processMessages(void); +int32_t getSignalCount(uint32_t priority); +uint32_t getSubscriberCount(void); + +// 进入临界区，禁用中断，并返回之前的中断状态 +static inline irq_state_t enter_critical_section(void) +{ + return arch_irq_save(); +} + +// 退出临界区，恢复之前的中断状态 +static inline void exit_critical_section(irq_state_t saved_state) +{ + arch_irq_restore(saved_state); +} + + + +#ifdef __cplusplus +} +#endif + +#endif // APP_FRM_SIGNAL_H diff --git a/app/app_frm_timer.c b/app/app_frm_timer.c new file mode 100644 index 0000000..bedf882 --- /dev/null +++ b/app/app_frm_timer.c @@ -0,0 +1,165 @@ +#include "app_config.h" +#include "interface.h" + +#include "app_frm_timer.h" +#include "app_frm_signal.h" + +// 全局计时器列表头指针 +static Timer *timer_list = NULL; + +// 添加一个静态变量来跟踪当前定时器数量 +static uint32_t current_timer_count = 0; + +// 初始化定时器 +void timerInit(Timer *timer) +{ + if (timer == NULL) + { + return; + } + timer->target_time = 0; + timer->elapsed_time = 0; + timer->start_time = 0; + timer->active = 0; + timer->priority = 1; // 默认优先级为1 + timer->next = NULL; +} + +// 修改 timerStart 函数 +void timerStart(Timer *timer, uint32_t target_time, unsigned char priority) +{ + if (timer == NULL || target_time == 0) + { + printf("Error: Null timer pointer or invalid target time\n"); + return; + } + + // 检查定时器是否已经在列表中 + Timer *current = timer_list; + while (current != NULL) + { + if (current == timer) + { + // 定时器已在列表中, 只需更新其参数 + irq_state_t saved_state = enter_critical_section(); + + timer->target_time = target_time; + timer->start_time = getCurrentTime(); + timer->elapsed_time = 0; + timer->active = 1; + timer->priority = priority; + + exit_critical_section(saved_state); + return; + } + current = current->next; + } + + // 检查是否达到最大定时器数量 + if (current_timer_count >= MAX_TIMERS) + { + printf("Error: Maximum number of timers reached\n"); + return; + } + + // 初始化新定时器 + irq_state_t saved_state = enter_critical_section(); + + timer->target_time = target_time; + timer->start_time = getCurrentTime(); + timer->elapsed_time = 0; + timer->active = 1; + timer->priority = priority; + + // 将定时器插入到定时器列表 + timer->next = timer_list; + timer_list = timer; + current_timer_count++; + + exit_critical_section(saved_state); +} + +// 停止定时器 +void timerStop(Timer *timer) +{ + if (timer == NULL) + { + return; + } + + irq_state_t saved_state = enter_critical_section(); + + Timer *current = timer_list; + Timer *prev = NULL; + while (current != NULL) + { + if (current == timer) + { + // 从链表中移除定时器 + if (prev == NULL) + { + timer_list = current->next; + } + else + { + prev->next = current->next; + } + current_timer_count--; + timer->active = 0; + timer->next = NULL; // 断开链接 + break; + } + prev = current; + current = current->next; + } + + exit_critical_section(saved_state); +} + +// 更新所有定时器, 在定时器中断中调用 +void timerUpdateAll(void) +{ + irq_state_t saved_state = enter_critical_section(); + + Timer *prev = NULL; + Timer *current = timer_list; + + while (current != NULL) + { + if (current->active) + { + current->elapsed_time++; + if (current->elapsed_time >= current->target_time) + { + current->active = 0; + // 发送定时器过期消息 + publishMessage((void *)(uintptr_t)current, current->priority); + + // 从链表中移除已过期的定时器 + if (prev == NULL) + { + timer_list = current->next; + } + else + { + prev->next = current->next; + } + current_timer_count--; + Timer *expired_timer = current; + current = current->next; + expired_timer->next = NULL; // 断开链接 + continue; // 跳过前进 prev 指针 + } + } + prev = current; + current = current->next; + } + + exit_critical_section(saved_state); +} + +// 获取当前定时器数量 +uint32_t getCurrentTimerCount(void) +{ + return current_timer_count; +} \ No newline at end of file diff --git a/app/app_frm_timer.h b/app/app_frm_timer.h new file mode 100644 index 0000000..3e76cc2 --- /dev/null +++ b/app/app_frm_timer.h @@ -0,0 +1,35 @@ +#ifndef APP_FRM_TIMER_H +#define APP_FRM_TIMER_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define MAX_TIMERS 100 // 定义最大定时器数量 + +// 定时器结构体定义 +typedef struct Timer { + uint32_t target_time; + uint32_t elapsed_time; + uint32_t start_time; + unsigned char active; + unsigned char priority; + struct Timer *next; +} Timer; + +// 函数声明 +void timerInit(Timer *timer); +void timerStart(Timer *timer, uint32_t target_time, unsigned char priority); +void timerStop(Timer *timer); +void timerUpdateAll(void); +uint32_t getCurrentTimerCount(void); + + + +#ifdef __cplusplus +} +#endif + +#endif // APP_FRM_TIMER_H \ No newline at end of file diff --git a/app/app_light.c b/app/app_light.c new file mode 100644 index 0000000..9bb85d9 --- /dev/null +++ b/app/app_light.c @@ -0,0 +1,227 @@ +#include "app_config.h" +#include "interface.h" +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" +#include "app_param_manage.h" + +#include "app_light.h" +#include "app_power.h" + +// 声明 light_data 变量 +LightSystem light_data; + +// ... existing code ... + +// 定义灯光按钮状态枚举 +typedef enum { + BUTTON_STATE_INITIAL, + BUTTON_STATE_SHORT_PRESS, + BUTTON_STATE_DOUBLE_PRESS_DETECTED, + BUTTON_STATE_SECOND_PRESS_DETECTED +} LightButtonState; + +// 定义灯光按钮结构体 +typedef struct { + LightButtonState state; + uint32_t press_start_time; + uint32_t release_start_time; + uint8_t is_light_on; + Timer timer; +} LightButton; + +// 全局变量 +static LightButton light_button = {BUTTON_STATE_INITIAL, 0, 0, 0, {0}}; + +static uint8_t power_state = 0; + +// 灯光按钮处理函数 +static void handleLightButton(void) +{ + switch (light_button.state) + { + case BUTTON_STATE_INITIAL: + if (light_data.light_switch == app_close()) + { + light_button.state = BUTTON_STATE_SHORT_PRESS; + timerStart(&light_button.timer, 300, 0); // 启动短按定时器，300ms + } + break; + + case BUTTON_STATE_SHORT_PRESS: + if (light_data.light_switch == app_open()) + { + if (light_button.timer.active) // 定时器未到期，短按完成，等待双击 + { + light_button.state = BUTTON_STATE_DOUBLE_PRESS_DETECTED; + timerStart(&light_button.timer, 300, 0); // 启动双击等待定时器，300ms + } + } + else if (!light_button.timer.active) // 短按定时器到期，重置为初始状态 + { + light_button.state = BUTTON_STATE_INITIAL; + } + break; + + case BUTTON_STATE_DOUBLE_PRESS_DETECTED: + if (light_data.light_switch == app_close()) + { + light_button.state = BUTTON_STATE_SECOND_PRESS_DETECTED; + timerStart(&light_button.timer, 300, 0); // 启动第二次按下定时器，300ms + } + else if (!light_button.timer.active) // 双击等待定时器到期，重置为初始状态 + { + light_button.state = BUTTON_STATE_INITIAL; + } + break; + + case BUTTON_STATE_SECOND_PRESS_DETECTED: + if (light_data.light_switch == app_open()) + { + if (!light_button.timer.active) // 第二次按下完成，切换灯光状态 + { + light_button.is_light_on = !light_button.is_light_on; + printf("LightButton: is_light_on = %d\n", light_button.is_light_on); + light_button.state = BUTTON_STATE_INITIAL; + } + } + else if (!light_button.timer.active) // 第二次按下定时器到期，重置为初始状态 + { + light_button.state = BUTTON_STATE_INITIAL; + } + break; + + default: + light_button.state = BUTTON_STATE_INITIAL; + break; + } +} + +// 灯光输出处理函数 +static void lightOutput(void *signal_id) +{ + (void)signal_id; + + // 根据当前状态,控制各个灯光 + for (int32_t i = 0; i < LIGHT_COUNT; i++) + { + uint8_t state_value = (light_data.states[i] == LIGHT_ON) ? setLightOn() : setLightOff(); + // 先判断灯光类型并设置状态 + switch (i) + {//正常所有灯光熄灭 + case LIGHT_HEAD://头灯，前面4个灯 + un_inf_can_kgf_output2.bit_data.KGF07 = state_value; + un_inf_can_kgf_output2.bit_data.KGF09 = state_value; + break; + case LIGHT_TAIL://尾灯，后面4个灯 + un_inf_can_kgf_output2.bit_data.KGF11 = state_value; + un_inf_can_kgf_output2.bit_data.KGF13 = state_value; + break; + case LIGHT_LEFT_TURN://左转向，左边4个灯 + un_inf_can_kgf_output2.bit_data.KGF08 = state_value; + un_inf_can_kgf_output2.bit_data.KGF12 = state_value; + break; + case LIGHT_RIGHT_TURN://右转向灯，右边4个灯 + un_inf_can_kgf_output2.bit_data.KGF10 = state_value; + un_inf_can_kgf_output2.bit_data.KGF14 = state_value; + break; + case LIGHT_BRAKE://刹车灯，四个黄灯 + un_inf_can_kgf_output2.bit_data.KGF11 = state_value; + un_inf_can_kgf_output2.bit_data.KGF13 = state_value; + break; + case LIGHT_ALARM://报警灯，四个红灯 + un_inf_can_kgf_output2.bit_data.KGF11 = state_value; + un_inf_can_kgf_output2.bit_data.KGF13 = state_value; + break; + } + } + + // 灯的状态有变化，就存入参数，一个灯对应一位，共6位 + for (int32_t i = 0; i < LIGHT_COUNT; i++) + { + light_data.light_state |= (light_data.states[i] << i); + } + if (light_data.light_state != light_data.old_light_state) + { + setParam("lightSt", (float)light_data.light_state); + light_data.old_light_state = light_data.light_state; + } + + publishMessage(&un_inf_can_kgf_output2, 1); +} + +// 灯光状态处理函数 +static void lightProcess(void *signal_id) +{ + (void)signal_id; + + if(0 != power_state)//处于非掉电模式才能打开 + { + // 调用按钮处理函数 + handleLightButton(); + } + else + { + light_button.state = BUTTON_STATE_INITIAL;//一直保持初始化模式 + light_button.is_light_on = LIGHT_OFF;//一直关闭 + } + + // 根据双击标志控制灯光输出，相当于灯光总开关 + for (int32_t i = 0; i < LIGHT_COUNT; i++) + { + light_data.states[i] = (LightState)(light_button.is_light_on); + } + lightOutput(NULL); + + timerStart(&light_data.timer_function, 100, 1); +} + +// 处理输入信号的函数 +static void lightInput(void *signal_id) +{ +// LightSystem old_data = light_data; + + // 填充数据 + if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据 + { + // 保存遥控器输入开关状态 + light_data.light_switch = un_remote_control_input.bit_data.switch_d ? LIGHT_ON : LIGHT_OFF; + } + else if(signal_id == &power_data) + { + power_state = power_data.current_state; + } +} + + +// APP模块的初始化 +void lightAppInit(void) +{ + // 初始化时恢复灯光状态 + light_data.light_state = (uint8_t)getParam("lightSt"); + light_data.old_light_state = light_data.light_state; + // 根据灯光状态恢复灯光 + for (int32_t i = 0; i < LIGHT_COUNT; i++) + { + light_data.states[i] = (LightState)( (light_data.light_state >> i) & 1 ); + } + // 初始化 + memset(&light_data, 0, sizeof(LightSystem)); + for (int32_t i = 0; i < LIGHT_COUNT; i++) + { + light_data.states[i] = LIGHT_OFF; + light_data.brightness[i] = 255; // 默认最大亮度 + light_data.blink_state[i] = 0; // 初始化闪烁状态 + light_data.blink_interval[i] = 500; // 默认闪烁间隔500ms + } + + // 订阅输入信号,处理灯光逻辑 + subscribe(&un_remote_control_input, lightInput); + subscribe(&power_data, lightInput); + + // 订阅定时器 + subscribe(&light_data.timer_function, lightProcess); + timerStart(&light_data.timer_function, 500, 1); + + printf("app_light: initial OK \n"); +} diff --git a/app/app_light.h b/app/app_light.h new file mode 100644 index 0000000..fb77356 --- /dev/null +++ b/app/app_light.h @@ -0,0 +1,62 @@ +#ifndef APP_LIGHT_H +#define APP_LIGHT_H + +#ifdef __cplusplus +extern "C" +{ +#endif + + + + +// 定义灯光状态枚举 +typedef enum +{ + LIGHT_OFF, + LIGHT_ON, + LIGHT_BLINK +} LightState; + +// 定义灯光类型枚举 +typedef enum +{ + LIGHT_HEAD, + LIGHT_TAIL, + LIGHT_LEFT_TURN, + LIGHT_RIGHT_TURN, + LIGHT_BRAKE, + LIGHT_ALARM, + LIGHT_COUNT +} LightType; + +typedef struct +{ + LightState states[LIGHT_COUNT]; + uint8_t brightness[LIGHT_COUNT]; + uint16_t blink_interval[LIGHT_COUNT]; // 闪烁间隔(毫秒) + uint8_t blink_state[LIGHT_COUNT]; // 闪烁状态 + Timer timer;// 设置定时器，定时调用灯光处理函数 + uint8_t old_light_state; // 旧的灯光状态 + uint8_t light_state; // 当前的灯光状态 + Timer timer_function; // 函数定时器 + uint8_t light_switch; // 当前的灯光开关状态 + uint8_t double_click_flag;//双击标志 +} LightSystem; + +// 在头文件中声明外部变量 +extern LightSystem light_data; + +// 使用内联函数 +static inline uint8_t setLightOn(void) { return 1; } +static inline uint8_t setLightOff(void) { return 0; } + +void lightAppInit(void); +void setLightState(LightType light, LightState state); +void setLightBrightness(LightType light, uint8_t brightness); +void setBlinkInterval(uint16_t interval); + +#ifdef __cplusplus +} +#endif + +#endif // APP_LIGHT_H \ No newline at end of file diff --git a/app/app_param_manage.c b/app/app_param_manage.c new file mode 100644 index 0000000..b7950d6 --- /dev/null +++ b/app/app_param_manage.c @@ -0,0 +1,464 @@ +#include "app_config.h" +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" +#include "app_param_manage.h" + + +#define E2_RESERVE_COUNT 0x20 //增加IP地址 修改为0x20 20250110 + +// 待发送的参数请求信号 +UnParamRequest un_param_request1 ; +UnParamRequest un_param_request2; + +RequestContext request_send ; +RequestContext request_context ; + +uint8_t read_write_e2_finished = 0; + +// 定义全局信号实例，读写信号现在包括 offset 和 size +ParamSignal param_signal = { + .param_ptr = NULL, // 参数指针初始化为 NULL + .type = READ_OPERATION, // 操作类型设置为读操作 + .offset = 0, // 整个数据块的偏移 + .size = sizeof(param_manager.arr) // 整个数据块的大小 +}; + + +// 全局变量：初始化参数名称结构体 +ParamNames param_names = { +#define X(name) .name = #name, + PARAM_LIST +#undef X +}; + + +UnParamManager param_manager ; + +static uint8_t is_param_initialized = 0; + +// 打印所有参数的名称和值, 每行 4 个参数 +void printParams() +{ + unsigned int param_count = 0; + + #define X(name) \ + printf("%-8s: %-8.2f", param_names.name, param_manager.bit_data.name); \ + param_count++; \ + if (param_count % 4 == 0) { \ + printf("\n"); \ + } else { \ + printf(" "); \ + } + PARAM_LIST + #undef X + + // 如果最后一行不足 4 个参数, 打印换行 + if (param_count % 4 != 0) + { + printf("\n"); + } +} + + + +void writeByte24c02(uint16_t addr, uint8_t data) +{ + if(0 != wrbyte_24c02(addr,data)) + { + printf("E2PROM write error!\n"); + } +} + +uint8_t readByte24c02(uint16_t addr) +{ + return rdbyte_24c02(addr); +} + +// 定义一个通用的 EEPROM 访问函数 +uint8_t accessEeprom(size_t offset, void *data, size_t size, OperationType type) +{ + if (data == NULL || size == 0) + { + return 1; // 返回错误状态，表示无效的参数 + } + + uint8_t *byte_data = (uint8_t *)data; // 将 void* 转换为 uint8_t*，方便逐字节操作 + size_t index; + + if (type == WRITE_OPERATION) + { + // 写入操作 + for (index = 0; index < size; index++) + { + writeByte24c02((uint16_t)(offset + index + E2_RESERVE_COUNT), byte_data[index]); + udelay(4000);//写入一个字节延时4ms + } + // 校验 + for (index = 0; index < size; index++) + { + if (readByte24c02((uint16_t)(offset + index + E2_RESERVE_COUNT)) != byte_data[index]) + { + return 2; // 返回错误状态，表示写入验证失败 + } + } + } + else + { + // 读取操作 + for (index = 0; index < size; index++) + { + byte_data[index] = readByte24c02((uint16_t)(offset + index + E2_RESERVE_COUNT)); + } + } + return 0; // 返回状态，表示成功 +} + +void handleParamOp(void *data) +{ + ParamSignal *signal = (ParamSignal *)data; + + if (signal->param_ptr == NULL) + { + // 操作整个参数管理器 + if (accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), signal->type) == 0) + { + read_write_e2_finished = 1; + publishMessage(&read_write_e2_finished, 1); // 读写成功 + } + else + { + read_write_e2_finished = 2; + publishMessage(&read_write_e2_finished, 1); // 读写失败 + } + } + else + { + // 根据信号中的偏移和大小操作单个参数 + if (accessEeprom(signal->offset, signal->param_ptr, signal->size, signal->type) == 0) + { + read_write_e2_finished = 1; + publishMessage(&read_write_e2_finished, 1); // 读写成功 + } + else + { + read_write_e2_finished = 2; + publishMessage(&read_write_e2_finished, 1); // 读写失败 + } + } +} + + +uint8_t calculateCRC(const uint8_t* data, uint32_t length) { + uint8_t crc = 0; + for (uint32_t i = 0; i < length; ++i) { + crc += data[i]; // 简单的校验和，按字节累加 + } + return crc; +} + + +float readParameter(const char *param_name) { + float float_value = 0; + unsigned int offset = 0; + + #define X(name) \ + if (strcmp(param_name, param_names.name) == 0) { \ + accessEeprom(offset, ¶m_manager.bit_data.name,sizeof(param_manager.bit_data.name), READ_OPERATION);\ + memcpy(&float_value, ¶m_manager.bit_data.name, sizeof(param_manager.bit_data.name)); \ + return float_value; \ + } \ + offset += 4; + + PARAM_LIST + #undef X + + printf("Parameter not found: %s\n", param_name); + return 0; +} + + + +void writeParameter(const char *param_name, const uint8_t *data) { + unsigned int offset = 0; + + #define X(name) \ + if (strcmp(param_name, param_names.name) == 0) { \ + memcpy(¶m_manager.bit_data.name, data, sizeof(param_manager.bit_data.name)); \ + accessEeprom(offset, ¶m_manager.bit_data.name,sizeof(param_manager.bit_data.name), WRITE_OPERATION);\ + return; \ + } \ + offset += 4; + + PARAM_LIST + #undef X + + printf("Parameter not found: %s\n", param_name); +} + + + +void sendParamRequestResponse(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port, uint8_t isWriteOperation) { + // 准备响应帧 + paramRequest->bit_data.frame_header = 0xFF80; + paramRequest->bit_data.frame_type = 0x002B; + paramRequest->bit_data.frame_length = sizeof(StrParamRequest); + paramRequest->bit_data.accumulated = 0; + paramRequest->bit_data.request_id = isWriteOperation ? 98 : 99; + + paramRequest->bit_data.crc = calculateCRC(paramRequest->arr, sizeof(paramRequest->arr) - 1); + + request_send.param_request = paramRequest; + request_send.sender_ip = sender_ip; + request_send.sender_port = sender_port; + // 发送信号，从UDP发送 + publishMessage(&request_send, 1); +} + + +void processReadAllParams(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) { + uint8_t allParams[256][4]; // Size based on E2 size + unsigned int i = 0; + float param_value; + uint8_t exceeded_max = 0; // 新增标志变量 + + // 清零 paramRequest + memset(paramRequest, 0, sizeof(UnParamRequest)); + + accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION); + + printf("Sending parameter data:\n"); + + #define X(name) \ + if (!exceeded_max) { \ + if (i < 256) { \ + strncpy((char *)paramRequest->bit_data.param_name[i], #name, sizeof(paramRequest->bit_data.param_name[i]) - 1); \ + paramRequest->bit_data.param_name[i][sizeof(paramRequest->bit_data.param_name[i]) - 1] = '\0'; \ + memcpy(allParams[i], ¶m_manager.bit_data.name, sizeof(param_manager.bit_data.name)); \ + memcpy(¶m_value, allParams[i], sizeof(float)); \ + printf("Parameter name: %-20s Value: %f\n", #name, param_value); \ + i++; \ + } else { \ + printf("Warning: Exceeded maximum number of parameters\n"); \ + exceeded_max = 1; \ + } \ + } + + PARAM_LIST + #undef X + + // Pack all parameter data into paramRequest + memcpy(paramRequest->bit_data.data, allParams, sizeof(allParams)); + + printf("Total parameters sent: %d\n", i); + + // Send response + sendParamRequestResponse(paramRequest, sender_ip, sender_port, 0); +} + + +void processWriteRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) { + float value; + printf("Processing write request.\n"); + + // 先发送信号，然后从结构体读数 + for (int i = 0; i < 256; ++i) { + if (strlen((char *)paramRequest->bit_data.param_name[i]) > 0) { + writeParameter(paramRequest->bit_data.param_name[i], paramRequest->bit_data.data[i]); + printf("paramRequest->bit_data.param_name[i]:%s \n",paramRequest->bit_data.param_name[i]); + memcpy(&value, paramRequest->bit_data.data[i], sizeof(float)); + printf("paramRequest->bit_data.data[i]:%f \n", value); + } + } + + // 发送响应，发送所有参数 + processReadAllParams(paramRequest, sender_ip, sender_port); +} + + + +void processReadRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) { + // 处理读请求的逻辑 + printf("Processing read request.\n"); + + // 清零 paramRequest + memset(paramRequest, 0, sizeof(UnParamRequest)); + + // 先发送信号，然后从结构体读数 + for (int i = 0; i < 256; ++i) { + if (strlen((char *)paramRequest->bit_data.param_name[i]) > 0) { + float readData = readParameter(paramRequest->bit_data.param_name[i]); + memcpy(paramRequest->bit_data.data[i], &readData, sizeof(paramRequest->bit_data.data[i])); + } + } + + // 发送响应 + sendParamRequestResponse(paramRequest, sender_ip, sender_port, 0); +} + + + + + +void OnParamSignal(void *data) +{ + RequestContext *signal = (RequestContext *)data; + + uint8_t *datagram = (uint8_t *)signal->param_request->arr; + uint16_t request_id = ((uint16_t)datagram[7] << 8) | (uint16_t)datagram[8];// 大端模式 + + // 调试输出 + printf("Received request ID: 0x%04X\n", request_id); + + // 计算CRC + uint8_t calculatedCrc = calculateCRC(datagram, sizeof(UnParamRequest) - 1); + uint8_t receivedCrc = datagram[sizeof(UnParamRequest) - 1]; + + // 比较CRC + if (calculatedCrc != receivedCrc) + { + printf("CRC check failed, discarding data\n"); + printf("Calculated CRC: 0x%02X, Received CRC: 0x%02X\n", calculatedCrc, receivedCrc); + return; + } + + printf("CRC check passed\n"); + + if (request_id == 100) + { // 读请求 + processReadRequestFrame(signal->param_request, signal->sender_ip, signal->sender_port); + } + else if (request_id == 101) + { // 写请求 + processWriteRequestFrame(signal->param_request, signal->sender_ip, signal->sender_port); + } + else if (request_id == 102) + { // 读取所有参数 + processReadAllParams(signal->param_request, signal->sender_ip, signal->sender_port); + } + else + { + printf("Unknown request ID.\n"); + return; + } +} + +float getParam(const char *param_name) +{ + // 检查是否已初始化 + if (!is_param_initialized) + { + printf("Parameters not initialized, reinitializing\n"); + accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION);//Read all parameters from E2 + is_param_initialized = 1; // Mark as initialized + printParams(); + return 0.0f; + } + + // 检查参数名是否为空 + if (param_name == NULL) + { + printf("Error: Parameter name is empty\n"); + return 0.0f; + } + + // 遍历所有参数 + #define X(name) \ + if (strcmp(param_name, #name) == 0) \ + { \ + return param_manager.bit_data.name; \ + } + PARAM_LIST + #undef X + + // 如果没有找到匹配的参数名 + printf("Error: Parameter %s not found\n", param_name); + return 0.0f; +} + +// setParam 函数 +uint8_t setParam(const char *param_name, float value) +{ + // 检查参数名是否为空 + if (param_name == NULL) + { + printf("Error: Parameter name is empty\n"); + return 2; // 返回错误码 + } + + // 参数名和值写入EEPROM，先转成字节数组 + uint8_t data[sizeof(float)]; + memcpy(data, &value, sizeof(float)); + writeParameter(param_name, data); + + // 更新参数 + #define X(name) \ + if (strcmp(param_name, #name) == 0) \ + { \ + memcpy(¶m_manager.bit_data.name, data, sizeof(param_manager.bit_data.name)); \ + } + PARAM_LIST + #undef X + + return 0; +} + +void paramAppInit(void) +{ + // 初始化全局变量 + memset(&un_param_request1, 0, sizeof(UnParamRequest)); + memset(&un_param_request2, 0, sizeof(UnParamRequest)); + + // 正确初始化 RequestContext 结构体 + request_send.param_request = &un_param_request1; + request_send.sender_ip = 0; + request_send.sender_port = 0; + + request_context.param_request = &un_param_request2; + request_context.sender_ip = 0; + request_context.sender_port = 0; + + // 上电读取所有参数 + memset(param_manager.arr, 0, sizeof(param_manager.arr)); + accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION); + + // 初始化每个参数 + // param_manager.bit_data.whl_bas = 1.5f; // 初始化轮距 + // param_manager.bit_data.max_rpm = 5500.0f; // 初始化最大转速 + // param_manager.bit_data.whl_dia = 0.6f; // 初始化轮直径 + // param_manager.bit_data.max_acc = 1.0f; // 初始化最大加速度 + // param_manager.bit_data.spd_kp = 5.0f; // 初始化速度控制 KP + // param_manager.bit_data.spd_ki = 1.0f; // 初始化速度控制 KI + // param_manager.bit_data.spd_kd = 0.0f; // 初始化速度控制 KD + // param_manager.bit_data.spd_il = 5.0f; // 初始化速度控制 IL + // param_manager.bit_data.spd_ol = 5.0f; // 初始化速度控制 OL + // param_manager.bit_data.crv_kp = 1.0f; // 初始化曲线控制 KP + // param_manager.bit_data.crv_ki = 0.0f; // 初始化曲线控制 KI + // param_manager.bit_data.crv_kd = 0.0f; // 初始化曲线控制 KD + // param_manager.bit_data.crv_il = 2.0f; // 初始化曲线控制 IL + // param_manager.bit_data.crv_ol = 2.0f; // 初始化曲线控制 OL + // param_manager.bit_data.brk_on = 1500.0f; // 初始化制动开启参数 + // param_manager.bit_data.brk_off = 800.0f; // 初始化制动关闭参数 + // param_manager.bit_data.maxTorq = 60.0f; // 初始化最大扭矩 + // param_manager.bit_data.feedPwr = 10000.0f; // 初始化馈电功率 + // param_manager.bit_data.dispPwr = 10000.0f; // 初始化显示功率 + // param_manager.bit_data.VehMass = 700.0f; // 初始化车辆质量 + // param_manager.bit_data.gRatio = 28.0f; // 初始化减速比 + // param_manager.bit_data.prCTime = 5.0f; // 初始化预充时间 + // param_manager.bit_data.brk_pos = 0.0f; // 初始化刹车位置, 0表示未刹车 + // param_manager.bit_data.pwr_sta = 0.0f; // 初始化电源状态 + // param_manager.bit_data.lightSt = 0.0f; // 初始化灯光状态 + // param_manager.bit_data.pwr_btn = 0.0f; // 初始化电源按钮状态 + // param_manager.bit_data.test = 0.0f; // 初始化测试参数 + + + // 订阅信号 + subscribe(¶m_signal, handleParamOp); + subscribe(&request_context, OnParamSignal);// 接收到上位机读写参数信号 + + printParams();//打印所有参数 + + is_param_initialized = 1; // 标记初始化完成 + + printf("paramAPP init OK! %d\n",getCurrentTime()); +} diff --git a/app/app_param_manage.h b/app/app_param_manage.h new file mode 100644 index 0000000..72508f6 --- /dev/null +++ b/app/app_param_manage.h @@ -0,0 +1,148 @@ +#ifndef APP_PARAM_MANAGE_H +#define APP_PARAM_MANAGE_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "app_config.h" + + +// 定义参数列表宏 +#define PARAM_LIST \ + X(whl_bas) \ + X(max_rpm) \ + X(whl_dia) \ + X(max_acc) \ + X(spd_kp) \ + X(spd_ki) \ + X(spd_kd) \ + X(spd_il) \ + X(spd_ol) \ + X(crv_kp) \ + X(crv_ki) \ + X(crv_kd) \ + X(crv_il) \ + X(crv_ol) \ + X(brk_on) \ + X(brk_off) \ + X(maxTorq) \ + X(feedPwr) \ + X(dispPwr) \ + X(VehMass) \ + X(gRatio) \ + X(prCTime) \ + X(brk_pos) \ + X(pwr_sta) \ + X(lightSt) \ + X(pwr_btn) \ + X(sleepTm) \ + X(wakeTm) \ + X(R_maxP) \ + X(R_minP) \ + X(R_zeroP) \ + X(R_speed) \ + X(L_maxP) \ + X(L_minP) \ + X(L_zeroP) \ + X(L_speed) \ + X(brk_rev) \ + X(tra_wid) \ + X(mot_kp) \ + X(mot_ki) \ + X(mot_kd) \ + X(mot_il) \ + X(mot_ol) \ + X(diff_sp) \ + X(test) + +// 定义一个包含所有参数名称的结构体 +typedef struct { + #define X(name) const char* name; + PARAM_LIST + #undef X +} ParamNames; + +// 参数结构体，不能使用位域，不要超过E2的大小, 1K byte +typedef struct +{ + #define X(name) float name; + PARAM_LIST + #undef X +} ParamData; + +typedef union +{ + ParamData bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(ParamData)]; // 通过结构体类型确定大小 +} UnParamManager; + +// 定义信号操作类型 +typedef enum +{ + READ_OPERATION, + WRITE_OPERATION +} OperationType; + +// 定义信号数据结构 +typedef struct +{ + void *param_ptr; // 参数数据的指针 + OperationType type; // 操作类型 + size_t offset; // 参数在结构体中的偏移 + size_t size; // 参数大小 +} ParamSignal; + +#pragma pack(push, 1) + +typedef struct _StrParamRequest { + //-------------------------------------------------- + uint16_t frame_header; // 帧头 固定值0xFF80 (16位) + uint16_t frame_type; // 帧类型 固定值0x002A (16位) + uint16_t frame_length; // 帧长 根据参数数据的长度动态设置 (16位) + uint8_t accumulated; // 累加值 按帧累加 (8位) + uint16_t request_id; // 请求帧ID 请求ID 100表示读，101 表示写参数 (16位) + char param_name[256][8]; // 参数名称 标识要写入或读出的参数 + uint8_t data[256][4]; // 数据 用于写入或读出的参数值，一个参数最大4字节 (8位*4) + uint8_t crc; // CRC 按字节累加之和 取低8位 (8位) +} StrParamRequest; + +typedef union _UnParamRequest { + StrParamRequest bit_data; // 使用定义的结构体变量名 + unsigned char arr[sizeof(StrParamRequest)]; // 通过结构体类型确定大小 +} UnParamRequest; + +typedef struct { + UnParamRequest *param_request; // 指向 UnParamRequest 的指针 + uint32_t sender_ip; // 发送方的 IP 地址（使用标准的32位整数表示） + uint16_t sender_port; // 发送方的端口号（使用标准的16位整数表示） +} RequestContext; + +#pragma pack(pop) + +extern UnParamRequest un_param_request;// 声明用于参数响应的帧实例 +extern RequestContext request_context; + +// 声明全局信号实例 +extern ParamNames param_names; +extern UnParamManager param_manager; // 全局参数管理实例 +extern ParamSignal param_signal; +extern uint8_t read_write_e2_finished; +extern RequestContext request_send;// 待发送的参数请求信号 + +uint8_t access_eeprom(size_t offset, void *data, size_t size, OperationType type); +void paramAppInit(void); + +// 在适当的位置添加以下函数声明 +float getParam(const char *param_name); +uint8_t setParam(const char *param_name, float value); +void printParams(void); +void handleParamOp(void *data); +void OnParamSignal(void *data); + +#ifdef __cplusplus +} +#endif + +#endif // APP_PARAM_MANAGE_H diff --git a/app/app_pid.c b/app/app_pid.c new file mode 100644 index 0000000..f760ee7 --- /dev/null +++ b/app/app_pid.c @@ -0,0 +1,130 @@ +#include "app_config.h" + +#include "app_pid.h" + + + + +// PID控制器初始化 +void initializePid(PID_t *pid, pid_mode_t mode, float dtMin) +{ + pid->mode = mode; + pid->dt_min = dtMin > SIGMA ? dtMin : SIGMA; + pid->kp = 0.0f; + pid->ki = 0.0f; + pid->kd = 0.0f; + pid->integral = 0.0f; + pid->integral_limit = 0.0f; + pid->output_limit = 0.0f; + pid->error_previous = 0.0f; + pid->last_output = 0.0f; +} + +// 设置PID参数 +int32_t setPidParameters(PID_t *pid, float kp, float ki, float kd, float integralLimit, float outputLimit) +{ + int32_t ret = 0; + + if (isfinite(kp)) { + pid->kp = kp; + } else { + ret = -1; + } + + if (isfinite(ki)) { + pid->ki = ki; + } else { + ret = -2; + } + + if (isfinite(kd)) { + pid->kd = kd; + } else { + ret = -3; + } + + if (isfinite(integralLimit)) { + pid->integral_limit = integralLimit; + } else { + ret = -4; + } + + if (isfinite(outputLimit)) { + pid->output_limit = outputLimit; + } else { + ret = -5; + } + + return ret; +} + +/** + * @brief 计算PID控制器的输出 + * + * @param pid 指向PID_t结构体的指针，包含PID控制器的状态和参数 + * @param sp 设定值(Setpoint)，期望系统达到的目标值 + * @param val 当前值(Current Value)，系统的实际测量值 + * @param val_dot 当前值的导数(Derivative of Current Value)，即测量值的变化率（用于微分项计算） + * @param dt 时间增量(Time Increment)，两次调用之间的时间间隔，用于计算积分和微分 + * + * @return float 返回PID控制器计算出的输出值 + */ +float calculatePidOutput(PID_t *pid, float sp, float val, float val_dot, float dt) +{ + // 检查输入参数的有效性 + if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt) || dt < pid->dt_min) { + return pid->last_output; + } + + // 计算误差 + float error = sp - val; + + // 根据模式计算微分项 + float derivative = 0.0f; + switch (pid->mode) { + case PID_MODE_DERIVATIVE_CALC: + derivative = (error - pid->error_previous) / dt; + pid->error_previous = error; + break; + case PID_MODE_DERIVATIVE_CALC_NO_SP: + derivative = (-val - pid->error_previous) / dt; + pid->error_previous = -val; + break; + case PID_MODE_DERIVATIVE_SET: + derivative = -val_dot; + break; + default: + derivative = 0.0f; + break; + } + + // 计算比例和微分项的输出 + float output = (pid->kp * error) + (pid->kd * derivative); + + // 计算积分项，并检查积分饱和 + if (pid->ki > SIGMA) { + pid->integral += error * dt; + if (pid->integral > pid->integral_limit) { + pid->integral = pid->integral_limit; + } else if (pid->integral < -pid->integral_limit) { + pid->integral = -pid->integral_limit; + } + output += pid->ki * pid->integral; + } + + // 限制输出范围 + if (output > pid->output_limit) { + output = pid->output_limit; + } else if (output < -pid->output_limit) { + output = -pid->output_limit; + } + + pid->last_output = output; + return output; +} + +// 重置积分器 +void resetPidIntegral(PID_t *pid) +{ + pid->integral = 0.0f; +} diff --git a/app/app_pid.h b/app/app_pid.h new file mode 100644 index 0000000..25a7962 --- /dev/null +++ b/app/app_pid.h @@ -0,0 +1,46 @@ +#ifndef PID_H +#define PID_H + +#ifdef __cplusplus +extern "C" { +#endif + +// 防止除零和其他误差的极小值 +#define EPSILON 1e-5f +#define SIGMA EPSILON + +#include "app_config.h" + +// PID 控制模式 +typedef enum { + PID_MODE_DERIVATIVE_NONE = 0, // PI 控制器模式，无微分项 + PID_MODE_DERIVATIVE_CALC, // 根据当前误差计算微分项 + PID_MODE_DERIVATIVE_CALC_NO_SP, // 根据当前值计算微分项，忽略设定值 + PID_MODE_DERIVATIVE_SET // 使用外部提供的微分项值 +} pid_mode_t; + +// PID 控制器结构体 +typedef struct { + pid_mode_t mode; // 控制模式 + float dt_min; // 最小时间间隔 + float kp; // 比例系数 + float ki; // 积分系数 + float kd; // 微分系数 + float integral; // 积分累积值 + float integral_limit; // 积分限幅 + float output_limit; // 输出限幅 + float error_previous; // 上一次的误差值 + float last_output; // 上一次的输出值 +} PID_t; + +// 函数声明 +void initializePid(PID_t *pid, pid_mode_t mode, float dtMin); +int32_t setPidParameters(PID_t *pid, float kp, float ki, float kd, float integralLimit, float outputLimit); +float calculatePidOutput(PID_t *pid, float sp, float val, float val_dot, float dt); +void resetPidIntegral(PID_t *pid); + +#ifdef __cplusplus +} +#endif + +#endif // PID_H diff --git a/app/app_power.c b/app/app_power.c new file mode 100644 index 0000000..8d0ce8a --- /dev/null +++ b/app/app_power.c @@ -0,0 +1,366 @@ +#include "app_config.h" +#include "interface.h" +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" +#include "app_param_manage.h" +#include "app_power.h" + + +// 定义按钮状态枚举 +typedef enum { + BUTTON_STATE_INITIAL, + BUTTON_STATE_SHORT_PRESS, + BUTTON_STATE_SHORT_PRESS_DETECTED, + BUTTON_STATE_WAIT_FOR_LONG_PRESS, + BUTTON_STATE_LONG_PRESS, + BUTTON_STATE_LONG_PRESS_WAIT +} ButtonState; + +// 定义按钮结构体 +typedef struct { + ButtonState state; + uint32_t press_start_time; + uint32_t release_start_time; + uint8_t is_power_on; + Timer timer; + uint8_t old_is_power_on; + Timer timer1; +} PowerButton; + +// 全局变量 +PowerSystem power_data; +static PowerButton power_button = {BUTTON_STATE_INITIAL, 0, 0, 0, {0},0}; + +// 电源按钮处理函数 +static void handlePowerButton(void) +{ + switch (power_button.state) + { + case BUTTON_STATE_INITIAL: + if (power_data.remote_power_switch == app_close()) + { + power_button.state = BUTTON_STATE_SHORT_PRESS_DETECTED; + timerStart(&power_button.timer, 500, 0); // 启动短按定时器，500ms + } + break; + + case BUTTON_STATE_SHORT_PRESS_DETECTED: + if (power_data.remote_power_switch == app_open()) + { + if (power_button.timer.active) // 定时器未到期,短按完成，启动等待长按定时器 + { + power_button.state = BUTTON_STATE_WAIT_FOR_LONG_PRESS; + timerStart(&power_button.timer, 500, 0); // 启动等待长按定时器，500ms + } + } + else if (!power_button.timer.active)// 短按定时器到期，按键仍被按下，视为无效，重置为初始状态 + { + power_button.state = BUTTON_STATE_INITIAL; + } + break; + + case BUTTON_STATE_WAIT_FOR_LONG_PRESS: + if (power_data.remote_power_switch == app_close())// 检测是否在等待时间内进行长按 + { + power_button.state = BUTTON_STATE_LONG_PRESS; + timerStart(&power_button.timer, 1000, 0); // 启动长按定时器，1000ms + } + else if (!power_button.timer.active) // 等待长按超时，重置为初始状态 + { + power_button.state = BUTTON_STATE_INITIAL; + } + break; + + case BUTTON_STATE_LONG_PRESS: + + if (!power_button.timer.active)// 长按完成，切换电源状态 20250423 修改不需要判断松开按键就打开控制器 + { + power_button.is_power_on = !power_button.is_power_on; + printf("PowerButton: is_power_on = %d\n", power_button.is_power_on); + power_button.state = BUTTON_STATE_LONG_PRESS_WAIT; + } + else if(power_data.remote_power_switch == app_open()) + { + power_button.state = BUTTON_STATE_INITIAL; + printf("Long press for short duration"); + } + else + break; + + case BUTTON_STATE_LONG_PRESS_WAIT: + if (power_data.remote_power_switch == app_open())// 检测按键释放 + { + power_button.state = BUTTON_STATE_INITIAL; + printf("Release the button"); + } + default: + power_button.state = BUTTON_STATE_INITIAL; + break; + } +} + +// 输出处理函数 +static void powerOutput(void *signal_id) +{ + (void)signal_id; + + // 根据当前状态,控制各个设备的电源 + switch (power_data.current_state) + { + case POWER_PRE_CHARGE: + publishMessage(&power_data.pre_charge_finish, 1);//发布预充完成信号，100ms发送一次，直到预充完成 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOn(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + break; + + case POWER_STANDBY: + // 初始状态,只开启基本设备 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOff(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOff(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + break; + + case POWER_WORKING: + // 工作状态,除预充继电器外所有设备开启 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOn(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOn(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + break; + + case POWER_EMERGENCY: + // 急停状态,断开高压 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + break; + + case POWER_SLEEP: + // 休眠状态,关闭所有设备 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOff(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOff(); // 低压继电器 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + break; + + default: + break; + } + publishMessage(&power_data, 1); + publishMessage(&un_inf_can_kgf_output1, 1); + publishMessage(&un_inf_can_kgf_output2, 1); +} + + +static void wakeupProcess(void *signal_id) +{ + (void)signal_id; + + un_gather_output.bit_data.sleep_duration = (uint16_t)getParam("sleepTm"); + un_gather_output.bit_data.wakeup_interval = (uint16_t)getParam("wakeTm"); + + if(un_gather_output.bit_data.sleep_duration < 5)//最小值限定 + { + un_gather_output.bit_data.sleep_duration = 5; + } + + if(un_gather_output.bit_data.wakeup_interval < 5)//最小值限定 + { + un_gather_output.bit_data.wakeup_interval = 5; + } + + un_gather_output.bit_data.vehicle_mode = power_data.current_state; + + publishMessage(&un_gather_output, 1); + timerStart(&power_data.timer1, 500, 1); //周期调用 +} + +// 定时器处理函数 +static void powerTimerProcess(void *signal_id) +{ + (void)signal_id; + + // 调用电源按钮处理函数 + handlePowerButton(); + // 电源按钮状态有变化，保存到参数 + if (power_button.is_power_on != power_button.old_is_power_on) + { + setParam("pwr_btn", (float)power_button.is_power_on); + power_button.old_is_power_on = power_button.is_power_on; + } + + // 状态转换逻辑 + switch (power_data.current_state) + { + case POWER_PRE_CHARGE: + if (!power_data.timer_pre_charge.active) // 预充时间到 + { + power_data.current_state = POWER_WORKING; // 工作 + power_data.pre_charge_finish = 1; // 预充完成 + printf("Power: Transitioning from PRE_CHARGE to WORKING state\n"); + } + break; + case POWER_STANDBY: + if (power_button.is_power_on == app_close() && power_data.emergency_stop == app_close()) // 遥控器电源开关闭合且急停开关闭合 + { + power_data.current_state = POWER_EMERGENCY; // 急停 + printf("Power: Transitioning from STANDBY to EMERGENCY state\n"); + } + break; + case POWER_WORKING: + if (power_data.emergency_stop == app_close()) // 急停开关闭合 + { + power_data.current_state = POWER_EMERGENCY; // 急停 + printf("Power: Transitioning from WORKING to EMERGENCY state\n"); + } + break; + case POWER_EMERGENCY: + if (power_button.is_power_on == app_open()) // 遥控器电源开关断开 + { + power_data.current_state = POWER_STANDBY; // 待机 + printf("Power: Transitioning from EMERGENCY to STANDBY state\n"); + } + else if (power_data.emergency_stop == app_open()) // 急停断开 + { + power_data.current_state = POWER_PRE_CHARGE; // 预充 + timerStart(&power_data.timer_pre_charge, (uint32_t)(getParam("prCTime") * 1000), 1); // 启动预充定时器 + printf("Power: Transitioning from EMERGENCY to PRE_CHARGE state\n"); + } + break; +// case POWER_SLEEP: +// if (power_data.high_voltage_switch == app_close()) // 高压开关闭合 +// { +// power_data.current_state = POWER_STANDBY; // 待机 +// printf("Power: Transitioning from SLEEP to STANDBY state\n"); +// } +// break; + default: + power_data.current_state = POWER_STANDBY; // 待机 + break; + } + + powerOutput(NULL); // 输出 + + // 电源状态有变化，记录到参数 + if (power_data.old_state != power_data.current_state) + { + power_data.old_state = power_data.current_state; + setParam("pwr_sta", (float)power_data.current_state); + } + + timerStart(&power_data.timer, 100, 1); //周期调用 +} + +// 处理所有输入信号的函数 +static void powerInput(void *signal_id) +{ + //不能直接赋值，用memcpy + PowerSystem old_data; + memcpy(&old_data, &power_data, sizeof(PowerSystem)); + + // 填充数据 + if (signal_id == &un_sw_sample) + { + power_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch;//急停开关 + power_data.high_voltage_switch = (uint8_t)un_sw_sample.bit_data.High_voltage_switch;//高压开关 + + printf("emergency_stop_switch: = %d,high_voltage_switch = %d\n", power_data.emergency_stop_switch,power_data.high_voltage_switch); + } + else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据 + { + power_data.remote_power_switch = (uint8_t)un_remote_control_input.bit_data.switch_d; // 遥控器电源开关 + power_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1;// 遥控器急停开关 + } + + // 急停开关 + power_data.emergency_stop = (uint8_t)( (power_data.emergency_stop_switch == app_close()) || (power_data.remote_emergency_stop == app_close()) ); + +} + + +// APP模块的初始化 +void powerAppInit(void) +{ + // 初始化变量 + memset(&power_data, 0, sizeof(PowerSystem)); + power_data.current_state = POWER_STANDBY; + // 初始化时恢复电源状态 + power_data.current_state = (PowerState)getParam("pwr_sta"); + power_data.old_state = power_data.current_state; + // 恢复电源按钮状态 + power_button.is_power_on = (uint8_t)getParam("pwr_btn"); + power_button.old_is_power_on = power_button.is_power_on; + + // 订阅输入信号 + subscribe(&un_sw_sample, powerInput); // 急停开关、高压开关 + subscribe(&un_remote_control_input, powerInput); // 遥控器电源开关 + // 定时器 + timerInit(&power_data.timer); + subscribe(&power_data.timer, powerTimerProcess); + timerStart(&power_data.timer, 500, 1); // 周期调用 + //定时器唤醒 + timerInit(&power_data.timer1); + subscribe(&power_data.timer1, wakeupProcess); + timerStart(&power_data.timer1, 500, 1); // 周期调用 + + //预充定时器 + timerInit(&power_data.timer_pre_charge); + subscribe(&power_data.timer_pre_charge, powerTimerProcess); + + printf("app_power: initial OK\n"); +} diff --git a/app/app_power.h b/app/app_power.h new file mode 100644 index 0000000..0cebf81 --- /dev/null +++ b/app/app_power.h @@ -0,0 +1,53 @@ +#ifndef APP_POWER_H +#define APP_POWER_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "app_config.h" +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" + +// 定义电源状态枚举 +typedef enum { + POWER_STANDBY, + POWER_WORKING, + POWER_EMERGENCY, + POWER_PRE_CHARGE, + POWER_SLEEP +} PowerState; + +// 使用内联函数 +static inline uint8_t setPowerOn(void) { return 1; } +static inline uint8_t setPowerOff(void) { return 0; } + + +// 声明 power_data 变量结构体 +typedef struct { + PowerState current_state; // 当前电源状态 + uint32_t start_time; // 定时器起始时间 + Timer timer; // 定时器 + Timer timer1; + Timer timer_pre_charge; // 预充定时器 + PowerState last_state; // 上一次状态 + uint8_t emergency_stop_switch; // 急停开关 + uint8_t high_voltage_switch; // 高压开关 + uint8_t remote_power_switch; // 遥控器电源开关 + uint8_t remote_emergency_stop; // 遥控器急停开关 + uint8_t emergency_stop; // 急停状态 + uint8_t pre_charge_finish; // 预充完成标志位 + uint8_t old_state; // 上一次状态 +} PowerSystem; + +// 声明外部变量 +extern PowerSystem power_data; + +void powerAppInit(void); + +#ifdef __cplusplus +} +#endif + +#endif // APP_POWER_H diff --git a/app/app_request.c b/app/app_request.c new file mode 100644 index 0000000..5390740 --- /dev/null +++ b/app/app_request.c @@ -0,0 +1,415 @@ +#include "app_config.h" +#include "interface.h" +#include "app_request.h" +#include "app_param_manage.h" + +uint16_t request_id = 0; + + +static void processRequestframe(uint16_t id) +{ + uint32_t TempAcc = 0; + uint8_t i = 0; + static uint8_t VehicleStaACC0 = 0; + static uint8_t VehicleStaACC1 = 0; + static uint8_t VehicleStaACC2 = 0; + static uint8_t VehicleStaACC3 = 0; + + uint16_t RgExchangeTemp = 0; + uint32_t Rg32ExchangeTemp = 0; +//----------------------------------------------------------- +// printf("request_read_id:%d\n",id); + switch (id)//注意是高位在前，低位在后 + { + case 0x2000://状态帧 + un_vehicle_Info_output.bit_data.frame_header = 0xCCAA;//帧头 + un_vehicle_Info_output.bit_data.frame_type = 0x2000;//帧类型 + un_vehicle_Info_output.bit_data.frame_length = 0x2900;//帧长 + un_vehicle_Info_output.bit_data.accumulated = VehicleStaACC0++;//累加值 + + TempAcc = 0; + for (i = 0; i < 40; i++)//累加前40个字节 + { + TempAcc = TempAcc + (uint32_t)(un_vehicle_Info_output.arr[i]); + } + un_vehicle_Info_output.bit_data.crc = (uint8_t)TempAcc;//累加值 + + publishMessage(&un_vehicle_Info_output, 1); + + break; + + case 0x2100://电机帧 + un_motor_status_output.bit_data.frame_header = 0xCCAA;//帧头 + un_motor_status_output.bit_data.frame_type = 0x2100;//帧类型 + un_motor_status_output.bit_data.frame_length = 0x1E00;//帧长 + un_motor_status_output.bit_data.accumulated = VehicleStaACC1++;//累加值 + RgExchangeTemp = ( (uint16_t)getParam("maxTorq") + 300 ) *100 ; +// un_motor_status_output.bit_data.left_torque_limit = ((RgExchangeTemp << 8) | (RgExchangeTemp >> 8));//左侧扭矩限制 +// un_motor_status_output.bit_data.right_torque_limit = ((RgExchangeTemp << 8) | (RgExchangeTemp >> 8));//右侧扭矩限制 +// un_motor_status_output.bit_data.left_power_in = (((uint16_t)getParam("feedPwr") << 8) | ((uint16_t)getParam("feedPwr") >> 8));//左侧馈电功率 +// un_motor_status_output.bit_data.right_power_in = (((uint16_t)getParam("feedPwr") << 8) | ((uint16_t)getParam("feedPwr") >> 8));//右侧馈电功率 +// un_motor_status_output.bit_data.left_power_out = (((uint16_t)getParam("dispPwr") << 8) | ((uint16_t)getParam("dispPwr") >> 8));//左侧放电功率 +// un_motor_status_output.bit_data.right_power_out = (((uint16_t)getParam("dispPwr") << 8) | ((uint16_t)getParam("dispPwr") >> 8));//右侧放电功率 + + TempAcc = 0; + for (i = 0; i < 31; i++)//累加前31个字节 + { + TempAcc = TempAcc + (uint32_t)(un_motor_status_output.arr[i]); + un_motor_status_output.bit_data.checksum = (uint8_t)TempAcc;//累加值 + } + + publishMessage(&un_motor_status_output, 1); + break; + + case 0x2200: + un_pid_output.bit_data.frame_header = 0xCCAA;//帧头 + un_pid_output.bit_data.frame_type = 0x2200;//帧类型 + un_pid_output.bit_data.frame_length = 0x3800;//帧长 + un_pid_output.bit_data.accumulated = VehicleStaACC2++;//累加值 + + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kp")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.rc_straight_p = Rg32ExchangeTemp;//遥控直行P参数 + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_ki")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.rc_straight_i = Rg32ExchangeTemp;//遥控直行I参数 + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kd")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.rc_straight_d = Rg32ExchangeTemp;//遥控直行D参数 + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_il")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.auto_straight_p = Rg32ExchangeTemp;//自主直行P参数 + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_ol")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.auto_straight_i = Rg32ExchangeTemp;//自主直行I参数 + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kd")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.auto_straight_d = Rg32ExchangeTemp;//自主直行D参数 + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_kp")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.rc_turn_p = Rg32ExchangeTemp;//遥控转弯P参数 + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_ki")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.rc_turn_i = Rg32ExchangeTemp;//遥控转弯I参数 + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_kd")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.rc_turn_d = Rg32ExchangeTemp;//遥控转弯D参数 + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_il")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.auto_turn_p = Rg32ExchangeTemp;//自主转弯P参数 + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_ol")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.auto_turn_i = Rg32ExchangeTemp;//自主转弯I参数 + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kd")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.auto_turn_d = Rg32ExchangeTemp;//自主转弯I参数 + + TempAcc = 0; + for (i = 0; i < 55; i++)//累加前40个字节 + { + TempAcc = TempAcc + (uint32_t)(un_pid_output.arr[i]); + } + un_pid_output.bit_data.checksum = (uint8_t)TempAcc;//累加值 + + publishMessage(&un_pid_output, 1); + + break; + + case 0x2300: +// ETHTxtempArr[0] = 0xAA; +// ETHTxtempArr[1] = 0xCC; +// ETHTxtempArr[2] = 0x00; +// ETHTxtempArr[3] = 0x23; +// ETHTxtempArr[4] = 0; +// ETHTxtempArr[5] = 27; +// ETHTxtempArr[6] = VehicleStaACC3++; +// //AIAO +// memcpy(ÐTxtempArr[7],(uint8_t *)&(UnAIAOSignal_1.ArrData.ArrRx[0]),17);//电池数据拷贝 +// ETHTxtempArr[24] = (u8)(RgCanToAiAOCnt>>8); +// ETHTxtempArr[25] = (u8)RgCanToAiAOCnt;//累加值 +// +// TempAcc = 0; +// for (i = 0; i < 26; i++)//累加前40个字节 +// { +// TempAcc = TempAcc + (u32)(ETHTxtempArr[i]); +// } +// ETHTxtempArr[26] = (uint8_t)TempAcc;//累加值 +// +// TxLen = 27; +// UdpSendToData(UDPCB_2,(uint8_t *)(&un_analog_signal_output), 27, (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); + break; + + case 0x2400: + un_remote_control_output.bit_data.frame_header = 0xCCAA;//帧头 + un_remote_control_output.bit_data.frame_type = 0x2400;//帧类型 + un_remote_control_output.bit_data.frame_length = 0x1400;//帧长 + un_remote_control_output.bit_data.accumulated = VehicleStaACC3++;//累加值 + //RCH_3 + + TempAcc = 0; + for (i = 0; i < 17; i++)//累加前40个字节 + { + TempAcc = TempAcc + (uint32_t)(un_remote_control_output.arr[i]); + } + un_remote_control_output.bit_data.crc = (uint8_t)TempAcc;//累加值 + + publishMessage(&un_remote_control_output, 1); + break; + + case 0x2500: + un_manual_control_output.bit_data.frame_type = 0x2500; + + TempAcc = 0; + for (i = 0; i < 14; i++)//累加前40个字节 + { + TempAcc = TempAcc + (uint32_t)(un_manual_control_output.arr[i]); + } + un_manual_control_output.bit_data.crc_2 = (uint8_t)TempAcc;//累加值 + + publishMessage(&un_manual_control_output, 1); + break; + + case 0x2600: + un_auto_control_output.bit_data.frame_type = 0x2600; + + TempAcc = 0; + for (i = 0; i < 27; i++)//累加前40个字节 + { + TempAcc = TempAcc + (uint32_t)(un_auto_control_output.arr[i]); + } + un_auto_control_output.bit_data.crc = (uint8_t)TempAcc;//累加值 + + publishMessage(&un_auto_control_output, 1); + break; + +// case 0x2700: +// memcpy(ÐTxtempArr[0],(uint8_t *)&RemoteControlRoomToVcuBuf,11);//手柄拷贝 +// ETHTxtempArr[2] = 0;//修改帧类型为0x0025 +// ETHTxtempArr[3] = 0x27; +// ETHTxtempArr[11] = (u8)(RgETHToRemoteControlCnt>>8); +// ETHTxtempArr[12] = (u8)RgETHToRemoteControlCnt;//累加值 +// TempAcc = 0; +// for (i = 0; i < 13; i++)//累加前40个字节 +// { +// TempAcc = TempAcc + (u32)(ETHTxtempArr[i]); +// } +// ETHTxtempArr[13] = (uint8_t)TempAcc;//累加值 +// +// TxLen = 14; +// UdpSendToData(SocketId2,ETHTxtempArr,&TxLen,ip_addr,port);//测试 +// break; + +// case 0xFFFF: +// memcpy(ÐTxtempArr[0],(uint8_t *)&RemoteControlRoomToVcuBuf,11);//手柄拷贝 +// ETHTxtempArr[2] = 0x27;//修改帧类型为0x0025 +// ETHTxtempArr[3] = 0; +// ETHTxtempArr[11] = (u8)RgETHToRemoteControlCnt; +// ETHTxtempArr[12] = (u8)(RgETHToRemoteControlCnt>>8);//累加值 +// TempAcc = 0; +// for (i = 0; i < 13; i++)//累加前40个字节 +// { +// TempAcc = TempAcc + (u32)(ETHTxtempArr[i]); +// } +// ETHTxtempArr[13] = (uint8_t)TempAcc;//累加值 +// +// TxLen = 14; +// UdpSendToData(SocketId2,ETHTxtempArr,&TxLen,ip_addr,port);//测试 +// break; + default: break; + } +} + + + +// 处理所有输入信号的函数 +static void requestInput(void *signal_id) +{ + uint16_t request16_temp = 0; + +//-------------------------------------------------------- + if (signal_id == &diff_data) + { + request16_temp = (uint16_t)(int16_t)(diff_data.desired_speed * 100.0); + un_vehicle_Info_output.bit_data.desired_speed = ((request16_temp << 8) | (request16_temp >> 8));//当前速度 + + request16_temp = (uint16_t)(int16_t)(diff_data.left_diff_touue * 10000.0); + un_vehicle_Info_output.bit_data.desired_curvature = ((request16_temp << 8) | (request16_temp >> 8));//当前曲率 + + request16_temp = (uint16_t)(int16_t)(diff_data.speed * 100.0); + un_vehicle_Info_output.bit_data.speed = ((request16_temp << 8) | (request16_temp >> 8));//当前速度 + + request16_temp = (uint16_t)(int16_t)(diff_data.right_diff_touue * 10000.0); + un_vehicle_Info_output.bit_data.curvature = ((request16_temp << 8) | (request16_temp >> 8));//当前曲率 + + request16_temp = (uint16_t)(int16_t)(diff_data.out_motor_speed[0]);// + un_vehicle_Info_output.bit_data.set_left_speed = ((request16_temp << 8) | (request16_temp >> 8));//左前 + + request16_temp = (uint16_t)(int16_t)(diff_data.out_motor_speed[1]);//out_right_motor_speed + un_vehicle_Info_output.bit_data.set_right_speed = ((request16_temp << 8) | (request16_temp >> 8));//右前 + + request16_temp = (uint16_t)(int16_t)(diff_data.out_motor_speed[2]);// + un_motor_status_output.bit_data.left_power_in = ((request16_temp << 8) | (request16_temp >> 8));//左后 + + request16_temp = (uint16_t)(int16_t)(diff_data.out_motor_speed[3]);//out_right_motor_speed + un_motor_status_output.bit_data.right_power_in = ((request16_temp << 8) | (request16_temp >> 8));//右后 + + un_vehicle_Info_output.bit_data.vehicle_fault_state = diff_data.state; + un_vehicle_Info_output.bit_data.battery_soc = diff_data.motor_dir; + + request16_temp = (uint16_t)(int16_t)(diff_data.left_rear_motor_speed + 30000); + un_motor_status_output.bit_data.left_wheel_speed = ((request16_temp << 8) | (request16_temp >> 8));;//左侧后轮速 + + request16_temp = (uint16_t)(int16_t)(diff_data.right_rear_motor_speed + 30000); + un_motor_status_output.bit_data.right_wheel_speed = ((request16_temp << 8) | (request16_temp >> 8));;//右侧后轮速 + + request16_temp = (uint16_t)(int16_t)( (diff_data.left_front_motor_speed + 300)*100); + un_motor_status_output.bit_data.left_torque = ((request16_temp << 8) | (request16_temp >> 8));;//左侧前轮速 + + request16_temp = (uint16_t)(int16_t)( (diff_data.right_front_motor_speed + 300)*100); + un_motor_status_output.bit_data.right_torque = ((request16_temp << 8) | (request16_temp >> 8));;//右侧前轮速 + } + else if(signal_id == &un_auto_computer_input) + { + un_vehicle_Info_output.bit_data.longitude = un_auto_computer_input.bit_data.longitude;//经度 + + un_vehicle_Info_output.bit_data.latitude = un_auto_computer_input.bit_data.latitude; + + un_vehicle_Info_output.bit_data.altitude = un_auto_computer_input.bit_data.altitude; + + un_vehicle_Info_output.bit_data.heading_angle = un_auto_computer_input.bit_data.heading;//航向 + + memcpy(&un_auto_control_output.arr[0],&(un_auto_computer_input.arr[0]),25);//手柄拷贝 + } + else if(signal_id == &un_bms_input) + { + un_vehicle_Info_output.bit_data.battery_voltage = un_bms_input.bit_data.bus_voltage/10;//电池电压读取得的10mV，所以输出的是100mV单位，所以缩小10倍 + un_vehicle_Info_output.bit_data.battery_soc = un_bms_input.bit_data.soc;//SOC + un_vehicle_Info_output.bit_data.battery_current = un_bms_input.bit_data.bus_current;//电池电流 + } + else if(signal_id == &un_motor_input1) + { +// un_motor_status_output.bit_data.left_wheel_speed = ((un_motor_input1.bit_data.speed << 8) | (un_motor_input1.bit_data.speed >> 8));//左侧轮速 +// un_motor_status_output.bit_data.left_torque = ((un_motor_input1.bit_data.torque << 8) | (un_motor_input1.bit_data.torque >> 8));//左侧扭矩 +// un_motor_status_output.bit_data.left_voltage = ((un_motor_input1.bit_data.bus_voltage << 8) | (un_motor_input1.bit_data.bus_voltage >> 8));//左侧电压 +// un_motor_status_output.bit_data.left_fault_code = un_motor_input1.bit_data.fault_code;//左侧故障码 +// + } + else if(signal_id == &un_motor_input2) + { +// un_motor_status_output.bit_data.right_wheel_speed = ((un_motor_input2.bit_data.speed << 8) | (un_motor_input2.bit_data.speed >> 8));//右侧轮速 +// un_motor_status_output.bit_data.right_torque = ((un_motor_input2.bit_data.torque << 8) | (un_motor_input2.bit_data.torque >> 8));//右侧扭矩 +// un_motor_status_output.bit_data.right_fault_code = un_motor_input2.bit_data.fault_code;//右侧故障码 +// un_motor_status_output.bit_data.right_voltage = ((un_motor_input2.bit_data.bus_voltage << 8) | (un_motor_input2.bit_data.bus_voltage >> 8));//右侧电压 +// + } + else if(signal_id == &un_remote_control_input) + { + //RCH_3 + un_remote_control_output.bit_data.speed = ((un_remote_control_input.bit_data.speed << 8) | (un_remote_control_input.bit_data.speed >> 8));//遥控器期望速度 + un_remote_control_output.bit_data.curvature = ((un_remote_control_input.bit_data.curvature << 8) | (un_remote_control_input.bit_data.curvature >> 8));//遥控器期望曲率 + memcpy(&un_remote_control_output.arr[11],&un_remote_control_input.arr[4],4);//遥控数据拷贝 + } + else if(signal_id == &un_manual_computer_input) + { + memcpy(&un_manual_control_output.arr[0],&(un_manual_computer_input.arr[0]),12);//手柄拷贝 + } + else if(signal_id == ðernet_fault_Info) + { + un_auto_control_output.arr[25] = (uint8_t)(ethernet_fault_Info.bit_data.auto_count>>8); + un_auto_control_output.arr[26] = (uint8_t)(ethernet_fault_Info.bit_data.auto_count);//累加值 + un_manual_control_output.arr[12] = (uint8_t)(ethernet_fault_Info.bit_data.manual_count>>8); + un_manual_control_output.arr[13] = (uint8_t)(ethernet_fault_Info.bit_data.manual_count);//累加值 + } + + else if(signal_id == &can_fault_info) + { + un_remote_control_output.arr[15] = (uint8_t)(can_fault_info.bit_data.remote_count>>8); + un_remote_control_output.arr[16] = (uint8_t)(can_fault_info.bit_data.remote_count);//累加值 + } + else if(signal_id == &un_request_frame) + { + request_id = REQUEST_READ_ID; + processRequestframe(request_id); + } + else{} +} + +// 修改APP模块的初始化函数 +void requestAppInit(void) +{ + // 订阅消息，使用静态成员函数作为回调 参数回馈 + subscribe(&diff_data, requestInput); + subscribe(&un_auto_computer_input, requestInput); + subscribe(&un_bms_input, requestInput); + subscribe(&un_motor_input1, requestInput); + subscribe(&un_motor_input2, requestInput); + subscribe(&un_remote_control_input, requestInput); + subscribe(&can_fault_info, requestInput); + subscribe(ðernet_fault_Info, requestInput); + +// // 订阅消息，使用静态成员函数作为回调 参数回馈 + subscribe(&un_request_frame, requestInput); + + printf("app_request: initial OK \n"); +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/app/app_request.h b/app/app_request.h new file mode 100644 index 0000000..0f51034 --- /dev/null +++ b/app/app_request.h @@ -0,0 +1,83 @@ +#ifndef APP_REQUEST_H +#define APP_REQUEST_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "app_config.h" +#include "app_differential_drive.h" + + + +//typedef enum +//{ +// MODE_MANUAL, // 手动模式 +// MODE_AUTO // 自动模式 +//} ControlMode; +// +typedef struct RequestData +{ + float desired_speed; // 期望速度 + float desired_curvature; // 期望曲率 + float left_motor_speed; // 当前左电机速度 + float right_motor_speed; // 当前右电机速度 + float speed; // 当前车速 + float curvature; // 当前曲率 + float yaw_rate; // 当前角速度 + float desired_yaw_rate; // 期望角速度 + float acceleration; // 当前加速度 + float deceleration; // 当前减速度 + float max_speed; // 最大速度 + float desired_acceleration; // 期望加速度 + float desired_deceleration; // 期望减速度 + float out_left_motor_speed; // 输出左电机速度 + float out_right_motor_speed; // 输出右电机速度 +} RequestData; + + + + +// 声明外部变量 +extern DiffData diff_data; + + +void requestAppInit(void); + + +#ifdef __cplusplus +} +#endif + +#endif // APP_DIFFERENTIAL_DRIVE_H + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/app/app_temp.c b/app/app_temp.c new file mode 100644 index 0000000..175f062 --- /dev/null +++ b/app/app_temp.c @@ -0,0 +1,115 @@ +#include "app_config.h" +#include "interface.h" +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" +#include "app_param_manage.h" + +#include "app_temp.h" + + +// 声明 temp_data 变量 +TempSystem temp_data; + +// 温度输出处理函数 +static void tempOutput(void *signal_id) +{ + (void)signal_id; + + // 根据当前状态,控制风扇 + switch (temp_data.state) + { + case TEMP_NORMAL: +// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOff();//电机控制器风扇 +// un_inf_can_kgf_output1.bit_data.KGF03 = setFanOff(); +// un_inf_can_kgf_output1.bit_data.KGF14 = setFanOff();//整车风扇 +// un_inf_can_kgf_output2.bit_data.KGF03 = setFanOff(); + break; + case TEMP_WARNING: + case TEMP_CRITICAL: +// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn(); +// un_inf_can_kgf_output1.bit_data.KGF03 = setFanOn(); +// un_inf_can_kgf_output1.bit_data.KGF14 = setFanOn(); +// un_inf_can_kgf_output2.bit_data.KGF03 = setFanOn(); + break; + } + + // // 设置风扇速度 + // temp_data.fan_speed = 0; + // un_inf_can_kgf_output1.bit_data.pwm_01 = temp_data.fan_speed; + + publishMessage(&un_inf_can_kgf_output1, 1); +} + +// 温度状态处理函数 +static void tempProcess(void *signal_id) +{ + (void)signal_id; + + // 获取当前最高温度 + int16_t max_temp = temp_data.current_temp[0]; + for (uint8_t i = 1; i < 8; i++) + { + if (temp_data.current_temp[i] > max_temp) + { + max_temp = temp_data.current_temp[i]; + } + } + // 根据最高温度设置状态 + if (max_temp > 80) // 假设80度为危险温度 + { + temp_data.state = TEMP_CRITICAL; + temp_data.fan_speed = 255; // 全速运转 + } + else if (max_temp > 60) // 假设60度为警告温度 + { + temp_data.state = TEMP_WARNING; + temp_data.fan_speed = 128; // 半速运转 + } + else + { + temp_data.state = TEMP_NORMAL; + temp_data.fan_speed = 0; // 停止运转 + } + + tempOutput(NULL); +} + +// 处理输入信号的函数 +static void tempInput(void *signal_id) +{ +// TempSystem old_data = temp_data; + + // 填充数据 + if (signal_id == &un_temp_module_input) + { + temp_data.current_temp[0] = (int16_t)un_temp_module_input.bit_data.channel_1; + temp_data.current_temp[1] = (int16_t)un_temp_module_input.bit_data.channel_2; + temp_data.current_temp[2] = (int16_t)un_temp_module_input.bit_data.channel_3; + temp_data.current_temp[3] = (int16_t)un_temp_module_input.bit_data.channel_4; + temp_data.current_temp[4] = (int16_t)un_temp_module_input.bit_data.channel_5; + temp_data.current_temp[5] = (int16_t)un_temp_module_input.bit_data.channel_6; + temp_data.current_temp[6] = (int16_t)un_temp_module_input.bit_data.channel_7; + temp_data.current_temp[7] = (int16_t)un_temp_module_input.bit_data.channel_8; + } +} + + +// APP模块的初始化 +void tempAppInit(void) +{ + // 初始化 + memset(&temp_data, 0, sizeof(TempSystem)); + temp_data.state = TEMP_NORMAL; + temp_data.mode = TEMP_MODE_AUTO; + temp_data.target_temp = 25; // 默认目标温度25度 + + // 订阅输入信号,处理温度逻辑 + subscribe(&un_temp_module_input, tempInput); + + // 启动定时器,每秒检查一次温度 + subscribe(&temp_data.timer, tempProcess); + timerStart(&temp_data.timer, 1000, 1); //1s + + printf("app_temp: initial OK \n"); +} diff --git a/app/app_temp.h b/app/app_temp.h new file mode 100644 index 0000000..4348120 --- /dev/null +++ b/app/app_temp.h @@ -0,0 +1,53 @@ +#ifndef APP_TEMP_H +#define APP_TEMP_H + +#ifdef __cplusplus +extern "C" +{ +#endif + + + + +// 定义温度状态枚举 +typedef enum +{ + TEMP_NORMAL, + TEMP_WARNING, + TEMP_CRITICAL +} TempState; + +// 定义温度控制模式枚举 +typedef enum +{ + TEMP_MODE_AUTO, + TEMP_MODE_MANUAL +} TempMode; + +typedef struct +{ + TempState state; + TempMode mode; + int16_t current_temp[8]; // 当前温度,对应8个通道 + int16_t target_temp; // 目标温度 + uint8_t fan_speed; // 风扇速度 + Timer timer; // 定时器 +} TempSystem; + +// 在头文件中声明外部变量 +extern TempSystem temp_data; + +// 使用内联函数 +static inline uint8_t setFanOn(void) { return 1; } +static inline uint8_t setFanOff(void) { return 0; } + +void tempAppInit(void); +void setTempMode(TempMode mode); +void setTargetTemp(int16_t temp); +void setFanSpeed(uint8_t speed); + +#ifdef __cplusplus +} +#endif + +#endif // APP_TEMP_H \ No newline at end of file diff --git a/app/app_test.c b/app/app_test.c new file mode 100644 index 0000000..60bfe6d --- /dev/null +++ b/app/app_test.c @@ -0,0 +1,62 @@ +#include "app_config.h" +#include "app_dependence.h" +#include "interface.h" + +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" + +#include "sdrv_vic.h" +#include "app_test.h" +#include "app_differential_drive.h" + + +// 定时器结构体 +Timer test_timer; + +// 定时器信号处理函数 +void testTimerProcess(void *signal_id) +{ + (void)signal_id; // 标记变量为已使用，避免编译器警告 +// static uint32_t start_time = 0; +// uint32_t test_irq[6] = {0}; + // 再次启动定时器，实现周期定时 + + timerStart(&test_timer, 5000,1); + + + + + +// printf("speed = %f ",diff_data.desired_speed); +// printf("curvature = %f ",diff_data.desired_curvature); +// printf("mode = %d\n",diff_data.mode); + + +// +// sdrv_vic_lld_read_all(test_irq);//打印所有中断使能位 +// printf("irq state = %#X ",test_irq[0]); +// printf("irq state = %#X ",test_irq[1]); +// printf("irq state = %#X ",test_irq[2]); +// printf("irq state = %#X ",test_irq[3]); +// printf("irq state = %#X ",test_irq[4]); +// printf("irq state = %#X\n",test_irq[5]); + +// printf("testAPP: %d us \n",getCurrentTime() - start_time); +// start_time = getCurrentTime(); + +} + + +// APP模块的初始化 +void testAppInit(void) +{ + // 初始化定时器 + timerInit(&test_timer); + // 订阅定时器信号，用于停止电机 + subscribe(&test_timer, testTimerProcess); + + printf("testAPP: initial OK %d\n",getCurrentTime()); + + timerStart(&test_timer, 5000,1); +} diff --git a/app/app_test.h b/app/app_test.h new file mode 100644 index 0000000..9bdf00e --- /dev/null +++ b/app/app_test.h @@ -0,0 +1,18 @@ +#ifndef TEST_H +#define TEST_H + +#ifdef __cplusplus +extern "C" { +#endif + + + +extern void testAppInit(void); + + + +#ifdef __cplusplus +} +#endif + +#endif // TEST_H diff --git a/app/app_turn.c b/app/app_turn.c new file mode 100644 index 0000000..4c6e1d8 --- /dev/null +++ b/app/app_turn.c @@ -0,0 +1,372 @@ +#include "app_config.h" +#include "app_dependence.h" +#include "interface.h" + +#include "app_turn.h" +#include "app_differential_drive.h" +#include "app_param_manage.h" +#include +#include + +TurnData turn_data; +Timer turn_timer2; // ʱṹ + + +// תuint32_tС洢 +void floatToUint32(float num, uint32_t *result) +{ + // ڴֱӿuint32_t + memcpy(result, &num, sizeof(num)); +} + + + + + + + + +static void turnOutput(void *signal_id) +{ + (void)signal_id; + uint32_t float_temp = 0; + + switch(turn_data.direction) + { + case 0: + // + floatToUint32(turn_data.left_zero_position,&float_temp); + un_sdo_output1.bit_data.data = float_temp; + floatToUint32(turn_data.left_speed*2,&float_temp); + un_sdo_output2.bit_data.data = float_temp; + //ҵ + floatToUint32(turn_data.right_zero_position,&float_temp); + un_sdo_output5.bit_data.data = float_temp; + floatToUint32(turn_data.right_speed*2,&float_temp); + un_sdo_output6.bit_data.data = float_temp; + break; + case 1://ת + // + floatToUint32(turn_data.inner_rad + turn_data.left_zero_position,&float_temp); + un_sdo_output1.bit_data.data = float_temp; + floatToUint32(turn_data.left_speed,&float_temp); + un_sdo_output2.bit_data.data = float_temp; + //ҵ + floatToUint32(turn_data.outer_rad + turn_data.right_zero_position,&float_temp); + un_sdo_output5.bit_data.data = float_temp; + floatToUint32(turn_data.right_speed,&float_temp); + un_sdo_output6.bit_data.data = float_temp; + break; + case 2://ת + // + floatToUint32(turn_data.outer_rad + turn_data.left_zero_position,&float_temp); + un_sdo_output1.bit_data.data = float_temp; + floatToUint32(turn_data.left_speed,&float_temp); + un_sdo_output2.bit_data.data = float_temp; + //ҵ + floatToUint32(turn_data.inner_rad + turn_data.right_zero_position,&float_temp); + un_sdo_output5.bit_data.data = float_temp; + floatToUint32(turn_data.right_speed,&float_temp); + un_sdo_output6.bit_data.data = float_temp; + break;default:; + } + + un_sdo_output1.bit_data.index = LOC_REF_INDEX; + un_sdo_output1.bit_data.object_index = 0x0000; +// publishMessage(&un_sdo_output1, 1);//趨λ + + un_sdo_output2.bit_data.index = LIMIT_SPEED_INDEX; + un_sdo_output2.bit_data.object_index = 0x0000; +// publishMessage(&un_sdo_output2, 1);//趨ҵλ + + un_sdo_output5.bit_data.index = LOC_REF_INDEX; + un_sdo_output5.bit_data.object_index = 0x0000; +// publishMessage(&un_sdo_output5, 1);//趨λ + + un_sdo_output6.bit_data.index = LIMIT_SPEED_INDEX; + un_sdo_output6.bit_data.object_index = 0x0000; +// publishMessage(&un_sdo_output6, 1);//趨ҵλ + +} + +//// Ԥ Ҫȷһʧܣʹ +//void turnChargeFinish(uint8_t *stt, uint8_t *cnt) +//{ +// switch(stt[0])//ȷлģʽԼʧܣֱʹ +// { +// case 0: +// if(cnt[0] >= 20)//50 +// { +// cnt[0] = 0; +// stt[0] = 1; +// } +// else +// { +// un_sdo_output5.bit_data.cmd = 0x2F; +// un_sdo_output5.bit_data.object_index = 0x6060; +// un_sdo_output5.bit_data.sub_index = 0; +// un_sdo_output5.bit_data.data = 0x01;//趨λģʽ +// publishMessage(&un_sdo_output5, 1); +// +// un_sdo_output4.bit_data.cmd = 0x2B; +// un_sdo_output4.bit_data.object_index = 0x6040; +// un_sdo_output4.bit_data.sub_index = 0; +// un_sdo_output4.bit_data.data = 0x06;//ʧ +// publishMessage(&un_sdo_output4, 1); +// cnt[0] ++; +// stt[0] = 0; +// printf("position! \n"); +// } +// break; +// +// case 1: +// +// cnt[0] = 0; +// stt[0] = 1; +// printf("disable! \n"); +// break;default:; +// } +//} + +/** + * 㰢ת򼸺еתǣƣ + * @param L ࣨǰ־룬λף + * @param W ־ࣨ־룬λף + * @param inner_rad תǣȣ + * @param[out] curvature ·ʣλ1/ף + * @return תǣȣ + */ +float calculate_outer_angle(float L, float W, float inner_rad, float* curvature) +{ + // ԭʼ + int sign = (inner_rad < 0) ? -1 : 1; + + // ʹþֵм + float inner_temp = fabsf(inner_rad); + + // ת뾶㣨ؼ޸ĵ㣩 + float R_inner = L / tanf(inner_temp); // ת뾶 + float R_outer = R_inner + W; // ת뾶 = ְ뾶 + ־ + + float R = R_inner + W / 2; // ת뾶 + + // ʣעţ + *curvature = sign / R; + + // תǣatan(L/R_outer) + float delta_o = atanf(L / R_outer); + + // ָԭʼ + return sign * delta_o; +} + + + + + +// ת +static void turnProcess(void *signal_id) +{ + static uint8_t turn_process_state = 0; + static uint8_t turn_process_cnt = 0; + + (void)signal_id; + if((turn_data.desired_curvature) > 0.01)//ת + { + turn_data.direction = 1; + } + else if((turn_data.desired_curvature) < -0.01)//ת + { + turn_data.direction = 2; + } + else// + { + turn_data.direction = 0; + } + + turn_data.inner_rad = turn_data.desired_curvature*turn_data.left_max_position/2.0;//ǶȣֽǶ -2-2ʶӦ תΪ޶Ƕ + turn_data.outer_rad = calculate_outer_angle(turn_data.wheel_base, turn_data.track_width, turn_data.inner_rad, &turn_data.curvature);//ֽǶԼ + + + + + +// //ϵδcanλԵբδ򿪵⡣ +// if(power_data.current_state == POWER_WORKING)//բ +// { +// un_inf_can_kgf_output1.bit_data.KGF01 = 1;//򿪱բ +// } +// else +// { +// un_inf_can_kgf_output1.bit_data.KGF01 = 0;//բ +// } + + if ((turn_data.current_state == POWER_WORKING) && (1 == turn_data.motor_sleep))//workҵϵ + { + switch(turn_process_state)//ȷлģʽԼʧܣֱʹ + { + case 0: + if(turn_process_cnt >= 20)//50 + { + turn_process_cnt = 0; + turn_process_state = 1; + } + else + { + turn_process_cnt ++; + + un_sdo_output4.bit_data.index = RUM_MODE; + un_sdo_output4.bit_data.object_index = 0x0; + un_sdo_output4.bit_data.data = POSITION_MODE_CSP;//趨λģʽ + publishMessage(&un_sdo_output4, 1); + + un_sdo_output8.bit_data.index = RUM_MODE; + un_sdo_output8.bit_data.object_index = 0x0; + un_sdo_output8.bit_data.data = POSITION_MODE_CSP;//趨λģʽ + publishMessage(&un_sdo_output8, 1); + } + break; + + case 1: + if(turn_process_cnt >= 10)//50 + { + turn_process_cnt = 0; + turn_process_state = 2; + } + else + { + turn_process_cnt ++; + + un_sdo_output3.bit_data.index = 0x0; + un_sdo_output3.bit_data.object_index = 0x0; + un_sdo_output3.bit_data.data = 0x0;// + publishMessage(&un_sdo_output3, 1); + + un_sdo_output7.bit_data.index = 0x0; + un_sdo_output7.bit_data.object_index = 0x0; + un_sdo_output7.bit_data.data = 0x0;// + publishMessage(&un_sdo_output7, 1); + } + break; + + case 2: + turnOutput(signal_id); + turn_process_cnt = 0; + turn_process_state = 2; + break;default:; + } + } + else//ÿε綼·ʹ + { + turn_process_state = 0; + turn_process_cnt = 0; + turn_data.motor_sleep = 0; + } +} + +// źŵĺ +static void turnInput(void *signal_id) +{ + (void)signal_id; + + if (signal_id == &diff_data) + { + turn_data.mode = diff_data.mode; + turn_data.desired_curvature = diff_data.desired_curvature; + } + else if(signal_id == &power_data)//ϵ + { + turn_data.current_state = power_data.current_state; + } + else{} + turn_data.motor_sleep = 1; + turnProcess(signal_id);//ӳ +} + + +void turnParametersInit(void *signal_id) +{ + (void)signal_id; // DZΪʹã + //ȡ + turn_data.right_max_position = (float)getParam("R_maxP"); // תλãrad + turn_data.right_zero_position = (float)getParam("R_zeroP"); // תλãrad + turn_data.right_speed = (float)getParam("R_speed"); // תλãrad + + turn_data.left_max_position = (float)getParam("L_maxP"); // תλãrad + turn_data.left_zero_position = (float)getParam("L_zeroP"); // תλãrad + turn_data.left_speed = (float)getParam("L_speed"); // תλãrad + + + turn_data.wheel_base = (float)getParam("whl_bas"); // + turn_data.track_width = (float)getParam("tra_wid"); //־ + + timerStart(&turn_data.turn_timer,1000,1);//1sһ +} + +void motorInit(void *signal_id) +{ + if(turn_data.current_state == POWER_WORKING) + { + un_sdo_output4.bit_data.index = RUM_MODE; + un_sdo_output4.bit_data.object_index = 0x0; + un_sdo_output4.bit_data.data = POSITION_MODE_CSP;//趨λģʽ + publishMessage(&un_sdo_output4, 1); + + un_sdo_output8.bit_data.index = RUM_MODE; + un_sdo_output8.bit_data.object_index = 0x0; + un_sdo_output8.bit_data.data = POSITION_MODE_CSP;//趨λģʽ + publishMessage(&un_sdo_output8, 1); + + un_sdo_output3.bit_data.index = 0x0; + un_sdo_output3.bit_data.object_index = 0x0; + un_sdo_output3.bit_data.data = 0x0;// + publishMessage(&un_sdo_output3, 1); + + un_sdo_output7.bit_data.index = 0x0; + un_sdo_output7.bit_data.object_index = 0x0; + un_sdo_output7.bit_data.data = 0x0;// + publishMessage(&un_sdo_output7, 1); + } +// timerStart(&turn_timer2,1000,1);//1sһ +} + + + + + +// ޸APPģijʼ +void turnAppInit(void) +{ + // ʼ + memset(&turn_data, 0, sizeof(TurnData)); + timerInit(&turn_data.turn_timer); + timerInit(&turn_timer2); + + + //ȡ + turn_data.right_max_position = (float)getParam("R_maxP"); // תλãrad +// turn_data.right_min_position = (float)getParam("R_minP"); // תλãrad + turn_data.right_zero_position = (float)getParam("R_zeroP"); // תλãrad + turn_data.right_speed = (float)getParam("R_speed"); // תλãrad + + turn_data.left_max_position = (float)getParam("L_maxP"); // תλãrad +// turn_data.left_min_position = (float)getParam("L_minP"); // תλãrad + turn_data.left_zero_position = (float)getParam("L_zeroP"); // תλãrad + turn_data.left_speed = (float)getParam("L_speed"); // תλãrad + + // ź,ɲ߼ + subscribe(&diff_data, turnInput); + subscribe(&power_data, turnInput); + + subscribe(&turn_data.turn_timer, turnParametersInit); + + subscribe(&turn_timer2, motorInit); + + + timerStart(&turn_data.turn_timer,1000,1);//1sһ + timerStart(&turn_timer2,1000,1);//1sһ + + printf("turnApp: initial OK \n"); +} + + diff --git a/app/app_turn.h b/app/app_turn.h new file mode 100644 index 0000000..449f886 --- /dev/null +++ b/app/app_turn.h @@ -0,0 +1,95 @@ +#ifndef TURN_H +#define TURN_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "app_power.h" + + + + + +#define LIMIT_SPEED_INDEX 0x7017//CSPٶ +#define LOC_REF_INDEX 0x7016//CSPλ +#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) + + + + + + + + + + + + + + +typedef struct TurnData +{ + uint8_t mode ; // ģʽ + Timer turn_timer; // ʱṹ + float desired_curvature; // + float curvature; // ǰ + uint32_t position; // תλ + uint8_t direction; // ת + uint8_t old_direction; // ת + float yaw_rate; // תٶȡ + float inner_rad; //ֽǶ + float outer_rad; //ֽǶ + uint8_t motor_sleep; // ״̬ 0δϵ磬1ϵ + uint8_t emergency_stop_switch; // ͣ + uint8_t remote_emergency_stop; // ңͣ + uint8_t emergency_stop_state; // ͣ״̬ + PowerState current_state; // ǰԴ״̬ + + float right_max_position; // ұǶ rad + float right_zero_position; // ұ߹Ƕ rad + float right_speed; // ұٶ rad/s + float left_max_position; // Ƕ rad + float left_zero_position; // ߹Ƕ rad + float left_speed; // ٶ rad/s + + float wheel_base; // m + float track_width; // ־ m + +} TurnData; + + + + + + + + + + + +//ⲿ +void turnAppInit(void); + + + + + + + + + +#ifdef __cplusplus +} +#endif + +#endif // TURN_H \ No newline at end of file diff --git a/app_brake.c b/app_brake.c new file mode 100644 index 0000000..a7b42c1 --- /dev/null +++ b/app_brake.c @@ -0,0 +1,127 @@ +#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 new file mode 100644 index 0000000..34f2250 --- /dev/null +++ b/app_brake.h @@ -0,0 +1,34 @@ +#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/crc16.h b/crc16.h new file mode 100644 index 0000000..d1f27ea --- /dev/null +++ b/crc16.h @@ -0,0 +1,51 @@ +#ifndef _CRC16_H_ +#define _CRC16_H_ + +static const unsigned short crc16tab[256]= { + 0x0000,0x1021,0x2042,0x3063,0x4084,0x50a5,0x60c6,0x70e7, + 0x8108,0x9129,0xa14a,0xb16b,0xc18c,0xd1ad,0xe1ce,0xf1ef, + 0x1231,0x0210,0x3273,0x2252,0x52b5,0x4294,0x72f7,0x62d6, + 0x9339,0x8318,0xb37b,0xa35a,0xd3bd,0xc39c,0xf3ff,0xe3de, + 0x2462,0x3443,0x0420,0x1401,0x64e6,0x74c7,0x44a4,0x5485, + 0xa56a,0xb54b,0x8528,0x9509,0xe5ee,0xf5cf,0xc5ac,0xd58d, + 0x3653,0x2672,0x1611,0x0630,0x76d7,0x66f6,0x5695,0x46b4, + 0xb75b,0xa77a,0x9719,0x8738,0xf7df,0xe7fe,0xd79d,0xc7bc, + 0x48c4,0x58e5,0x6886,0x78a7,0x0840,0x1861,0x2802,0x3823, + 0xc9cc,0xd9ed,0xe98e,0xf9af,0x8948,0x9969,0xa90a,0xb92b, + 0x5af5,0x4ad4,0x7ab7,0x6a96,0x1a71,0x0a50,0x3a33,0x2a12, + 0xdbfd,0xcbdc,0xfbbf,0xeb9e,0x9b79,0x8b58,0xbb3b,0xab1a, + 0x6ca6,0x7c87,0x4ce4,0x5cc5,0x2c22,0x3c03,0x0c60,0x1c41, + 0xedae,0xfd8f,0xcdec,0xddcd,0xad2a,0xbd0b,0x8d68,0x9d49, + 0x7e97,0x6eb6,0x5ed5,0x4ef4,0x3e13,0x2e32,0x1e51,0x0e70, + 0xff9f,0xefbe,0xdfdd,0xcffc,0xbf1b,0xaf3a,0x9f59,0x8f78, + 0x9188,0x81a9,0xb1ca,0xa1eb,0xd10c,0xc12d,0xf14e,0xe16f, + 0x1080,0x00a1,0x30c2,0x20e3,0x5004,0x4025,0x7046,0x6067, + 0x83b9,0x9398,0xa3fb,0xb3da,0xc33d,0xd31c,0xe37f,0xf35e, + 0x02b1,0x1290,0x22f3,0x32d2,0x4235,0x5214,0x6277,0x7256, + 0xb5ea,0xa5cb,0x95a8,0x8589,0xf56e,0xe54f,0xd52c,0xc50d, + 0x34e2,0x24c3,0x14a0,0x0481,0x7466,0x6447,0x5424,0x4405, + 0xa7db,0xb7fa,0x8799,0x97b8,0xe75f,0xf77e,0xc71d,0xd73c, + 0x26d3,0x36f2,0x0691,0x16b0,0x6657,0x7676,0x4615,0x5634, + 0xd94c,0xc96d,0xf90e,0xe92f,0x99c8,0x89e9,0xb98a,0xa9ab, + 0x5844,0x4865,0x7806,0x6827,0x18c0,0x08e1,0x3882,0x28a3, + 0xcb7d,0xdb5c,0xeb3f,0xfb1e,0x8bf9,0x9bd8,0xabbb,0xbb9a, + 0x4a75,0x5a54,0x6a37,0x7a16,0x0af1,0x1ad0,0x2ab3,0x3a92, + 0xfd2e,0xed0f,0xdd6c,0xcd4d,0xbdaa,0xad8b,0x9de8,0x8dc9, + 0x7c26,0x6c07,0x5c64,0x4c45,0x3ca2,0x2c83,0x1ce0,0x0cc1, + 0xef1f,0xff3e,0xcf5d,0xdf7c,0xaf9b,0xbfba,0x8fd9,0x9ff8, + 0x6e17,0x7e36,0x4e55,0x5e74,0x2e93,0x3eb2,0x0ed1,0x1ef0 +}; + +__inline unsigned short crc16_ccitt(const unsigned char *buf, unsigned int len) +{ + register unsigned int counter; + register unsigned short crc = 0; + for( counter = 0; counter < len; counter++) + crc = (crc<<8) ^ crc16tab[((crc>>8) ^ *(unsigned char *)buf++)&0x00FF]; + return crc; +} +extern __inline unsigned short crc16_ccitt(const unsigned char *buf, unsigned int len); + +#endif /* _CRC16_H_ */ + + diff --git a/interface.c b/interface.c new file mode 100644 index 0000000..810c118 --- /dev/null +++ b/interface.c @@ -0,0 +1,76 @@ + + +#include "interface_config.h" + +UnMotorInput un_motor_input1 ;//电机控制器1 左前侧 +UnMotorInput un_motor_input2 ;//电机控制器2 右前侧 +UnMotorInput un_motor_input3 ;//电机控制器1 左后侧 +UnMotorInput un_motor_input4 ;//电机控制器2 右后侧 + +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 ;//电机输出 + +UnInfCanKGFOutput un_inf_can_kgf_output1 ;//kgf输出 +UnInfCanKGFOutput un_inf_can_kgf_output2 ; +UnHBridgeOutput un_h_bridge_output ;//H桥输出 +UnWheelSpeedOutput un_wheel_wpeed_output ;//轮速输出 + +UnRemoteControlInput un_remote_control_input ;//遥控器输入 +UnRemoteControlInput un_remote_control_input1 ;//遥控器输入 + +UnHBridgeOutput un_h_bridge_output ;//H桥输出 + +UnHBridgeOutput un_h_bridge_output1 ;//左太阳能板电机 +UnHBridgeOutput un_h_bridge_output2 ;//右太阳能板电机 + +UnLifterOutput un_lifter_output ;//基站升降杆输出 +UnGatherOutput un_gather_output ;//采集模块输出 + + + +UnSdoOutput un_sdo_output1 ;//设定左转向电机位置 +UnSdoOutput un_sdo_output2 ;//设定左转向电机速度 +UnSdoOutput un_sdo_output3 ;//使能左电机 +UnSdoOutput un_sdo_output4 ;//设定左电机模式 + +UnSdoOutput un_sdo_output5 ;//设定右转向电机位置 +UnSdoOutput un_sdo_output6 ;//设定右转向电机速度 +UnSdoOutput un_sdo_output7 ;//使能右电机 +UnSdoOutput un_sdo_output8 ;//设定右电机模式 + +UnSdoOutput un_sdo_output9 ;//输入 +UnSdoOutput un_sdo_output10 ;//输入 +//IO口 +UnSwSample un_sw_sample ;//采集 + + +//以太网 +UnAutoComputerInput un_auto_computer_input ;//自主计算机自动数据 +UnManualComputerInput un_manual_computer_input ;//自主计算机手动数据 +UnRequestFrame un_request_frame ;//请求帧 + +UnComputerOutput un_computer_output ;//输出给自主计算机 + +//输出给上位机 +UnVehicleInfoOutput un_vehicle_Info_output ;// 车辆信息，输出给上位机 +UnMotorStatusOutput un_motor_status_output ;// 电机状态信息，输出给上位机 +UnPIDOutput un_pid_output ;// PID参数输出，输出给上位机 +UnAnalogSignalOutput un_analog_signal_output ;// 模拟信号输出，输出给上位机 +UnRemoteControlOutput un_remote_control_output ;// 遥控器数据输出，给上位机 +UnManualControlOutput un_manual_control_output ;// 手动控制数据，返回给请求者 +UnAutoControlOutput un_auto_control_output ;// 自动控制数据输出，返回给请求者 + + + + + + + + + + diff --git a/interface.h b/interface.h new file mode 100644 index 0000000..f693285 --- /dev/null +++ b/interface.h @@ -0,0 +1,921 @@ +#ifndef _INTERFACE_H_ +#define _INTERFACE_H_ + +#include "stdint.h" + +#include "app/app_config.h" // 假设包含基础配置和宏定义 +#include "irq.h" +#include "interface_config.h" +#include + + + +#pragma pack(1)//数据结构一个字节对齐 + + +#define REQUEST_READ_ID un_request_frame.bit_data.request_id//读取ID +#define REQUEST_HEADER_ID un_request_frame.bit_data.frame_header//帧头 + + + +#define FAULT 0 +#define NORMAL 1 + + +//-----CAN---------------------------------------------------------------- +// 接收电机控制器输入 +typedef struct _StrMotorInput +{ +//-----接收数据0x10F90708---------------------------------------------- + unsigned int control_data3 : 16; // 读取的控制数据 + unsigned int torque : 16; // 当前扭矩 + unsigned int phase_current : 16; // 当前相电流 + unsigned int speed : 16; // 当前转速 + + unsigned int gear : 2; // 反馈档位 + unsigned int speed_gear : 3; // 当前扭矩 + unsigned int reserve1 : 1; // 保留 + unsigned int reserve2 : 1; // 保留 + unsigned int reserve3 : 1; // 保留 + + unsigned int reserve4 : 8; // 保留 + + unsigned int ready : 1; // Bit0: READY, 0-有告警不能行驶, 1-工作正常可行驶 + unsigned int reserved1 : 1; // Bit1: ECO状态（预留） + unsigned int eabs : 1; // Bit2: EABS, 0-停止或驱动行驶, 1-能量回收状态 + unsigned int boost : 1; // Bit3: BOOST状态, 0-正常驱动, 1-BOOST驱动 + unsigned int reserved2 : 1; // Bit4: 预留 + unsigned int rotation_dir : 1; // Bit5: 转动方向, 0-电机逆时针转, 1-电机顺时针转 + unsigned int motor_dir : 1; // Bit6: 电机方向, 0-电机右置, 1-电机左置 + unsigned int main_relay : 1; // Bit7: 主继电器, 0-断开, 1-闭合 + + unsigned int reserve5 : 16; // 保留 + unsigned int reserve6 : 16; // 保留 + unsigned int reserve7 : 16; // 保留 +} StrMotorInput; + +typedef union _UnMotorInput +{ + StrMotorInput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrMotorInput)]; // 通过结构体类型确定大小 +} UnMotorInput; + + + + + + + + + + +// 接收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 +} StrBmsInput; + +typedef union _UnBmsInput +{ + StrBmsInput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrBmsInput)]; // 通过结构体类型确定大小 +} UnBmsInput; + + +// 接收温度模块输入 +typedef struct _StrTempModuleInput +{ + //-----接收数据0x301，302---------------------------------------------- + // 多字节数据，高位在前，低位在后 + unsigned int channel_1 : 16; // 通道1 温度传感器 系数为0.1 有符号 正数表示正温度，负数表示负温度 + unsigned int channel_2 : 16; // 通道2 + unsigned int channel_3 : 16; // 通道3 + unsigned int channel_4 : 16; // 通道4 + unsigned int channel_5 : 16; // 通道5 + unsigned int channel_6 : 16; // 通道6 + unsigned int channel_7 : 16; // 通道7 + unsigned int channel_8 : 16; // 通道8 +} StrTempModuleInput; + +typedef union _UnTempModuleInput +{ + StrTempModuleInput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrTempModuleInput)]; // 通过结构体类型确定大小 +} UnTempModuleInput; + + + + +//-----以太网------------------------------------------------------------- +// 接收自主计算机自动输入 +typedef struct _StrAutoComputerInput +{ + // 多字节数据，高位在前，低位在后 + 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^-7，431234567表示43.1234567度 + unsigned int longitude : 32; // 经度 系数10^-7，431234567表示43.1234567度 + unsigned int altitude : 32; // 高度 单位mm + unsigned int heading : 16; // 航向 车辆航向，35999表示359.99度 + unsigned int crc : 8; // CRC 按字节累加之和，溢出取低8位 +} StrAutoComputerInput; + +typedef union _UnAutoComputerInput +{ + StrAutoComputerInput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrAutoComputerInput)]; // 通过结构体类型确定大小 +} UnAutoComputerInput; + + + + + +// 接收自主计算机手动输入 + +typedef struct _StrManualComputerInput +{ + // 手动控制数据 高位在前，低位在后 + unsigned int frame_header : 16; // 帧头 固定值0xFFBB + unsigned int frame_type : 16; // 帧类型 固定值0x0011 + unsigned int frame_length : 8; // 帧长 固定值0x0B + unsigned int accumulated : 8; // 累加值 按帧累加 + unsigned int set_speed : 16; // 期望速度 系数0.01，正为前进，负为后退 单位m/s + unsigned int set_curvature : 16; // 期望曲率 系数0.0001，正为左转，负为右转 + unsigned int reserved : 8; // 保留 同时按下LB+Y时发0xFF，松开发0，同时按下LT+Y时发0x10，松开发0 + unsigned int crc : 8; // CRC 按字节累加之和，溢出取低8位 +} StrManualComputerInput; + +typedef union _UnManualComputerInput +{ + StrManualComputerInput bit_data; // 使用定义的结构体变量名 + unsigned int arr[sizeof(StrManualComputerInput) / sizeof(unsigned int)]; // 通过结构体类型确定大小 +} UnManualComputerInput; + + + + + + + + + +// 接收请求帧 +typedef struct _StrRequestFrame +{ + //-------------------------------------------------- + unsigned int frame_header : 16; // 帧头 固定值0x0080 + unsigned int frame_type : 16; // 帧类型 固定值0x0029 + unsigned int frame_length : 16; // 帧长 固定值10 + unsigned int accumulated : 8; // 累加值 按帧累加 + unsigned int request_id : 16; // 请求帧ID 请求ID 0-FFFF，FFFF表示全部帧 + unsigned int crc : 8; // CRC 按字节累加之和 取低8位 +} StrRequestFrame; + +typedef union _UnRequestFrame +{ + StrRequestFrame bit_data; // 使用定义的结构体变量名 + unsigned char arr[sizeof(StrRequestFrame)]; // 通过结构体类型确定大小 +} UnRequestFrame; + + +// 接收遥控器输入 +typedef struct _StrRemoteControlInput +{ + //-----接收数据0x12000013---------------------------------------------- + unsigned int speed : 16; // 速度 + unsigned int curvature : 16; // 曲率 + unsigned int reserve1 : 16; // 遥控轴 + + unsigned int switch_a : 1; // SwA + unsigned int switch_b : 2; // SwB 急停开关 1释放 0/2是急停 + unsigned int switch_c : 2; // SwC 模式开关 1自动 0/2是手动 + unsigned int switch_d : 1; // SwD 上电开关 先短按再长按 + unsigned int reserve2 : 1; // 保留 + unsigned int reserve3 : 1; // 保留 + + unsigned int enable : 8; // 使能 +} StrRemoteControlInput; + +typedef union _UnRemoteControlInput +{ + StrRemoteControlInput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrRemoteControlInput)]; // 通过结构体类型确定大小 +} UnRemoteControlInput; + + +//-----IO口--------------------------------------------------------------- +// 从IO口输入 + +typedef struct _StrSwSample +{ + unsigned int emergency_stop_switch : 1; // 通道1，急停开关 + unsigned int High_voltage_switch : 1; // 通道2，高压开关 + unsigned int Left_switch : 1; // 通道3 左限位开关 + unsigned int right_switch : 1; // 通道4 右限位开关 + unsigned int CH05 : 1; // 通道5 + unsigned int CH06 : 1; // 通道6 + unsigned int CH07 : 1; // 通道7 + unsigned int CH08 : 1; // 通道8 + unsigned int CH09 : 1; // 通道9 + unsigned int CH10 : 1; // 通道10 + unsigned int CH11 : 1; // 通道11 + unsigned int CH12 : 1; // 通道12 + unsigned int CH13 : 1; // 通道13 + unsigned int CH14 : 1; // 通道14 + unsigned int CH15 : 1; // 通道15 + unsigned int CH16 : 1; // 通道16 + unsigned int CH17 : 1; // 通道17 + unsigned int CH18 : 1; // 通道18 + unsigned int CH19 : 1; // 通道19 + unsigned int CH20 : 1; // 通道20 + unsigned int CH21 : 1; // 通道21 + unsigned int CH22 : 1; // 通道22 + unsigned int CH23 : 1; // 通道23 + unsigned int CH24 : 1; // 通道24 + unsigned int CH25 : 1; // 通道25 + unsigned int CH26 : 1; // 通道26 + unsigned int CH27 : 1; // 通道27 + unsigned int CH28 : 1; // 通道28 + unsigned int CH29 : 1; // 通道29 + unsigned int CH30 : 1; // 通道30 + unsigned int CH31 : 1; // 通道31 + unsigned int CH32 : 1; // 通道32 + unsigned int CH33 : 1; // 通道33 + unsigned int CH34 : 1; // 通道34 + unsigned int CH35 : 1; // 通道35 + unsigned int CH36 : 1; // 通道36 + unsigned int CH37 : 1; // 通道37 + unsigned int CH38 : 1; // 通道38 + unsigned int CH39 : 1; // 通道39 + unsigned int CH40 : 1; // 通道40 + unsigned int CH41 : 1; // 通道41 + unsigned int CH42 : 1; // 通道42 + unsigned int CH43 : 1; // 通道43 + unsigned int CH44 : 1; // 通道44 + unsigned int reserve : 4; +} StrSwSample; + +typedef union _UnSwSample +{ + StrSwSample bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrSwSample)]; // 通过结构体类型确定大小 +} UnSwSample; + + +//-----输出数据结构--------------------------------------------------------------- +//-----CAN---------------------------------------------------------------- +// 输出到电机控制器 +//typedef struct _StrMotorOutput +//{ +////-----发送数据0x201或者0x202---------------------------------------------- +// unsigned int mode : 8; // 模式 0x1恒速模式，0x2恒扭模式，其他无效 +// unsigned int gear : 8; // 档位 0x0空挡模式，0x1前进挡，0x2倒退档，其他无效 +// unsigned int set_torque : 16; // 给定扭矩 系数 0.01 偏移量 -300 实际物理量＝数据×系数＋偏移量 +// unsigned int set_rotation_speed : 16; // 给定转速 偏移量 -30000 +// unsigned int fault_code : 8; // 故障码 +// unsigned int heartbeat : 8; // 心跳 +////-----发送数据0x401或者0x402---------------------------------------------- +// unsigned int feed_power : 16; // 馈电功率 单位为 W 最大为10KW +// unsigned int discharge_power : 16; // 放电功率 单位为 W 最大为15kW +// unsigned int reserve1 : 16; // 保留 +// unsigned int reserve2 : 16; // 保留 +//} StrMotorOutput; +// +//typedef union _UnMotorOutput +//{ +// StrMotorOutput bit_data; // 使用定义的结构体变量名 +// uint8_t arr[sizeof(StrMotorOutput)]; // 通过结构体类型确定大小 +//} UnMotorOutput; + + + +// canoe协议输出 +typedef struct _StrSdoOutput +{ +//-----发送数据0x601---------------------------------------------- + uint16_t index; //索引，类似寄存器地址 + uint16_t object_index; // 从索引 为0x0000 + uint32_t data; // 数据 +} StrSdoOutput; + +typedef union _UnSdoOutput +{ + StrSdoOutput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrSdoOutput)]; // 通过结构体类型确定大小 +} UnSdoOutput; + + +// 输出到采集模块 +typedef struct _StrGatherOutput +{ +//----- Send Data 0x15000002 ---------------------------------------------- + unsigned int sleep_duration : 16; // 休眠时间 + unsigned int wakeup_interval : 16; // 唤醒时间 + unsigned int vehicle_mode : 8; // 整车模式 + unsigned int reserved1 : 8; // 保留1 + unsigned int reserved2 : 8; // 保留2 + unsigned int reserved3 : 8; // 保留3 +} StrGatherOutput; + +typedef union _UnGatherOutput +{ + StrGatherOutput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrGatherOutput)]; // 通过结构体类型确定大小 +} UnGatherOutput; + + + + + + + + + + + + + + + + + + + + +typedef struct _StrMotorOutput +{ +//-----发送数据0x10F80807---------------------------------------------- + unsigned int gear : 2; // 0 表示空挡，1 表示前进，2 表示后退 + unsigned int can_gear : 1; // 0-无效 1-有效。无效时 BIT10 的挡位无效，以线路控制挡位为准 + unsigned int can_break : 1; // 0:不刹车，1-刹车(刹车时才能启动电子刹车) + unsigned int reserve1 : 1; // 0-只有高速档 1-可以通过接线切换 123 档。 + unsigned int reserve2 : 1; // 0-无效 1-有效：线控防盗使能 + unsigned int reserve3 : 1; // 0-无效 1-有效：线控巡航使能 + unsigned int reserve4 : 1; // 0-无效 1-有效：线控座桶使能 + + unsigned int reserve5 : 1; // 0-无效 1-有效：线控边撑使能 + unsigned int reserve6 : 1; // 0-无效 1-有效：线控防盗使能 + unsigned int reserve7 : 1; // 0-无效 1-有效：线控巡航使能 + unsigned int reserve8 : 1; // 0-无效 1-有效：线控座桶使能 + unsigned int reserve9 : 1; // 0-无效 1-有效：线控巡航使能 + unsigned int reserve10 : 1; // 0-无效 1-有效：线控座桶使能 + unsigned int reserve11 : 1; // 0-无效 1-有效：线控巡航使能 + unsigned int reserve12 : 1; // 0-无效 1-有效：线控座桶使能 + + unsigned int reserve13 : 1; // 0-无效 1-有效：线控边撑使能 + unsigned int reserve14 : 1; // 0-无效 1-有效：线控防盗使能 + unsigned int reserve15 : 1; // 0-无效 1-有效：线控巡航使能 + unsigned int reserve16 : 1; // 0-无效 1-有效：线控座桶使能 + unsigned int reserve17 : 1; // 0-无效 1-有效：线控巡航使能 + unsigned int reserve18 : 1; // 0-无效 1-有效：线控座桶使能 + unsigned int motor_direction : 1; // 0 和 1 （静止空闲状态设置有效，在油门、 转矩、转速模式下允许设置，否则使用控制器 内部保存的参数控制电机方向） + unsigned int Contactor : 1; // 0-断开 1-闭合，（部分控制器支持） + + unsigned int reserve19 : 1; // 0-无效 1-有效：线控边撑使能 + unsigned int reserve20 : 1; // 0-无效 1-有效：线控防盗使能 + unsigned int reserve21 : 1; // 0-无效 1-有效：线控巡航使能 + unsigned int reserve22 : 1; // 0-无效 1-有效：线控座桶使能 + unsigned int mode : 4; //0x0:转把控制，0x5:油门模式，0xA:转矩模式， 0xC:转速模式，其它无效停机。在这三种模式下，电机方向设置有效。只允许在停机状态下设置控制模式。 + + unsigned int control_data1 : 16; // 油门-256~+256：(0~+256 表示最大油门,-256 表 示最大电子刹车力度。转矩 -256~10000，（正数 驱 动 扭 矩 0~1000.0Nm，负数表示刹车，-256 表示最大电子刹车力度）转速-256~12000,（正数表示转速 0~12000rpm， 负数表示刹车，-256 最大刹车力度） + unsigned int control_data2 : 16; // 控制数据和上一个一样 +} StrMotorOutput; + +typedef union _UnMotorOutput +{ + StrMotorOutput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrMotorOutput)]; // 通过结构体类型确定大小 +} UnMotorOutput; + + + + + + + + + + + +//电机控制器风扇左 +//kgf1 +//电机控制器风扇右 +//kgf3 +//预充 +//kgf4 +//高压压继电器 +//kgf7，8 +//低压继电器 +//kgf11-12 +//左前红灯 +//kgf6 +//左前黄灯 +//kgf9 +//左风扇 +//kgf14 +//右风扇 +//kgf19 +//电脑 +//kgf 17-18 +//交换机 +//kgf 21-22 +//右前红灯 +//kgf24 +//右前黄灯 +//kgf25 +//左后红灯 +//kgf26 +//左后黄灯 +//kgf27 +//右后红灯 +//kgf29 +//右后黄灯 +//kgf30 + + +// 输出到开关阀模块 +typedef struct _StrInfCanKGFOutput +{ + //----------主机发送命令字0x11000002 -------------------------------------------- + unsigned int KGF01 : 1; // 通道1 + unsigned int KGF02 : 1; // 通道2 + unsigned int KGF03 : 1; // 通道3 + unsigned int KGF04 : 1; // 通道4 + unsigned int KGF05 : 1; // 通道5 + unsigned int KGF06 : 1; // 通道6 + unsigned int KGF07 : 1; // 通道7 + unsigned int KGF08 : 1; // 通道8 + unsigned int KGF09 : 1; // 通道9 + unsigned int KGF10 : 1; // 通道10 + unsigned int KGF11 : 1; // 通道11 + unsigned int KGF12 : 1; // 通道12 + unsigned int KGF13 : 1; // 通道13 + unsigned int KGF14 : 1; // 通道14 + unsigned int KGF15 : 1; // 通道15 + unsigned int KGF16 : 1; // 通道16 + + unsigned int can_rx2 : 8; // 保留 + unsigned int can_rx3 : 8; // 保留 + unsigned int can_rx4 : 8; // 保留 + unsigned int can_rx5 : 8; // 保留 + unsigned int can_period_h : 8; // 发送周期高位 + unsigned int can_period_l : 8; // 发送周期低位 + + //----------发送0x11000003 --------------------------------------------------- + unsigned int pwm_01 : 4; // PWM通道1 + unsigned int pwm_02 : 4; // PWM通道2 + unsigned int pwm_03 : 4; // PWM通道3 + unsigned int pwm_04 : 4; // PWM通道4 + unsigned int pwm_05 : 4; // PWM通道5 + unsigned int pwm_06 : 4; // PWM通道6 + unsigned int pwm_07 : 4; // PWM通道7 + unsigned int pwm_08 : 4; // PWM通道8 + unsigned int pwm_09 : 4; // PWM通道9 + unsigned int pwm_10 : 4; // PWM通道10 + unsigned int pwm_11 : 4; // PWM通道11 + unsigned int pwm_12 : 4; // PWM通道12 + unsigned int pwm_13 : 4; // PWM通道13 + unsigned int pwm_14 : 4; // PWM通道14 + unsigned int pwm_15 : 4; // PWM通道15 + unsigned int pwm_16 : 4; // PWM通道16 +} StrInfCanKGFOutput; + +typedef union _UnInfCanKGFOutput +{ + StrInfCanKGFOutput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrInfCanKGFOutput)]; // 通过结构体类型确定大小 +} UnInfCanKGFOutput; + + + +// 输出到H桥模块 +typedef struct _StrHBridgeOutput +{ + //----------主机发送命令字0x7F2 -------------------------------------------- + unsigned int channel_01 : 1; // 通道1 + unsigned int channel_02 : 1; // 通道2 + unsigned int channel_03 : 1; // 通道3 + unsigned int channel_04 : 1; // 通道4 + unsigned int channel_05 : 1; // 通道5 + unsigned int channel_06 : 1; // 通道6 + unsigned int channel_07 : 1; // 通道7 + unsigned int channel_08 : 1; // 通道8 + unsigned int sleep_01 : 1; // 休眠1 + unsigned int sleep_02 : 1; // 休眠2 + unsigned int sleep_03 : 1; // 休眠3 + unsigned int sleep_04 : 1; // 休眠4 + unsigned int reserve_01 : 1; // 保留1 + unsigned int reserve_02 : 1; // 保留2 + unsigned int reserve_03 : 1; // 保留3 + unsigned int reserve_04 : 1; // 保留4 + + unsigned int reserve_05 : 8; // 保留5 + unsigned int reserve_06 : 8; // 保留6 + unsigned int reserve_07 : 8; // 保留7 + unsigned int reserve_08 : 8; // 保留8 + unsigned int can_period_h : 8; // 发送周期高位 + unsigned int can_period_l : 8; // 发送周期低位 + + //----------发送0x7F3 --------------------------------------------------- + unsigned int pwm_channel_01 : 4; // PWM通道1 + unsigned int pwm_channel_02 : 4; // PWM通道2 + unsigned int pwm_channel_03 : 4; // PWM通道3 + unsigned int pwm_channel_04 : 4; // PWM通道4 + unsigned int pwm_channel_05 : 4; // PWM通道5 + unsigned int pwm_channel_06 : 4; // PWM通道6 + unsigned int pwm_channel_07 : 4; // PWM通道7 + unsigned int pwm_channel_08 : 4; // PWM通道8 + unsigned int pwm_reserve_01 : 4; // 保留pwm1 + unsigned int pwm_reserve_02 : 4; // 保留pwm2 + unsigned int pwm_reserve_03 : 4; // 保留pwm3 + unsigned int pwm_reserve_04 : 4; // 保留pwm4 + unsigned int pwm_reserve_05 : 4; // 保留pwm5 + unsigned int pwm_reserve_06 : 4; // 保留pwm6 + unsigned int pwm_reserve_07 : 4; // 保留pwm7 + unsigned int pwm_reserve_08 : 4; // 保留pwm8 +} StrHBridgeOutput; + +typedef union _UnHBridgeOutput +{ + StrHBridgeOutput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrHBridgeOutput)]; // 通过结构体类型确定大小 +} UnHBridgeOutput; + + +// 输出给导航仪 +typedef struct _StrWheelSpeedOutput +{ + //-----发送数据0x98---------------------------------------------- + unsigned int left_front : 16; // 左前轮速 + unsigned int right_front : 16; // 右前轮速 + unsigned int left_rear : 16; // 左后轮速 + unsigned int right_rear : 16; // 右后轮速 +} StrWheelSpeedOutput; + +typedef union _UnWheelSpeedOutput +{ + StrWheelSpeedOutput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrWheelSpeedOutput)]; // 通过结构体类型确定大小 +} UnWheelSpeedOutput; + + +// 输出给基站升降杆 +typedef struct _StrLifterOutput +{ + //-----发送数据0x6F2---------------------------------------------- + unsigned int cmd : 8 ; // 上升命令 + unsigned int reserve1 : 8 ; // 保留 + unsigned int reserve2 : 16; // 保留 + unsigned int reserve3 : 16; // 保留 + unsigned int reserve4 : 16; // 保留 +} StrLifterOutput; + +typedef union _UnrLifterOutput +{ + StrLifterOutput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrLifterOutput)]; // 通过结构体类型确定大小 +} UnLifterOutput; + + +//-----以太网------------------------------------------------------------- +// 输出给自主计算机 +typedef struct _StrComputerOutput +{ + //-------------------------------------------------- + unsigned int frame_header : 16; // 帧头 0xFFCC + unsigned int frame_type : 16; // 帧类型 0x0011 + unsigned int frame_length : 8; // 帧长 0x000b，11 + unsigned int accumulated : 8; // 累加值 + unsigned int speed : 16; // 左侧轮速 + unsigned int curvature : 16; // 右侧轮速 + unsigned int crc : 8; // CRC +} StrComputerOutput; + +typedef union _UnComputerOutput +{ + StrComputerOutput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrComputerOutput)]; // 通过结构体类型确定大小 +} UnComputerOutput; + + + + +// 车辆信息，输出给上位机 +typedef struct _StrVehicleInfoOutput +{ + //-------------------------------------------------- + unsigned int frame_header : 16; // 帧头 + unsigned int frame_type : 16; // 帧类型 + unsigned int frame_length : 16; // 帧长 + unsigned int accumulated : 8; // 累加值 + unsigned int desired_speed : 16; // 期望速度 + unsigned int desired_curvature : 16; // 期望曲率 + unsigned int set_left_speed : 16; // 设定左转速度 + unsigned int set_right_speed : 16; // 设定右转速度 + unsigned int longitude : 32; // 经度 + unsigned int latitude : 32; // 纬度 + unsigned int altitude : 32; // 高度 + unsigned int heading_angle : 16; // 航向角 + unsigned int speed : 16; // 当前车速 + unsigned int curvature : 16; // 当前曲率 + unsigned int battery_voltage : 16; // 电池电压 + unsigned int battery_soc : 16; // 电池SOC + unsigned int battery_current : 16; // 电池电流 + unsigned int vehicle_fault_state : 8; // 整车故障状态 + unsigned int crc : 8; // CRC +} StrVehicleInfoOutput; + +typedef union _UnVehicleInfoOutput +{ + StrVehicleInfoOutput bit_data; // 使用定义的结构体变量名 + unsigned char arr[sizeof(StrVehicleInfoOutput)]; // 通过结构体类型确定大小 +} UnVehicleInfoOutput; + + + + +// 电机状态信息，输出给上位机 +typedef struct _StrMotorStatusOutput +{ + //-------------------------------------------------- + unsigned int frame_header : 16; // 帧头 固定值0xAACC + unsigned int frame_type : 16; // 帧类型 固定值0x0021 + unsigned int frame_length : 16; // 帧长 固定值30 + unsigned int accumulated : 8; // 累加值 按帧累加 + unsigned int left_wheel_speed : 16; // 当前左侧轮速 系数 1 偏移量 -30000 单位rad/min + unsigned int right_wheel_speed : 16; // 当前右侧轮速 系数 1 偏移量 -30000 单位rad/min + unsigned int left_torque : 16; // 左侧电机扭矩 系数 0.01 偏移量 -300 单位N/s + unsigned int right_torque : 16; // 右侧电机扭矩 系数 0.01 偏移量 -300 单位N/s + unsigned int left_fault_code : 8; // 左侧电机故障码 + unsigned int right_fault_code : 8; // 右侧电机故障码 + unsigned int left_torque_limit : 16; // 左侧电机扭矩限制 + unsigned int right_torque_limit : 16; // 右侧电机扭矩限制 + unsigned int left_power_in : 16; // 左侧电机馈电功率 + unsigned int right_power_in : 16; // 右侧电机馈电功率 + unsigned int left_power_out : 16; // 左侧电机放电功率 + unsigned int right_power_out : 16; // 右侧电机放电功率 + unsigned int left_voltage : 16; // 左侧电机电压 + unsigned int right_voltage : 16; // 右侧电机电压 + unsigned int checksum : 8; // 校验和 按字节累加之和 取低8位 +} StrMotorStatusOutput; + +typedef union _UnMotorStatusOutput +{ + StrMotorStatusOutput bit_data; // 使用定义的结构体变量名 + unsigned char arr[sizeof(StrMotorStatusOutput)]; // 通过结构体类型确定大小 +} UnMotorStatusOutput; + + + + +// PID参数输出，输出给上位机 +typedef struct _StrPIDOutput +{ + //-------------------------------------------------- + unsigned int frame_header : 16; // 帧头 + unsigned int frame_type : 16; // 帧类型 + unsigned int frame_length : 16; // 帧长 + unsigned int accumulated : 8; // 累加值 + unsigned int rc_straight_p : 32; // 遥控直行P参数 + unsigned int rc_straight_i : 32; // 遥控直行I参数 + unsigned int rc_straight_d : 32; // 遥控直行D参数 + unsigned int auto_straight_p : 32; // 自主直行P参数 + unsigned int auto_straight_i : 32; // 自主直行I参数 + unsigned int auto_straight_d : 32; // 自主直行D参数 + unsigned int rc_turn_p : 32; // 遥控转弯P参数 + unsigned int rc_turn_i : 32; // 遥控转弯I参数 + unsigned int rc_turn_d : 32; // 遥控转弯D参数 + unsigned int auto_turn_p : 32; // 自主转弯P参数 + unsigned int auto_turn_i : 32; // 自主转弯I参数 + unsigned int auto_turn_d : 32; // 自主转弯D参数 + unsigned int checksum : 8; // 校验和 +} StrPIDOutput; + +typedef union _UnPIDOutput +{ + StrPIDOutput bit_data; // 使用定义的结构体变量名 + unsigned char arr[sizeof(StrPIDOutput)]; // 通过结构体类型确定大小 +} UnPIDOutput; + + + + + +// 模拟信号输出，输出给上位机 +typedef struct _StrAnalogSignalOutput +{ + //-------------------------------------------------- + unsigned int frame_header : 16; // 帧头 固定值0xAACC + unsigned int frame_type : 16; // 帧类型 固定值0x0023 + unsigned int frame_length : 16; // 帧长 固定值24 + unsigned int accumulated : 8; // 累加值 按帧累加 + unsigned int channel_1 : 16; // 通道1 温度传感器 系数为0.1 有符号 正数表示正温度，负数标表示负温度 + unsigned int channel_2 : 16; // 通道2 + unsigned int channel_3 : 16; // 通道3 + unsigned int channel_4 : 16; // 通道4 + unsigned int channel_5 : 16; // 通道5 温度传感器 系数为0.1 有符号 正数表示正温度，负数标表示负温度 + unsigned int channel_6 : 16; // 通道6 + unsigned int channel_7 : 16; // 通道7 + unsigned int channel_8 : 16; // 通道8 + unsigned int crc : 8 ; // 校验和 按字节累加之和 取低8位 + +} StrAnalogSignalOutput; + +typedef union _UnAnalogSignalOutput +{ + StrAnalogSignalOutput bit_data; // 使用定义的结构体变量名 + unsigned char arr[sizeof(StrAnalogSignalOutput)]; // 通过结构体类型确定大小 +} UnAnalogSignalOutput; + + + +// 遥控器数据输出，给上位机 +typedef struct _StrRemoteControlOutput +{ + //-------------------------------------------------- + unsigned int frame_header : 16; // 帧头 固定值0xAACC + unsigned int frame_type : 16; // 帧类型 固定值0x0024 + unsigned int frame_length : 16; // 帧长 固定值20 + unsigned int accumulated : 8; // 累加值 按帧累加 + + // RC6GS遥控原始数据 + unsigned int speed : 16; // 速度 系数0.01，正为前进，负为后退 单位m/s + unsigned int curvature : 16; // 曲率 系数0.0001，正为左转，负为右转 + unsigned int reserve_1 : 16; // 备用1 + + unsigned int switch_a : 1; // SwA + unsigned int switch_b : 2; // SwB 0和2刹车状态 1正常状态 + unsigned int switch_c : 2; // SwC 0和2自主状态 1手动状态 + unsigned int switch_d : 1; // SwD + unsigned int hv_relay_state : 1; // 高压继电机状态 0 断开， 1开启 + unsigned int reserve_2 : 1; // 备用 + unsigned int enable : 8; // 使能 1遥控数据有效，0无效 + unsigned int accumulated_value : 16; // 接收累加值 + unsigned int crc : 8; // 校验和 按字节累加之和 取低8位 +} StrRemoteControlOutput; + +typedef union _UnRemoteControlOutput +{ + StrRemoteControlOutput bit_data; // 使用定义的结构体变量名 + unsigned char arr[sizeof(StrRemoteControlOutput)]; // 通过结构体类型确定大小 +} UnRemoteControlOutput; + + + +// 手动控制数据，返回给请求者 +typedef struct _StrManualControlOutput +{ + //-------------------------------------------------- + // 手动控制数据 高位在前，低位在后 + unsigned int frame_header : 16; // 帧头 固定值0xFFBB + unsigned int frame_type : 16; // 帧类型 固定值0x0025 + unsigned int frame_length : 8; // 帧长 固定值0x0C + unsigned int accumulated : 8; // 累加值 按帧累加 + unsigned int set_speed : 16; // 设定速度 系数0.01，正为前进，负为后退 单位m/s + unsigned int set_curvature : 16; // 设定曲率 系数0.0001，正为左转，负为右转 + unsigned int reserved : 8; // 保留 + unsigned int crc_1 : 8 ; // CRC 按字节累加之和，溢出取低8位 + unsigned int accumulated_value : 16; // 接收累加值 + unsigned int crc_2 : 8 ; // CRC 按字节累加之和，溢出取低8位 +} StrManualControlOutput; + +typedef union _UnManualControlOutput +{ + StrManualControlOutput bit_data; // 使用定义的结构体变量名 + unsigned char arr[sizeof(StrManualControlOutput)]; // 通过结构体类型确定大小 +} UnManualControlOutput; + + + + +// 自动控制数据输出，返回给请求者 +typedef struct _StrAutoControlOutput +{ + //-------------------------------------------------- + // 自主计算机自动数据 高位在前，低位在后 + unsigned int frame_header : 16; // 帧头 固定值0xFFCC + unsigned int frame_type : 16; // 帧类型 固定值0x0026 + unsigned int frame_length : 8; // 帧长 固定值0x19 + unsigned int accumulated : 8; // 累加值 按帧累加 + unsigned int set_speed : 16; // 设定速度 系数0.01，正为前进，负为后退 单位m/s + unsigned int set_curvature : 16; // 设定曲率 系数0.0001，正为左转，负为右转 + unsigned int latitude : 32; // 纬度 系数10^-7，431234567表示43.1234567度 + unsigned int longitude : 32; // 经度 系数10^-7，431234567表示43.1234567度 + unsigned int altitude : 32; // 高度 单位mm + unsigned int heading : 16; // 航向 车辆航向，35999表示359.99度。 + unsigned int accumulated_value : 16; // 累加值 按帧累加 + unsigned int crc : 8 ; // CRC 总体累加CRC +} StrAutoControlOutput; + +typedef union _UnAutoControlOutput +{ + StrAutoControlOutput bit_data; // 使用定义的结构体变量名 + unsigned char arr[sizeof(StrAutoControlOutput)]; // 通过结构体类型确定大小 +} UnAutoControlOutput; + + + + + +//-----IO口--------------------------------------------------------------- +// 电压信号输出，到遥控器 +typedef struct _StrVoltageSignalOutput +{ + unsigned int pwm_value : 16; // PWM值 +} StrVoltageSignalOutput; + +typedef union _UnVoltageSignalOutput +{ + StrVoltageSignalOutput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrVoltageSignalOutput)]; // 通过结构体类型确定大小 +} 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 UnBmsInput un_bms_input ;//BMS接收数据 +extern UnTempModuleInput un_temp_module_input;//温度采集模块 +extern UnAutoComputerInput un_auto_computer_input;//自主计算机自动数据 +extern UnManualComputerInput un_manual_computer_input;//自主计算机手动数据 + +extern UnGatherOutput un_gather_output ;//采集模块输出 + + + +extern UnMotorOutput un_motor_output1; //电机输出 +extern UnMotorOutput un_motor_output2; //电机输出 +extern UnMotorOutput un_motor_output3; //电机输出 +extern UnMotorOutput un_motor_output4; //电机输出 +extern UnInfCanKGFOutput un_inf_can_kgf_output1; +extern UnInfCanKGFOutput un_inf_can_kgf_output2; +extern UnHBridgeOutput un_h_bridge_output; +extern UnHBridgeOutput un_h_bridge_output1; +extern UnHBridgeOutput un_h_bridge_output2; +extern UnWheelSpeedOutput un_wheel_wpeed_output; +extern UnLifterOutput un_lifter_output;//基站升降杆输出 + +extern UnSdoOutput un_sdo_output1 ;//设定左转向电机位置 +extern UnSdoOutput un_sdo_output2 ;//设定左转向电机速度 +extern UnSdoOutput un_sdo_output3 ;//使能左电机 +extern UnSdoOutput un_sdo_output4 ;//设定左电机模式 + +extern UnSdoOutput un_sdo_output5 ;//设定右转向电机位置 +extern UnSdoOutput un_sdo_output6 ;//设定右转向电机速度 +extern UnSdoOutput un_sdo_output7 ;//使能右电机 +extern UnSdoOutput un_sdo_output8 ;//设定右电机模式 + + +//串口 +extern UnRemoteControlInput un_remote_control_input; //遥控器输入 +extern UnRemoteControlInput un_remote_control_input1;//遥控器输入 +//IO口 +extern UnSwSample un_sw_sample; + +//以太网 +extern UnRequestFrame un_request_frame; //请求帧 + +extern UnComputerOutput un_computer_output; //输出给自主计算机 + +//输出给上位机 +extern UnVehicleInfoOutput un_vehicle_Info_output; // 车辆信息，输出给上位机 +extern UnMotorStatusOutput un_motor_status_output; // 电机状态信息，输出给上位机 +extern UnPIDOutput un_pid_output; // PID参数输出，输出给上位机 +extern UnAnalogSignalOutput un_analog_signal_output; // 模拟信号输出，输出给上位机 +extern UnRemoteControlOutput un_remote_control_output;// 遥控器数据输出，给上位机 +extern UnManualControlOutput un_manual_control_output;// 手动控制数据，返回给请求者 +extern UnAutoControlOutput un_auto_control_output; // 自动控制数据输出，返回给请求者 +extern UnSdoOutput un_sdo_output ;//转向电机输出 + + +//变量 +extern uint8_t test_app[26]; + + + + +//函数 +void canSendAll(void *signal_id); +void ethernetSendAll(void *signal_id); + +#pragma pack() + +#endif /* _INTERFACE_H_ */ diff --git a/interface_24c02.c b/interface_24c02.c new file mode 100644 index 0000000..095b86e --- /dev/null +++ b/interface_24c02.c @@ -0,0 +1,426 @@ + + +#include +#include +#include +#include "sdrv_spi.h" +#include "irq_num.h" +#include "clock_ip.h" + + + +#include "interface_config.h" + + +#define _nop_() udelay(1); +#define somenop {_nop_();_nop_();_nop_();_nop_();_nop_();} +#define bit bool + +#define MASTER_TEST_LEN 100//෢100ֽڵ + +static uint8_t m_rx_buf_u8[MASTER_TEST_LEN] __attribute__((__aligned__(32))); +static uint8_t m_tx_buf_u8[MASTER_TEST_LEN] __attribute__((__aligned__(32))); + +static struct sdrv_spi g_master; + +void iic_start(void); +void iic_stop(void); +void iic_ack(bit ackbit); +void iic_sendbyte(unsigned char byt); +//void wrbyte_24c02(unsigned int add,unsigned char dat); +void delay(unsigned char t); + +bit iic_waitack(void); +unsigned char i2c_recbyte(void); +unsigned char rdbyte_24c02(unsigned int add); +unsigned char wrEE_CRC_Bak(unsigned char add,unsigned char *eeData,unsigned char len,unsigned char addbak); +unsigned char rdEE_CRC_Bak(unsigned char add,unsigned char *eeData,unsigned char len,unsigned char addbak); + + +////FM25CL64ָ +#define FM25CL64_WREN 0x06 //ʹ +#define FM25CL64_WRDI 0x04 //ʧ +#define FM25CL64_RDSR 0x05 //״̬ +#define FM25CL64_WRSR 0x01 //д״̬ +#define FM25CL64_READ 0x03 // +#define FM25CL64_WRITE 0x02 //д + +#define MAX_FM25CL64_LEN 8192//8kֽ + +static const struct spi_device_config device_mode0_8bits_ss0 = { + .sclk_freq = 1000000, + .cpol = SCK_IDLE_LOW, + .cpha = DATA_CPT_ON_FIRST_SCK_EDGE, + .cs_pol = CS_ACTIVE_LOW, + .cs_sel = CS_SEL_SS0, + .width = SPI_DATA_WIDTH_BYTE, + .fream_delay = 0, + .clk2cs_delay = 0, + .clk2cs_end_delay = 0, + .is_lsb_mode = false, + +}; + +static struct spi_common_config spi_master = { + .is_spi_mode = true, + .is_half_mode = false, + .is_master = true, + .base = APB_SPI6_BASE, + .irq = SPI6_SPI_INTR_NUM, +#if CONFIG_E3 + .clk = &g_ckgen_ip_spi_sf_1_to_4, +#else + .clk = &g_ckgen_ip_spi_sf_1_to_3, +#endif +}; + + + + + +void initSpi() +{ + /* Init Spi Instance */ + sdrv_spi_init(&g_master, &spi_master); + sdrv_spi_config_device(&g_master, &device_mode0_8bits_ss0); +} + + + + + + + +void SDAOut(bit c) +{ +// sdrv_pinctrl_set_input_select(GPIO_Y3, PIN_IS_CMOS); +// sdrv_gpio_set_pin_direction(GPIO_Y3, GPIO_DIR_OUT); + sdrv_gpio_set_pin_output_level(GPIO_Y3, c); +} + +void SCLOut(bit c) +{ + sdrv_gpio_set_pin_output_level(GPIO_Y2, c); +} + +void iic_start(void) +{ + SDAOut(1); + _nop_(); + SCLOut(1); + somenop; + SDAOut(0); + somenop; + SCLOut(0); +} + +void iic_stop(void) +{ + SDAOut(0); + _nop_(); + SCLOut(1); + somenop; + SDAOut(1); +} + +void iic_ack(bit ackbit) +{ + if(ackbit) + SDAOut(0); + else + SDAOut(1); + somenop; + SCLOut(1); + somenop; + SCLOut(0); + SDAOut(1); + somenop; +} + +bit iic_waitack(void) +{ + SDAOut(1); + somenop; + SCLOut(1); +// sdrv_pinctrl_set_input_select(GPIO_Y3, PIN_IS_CMOS_SCHMITT); +// sdrv_gpio_set_pin_direction(GPIO_Y3, GPIO_DIR_IN); + somenop; + if(sdrv_gpio_read_pin_input_level(GPIO_Y3)) + { + SCLOut(0); + iic_stop(); + return 0; + } + else + { + SCLOut(0); + return 1; + } +} + +void iic_sendbyte(unsigned char byt) +{ + unsigned char i; + for(i=0;i<8;i++) + { + if(byt&0x80) + SDAOut(1); + else + SDAOut(0); + somenop; + SCLOut(1); + byt <<= 1; + somenop; + SCLOut(0); + } +} + +unsigned char iic_recbyte(void) +{ + unsigned char da; + unsigned char i; + for(i=0;i<8;i++) + { + SCLOut(1); +// sdrv_pinctrl_set_input_select(GPIO_Y3, PIN_IS_CMOS_SCHMITT); +// sdrv_gpio_set_pin_direction(GPIO_Y3, GPIO_DIR_IN); + somenop; + da <<= 1; + +// P1_IOCR01 = 0x0020; // load port control register 1 + if(sdrv_gpio_read_pin_input_level(GPIO_Y3)) + da |= 0x01; + SCLOut(0); + somenop; + } + return da; +} + +unsigned char wrbyte_24c02(unsigned int add,unsigned char dat) +{ +#if EN_24C02 + unsigned char eeprom_page = 0; + unsigned char eeprom_add = 0; + unsigned char device_add = 0; +//------------------------------------------------- + + eeprom_page = add/256;//ҳ + eeprom_add = add%256; + if(eeprom_page >= 3)//ĿǰE2һ4ҳ1k + { + eeprom_page = 3; + } + // Device Address 1100 0 p1 p0 R/W + device_add = (0xa0 | (eeprom_page << 1));// + +// ssdk_printf(SSDK_CRIT, "eeprom_page:0x%x\r\n", eeprom_page); +// ssdk_printf(SSDK_CRIT, "eeprom_add:0x%x\r\n", eeprom_add); +// ssdk_printf(SSDK_CRIT, "device_add:0x%x\r\n", device_add); + + iic_start(); + iic_sendbyte(device_add); + iic_waitack(); + iic_sendbyte(eeprom_add); + iic_waitack(); + iic_sendbyte(dat); + iic_waitack(); + iic_stop(); +#else + if(add >= MAX_FM25CL64_LEN)//ַ + { + printf("E2 len error!\r\n"); + } + else + { + m_tx_buf_u8[0] = FM25CL64_WREN;//дʹ + if (sdrv_spi_sync_transmit(&g_master, m_tx_buf_u8, m_rx_buf_u8,1,1)) + { + printf("E2 write Failed!\r\n"); + } + + m_tx_buf_u8[0] = FM25CL64_WRITE;//д + m_tx_buf_u8[1] = (uint8_t)( add >> 8);// 8λַ + m_tx_buf_u8[2] = (uint8_t)( add );// + m_tx_buf_u8[3] = dat;// + + if (sdrv_spi_sync_transmit(&g_master, m_tx_buf_u8, m_rx_buf_u8,4,4)) + { + printf("E2 write Failed!\r\n"); + } + } +#endif + return 0; +} + +unsigned char rdbyte_24c02(unsigned int add) +{ +#if EN_24C02 + unsigned char eeprom_page = 0; + unsigned char eeprom_add = 0; + unsigned char device_add = 0; + unsigned char da; +//------------------------------------------------- + + eeprom_page = add/256;//ҳ + eeprom_add = add%256; + if(eeprom_page >= 3)//ĿǰE2һ4ҳ1k + { + eeprom_page = 3; + } + + // Device Address 1100 000 R/W + iic_start(); + device_add = (0xa0 | (eeprom_page << 1));// αд + iic_sendbyte(device_add); + iic_waitack(); + iic_sendbyte(eeprom_add); + iic_waitack(); + iic_start(); + + device_add = (0xa1 | (eeprom_page << 1));// + + ssdk_printf(SSDK_CRIT, "device_add:0x%x\r\n", device_add); + + iic_sendbyte(device_add); + iic_waitack(); + da = iic_recbyte(); + iic_ack(0); + iic_stop(); +//CAN_sendAck(add, da); + return da; +#else + if(add >= MAX_FM25CL64_LEN)//ַ + { + printf("E2 len error!\r\n"); + } + else + { + m_tx_buf_u8[0] = FM25CL64_READ;// + m_tx_buf_u8[1] = (uint8_t)( add >> 8);// 8λַ + m_tx_buf_u8[2] = (uint8_t)( add );// + m_tx_buf_u8[3] = 0xFF;// + + if (sdrv_spi_sync_transmit(&g_master, m_tx_buf_u8, m_rx_buf_u8,4,4)) + { + printf("E2 write Failed!\r\n"); + } + + return m_rx_buf_u8[3]; + } +#endif + return 0xFF; +} + + +#ifdef _EEROM_CRC_ +#define POLY 0x1021 +/** +* Calculating CRC-16 in 'C' +* @para addr, start of data +* @para num, length of data +* @para crc, incoming CRC +*/ +unsigned int crc16(unsigned char *addr, int num, unsigned int crc) +{ + int i; + for (; num > 0; num--) /* Step through bytes in memory */ + { + crc = crc ^ ((*addr) << 8); /* Fetch byte from memory, XOR into CRC top byte*/ + addr++; + for (i = 0; i < 8; i++) /* Prepare to rotate 8 bits */ + { + if (crc & 0x8000) /* b15 is set... */ + crc = (crc << 1) ^ POLY; /* rotate and XOR with polynomic */ + else /* b15 is clear... */ + crc <<= 1; /* just rotate */ + } /* Loop for 8 bits */ + crc &= 0xFFFF; /* Ensure CRC remains 16-bit value */ + } /* Loop until num=0 */ + return(crc); /* Return updated CRC */ +} + +//дȷԶдһݣдͲд +unsigned char wrEE_CRC_Bak(unsigned char add,unsigned char *eeData,unsigned char len,unsigned char addbak) +{ + uword crc_Res; + unsigned char index; + unsigned char flagEEwrOK; + + crc_Res = crc16(eeData, len-2, 0xffff); //ݵУ + eeData[len-2] = (ubyte)(crc_Res ); + eeData[len-1] = (ubyte)(crc_Res >> 8); + + for(index=0;index +#include +#include +#include +#include +#include "sdrv_rstgen.h" +#include "interface_config.h" + + + +uint16_t FrameHeader = 0;//֡ͷ +bool boot_can_flag = false;//֡ͷ +bool boot_eth_flag = false;//֡ͷ + + +SystemDataRecord g_systemDataRecord = {0,0,0,0}; + +bool WDTReFresh_flag = false;// + +Timer boot_timer_interface; + + + +void bootmian(void *signal_id) +{ + static uint16_t bootmianSTT = 0; +// uint32_t time_boot = getCurrentTime();//¼ǰʱ +//------------------------------------------------------------------------------ + (void)signal_id; // DZΪʹã + + if( (true == boot_can_flag) || (true == boot_eth_flag) )//־жнպ1 + { + boot_can_flag = false; + boot_eth_flag = false; + if(EXCUTE_APP == FrameHeader)//д־0APP + { + printf("go to app!\r\n"); + wrbyte_24c02(Update_Flg_E2adr,NORMAL_ON); //д־λΪ0һγֱӽapp +// udelay(3000);//ʱ3ms + g_systemDataRecord.canBootloaderUpgrade = rdbyte_24c02(Update_Flg_E2adr); //ݷι + +// sdrv_rstgen_global_reset(&rstctl_glb); + for(;;) + { + printf("Restart!\r\n"); + } + } + else if(ENTER_UPDATE_MODE == FrameHeader)//д־1boot + { + ssdk_printf(SSDK_INFO, "go to boot!\r\n"); + wrbyte_24c02(Update_Flg_E2adr,CAN_BOOTLOADER_UPGRADE); //д־ +// udelay(3000);//ʱ3ms + //20250822 ޸ΪӾ޸IPҪⲿ޸IJ + wrbyte_24c02(BOOT_DES_IP, ethernet_parameter.download_ip[0]); + wrbyte_24c02(BOOT_DES_IP+1, ethernet_parameter.download_ip[1]); + wrbyte_24c02(BOOT_DES_IP+2, ethernet_parameter.download_ip[2]); + wrbyte_24c02(BOOT_DES_IP+3, ethernet_parameter.download_ip[3]); + +// sdrv_rstgen_global_reset(&rstctl_glb); + for(;;) + { + printf("Restart!\r\n"); + } + } + } + + //жϴe2ȡboot־󣬾ǷAPPʱι + if( CAN_BOOTLOADER_UPGRADE == (g_systemDataRecord.canBootloaderUpgrade) )//boot־ + { + //boot־Ϊ1ϵbootλι + if(true == WDTReFresh_flag)//յIDŸWDT յ־ι + { + feedWatchdog();//ι + WDTReFresh_flag = false; + } + } + else//APPԼι + { + bootmianSTT ++; + if(bootmianSTT >= 5)//5*1000*100us = 500ms + { + bootmianSTT = 0; + feedWatchdog();//ι + } + } + +// printf("bootrun !\r\n"); + + timerStart(&boot_timer_interface, 100,0); + +// printf("bootAPP spend time:%d\n",getCurrentTime() - time_boot);//app˶೤ʱ +} + + + +static uint32_t time_wdt = 0; +// ι +void feedWatchdog(void) +{ + sdrv_gpio_toggle_pin_output_level(WDTGPIO); //ⲿwdtι + printf("WDT:%d \n",getCurrentTime() - time_wdt); + time_wdt = getCurrentTime(); +} + + + + + + + +// APPģijʼ +void bootInterfaceInit(void) +{ + // ʼʱ + timerInit(&boot_timer_interface); + // Ķʱźţڶʱɼ + subscribe(&boot_timer_interface, bootmian); + + timerStart(&boot_timer_interface, 100,0); //100ms + + feedWatchdog();//ι,ʼιһ + + printf("bootInterface: initial OK %d\n",getCurrentTime()); +} + + + + + + + + + + + + diff --git a/interface_boot.h b/interface_boot.h new file mode 100644 index 0000000..9835496 --- /dev/null +++ b/interface_boot.h @@ -0,0 +1,69 @@ + +#ifndef __INTERFACE_BOOT_H_ +#define __INTERFACE_BOOT_H_ + +#include +#include +#include "stdint.h" +#include + + + + +//#define WDTGPIO GPIO_H13 +#define WDTGPIO GPIO_F5 + + + +typedef struct { + volatile uint8_t canBootloaderUpgrade; //CAN־λ + uint8_t test1; + uint8_t test2; + uint8_t test3; +} SystemDataRecord;//ṹAPPͬ + +typedef enum{ + NORMAL_ON = 0, + CAN_BOOTLOADER_UPGRADE, +}UpgradeFlag; + + +#define Update_Flg_E2adr 0x0 //ռ1ֽ +#define BOOT0_VERSION_E2adr 0x1 //ռ5ֽ +ʱ+汾 20 24 08 08 01 +#define BOOT1_VERSION_E2adr 0x6 //ռ5ֽ +ʱ+汾 20 24 08 08 01 +#define APP_VERSION_E2adr 0xB //ռ5ֽ +ʱ+汾 20 24 08 08 01 +#define BOOT_DES_IP 0x10 //ռ4ֽ IP 192.168.17.3 + + + + +#define ENTER_UPDATE_MODE 0x1 +#define READ_VERSION 0x100 +#define CRC_UPDATE 0x101 +#define Write_VERSION 0x103 +#define ENTER_APP 0x102 +#define EXCUTE_APP 0x02 +#define FEEDDOG_HEADER 0x200 //յιcan֡֡ͷ + +//ⲿ +void bootmian(void *signal_id); +void reboot_global(sdrv_rom_ctrl_boot_mode_e boot_mode); +void bootInterfaceInit(void); + + +//ⲿ +extern uint32_t OTA_CANTxID;//ĬϷIDΪ0x02 +extern uint32_t OTA_CANRxID;//ĬϽIDΪ0x01 +extern uint32_t WDT_CANRxID;//ĬϽIDΪ0x03 + +extern SystemDataRecord g_systemDataRecord ; +extern uint16_t FrameHeader;//֡ͷ +extern bool boot_can_flag;//֡ͷ +extern bool boot_eth_flag;//֡ͷ +//ι +extern void feedWatchdog(void); + + +#endif /*__INTERFACE_BOOT_H_*/ + + diff --git a/interface_btm.c b/interface_btm.c new file mode 100644 index 0000000..14f5390 --- /dev/null +++ b/interface_btm.c @@ -0,0 +1,99 @@ +#include "regs_base.h" +#include "irq_num.h" +#include "sdrv_btm.h" + +#include "interface_config.h" + +//app +#include "app/app_config.h" + +static sdrv_btm_t gstsdrv_btm_timer0; +static sdrv_btm_t gstsdrv_btm_timer1; + +volatile uint8_t Btm10ms = 0; + + +#define DEVICE_BASE(dev) _DEVICE_BASE(dev) +#define _DEVICE_BASE(dev) APB_##dev##_BASE + +#define DEVICE_INTR(dev) _DEVICE_INTR(dev) +#define _DEVICE_INTR(dev) dev##_INTR_NUM + + +/*SDRV_BTM_G0ʱԴAPB clock, APB clock150MHz*/ +/*SDRV_BTM_G0Ƶ֮clock1M*/ +static sdrv_btm_cfg_t timer0_config = { + .base = DEVICE_BASE(BTM2), + .irq = DEVICE_INTR(BTM2_O_BTM), + .tmr_id = SDRV_BTM_G0, + .tmr_cfg = { + .si_val = 74, /*ʱÿ(si_val + 1)cycle, G0inc_val*/ + .inc_val = 1, + .frc_rld_rst_cnt_en = true, /*ĬtrueʾʼʱǷcounter㡣*/ + .term_use_mode = SDRV_BTM_DIRECT, /*overflowֵЧǵȵһЧ*/ + .cmp_use_mode = SDRV_BTM_DIRECT, /*compareֵЧǵȵһЧ*/ + .cnt_dir = SDRV_BTM_CNT_UP, /*üֵ*/ + .cnt_mode = SDRV_BTM_CONTINOUS_MODE, /*üģʽ*/ + } +}; + + +/*SDRV_BTM_G1ʱԴⲿ񣬵24MHz*/ +/*SDRV_BTM_G1Ƶ֮clock1M*/ +static sdrv_btm_cfg_t timer1_config = { + .base = DEVICE_BASE(BTM2), + .irq = DEVICE_INTR(BTM2_O_BTM), + .tmr_id = SDRV_BTM_G1, + .tmr_cfg = { + .si_val = 23, + .inc_val = 1, + .frc_rld_rst_cnt_en = true, + .term_use_mode = SDRV_BTM_DIRECT, + .cmp_use_mode = SDRV_BTM_DIRECT, + .cnt_dir = SDRV_BTM_CNT_UP, + .cnt_mode = SDRV_BTM_CONTINOUS_MODE, + } +}; + +//ص10msʱ־ +static void timer1_handle(void *arg) +{ + static int timer1_count = 0; + timer1_count++; + + if(timer1_count >= 100) + { + timer1_count = 0; + Btm10ms = 1; + } + + //app + timerUpdateAll(); // öʱº + +} + + +/*g1*/ +int btm_init(void) +{ + sdrv_btm_init(&gstsdrv_btm_timer1, &timer0_config);//ʱ1ʼ + sdrv_btm_set_callback(&gstsdrv_btm_timer1, timer1_handle, &gstsdrv_btm_timer1);//趨ص + sdrv_btm_start(&gstsdrv_btm_timer1, BTM_TYPE_PERIOD, BTM_TIME_US, 1000);//趨1000usʱ + + sdrv_btm_init(&gstsdrv_btm_timer0, &timer1_config);//ʱ0ʼ + sdrv_btm_start(&gstsdrv_btm_timer0, BTM_TYPE_PERIOD, BTM_TIME_MS, ~0);//趨1msʱ + return 0; +} + +//ȡǰʱ,lwip +uint32_t current_time(void) +{ + return sdrv_btm_get_current_time(&gstsdrv_btm_timer0) / 1000; +} + +//ȡǰʱus +uint32_t getCurrentTime(void) +{ + return sdrv_btm_get_current_time(&gstsdrv_btm_timer0); +} + diff --git a/interface_btm.h b/interface_btm.h new file mode 100644 index 0000000..f96aa52 --- /dev/null +++ b/interface_btm.h @@ -0,0 +1,19 @@ +#ifndef __INTERFACE_BTM_H__ +#define __INTERFACE_BTM_H__ + +#include +#include +#include +#include "sdrv_btm.h" + +// +extern volatile uint8_t Btm10ms; + + +// +int btm_init(void); +extern uint32_t current_time(void); +extern uint32_t getCurrentTime(void); + + +#endif /* __BTM_INIT_H__ */ \ No newline at end of file diff --git a/interface_can.c b/interface_can.c new file mode 100644 index 0000000..cd9b72f --- /dev/null +++ b/interface_can.c @@ -0,0 +1,1426 @@ + +#include +#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" + + +uint32_t OTA_CANTxID = 0x02;//ĬϷIDΪ0x02 +uint32_t OTA_CANRxID = 0x01;//ĬϽIDΪ0x01 +uint32_t WDT_CANRxID = 0x03;//ĬϽIDΪ0x03 + + +Timer can_timer_interface1; +Timer can_timer_interface2; + + + +UnCanFault can_fault_info = { + .bit_data.navigator_count = 0, //Ǽ + .bit_data.motor1_count = 0, //ֶݼ + .bit_data.motor2_count = 0, //ֶݼ + .bit_data.bms_count = 0, //ֶݼ + .bit_data.temperature_count = 0, //¶ȼ + .bit_data.remote_count = 0, //ңؼ + + .bit_data.navigator_state = 0, //Զݼ + .bit_data.motor1_state = 0, //ֶݼ + .bit_data.motor2_state = 0, //ֶݼ + .bit_data.bms_state = 0, //ֶݼ + .bit_data.temperature_state = 0, //¶״̬ + .bit_data.remote_state = 0, //ң״̬ +}; + + + + +//StrCanFifoQueue can0_Queue;//8֡can棬Ͷ +//StrCanFifoQueue can1_Queue; +//StrCanFifoQueue can2_Queue; +//StrCanFifoQueue can3_Queue; +//StrCanFifoQueue can4_Queue; +//StrCanFifoQueue can5_Queue; +//StrCanFifoQueue can6_Queue; +//StrCanFifoQueue can7_Queue; + + + + +//bool busoff_occur_flag = false;//busoff־ + +bool can_0_busoff_flag = false;//busoff־ +bool can_1_busoff_flag = false;//busoff־ +bool can_2_busoff_flag = false;//busoff־ +bool can_3_busoff_flag = false;//busoff־ +bool can_4_busoff_flag = false;//busoff־ +bool can_5_busoff_flag = false;//busoff־ +bool can_6_busoff_flag = false;//busoff־ +bool can_7_busoff_flag = false;//busoff־ + + +flexcan_handle_t can_handle_0;//canʼṹ +flexcan_handle_t can_handle_1; +flexcan_handle_t can_handle_2; +flexcan_handle_t can_handle_3; +flexcan_handle_t can_handle_4; +flexcan_handle_t can_handle_5; +flexcan_handle_t can_handle_6; +flexcan_handle_t can_handle_7; + + + +static uint8_t fifo_Data_0[8]; +static flexcan_frame_t fifo_Buf_0; + +static uint8_t fifo_Data_1[8]; +static flexcan_frame_t fifo_Buf_1; + +static uint8_t fifo_Data_2[8]; +static flexcan_frame_t fifo_Buf_2; + +static uint8_t fifo_Data_3[8]; +static flexcan_frame_t fifo_Buf_3; + +static uint8_t fifo_Data_4[8]; +static flexcan_frame_t fifo_Buf_4; + +static uint8_t fifo_Data_5[8]; +static flexcan_frame_t fifo_Buf_5; + +static uint8_t fifo_Data_6[8]; +static flexcan_frame_t fifo_Buf_6; + +static uint8_t fifo_Data_7[8]; +static flexcan_frame_t fifo_Buf_7; + + +uint8_t flexcan_config_rx_fifo(flexcan_handle_t *handle, + flexcan_rx_fifo_config_t *config) +{ + flexcan_frame_t *rx_fifo_frame; +//------------------------------------------------------------------------------ + if(CAN_INDEX_0 == (handle->controller_id))//ѡͬжϺԼַ + { + rx_fifo_frame = &fifo_Buf_0; + rx_fifo_frame->dataBuffer = &fifo_Data_0[0]; + } + else if(CAN_INDEX_1 == (handle->controller_id)) + { + rx_fifo_frame = &fifo_Buf_1; + rx_fifo_frame->dataBuffer = &fifo_Data_1[0]; + } + else if(CAN_INDEX_2 == (handle->controller_id)) + { + rx_fifo_frame = &fifo_Buf_2; + rx_fifo_frame->dataBuffer = &fifo_Data_2[0]; + } + else if(CAN_INDEX_3 == (handle->controller_id)) + { + rx_fifo_frame = &fifo_Buf_3; + rx_fifo_frame->dataBuffer = &fifo_Data_3[0]; + } + else if(CAN_INDEX_4 == (handle->controller_id)) + { + rx_fifo_frame = &fifo_Buf_4; + rx_fifo_frame->dataBuffer = &fifo_Data_4[0]; + } + else if(CAN_INDEX_5 == (handle->controller_id)) + { + rx_fifo_frame = &fifo_Buf_5; + rx_fifo_frame->dataBuffer = &fifo_Data_5[0]; + } + else if(CAN_INDEX_6 == (handle->controller_id)) + { + rx_fifo_frame = &fifo_Buf_6; + rx_fifo_frame->dataBuffer = &fifo_Data_6[0]; + } + else if(CAN_INDEX_7 == (handle->controller_id)) + { + rx_fifo_frame = &fifo_Buf_7; + rx_fifo_frame->dataBuffer = &fifo_Data_7[0]; + } + else + { + return 0; + } + + flexcan_set_rx_fifo_config(handle, config);//fifo + + for (uint8_t i = 0; i < USED_MB_FOR_FIFO; + i++) { + flexcan_set_rx_individual_mask(handle, i, config->filter_tab[i].filter_mask); + } + + /* rx by interrupt */ + flexcan_fifo_transfer_t Xfer = {rx_fifo_frame}; + flexcan_receive_fifo_nonblocking(handle, &Xfer); + + return 1; +} + + +/********************************************************************* + * @fn CAN_Send_Msg + * + * @brief CAN Transmit function. + * + * @param CANx- CAN1 or CAN2 + * ID - Message ID + * IDE - CAN_Id_Standard or CAN_Id_Extended + * RTR - CAN_RTR_Data or CAN_RTR_Remote + * msg - Transmit data buffer. + * len - Data length. + * + * @return 0 - Send successful. + * 1 - Send failed. + */ +uint8_t CAN_Send_Msg( flexcan_handle_t *handle, uint32_t ID, flexcan_frame_format_e ide, flexcan_frame_type_e rtr, uint8_t *msg, uint8_t len, uint8_t tx_index) +{ + uint16_t i = 0; +//------------------------------------------------------------------------------ + + flexcan_frame_t frame = {.id = ID, + .length = len, + .type = rtr, + .format = ide, + .dataBuffer = msg}; + flexcan_mb_transfer_t xfer = {&frame, tx_index}; + + while( ( flexcan_get_mb_state(handle, tx_index) != FLEXCAN_StateIdle ) && (i < 0xFFF) ) + { + i++; + } + + if( i == 0xFFF ) + { + return 1; + + } + else + { + flexcan_send_nonblocking(handle, &xfer, TX_PADDING_VAL); + return 0; + } +} + + + +/********************************************************************* + * @fn flexcan_Receive_callback + * + * @brief erase Data-Flash block, minimal block is 256B + * + * @param Page_Address - the address of the page being erased. + * Length - Erased data length + * + * @return none + */ +void flexcan_Receive_callback_0(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) + { + case FLEXCAN_RX_IDLE: + + break; + + case FLEXCAN_RX_FIFO_IDLE: + if(TEMP_MODULE_INPUT_ID_1 == (buf->id))//1 + { + for(i = 0; i < (buf->length); i++) + { + un_sw_sample.arr[i] = buf->dataBuffer[i]; + } + //ź + publishMessage(&un_sw_sample, 1);//ź + } + break; + + case FLEXCAN_TX_IDLE: + break; + + case FLEXCAN_WAKEUP_TIMEOUT: + break; + + case FLEXCAN_WAKEUP_MATCH: + break; + + case FLEXCAN_ERROR_STATUS: + can_0_busoff_flag = true; + break; + + default: + break; + } +} + + + + + + +/********************************************************************* + * @fn flexcan_Receive_callback + * + * @brief erase Data-Flash block, minimal block is 256B + * + * @param Page_Address - the address of the page being erased. + * Length - Erased data length + * + * @return none + */ + +uint32_t time_elapsed = 0; +uint32_t time_elapsed1 = 0; +void flexcan_Receive_callback_1(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; + static uint32_t start_time = 0; +//-------------------------------------------------------------- + switch (status) + { + case FLEXCAN_RX_IDLE: + break; + + case FLEXCAN_RX_FIFO_IDLE: + if(MOTOR_INPUT_ID_1 == (buf->id))//1 + { + time_elapsed = getCurrentTime() - start_time; + start_time = getCurrentTime(); + + 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); + +// CAN_Send_Msg(&can_handle_1, 0x11111111, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_input1, 8, 18);// + } + else if(MOTOR_INPUT_ID_3 == (buf->id)) + { +// can_fault_info.bit_data.motor1_count ++; + for(i = 0; i < (buf->length); i++) + { + un_motor_input3.arr[i] = buf->dataBuffer[i]; + } + //ź + publishMessage(&un_motor_input3, 1); +// CAN_Send_Msg(&can_handle_1, 0x11111113, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_input3, 8, 18);// + } + else if(MOTOR_INPUT_ID_5 == (buf->id))//յλ + { + for(i = 0; i < (buf->length); i++) + { + un_motor_input1.arr[i+8] = buf->dataBuffer[i]; + } +// CAN_Send_Msg(&can_handle_1, 0x11111115, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_input1.arr[8], 8, 18);// + + } + else if(MOTOR_INPUT_ID_7 == (buf->id))//յλ + { + for(i = 0; i < (buf->length); i++) + { + un_motor_input3.arr[i+8] = buf->dataBuffer[i]; + +// CAN_Send_Msg(&can_handle_1, 0x11111117, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_input3.arr[8], 8, 18);// + } + } + else{} + break; + + case FLEXCAN_TX_IDLE: + break; + + case FLEXCAN_WAKEUP_TIMEOUT: + break; + + case FLEXCAN_WAKEUP_MATCH: + break; + + case FLEXCAN_ERROR_STATUS: + can_1_busoff_flag = true; + break; + + default: + break; + } +} + + + +/********************************************************************* + * @fn flexcan_Receive_callback + * + * @brief erase Data-Flash block, minimal block is 256B + * + * @param Page_Address - the address of the page being erased. + * Length - Erased data length + * + * @return none + */ +void flexcan_Receive_callback_2(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; + static uint32_t start_time1 = 0; +//-------------------------------------------------------------- + switch (status) + { + case FLEXCAN_RX_IDLE: + break; + + case FLEXCAN_RX_FIFO_IDLE: + if(MOTOR_INPUT_ID_2 == (buf->id))//2 + { + time_elapsed1 = getCurrentTime() - start_time1; + start_time1 = getCurrentTime(); + + 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); + +// CAN_Send_Msg(&can_handle_2, 0x11111112, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_input2, 8, 18);// + + } + else if(MOTOR_INPUT_ID_4 == (buf->id)) + { + + for(i = 0; i < (buf->length); i++) + { + un_motor_input4.arr[i] = buf->dataBuffer[i]; + } + //ź + publishMessage(&un_motor_input4, 1); + +// CAN_Send_Msg(&can_handle_2, 0x11111114, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_input4, 8, 18);// + + } + else if(MOTOR_INPUT_ID_6 == (buf->id))//յλ + { + for(i = 0; i < (buf->length); i++) + { + un_motor_input2.arr[i+8] = buf->dataBuffer[i]; + } + +// CAN_Send_Msg(&can_handle_2, 0x11111116, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_input2.arr[8], 8, 18);// + + } + else if(MOTOR_INPUT_ID_8 == (buf->id))//յλ + { + for(i = 0; i < (buf->length); i++) + { + un_motor_input4.arr[i+8] = buf->dataBuffer[i]; + } +// CAN_Send_Msg(&can_handle_2, 0x11111118, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_input4.arr[8], 8, 18);// + + } + else{} + break; + + case FLEXCAN_TX_IDLE: + break; + + case FLEXCAN_WAKEUP_TIMEOUT: + break; + + case FLEXCAN_WAKEUP_MATCH: + break; + + case FLEXCAN_ERROR_STATUS: + can_2_busoff_flag = true; + break; + + default: + break; + } +} + + + +/********************************************************************* + * @fn flexcan_Receive_callback + * + * @brief erase Data-Flash block, minimal block is 256B + * + * @param Page_Address - the address of the page being erased. + * Length - Erased data length + * + * @return none + */ +void flexcan_Receive_callback_3(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) + { + case FLEXCAN_RX_IDLE: + break; + + case FLEXCAN_RX_FIFO_IDLE: + + if(BMS_INPUT_ID1 == (buf->id))//BMS + { + can_fault_info.bit_data.bms_count ++; + 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 if(0x701 == (buf->id))//תϵᷢһ֡0x701 +// { +// un_sdo_output6.bit_data.cmd = 1; +// publishMessage(&un_sdo_output6, 1); +// } + else{} + + break; + + case FLEXCAN_TX_IDLE: + break; + + case FLEXCAN_WAKEUP_TIMEOUT: + break; + + case FLEXCAN_WAKEUP_MATCH: + break; + + case FLEXCAN_ERROR_STATUS: + can_3_busoff_flag = true; + break; + + default: + break; + } +} + +/********************************************************************* + * @fn flexcan_Receive_callback + * + * @brief erase Data-Flash block, minimal block is 256B + * + * @param Page_Address - the address of the page being erased. + * Length - Erased data length + * + * @return none + */ +void flexcan_Receive_callback_4(flexcan_handle_t *handle, + flexcan_status_e status, uint32_t result, + void *userData) +{ +//-------------------------------------------------------------- + switch (status) + { + case FLEXCAN_RX_IDLE: + break; + + case FLEXCAN_RX_FIFO_IDLE: + + break; + + case FLEXCAN_TX_IDLE: + break; + + case FLEXCAN_WAKEUP_TIMEOUT: + break; + + case FLEXCAN_WAKEUP_MATCH: + break; + + case FLEXCAN_ERROR_STATUS: + can_4_busoff_flag = true; + break; + + default: + break; + } +} + +/********************************************************************* + * @fn flexcan_Receive_callback + * + * @brief erase Data-Flash block, minimal block is 256B + * + * @param Page_Address - the address of the page being erased. + * Length - Erased data length + * + * @return none + */ +void flexcan_Receive_callback_5(flexcan_handle_t *handle, + flexcan_status_e status, uint32_t result, + void *userData) +{ +//-------------------------------------------------------------- + switch (status) + { + case FLEXCAN_RX_IDLE: + break; + + case FLEXCAN_RX_FIFO_IDLE: + + break; + + case FLEXCAN_TX_IDLE: + break; + + case FLEXCAN_WAKEUP_TIMEOUT: + break; + + case FLEXCAN_WAKEUP_MATCH: + break; + + case FLEXCAN_ERROR_STATUS: + can_5_busoff_flag = true; + break; + + default: + break; + } +} + +/********************************************************************* + * @fn flexcan_Receive_callback + * + * @brief erase Data-Flash block, minimal block is 256B + * + * @param Page_Address - the address of the page being erased. + * Length - Erased data length + * + * @return none + */ +void flexcan_Receive_callback_6(flexcan_handle_t *handle, + flexcan_status_e status, uint32_t result, + void *userData) +{ + flexcan_frame_t *buf = (flexcan_frame_t *)userData; +//-------------------------------------------------------------- + switch (status) + { + case FLEXCAN_RX_IDLE: + break; + + case FLEXCAN_RX_FIFO_IDLE: + if(OTA_CANRxID == (buf->id))//IDΪ1 boot + { + boot_can_flag = true; + FrameHeader = ( (buf->dataBuffer[0] << 8) | (buf->dataBuffer[1]) ); + } + else if(WDT_CANRxID == (buf->id))//յιID + { + if(FEEDDOG_HEADER == ( (buf->dataBuffer[0] << 8) | (buf->dataBuffer[1]) )) + { + WDTReFresh_flag = true;//ι־ + printf("Feed dog flag received %d\n",getCurrentTime()); + } + } + else + { + } + break; + + case FLEXCAN_TX_IDLE: + break; + + case FLEXCAN_WAKEUP_TIMEOUT: + break; + + case FLEXCAN_WAKEUP_MATCH: + break; + + case FLEXCAN_ERROR_STATUS: + can_6_busoff_flag = true; + break; + + default: + break; + } +} + + + +/********************************************************************* + * @fn flexcan_Receive_callback + * + * @brief erase Data-Flash block, minimal block is 256B + * + * @param Page_Address - the address of the page being erased. + * Length - Erased data length + * + * @return none + */ +void flexcan_Receive_callback_7(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; + //uintptr_t +//-------------------------------------------------------------- + switch (status) + { + case FLEXCAN_RX_IDLE: + break; + + case FLEXCAN_RX_FIFO_IDLE: +// if(TEMP_MODULE_INPUT_ID_1 == (buf->id))//¶Ȳɼ1 +// { +// can_fault_info.bit_data.temperature_count ++; +// for(i = 0; i < (buf->length); i++) +// { +// un_temp_module_input.arr[i] = buf->dataBuffer[i]; +// } +// //ź +// publishMessage(&un_temp_module_input, 1); +// } +// else if(TEMP_MODULE_INPUT_ID_2 == (buf->id))//¶Ȳɼ2 +// { +// for(i = 0; i < (buf->length); i++) +// { +// un_temp_module_input.arr[i+8] = buf->dataBuffer[i]; +// } +// //ź ֻһֻҪǰһ֡ +//// publishMessage(&un_temp_module_input, 1); +// } + if(REMOTE_ID == (buf->id))//ңCAN + { + can_fault_info.bit_data.remote_count ++; + for(i = 0; i < (buf->length); i++) + { + un_remote_control_input.arr[i] = buf->dataBuffer[i]; + } + //ź + publishMessage(&un_remote_control_input, 1); + } + else if(REMOTE_ID1 == (buf->id)) + { + can_fault_info.bit_data.remote_count ++; + for(i = 0; i < (buf->length); i++) + { + un_remote_control_input1.arr[i] = buf->dataBuffer[i]; + } +// printf("remote_input1 recive\n"); + publishMessage(&un_remote_control_input1, 1); //ź + } + else + { + } + break; + + case FLEXCAN_TX_IDLE: + break; + + case FLEXCAN_WAKEUP_TIMEOUT: + break; + + case FLEXCAN_WAKEUP_MATCH: + break; + + case FLEXCAN_ERROR_STATUS: + can_7_busoff_flag = true; + break; + + default: + break; + } +} + + +//fifo_config ˲ +//can_config Լ +//call_back жϻص +uint8_t initialization_Flexcan(uint8_t can_index, const flexcan_config_t *can_config, flexcan_rx_fifo_config_t *fifo_config, flexcan_transfer_callback_t call_back) +{ + flexcan_handle_t *handle; + uint8_t can_irq_num = 0; + uint32_t can_reg_base = 0; +//------------------------------------------------------------------ + if(FLEXCAN7 == can_index)//ѡͬжϺԼַ + { + handle = &can_handle_0; + can_irq_num = CANFD7_CANFD_INTR_NUM; + can_reg_base = APB_CANFD7_BASE; + } + else if(FLEXCAN21 == can_index) + { + handle = &can_handle_1; + can_irq_num = CANFD21_CANFD_INTR_NUM; + can_reg_base = APB_CANFD21_BASE; + } + else if(FLEXCAN16 == can_index) + { + handle = &can_handle_2; + can_irq_num = CANFD16_CANFD_INTR_NUM; + can_reg_base = APB_CANFD16_BASE; + } + else if(FLEXCAN3 == can_index) + { + handle = &can_handle_3; + can_irq_num = CANFD3_CANFD_INTR_NUM; + can_reg_base = APB_CANFD3_BASE; + } + else if(FLEXCAN23 == can_index) + { + handle = &can_handle_4; + can_irq_num = CANFD23_CANFD_INTR_NUM; + can_reg_base = APB_CANFD23_BASE; + } + else if(FLEXCAN5 == can_index) + { + handle = &can_handle_5; + can_irq_num = CANFD5_CANFD_INTR_NUM; + can_reg_base = APB_CANFD5_BASE; + } + else if(FLEXCAN6 == can_index) + { + handle = &can_handle_6; + can_irq_num = CANFD6_CANFD_INTR_NUM; + can_reg_base = APB_CANFD6_BASE; + } + else if(FLEXCAN4 == can_index) + { + handle = &can_handle_7; + can_irq_num = CANFD4_CANFD_INTR_NUM; + can_reg_base = APB_CANFD4_BASE; + } + else + { + return 0; + } + + //initialize the FlexCAN handle + flexcan_create_handle(handle, can_index, (void *)can_reg_base, can_irq_num, call_back, NULL); + + /* initialize flexCAN. */ + flexcan_init(handle, can_config); + + /* unfreeze the flexCAN by demo. */ + flexcan_freeze(handle, false); + + /* enable busoff interrupt. */ + flexcan_enable_interrupts(handle, FLEXCAN_BusOffInterruptEnable);// + + /* enable irq. */ + irq_attach(handle->irq_num, flexcan_irq_handler, handle); + irq_enable(handle->irq_num); + + /* assign fifo only for rx. */ + flexcan_config_rx_fifo(handle, fifo_config); + + /* assign mailbox14-20 for tx. */ + for (uint8_t i = TX_MB_INDEX; i < (TX_MB_INDEX + TX_MAILBOX_NUM); i++) + { + flexcan_set_tx_mb_config(handle, i, true); + } + + return 1; +} + +/* reinitialize the controller. */ +void flexcan_Busoff_Recovery(void) +{ +//---------------------------------------------------------- + if (can_0_busoff_flag == true) //busoff + { + printf("can0 busoff !\n"); + flexcan_deinit(&can_handle_0); + + initialization_Flexcan(CAN_INDEX_0, &can_0_config, &can_0_fifo_config, flexcan_Receive_callback_0); + + can_0_busoff_flag = false; + } + + if (can_1_busoff_flag == true) //busoff + { + printf("can1 busoff !\n"); + flexcan_deinit(&can_handle_1); + + initialization_Flexcan(CAN_INDEX_1, &can_1_config, &can_1_fifo_config, flexcan_Receive_callback_1); + + can_1_busoff_flag = false; + } + + if (can_2_busoff_flag == true) //busoff + { + printf("can2 busoff !\n"); + flexcan_deinit(&can_handle_2); + + initialization_Flexcan(CAN_INDEX_2, &can_2_config, &can_2_fifo_config, flexcan_Receive_callback_2); + + can_2_busoff_flag = false; + } + + if (can_3_busoff_flag == true) //busoff + { + printf("can3 busoff !\n"); + flexcan_deinit(&can_handle_3); + + initialization_Flexcan(CAN_INDEX_3, &can_3_config, &can_3_fifo_config, flexcan_Receive_callback_3); + + can_3_busoff_flag = false; + } + + if (can_4_busoff_flag == true) //busoff + { + printf("can4 busoff !\n"); + flexcan_deinit(&can_handle_4); + + initialization_Flexcan(CAN_INDEX_4, &can_4_config, &can_4_fifo_config, flexcan_Receive_callback_4); + + can_4_busoff_flag = false; + } + + if (can_5_busoff_flag == true) //busoff + { + printf("can5 busoff !\n"); + flexcan_deinit(&can_handle_5); + + initialization_Flexcan(CAN_INDEX_5, &can_5_config, &can_5_fifo_config, flexcan_Receive_callback_5); + + can_5_busoff_flag = false; + } + + if (can_6_busoff_flag == true) //busoff + { + printf("can6 busoff !\n"); + flexcan_deinit(&can_handle_6); + + initialization_Flexcan(CAN_INDEX_6, &can_6_config, &can_6_fifo_config, flexcan_Receive_callback_6); + + can_6_busoff_flag = false; + } + + if (can_7_busoff_flag == true) //busoff + { + printf("can7 busoff !\n"); + flexcan_deinit(&can_handle_7); + + initialization_Flexcan(CAN_INDEX_7, &can_7_config, &can_7_fifo_config, flexcan_Receive_callback_7); + + can_7_busoff_flag = false; + } +} + + + +/* initialization_all_flexcan. */ +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_1, &can_1_config, &can_1_fifo_config, flexcan_Receive_callback_1);//250k + + initialization_Flexcan(CAN_INDEX_2, &can_2_config, &can_2_fifo_config, flexcan_Receive_callback_2);//250k + + initialization_Flexcan(CAN_INDEX_3, &can_3_config, &can_3_fifo_config, flexcan_Receive_callback_3);//1M + + initialization_Flexcan(CAN_INDEX_4, &can_4_config, &can_4_fifo_config, flexcan_Receive_callback_4);//250k + + initialization_Flexcan(CAN_INDEX_5, &can_5_config, &can_5_fifo_config, flexcan_Receive_callback_5);//250k + + initialization_Flexcan(CAN_INDEX_6, &can_6_config, &can_6_fifo_config, flexcan_Receive_callback_6);//1M + + initialization_Flexcan(CAN_INDEX_7, &can_7_config, &can_7_fifo_config, flexcan_Receive_callback_7);//250k +} + + + + + + +//100ms һ +void canTimerProcess(void *signal_id) +{ + static uint8_t can_timer[5] = {0,0,0,0,0}; + static uint8_t can_temp[10] = {0,0,0,0,0,0,0,0,0,0};//мжֵ + uint8_t temp = 0; + + can_timer[0] ++; + if(can_timer[0] >= 1)//100msжһ + { + can_timer[0] = 0; + + if(can_fault_info.bit_data.motor1_count == can_temp[0])//һʾ + { + can_fault_info.bit_data.motor1_state = FAULT; + } + else + { + can_fault_info.bit_data.motor1_state = NORMAL; + can_temp[0] = can_fault_info.bit_data.motor1_count;//ݸ + } + + if(can_fault_info.bit_data.motor1_state != can_temp[1]) + { + can_temp[1] = can_fault_info.bit_data.motor1_state; + temp ++; + } + + + if(can_fault_info.bit_data.motor2_count == can_temp[2])//һʾ + { + can_fault_info.bit_data.motor2_state = FAULT; + } + else + { + can_fault_info.bit_data.motor2_state = NORMAL; + can_temp[2] = can_fault_info.bit_data.motor2_count;//ݸ + } + + if(can_fault_info.bit_data.motor2_state != can_temp[3]) + { + can_temp[3] = can_fault_info.bit_data.motor2_state; + temp ++; + } + } + + can_timer[1] ++;//BMSж + if(can_timer[1] >= 20)//2sжһ + { + can_timer[1] = 0; + //BMS + if(can_fault_info.bit_data.bms_count == can_temp[4])//һʾ + { + can_fault_info.bit_data.bms_state = FAULT; + } + else + { + can_fault_info.bit_data.bms_state = NORMAL; + can_temp[4] = can_fault_info.bit_data.bms_count;//ݸ + } + if(can_fault_info.bit_data.bms_state != can_temp[5]) + { + can_temp[5] = can_fault_info.bit_data.bms_state; + temp ++; + } + + //¶ģ + if(can_fault_info.bit_data.temperature_count == can_temp[6])//һʾ + { + can_fault_info.bit_data.temperature_state = FAULT; + } + else + { + can_fault_info.bit_data.temperature_state = NORMAL; + can_temp[6] = can_fault_info.bit_data.temperature_count;//ݸ + } + if(can_fault_info.bit_data.temperature_state != can_temp[7]) + { + can_temp[7] = can_fault_info.bit_data.temperature_state; + temp ++; + } + } + + can_timer[2] ++;//ңж + if(can_timer[2] >= 5)//500msжһ + { + can_timer[2] = 0; + //ң + if(can_fault_info.bit_data.remote_count == can_temp[8])//һʾ + { + can_fault_info.bit_data.remote_state = FAULT; + } + else + { + can_fault_info.bit_data.remote_state = NORMAL; + can_temp[8] = can_fault_info.bit_data.remote_count;//ݸ + } + if(can_fault_info.bit_data.remote_state != can_temp[9]) + { + can_temp[9] = can_fault_info.bit_data.remote_state; + temp ++; + } + } + + can_timer[4] ++;//ڷź + if( (0 != temp) || (can_timer[4] >= 5) )//500źţ仯 + { + can_timer[4] = 0; + publishMessage(&can_fault_info, 1);// + } + + //busoffж + can_timer[3] ++; + if(can_timer[3] >= 10)//100*100*10 = 1sһ + { + can_timer[3] = 0; + + flexcan_Busoff_Recovery();//BUSS OFF + } +} + + +static void processSdoOutput1(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_3, TURN_MOTOR_WRITE, FLEXCAN_EXTEND_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, TURN_MOTOR_WRITE, FLEXCAN_EXTEND_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, TURN_MOTOR_ENABLE, FLEXCAN_EXTEND_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, TURN_MOTOR_WRITE, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output4, 8, 17); +} + +static void processSdoOutput5(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_3, RIGHT_MOTOR_WRITE, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output5, 8, 18); +} + +static void processSdoOutput6(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_3, RIGHT_MOTOR_WRITE, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output6, 8, 19); +} + +static void processSdoOutput7(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_3, RIGHT_MOTOR_ENABLE, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output7, 8, 20); +} + +static void processSdoOutput8(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_3, RIGHT_MOTOR_WRITE, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output8, 8, 20); //can_handle_3 +} + + +static void processMotorOutput1(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_1, 0x10F81807, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1, 8, 15);//1Ťغת + CAN_Send_Msg(&can_handle_1, 0x10F83807, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output3, 8, 16);// +} + +static void processMotorOutput2(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_2, 0x10F82807, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2, 8, 15);//2Ť ת + CAN_Send_Msg(&can_handle_2, 0x10F84807, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output4, 8, 16);// +} + + +static void processKgfOutput1(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_0, 0x11000002, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_inf_can_kgf_output1, 8, 15);//2Ťغת +} + +static void processKgfOutput2(void *signal_id) +{ + (void)signal_id; // DZΪʹã + 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);//2Ťغת +} + +static void processWheelSpeedOutput(void *signal_id) +{ + float CANPressSpeedTemp = 0.0;//ٶȼмֵ + (void)signal_id; // DZΪʹã + + + CANPressSpeedTemp = (diff_data.left_motor_speed*(float)getParam("whl_dia") * M_PI*60)/1000.0/(float)getParam("gRatio");// ת/ӡkm/h һȦ1.8 20241016 Լٱ + CANPressSpeedTemp = CANPressSpeedTemp*100;//,Ҫ100 ,ϵ0.01 + un_wheel_wpeed_output.bit_data.left_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.left_rear = (uint16_t)((int16_t)(CANPressSpeedTemp));//תΪintתΪ޷ + + CANPressSpeedTemp = (diff_data.right_motor_speed*(float)getParam("whl_dia") * M_PI*60)/1000.0/(float)getParam("gRatio");// ת/ӡkm/h һȦ1.8 + CANPressSpeedTemp = CANPressSpeedTemp*100;//,Ҫ100 ,ϵ0.01 + 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);// +} + +static void processHBridgeOutput(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_0, 0x7F2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_h_bridge_output, 8, 17);//H +} + +static void processHBridgeOutput1(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_5, 0x712, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_h_bridge_output1, 8, 15);//H +} + +static void processHBridgeOutput2(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_5, 0x722, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_h_bridge_output2, 8, 16);//H +} + + +static void processLifterOutput(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_5, 0x6F2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_lifter_output, 8, 17);//Ƹ +} + +static void processUnGatherOutput(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_0, 0x15000002, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_gather_output, 8, 18);//ɼ +} + + +//can 100msһ +void canSendAll(void *signal_id) +{ + static uint16_t wheel_speed_cnt = 0; +// static uint8_t kgf_cnt = 0; +// static uint8_t bms_cnt1 = 0; +// static uint8_t bms_cnt2 = 0; + static uint16_t motor_speed_cnt = 0; + static uint16_t motor_power_cnt = 0; +// static uint8_t h_bridge_cnt = 0; + uint8_t CanData[8] = {0,0,0,0,0,0,0,0}; +//------------------------------------------------------------------------- + (void)signal_id; // DZΪʹã + +// bms_cnt1 ++; +// if(bms_cnt1 >= 1000)//1s +// { +// bms_cnt1 = 0; +// +// CanData[0] = 0x5A; +// +// CAN_Send_Msg(&can_handle_3, 0x100, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 15);//BMS +// } +// +// bms_cnt2 ++; +// if(bms_cnt2 >= 9)//1s +// { +// bms_cnt2 = 0; +// +// CanData[0] = 0x5A; +// +// CAN_Send_Msg(&can_handle_3, 0x101, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 16);//BMS +// } + + + motor_speed_cnt ++; + if(motor_speed_cnt >= 200)//1msһ + { + motor_speed_cnt = 0; + //дߵת + processSdoOutput1(CanData); +// //дұߵת + processSdoOutput2(CanData); +//// дߵת + processSdoOutput5(CanData); +// //дұߵת + processSdoOutput6(CanData); + + } + + motor_power_cnt ++; + if(motor_power_cnt >= 1000)//д 1s + { + motor_power_cnt = 0; +// processMotorOutput3(CanData); +// processMotorOutput4(CanData); + } + +// kgf_cnt ++; +// if(kgf_cnt >= 5)//20210312 CANͨѶģ 20210710޸Ϊ50ms +// { +// kgf_cnt = 0; +// +// processKgfOutput1(CanData); +// processKgfOutput2(CanData); +// } +// + //ٷ + wheel_speed_cnt ++; + if(wheel_speed_cnt >= 500) + { + wheel_speed_cnt = 0; + +// processWheelSpeedOutput(CanData); + +// CAN_Send_Msg(&can_handle_0, 0x122, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 16);//BMS +// CAN_Send_Msg(&can_handle_1, 0x122, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 16);//BMS +// CAN_Send_Msg(&can_handle_2, 0x122, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 16);//BMS +// CAN_Send_Msg(&can_handle_3, 0x122, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 16);//BMS +// CAN_Send_Msg(&can_handle_4, 0x122, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 16);//BMS +// CAN_Send_Msg(&can_handle_5, 0x122, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 16);//BMS +// CAN_Send_Msg(&can_handle_6, 0x122, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 16);//BMS +// CAN_Send_Msg(&can_handle_7, 0x122, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 16);//BMS + } +// + + //Hݷ +// h_bridge_cnt ++; +// if(h_bridge_cnt >= 5) +// { +// h_bridge_cnt = 0; +// +// processHBridgeOutput(CanData); +// } + + timerStart(&can_timer_interface1,1,1);//1msһ +} + + + + +// canʱźŴ +static void canInterfaceTimerProcess(void *signal_id) +{ + (void)signal_id; // DZΪʹã +// uint32_t time_boot = getCurrentTime();//¼ǰʱ + + canTimerProcess(signal_id); + +// printf("motor1 interval:%d,motor2 interval:%d\n",time_elapsed,time_elapsed1);//ӡ12ռ + + timerStart(&can_timer_interface2,100,1);//100msһ + +// printf("canInterface spend time:%d\n",getCurrentTime() - time_boot);//app˶೤ʱ +} + + + + +// APPģijʼ +void canInterfaceInit(void) +{ + memset(&un_motor_input1, 0, sizeof(UnMotorInput)); + memset(&un_motor_input2, 0, sizeof(UnMotorInput)); + memset(&un_bms_input , 0, sizeof(UnBmsInput)); + memset(&un_temp_module_input, 0, sizeof(UnTempModuleInput)); + + memset(&un_motor_output1, 0, sizeof(UnMotorOutput)); + memset(&un_motor_output2, 0, sizeof(UnMotorOutput)); + + un_motor_output1.bit_data.mode = 0x05; + un_motor_output1.bit_data.gear = 0x01; + un_motor_output1.bit_data.can_gear = 0x01; + un_motor_output1.bit_data.motor_direction = 0x01; + un_motor_output1.bit_data.control_data1 = 0x0; + un_motor_output1.bit_data.control_data2 = 0x0; + + un_motor_output2.bit_data.mode = 0x05; + un_motor_output2.bit_data.gear = 0x01; + un_motor_output2.bit_data.can_gear = 0x01; + un_motor_output2.bit_data.motor_direction = 0x00; + un_motor_output2.bit_data.control_data1 = 0x0; + un_motor_output2.bit_data.control_data2 = 0x0; + + un_motor_output3.bit_data.mode = 0x05; + un_motor_output3.bit_data.gear = 0x01; + un_motor_output3.bit_data.can_gear = 0x01; + un_motor_output3.bit_data.motor_direction = 0x01; + un_motor_output3.bit_data.control_data1 = 0x0; + un_motor_output3.bit_data.control_data2 = 0x0; + + un_motor_output4.bit_data.mode = 0x05; + un_motor_output4.bit_data.gear = 0x01; + un_motor_output4.bit_data.can_gear = 0x01; + un_motor_output4.bit_data.motor_direction = 0x00; + un_motor_output4.bit_data.control_data1 = 0x0; + un_motor_output4.bit_data.control_data2 = 0x0; + + + +// un_motor_output1.bit_data.set_torque = 30000;//ƫʼ +// un_motor_output1.bit_data.set_rotation_speed = 30000; +// un_motor_output1.bit_data.mode = 0; +// un_motor_output2.bit_data.set_torque = 30000; +// un_motor_output2.bit_data.set_rotation_speed = 30000; +// un_motor_output2.bit_data.mode = 0; + can_fault_info.bit_data.navigator_state = 1; + + memset(&un_inf_can_kgf_output1, 0, sizeof(UnInfCanKGFOutput)); + memset(&un_inf_can_kgf_output2, 0, sizeof(UnInfCanKGFOutput)); + memset(&un_h_bridge_output, 0, sizeof(UnHBridgeOutput)); + memset(&un_wheel_wpeed_output, 0, sizeof(UnWheelSpeedOutput)); + memset(&un_remote_control_input, 0, sizeof(UnRemoteControlInput)); + + + // ʼʱʹ brake_timer ĵַΪźID + timerInit(&can_timer_interface1); + timerInit(&can_timer_interface2); + + subscribe(&can_timer_interface1, canSendAll); + subscribe(&can_timer_interface2, canInterfaceTimerProcess); + subscribe(&un_motor_output1, processMotorOutput1); + subscribe(&un_motor_output2, processMotorOutput2); + subscribe(&un_inf_can_kgf_output1, processKgfOutput1); + subscribe(&un_inf_can_kgf_output2, processKgfOutput2); + subscribe(&un_wheel_wpeed_output, processWheelSpeedOutput); + subscribe(&un_h_bridge_output, processHBridgeOutput); + subscribe(&un_h_bridge_output1, processHBridgeOutput1); + subscribe(&un_h_bridge_output2, processHBridgeOutput2); + subscribe(&un_lifter_output, processLifterOutput); + subscribe(&un_gather_output, processUnGatherOutput); + + subscribe(&un_sdo_output1, processSdoOutput1); + subscribe(&un_sdo_output2, processSdoOutput2); + subscribe(&un_sdo_output3, processSdoOutput3); + subscribe(&un_sdo_output4, processSdoOutput4); + subscribe(&un_sdo_output5, processSdoOutput5); + subscribe(&un_sdo_output6, processSdoOutput6); + subscribe(&un_sdo_output7, processSdoOutput7); + subscribe(&un_sdo_output8, processSdoOutput8); + + + + timerStart(&can_timer_interface1,1,1);//1msһ + + timerStart(&can_timer_interface2,100,1);//100msһ + + printf( "canInterface: initial OK %d\n",getCurrentTime()); +} + + + diff --git a/interface_can.h b/interface_can.h new file mode 100644 index 0000000..a761afa --- /dev/null +++ b/interface_can.h @@ -0,0 +1,189 @@ + +#ifndef _INTERFACE_CAN_H_ +#define _INTERFACE_CAN_H_ + +#include "regs_base.h" +#include "irq_num.h" +#include + + +#define CAN_INDEX_0 FLEXCAN7 +#define CAN_INDEX_1 FLEXCAN21 +#define CAN_INDEX_2 FLEXCAN16 +#define CAN_INDEX_3 FLEXCAN3 +#define CAN_INDEX_4 FLEXCAN23 +#define CAN_INDEX_5 FLEXCAN5 +#define CAN_INDEX_6 FLEXCAN6 +#define CAN_INDEX_7 FLEXCAN4 + + + +#define TX_MB_INDEX (USED_MB_FOR_FIFO) + + +//#define MOTOR_INPUT_ID_1 0x101 +//#define MOTOR_INPUT_ID_2 0x102 + +#define MOTOR_INPUT_ID_1 0x10F91708//ǰ ת +#define MOTOR_INPUT_ID_2 0x10F92708//ǰ +#define MOTOR_INPUT_ID_3 0x10F93708// +#define MOTOR_INPUT_ID_4 0x10F94708//Һ + + +#define MOTOR_INPUT_ID_5 0x10F81708//ǰ λ +#define MOTOR_INPUT_ID_6 0x10F82708//ǰ +#define MOTOR_INPUT_ID_7 0x10F83708// +#define MOTOR_INPUT_ID_8 0x10F84708//Һ + + + +#define BMS_INPUT_ID1 0x100 +#define BMS_INPUT_ID2 0x101 +#define REMOTE_ID 0x12000023 +//#define TEMP_MODULE_INPUT_ID_1 0x301 +#define TEMP_MODULE_INPUT_ID_1 0x15000003 + +#define REMOTE_ID1 0x12000024 + + + +#define TURN_MOTOR_CANID 0x7F +#define RIGHT_MOTOR_CANID 0x7E + + +//ʹID +#define TURN_MOTOR_ENABLE 0x300FD00 + TURN_MOTOR_CANID +#define RIGHT_MOTOR_ENABLE 0x300FD00 + RIGHT_MOTOR_CANID + +//дID +#define TURN_MOTOR_WRITE 0x1200FD00 + TURN_MOTOR_CANID +#define RIGHT_MOTOR_WRITE 0x1200FD00 + RIGHT_MOTOR_CANID + + + + + +#define CAN_FIFO_SIZE 8 + +//#define TEMP_MODULE_INPUT_ID_2 0x302 + + +// Ϣ +typedef struct _StrCanFault +{ + uint8_t navigator_count; //Ǽ + uint8_t motor1_count; //ֶݼ + uint8_t motor2_count; //ֶݼ + uint8_t bms_count; //bms + uint8_t temperature_count; //¶ȼ + uint8_t remote_count; //ңؼ + + uint8_t navigator_state; //״̬ + uint8_t motor1_state; //1״̬ + uint8_t motor2_state; //2״̬ + uint8_t bms_state; //bms״̬ + uint8_t temperature_state; //¶ȼ + uint8_t remote_state; //ң״̬ +} StrCanFault; + +typedef union _UnCanFault +{ + StrCanFault bit_data; // ʹöĽṹ + uint8_t arr[sizeof(StrCanFault)]; // ͨṹȷС +} UnCanFault; + + + + + +// canڵ +typedef struct _StrCanBuffer +{ + uint32_t id; //**< CAN Frame Identifier. */ + uint8_t dataBuffer[8]; //ݻ + unsigned int length : 7; //**< CAN frame payload length in bytes(Range: 0~8). */ + unsigned int type : 1; //**< CAN Frame Type(DATA or REMOTE). */ + unsigned int format : 1; //**< CAN Frame Identifier(STD or EXT format). */ + +}StrCanBuffer; + + + +typedef struct Fifo_Queue_Tag +{ + StrCanBuffer Data[CAN_FIFO_SIZE];// + uint8_t front;//ͷָ + uint8_t rear;//βָ +}StrCanFifoQueue; + + + + + + + + + + + + + + + +#define ON 1 +#define OFF 0 + +#define left_motor_fan(Value) un_inf_can_kgf_output1.bit_data.KGF01 = Value;// +#define right_motor_fan(Value) un_inf_can_kgf_output1.bit_data.KGF03 = Value;// +#define pre_charge_relay(Value) un_inf_can_kgf_output1.bit_data.KGF04 = Value;//Ԥ +#define high_voltage_relay(Value) un_inf_can_kgf_output1.bit_data.KGF07 = Value;un_inf_can_kgf_output1.bit_data.KGF08 = Value//ѹ̵ +#define low_voltage_relay(Value) un_inf_can_kgf_output1.bit_data.KGF11 = Value;un_inf_can_kgf_output1.bit_data.KGF12 = Value//ѹ̵ +#define yellow_light(Value) un_inf_can_kgf_output1.bit_data.KGF06 = Value;//Ƶ +#define red_light(Value) un_inf_can_kgf_output1.bit_data.KGF09 = Value;// +#define left_vehicle_fan(Value) un_inf_can_kgf_output1.bit_data.KGF14 = Value;// +#define right_vehicle_fan(Value) un_inf_can_kgf_output2.bit_data.KGF03 = Value;//ҷ +#define computer(Value) un_inf_can_kgf_output2.bit_data.KGF01 = Value;un_inf_can_kgf_output2.bit_data.KGF02 = Value// +#define network_switch(Value) un_inf_can_kgf_output2.bit_data.KGF05 = Value;un_inf_can_kgf_output2.bit_data.KGF06 = Value//罻· + +//H +#define brake_motor_forward un_h_bridge_output.bit_data.channel_01 = ON ;un_h_bridge_output.bit_data.channel_04 = ON ;un_h_bridge_output.bit_data.channel_02 = OFF;un_h_bridge_output.bit_data.channel_03 = OFF;un_h_bridge_output.bit_data.sleep_01 = ON ;un_h_bridge_output.bit_data.sleep_02 = ON//ת +#define brake_motor_reversal un_h_bridge_output.bit_data.channel_01 = OFF;un_h_bridge_output.bit_data.channel_04 = OFF;un_h_bridge_output.bit_data.channel_02 = ON ;un_h_bridge_output.bit_data.channel_03 = ON ;un_h_bridge_output.bit_data.sleep_01 = ON ;un_h_bridge_output.bit_data.sleep_02 = ON//ת +#define brake_motor_off un_h_bridge_output.bit_data.channel_01 = OFF;un_h_bridge_output.bit_data.channel_04 = OFF;un_h_bridge_output.bit_data.channel_02 = OFF;un_h_bridge_output.bit_data.channel_03 = OFF;un_h_bridge_output.bit_data.sleep_01 = OFF;un_h_bridge_output.bit_data.sleep_02 = OFF//ر + + + + +//ⲿ +void flexcan_transfer_callback(flexcan_handle_t *handle,flexcan_status_e status, uint32_t result,void *userData); +uint8_t flexcan_config_rx_fifo(flexcan_handle_t *handle,flexcan_rx_fifo_config_t *config); +uint8_t CAN_Send_Msg( flexcan_handle_t *handle, uint32_t ID, flexcan_frame_format_e ide, flexcan_frame_type_e rtr, uint8_t *msg, uint8_t len, uint8_t tx_index); +void flexcan_Busoff_Recovery(void); +void initialization_All_Flexcan(void); + + + + +//ⲿ +extern flexcan_handle_t g_flexcan_handle; +extern bool busoff_occur_flag;//־ +extern bool WDTReFresh_flag;//±־ +extern flexcan_handle_t g_flexcan_handle; +extern uint16_t FrameHeader;//֡ͷ +extern flexcan_handle_t can_handle_0;//canʼṹ +extern flexcan_handle_t can_handle_1; +extern flexcan_handle_t can_handle_2; +extern flexcan_handle_t can_handle_3; +extern flexcan_handle_t can_handle_4; +extern flexcan_handle_t can_handle_5; +extern flexcan_handle_t can_handle_6; +extern flexcan_handle_t can_handle_7; +extern UnCanFault can_fault_info; + +extern uint32_t time_elapsed; +extern uint32_t time_elapsed1; + +//ⲿ +void canInterfaceInit(void); + + +#endif /* INTERFACE_CAN_H */ \ No newline at end of file diff --git a/interface_config.h b/interface_config.h new file mode 100644 index 0000000..2e3bd1e --- /dev/null +++ b/interface_config.h @@ -0,0 +1,70 @@ +#ifndef _INTERFACE_CONFIG_H_ +#define _INTERFACE_CONFIG_H_ + +//ͷļ +#include "interface.h" +#include "interface_can.h" +#include "interface_gpio.h" +#include "interface_ethernet.h" +#include "interface_btm.h" +#include "interface_boot.h" +#include "interface_wdt.h" +#include "interface_24c02.h" + +#include "app/app_frm_monitor.h" +#include "app/app_frm_signal.h" +#include "app/app_frm_timer.h" + + + +#include "irq.h" +#include +#include +#include "lwip/udp.h" +#include +#include +#include "debug.h" +#include +#include "board.h" + + + + + + + + + + + + +#endif /* _INTERFACE_CONFIG_H_ */ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/interface_ethernet.c b/interface_ethernet.c new file mode 100644 index 0000000..fb0a75f --- /dev/null +++ b/interface_ethernet.c @@ -0,0 +1,841 @@ +#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; +Timer ethernet_timer_interface2; + + +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); + uint8_t ip_addr[4] = {0,0,0,0}; +//------------------------------------------------------------------------------------------------ + //20250822 ӱˢдλIP + ip_addr[0] = addr->addr & 0xff; /* IADDR4 */ + ip_addr[1] = (addr->addr >> 8) & 0xff; /* IADDR3 */ + ip_addr[2] = (addr->addr >> 16) & 0xff; /* IADDR2 */ + ip_addr[3] = (addr->addr >> 24) & 0xff; /* IADDR1 */ + + ethernet_parameter.download_ip[0] = ip_addr[0]; + ethernet_parameter.download_ip[1] = ip_addr[1]; + ethernet_parameter.download_ip[2] = ip_addr[2]; + ethernet_parameter.download_ip[3] = ip_addr[3]; + + + boot_eth_flag = true; + FrameHeader = ( (buf[0] << 8) | (buf[1]) ); + printf("FrameHeader %d\n",FrameHeader); + + 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); +} + +void ethernetRequest(void *signal_id) +{ + static uint8_t ethernetRequestcnt = 0; + uint8_t request_ip[4] = {192,168,17,88}; + uint16_t request_port = 8080; + uint8_t request_buf[200] = {0}; + static uint8_t RequestStaACC0 = 0; + static uint8_t RequestStaACC1 = 0; + static uint8_t RequestStaACC2 = 0; + static uint8_t RequestStaACC3 = 0; + + uint16_t RgExchangeTemp = 0; + uint32_t Rg32ExchangeTemp = 0; + uint32_t TempAcc = 0; + uint8_t i = 0; + + ethernetRequestcnt ++; + + if(ethernetRequestcnt >= 10) + { + ethernetRequestcnt = 0; + + //״̬ + un_vehicle_Info_output.bit_data.frame_header = 0xCCAA;//֡ͷ + un_vehicle_Info_output.bit_data.frame_type = 0x2000;//֡ + un_vehicle_Info_output.bit_data.frame_length = 0x2900;//֡ + un_vehicle_Info_output.bit_data.accumulated = RequestStaACC0++;//ۼֵ + + TempAcc = 0; + for (i = 0; i < 40; i++)//ۼǰ40ֽ + { + TempAcc = TempAcc + (uint32_t)(un_vehicle_Info_output.arr[i]); + } + un_vehicle_Info_output.bit_data.crc = (uint8_t)TempAcc;//ۼֵ + + + //״̬ + un_motor_status_output.bit_data.frame_header = 0xCCAA;//֡ͷ + un_motor_status_output.bit_data.frame_type = 0x2100;//֡ + un_motor_status_output.bit_data.frame_length = 0x2200;//֡ + un_motor_status_output.bit_data.accumulated = RequestStaACC1++;//ۼֵ + RgExchangeTemp = ( (uint16_t)getParam("maxTorq") + 300 ) *100 ; +// un_motor_status_output.bit_data.left_torque_limit = ((RgExchangeTemp << 8) | (RgExchangeTemp >> 8));//Ť +// un_motor_status_output.bit_data.right_torque_limit = ((RgExchangeTemp << 8) | (RgExchangeTemp >> 8));//ҲŤ +// un_motor_status_output.bit_data.left_power_in = (((uint16_t)getParam("feedPwr") << 8) | ((uint16_t)getParam("feedPwr") >> 8));//繦 +// un_motor_status_output.bit_data.right_power_in = (((uint16_t)getParam("feedPwr") << 8) | ((uint16_t)getParam("feedPwr") >> 8));//Ҳ繦 +// un_motor_status_output.bit_data.left_power_out = (((uint16_t)getParam("dispPwr") << 8) | ((uint16_t)getParam("dispPwr") >> 8));//ŵ繦 +// un_motor_status_output.bit_data.right_power_out = (((uint16_t)getParam("dispPwr") << 8) | ((uint16_t)getParam("dispPwr") >> 8));//Ҳŵ繦 + + TempAcc = 0; + for (i = 0; i < 32; i++)//ۼǰ31ֽ + { + TempAcc = TempAcc + (uint32_t)(un_motor_status_output.arr[i]); + un_motor_status_output.bit_data.checksum = (uint8_t)TempAcc;//ۼֵ + } + + //PID + + un_pid_output.bit_data.frame_header = 0xCCAA;//֡ͷ + un_pid_output.bit_data.frame_type = 0x2200;//֡ + un_pid_output.bit_data.frame_length = 0x3800;//֡ + un_pid_output.bit_data.accumulated = RequestStaACC2++;//ۼֵ + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kp")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.rc_straight_p = Rg32ExchangeTemp;//ңֱP + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_ki")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.rc_straight_i = Rg32ExchangeTemp;//ңֱI + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kd")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.rc_straight_d = Rg32ExchangeTemp;//ңֱD + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_il")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.auto_straight_p = Rg32ExchangeTemp;//ֱP + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_ol")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.auto_straight_i = Rg32ExchangeTemp;//ֱI + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kd")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.auto_straight_d = Rg32ExchangeTemp;//ֱD + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_kp")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.rc_turn_p = Rg32ExchangeTemp;//ңתP + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_ki")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.rc_turn_i = Rg32ExchangeTemp;//ңתI + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_kd")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.rc_turn_d = Rg32ExchangeTemp;//ңתD + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_il")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.auto_turn_p = Rg32ExchangeTemp;//תP + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_ol")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.auto_turn_i = Rg32ExchangeTemp;//תI + + Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kd")*1000); + Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) ); + un_pid_output.bit_data.auto_turn_d = Rg32ExchangeTemp;//תI + + TempAcc = 0; + for (i = 0; i < 55; i++)//ۼǰ40ֽ + { + TempAcc = TempAcc + (uint32_t)(un_pid_output.arr[i]); + } + un_pid_output.bit_data.checksum = (uint8_t)TempAcc;//ۼֵ + + + //ң + un_remote_control_output.bit_data.frame_header = 0xCCAA;//֡ͷ + un_remote_control_output.bit_data.frame_type = 0x2400;//֡ + un_remote_control_output.bit_data.frame_length = 0x1400;//֡ + un_remote_control_output.bit_data.accumulated = RequestStaACC3++;//ۼֵ + //RCH_3 + + TempAcc = 0; + for (i = 0; i < 17; i++)//ۼǰ40ֽ + { + TempAcc = TempAcc + (uint32_t)(un_remote_control_output.arr[i]); + } + un_remote_control_output.bit_data.crc = (uint8_t)TempAcc;//ۼֵ + + //ֶ + un_manual_control_output.bit_data.frame_header = 0xBBFF; + un_manual_control_output.bit_data.frame_type = 0x2500; + un_manual_control_output.bit_data.frame_length = 0x0F; + TempAcc = 0; + for (i = 0; i < 14; i++)//ۼǰ40ֽ + { + TempAcc = TempAcc + (uint32_t)(un_manual_control_output.arr[i]); + } + un_manual_control_output.bit_data.crc_2 = (uint8_t)TempAcc;//ۼֵ + + // + un_auto_control_output.bit_data.frame_header = 0xCCFF; + un_auto_control_output.bit_data.frame_type = 0x2600; + un_auto_control_output.bit_data.frame_length = 0x1B; + TempAcc = 0; + for (i = 0; i < 27; i++)//ۼǰ40ֽ + { + TempAcc = TempAcc + (uint32_t)(un_auto_control_output.arr[i]); + } + un_auto_control_output.bit_data.crc = (uint8_t)TempAcc;//ۼֵ + + memcpy(request_buf,(uint8_t *)(&un_vehicle_Info_output),sizeof(un_vehicle_Info_output)); + memcpy(&request_buf[sizeof(un_vehicle_Info_output)],(uint8_t *)(&un_motor_status_output),sizeof(un_motor_status_output)); + memcpy(&request_buf[sizeof(un_vehicle_Info_output) + sizeof(un_motor_status_output)],(uint8_t *)(&un_pid_output),sizeof(un_pid_output)); + memcpy(&request_buf[sizeof(un_vehicle_Info_output) + sizeof(un_motor_status_output) + sizeof(un_pid_output)],(uint8_t *)(&un_remote_control_output),sizeof(un_remote_control_output)); + memcpy(&request_buf[sizeof(un_vehicle_Info_output) + sizeof(un_motor_status_output) + sizeof(un_pid_output) + sizeof(un_remote_control_output)],(uint8_t *)(&un_manual_control_output),sizeof(un_manual_control_output)); + memcpy(&request_buf[sizeof(un_vehicle_Info_output) + sizeof(un_motor_status_output) + sizeof(un_pid_output) + sizeof(un_remote_control_output) + sizeof(un_manual_control_output)],(uint8_t *)(&un_auto_control_output),sizeof(un_auto_control_output)); + + UdpSendToData(UDPCB_2,request_buf,sizeof(un_vehicle_Info_output) + sizeof(un_motor_status_output) + sizeof(un_pid_output) + sizeof(un_remote_control_output) + sizeof(un_manual_control_output) + sizeof(un_auto_control_output), request_ip, request_port); + } + timerStart(ðernet_timer_interface2, 1,1); //10msһ +} + +// 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); +// timerInit(ðernet_timer_interface2); + + // Ķʱźţڶʱɼ + subscribe(ðernet_timer_interface, ethernetInterfaceTimerProcess); + subscribe(ðernet_timer_interface1, lwip_Ethernet); +// subscribe(ðernet_timer_interface2, ethernetRequest); + + 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һ +// timerStart(ðernet_timer_interface2, 1,1); //10msһ + + + printf( "ethernetInterface: initial OK %d\n",getCurrentTime()); +} + + \ No newline at end of file diff --git a/interface_ethernet.h b/interface_ethernet.h new file mode 100644 index 0000000..e6abef4 --- /dev/null +++ b/interface_ethernet.h @@ -0,0 +1,121 @@ +#ifndef _INTERFACE_ETHERNET_H_ +#define _INTERFACE_ETHERNET_H_ + +#include "interface_config.h" +#include "lwip/err.h" +#include "lwip/inet.h" +#include "lwip/tcp.h" +#include "lwip/udp.h" + +#include "lwip/netif.h" +#include "lwip/ip.h" +#include "lwip/init.h" +#include "netif/etharp.h" +#include "lwip/pbuf.h" + + + + + +//̫Ϣ +typedef struct _StrEthernetFault +{ + uint8_t auto_count; //Զݼ + uint8_t manual_count; //ֶݼ + uint8_t auto_state; // Զ״̬ | 0 1 + uint8_t manual_state; // ֶ״̬ | 0 1 +} StrEthernetFault; + +typedef union _UnEthernetFault +{ + StrEthernetFault bit_data; // ʹöĽṹ + uint8_t arr[sizeof(StrEthernetFault)]; // ͨṹȷС +} UnEthernetFault; + + +extern UnEthernetFault ethernet_fault_Info; + + + + + + + + + + + +typedef struct +{ + uint8_t local_ip[4]; + uint8_t mask[4]; + uint16_t Local_upper_port; + uint16_t Local_download_port; + uint16_t local_communication_port; + uint8_t computer_ip[4]; + uint8_t upper_ip[4]; + uint8_t download_ip[4]; + uint16_t target_upper_port; + uint16_t target_download_port; + uint16_t target_communication_port; + +} StrEthernetParameter; + + + + + + + + + + +#define UDPCB_1 1//ͨѶ˿ +#define UDPCB_2 2//λ˿ +#define UDPCB_3 3//λ˿ +#define UDPCB_4 4//λ˿ +#define UDPCB_5 5//λ˿ + + + +#define COMMUNICATION_PORT 8011 +#define UPPER_PORT 8000 +#define PARAM_PORT 8080 + +#define DOWNLOAR_PORT 7812 +#define BOOT_TX_PORT 7811 //ʱдĶ˿ڣĿ˿ڣ˭ +#define WDT_PORT 7813 //ʼʱĶ˿ڣն˿ + + + + +//ⲿ +void UDP_Echo_Init(uint8_t udpcbID, udp_recv_fn recv, uint16_t port); +int8_t UdpSendToData(uint8_t udpcbID, uint8_t *buf, uint16_t len, uint8_t *sip, uint16_t port); + +void udp_Callback_1(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port); +void udp_Callback_2(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port); +void udp_Callback_3(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port); +void udp_Callback_4(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port); +void udp_Callback_5(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port); + +void ethernetTimerProcess(void); +void ethernetInterfaceInit(void); + + + +//ⲿ +extern struct udp_pcb *udpcb_1; +extern StrEthernetParameter ethernet_parameter; + +extern struct udp_pcb *udpcb_1; +extern struct udp_pcb *udpcb_2; +extern struct udp_pcb *udpcb_3; +extern struct udp_pcb *udpcb_4; +extern struct udp_pcb *udpcb_5; + +extern bool eth_pool_rx; + + + +#endif /*ETH_DEMO_H */ \ No newline at end of file diff --git a/interface_gpio.c b/interface_gpio.c new file mode 100644 index 0000000..f4b94ca --- /dev/null +++ b/interface_gpio.c @@ -0,0 +1,145 @@ +#include "interface_gpio.h" + + +Timer gpio_timer_interface; + +void gpioTimerProcess(void *signal_id) +{ + (void)signal_id; // 标记变量为已使用，避免编译器警告 +// uint32_t time_boot = getCurrentTime();//记录当前时间 +//-------------------------------------------- + un_sw_sample.bit_data.emergency_stop_switch = sdrv_gpio_read_pin_input_level(GPIO_A4); + un_sw_sample.bit_data.High_voltage_switch = sdrv_gpio_read_pin_input_level(GPIO_C7); + + publishMessage(&un_sw_sample, 1);//发射信号 + + timerStart(&gpio_timer_interface, 100,1); + printf("sw state %d\n",un_sw_sample.arr[0]); + } + + +// APP模块的初始化 +void gpioInterfaceInit(void) +{ + // 初始化定时器 + timerInit(&gpio_timer_interface); + // 订阅定时器信号，用于定时采集 + subscribe(&gpio_timer_interface, gpioTimerProcess); + + timerStart(&gpio_timer_interface, 100,1); + + printf("gpioInterface: initial OK %d\n",getCurrentTime()); +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/interface_gpio.h b/interface_gpio.h new file mode 100644 index 0000000..029f4ab --- /dev/null +++ b/interface_gpio.h @@ -0,0 +1,25 @@ +#ifndef _INTERFACE_GPIO_H_ +#define _INTERFACE_GPIO_H_ + + +#include "interface_config.h" + + + + + + +//#deifne emergency_stop_switch sdrv_gpio_read_pin_input_level(GPIO_A6) +// +//#deifne high_voltage_switch sdrv_gpio_read_pin_input_level(GPIO_A7) + + +void gpioTimerProcess(void *signal_id); + +void gpioInterfaceInit(void); + + + + + +#endif /* _INTERFACE_GPIO_H_ */ \ No newline at end of file diff --git a/interface_power.c b/interface_power.c new file mode 100644 index 0000000..d5225a6 --- /dev/null +++ b/interface_power.c @@ -0,0 +1,139 @@ + + +#include "interface_power.h" +#include "app/app_config.h" + +Timer ppwer_timer; + + +// 100ms +void powerOn(void) +{ + static uint8_t power_state = 0; + static uint8_t power_count = 0; +//--------------------------------------- + switch(power_state) + { + case 0: + left_motor_fan(ON); + right_motor_fan(ON); + pre_charge_relay(ON); + high_voltage_relay(OFF); + low_voltage_relay(ON); + yellow_light(ON); + red_light(ON); + left_vehicle_fan(ON); + right_vehicle_fan(ON); + computer(ON); + network_switch(ON); + power_state = 10; + power_count = 0; + break; + case 10: + if (power_count >= 100)//10s + { + left_motor_fan(ON); + right_motor_fan(ON); + pre_charge_relay(OFF); + high_voltage_relay(ON); + low_voltage_relay(ON); + yellow_light(ON); + red_light(ON); + left_vehicle_fan(ON); + right_vehicle_fan(ON); + computer(ON); + network_switch(ON); + power_state = 11; + power_count = 0; + } + else + { + left_motor_fan(ON); + right_motor_fan(ON); + pre_charge_relay(ON); + high_voltage_relay(OFF); + low_voltage_relay(ON); + yellow_light(ON); + red_light(ON); + left_vehicle_fan(ON); + right_vehicle_fan(ON); + computer(ON); + network_switch(ON); + power_state = 10; + power_count ++; + } + break; + case 11: + power_state = 11; +// left_motor_fan(ON); +// left_motor_fan(ON); +// right_motor_fan(ON); +// pre_charge_relay(OFF); +// high_voltage_relay(ON); +// low_voltage_relay(ON); +// yellow_light(ON); +// red_light(ON); +// left_vehicle_fan(ON); +// right_vehicle_fan(ON); +// computer(ON); +// network_switch(ON); + break; + default:break; + } + + canSendAll(); +// ethernetSendAll(&power_state); +} + +//ϵ紦 +//void powerOnSignal(void *data) +//{ +// +// +// +// +// +// +// +// +// +// +//} + + +// APPģijʼ +void powerAppInit(void) +{ + + // յλдź +// subscribe(&un_remote_control_input, powerOnSignal); +// timerStart(&test_timer, 5000); +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/interface_power.h b/interface_power.h new file mode 100644 index 0000000..9aec95b --- /dev/null +++ b/interface_power.h @@ -0,0 +1,115 @@ + +#ifndef _INTERFACE_POWER_H_ +#define _INTERFACE_POWER_H_ + + +#include "interface_config.h" + +#define ON 1 +#define OFF 0 + +// +//kgf1 + +// +//kgf3 + +//Ԥ +//kgf4 + +//ѹѹ̵ +//kgf78 + +//ѹ̵ +//kgf11-12 + +//1 +//kgf6 + +//2 +//kgf9 + +// +//kgf14 + +//ҷ +//kgf19 + +// +//kgf 17-18 + +#define left_motor_fan(Value) un_inf_can_kgf_output1.bit_data.KGF01 = Value;// +#define right_motor_fan(Value) un_inf_can_kgf_output1.bit_data.KGF03 = Value;// +#define pre_charge_relay(Value) un_inf_can_kgf_output1.bit_data.KGF04 = Value;//Ԥ +#define high_voltage_relay(Value) un_inf_can_kgf_output1.bit_data.KGF07 = Value;un_inf_can_kgf_output1.bit_data.KGF08 = Value//ѹ̵ +#define low_voltage_relay(Value) un_inf_can_kgf_output1.bit_data.KGF11 = Value;un_inf_can_kgf_output1.bit_data.KGF12 = Value//ѹ̵ +#define yellow_light(Value) un_inf_can_kgf_output1.bit_data.KGF06 = Value;//Ƶ +#define red_light(Value) un_inf_can_kgf_output1.bit_data.KGF09 = Value;// +#define left_vehicle_fan(Value) un_inf_can_kgf_output1.bit_data.KGF14 = Value;// +#define right_vehicle_fan(Value) un_inf_can_kgf_output2.bit_data.KGF03 = Value;//ҷ +#define computer(Value) un_inf_can_kgf_output2.bit_data.KGF01 = Value;un_inf_can_kgf_output2.bit_data.KGF02 = Value//ѹ̵ +#define network_switch(Value) un_inf_can_kgf_output2.bit_data.KGF05 = Value;un_inf_can_kgf_output2.bit_data.KGF06 = Value//罻· + +//H +#define brake_motor_forward un_h_bridge_output.bit_data.channel_01 = ON ;un_h_bridge_output.bit_data.channel_04 = ON ;un_h_bridge_output.bit_data.channel_02 = OFF;un_h_bridge_output.bit_data.channel_03 = OFF;un_h_bridge_output.bit_data.sleep_01 = ON ;un_h_bridge_output.bit_data.sleep_02 = ON//ת +#define brake_motor_reversal un_h_bridge_output.bit_data.channel_01 = OFF;un_h_bridge_output.bit_data.channel_04 = OFF;un_h_bridge_output.bit_data.channel_02 = ON ;un_h_bridge_output.bit_data.channel_03 = ON ;un_h_bridge_output.bit_data.sleep_01 = ON ;un_h_bridge_output.bit_data.sleep_02 = ON//ת +#define brake_motor_off un_h_bridge_output.bit_data.channel_01 = OFF;un_h_bridge_output.bit_data.channel_04 = OFF;un_h_bridge_output.bit_data.channel_02 = OFF;un_h_bridge_output.bit_data.channel_03 = OFF;un_h_bridge_output.bit_data.sleep_01 = OFF;un_h_bridge_output.bit_data.sleep_02 = OFF//ر + + + +//#define OutputL01_13(Value) UnInfCanKGF_2.bit_data.KGF03 = Value;UnBCM_1Signal_1.bit_data.BCM_1SignalW9 = Value//ת 20200106ת޸ӳΪK19 ϵƲܹͬһͨϡ +//#define OutputL01_14(Value) UnInfCanKGF_1.bit_data.KGF13 = Value;UnBCM_1Signal_1.bit_data.BCM_1SignalW7 = Value//ת +//#define OutputL01_19(Value) UnInfCanKGF_1.bit_data.KGF05 = Value;UnInfCanKGF_1.bit_data.KGF06 = Value; UnBCM_1Signal_1.bit_data.BCM_1SignalW22 = Value //Զ +//#define OutputL01_18(Value) UnInfCanKGF_1.bit_data.KGF03 = Value;UnInfCanKGF_1.bit_data.KGF04 = Value; UnBCM_1Signal_1.bit_data.BCM_1SignalW23 = Value // +//#define OutputL01_22(Value) UnInfCanKGF_1.bit_data.KGF11 = Value;UnInfCanKGF_1.bit_data.KGF12 = Value // +//#define OutputL01_06(Value) UnInfCanKGF_1.bit_data.KGF01 = Value;UnInfCanKGF_1.bit_data.KGF02 = Value; UnBCM_1Signal_1.bit_data.BCM_1SignalW2 = Value//λ +//#define OutputL01_04(Value) UnInfCanKGF_2.bit_data.KGF05 = Value//յ 20200106յ޸ӳΪK21 +//#define OutputL01_02(Value) UnInfCanKGF_1.bit_data.KGF16 = Value//ǰյ +//#define OutputL01_20(Value) UnInfCanKGF_1.bit_data.KGF09 = Value;UnInfCanKGF_1.bit_data.KGF10 = Value //ƶ +//#define OutputL01_21(Value) UnInfCanKGF_1.bit_data.KGF07 = Value;UnInfCanKGF_1.bit_data.KGF08 = Value;UnBCM_1Signal_1.bit_data.BCM_1SignalW28 = Value// +// +//#define OutputL01_05(Value) UnInfCanKGF_2.bit_data.KGF01 = Value//ƶ ҲΪƶ +//#define OutputL01_11(Value) UnInfCanKGF_2.bit_data.KGF02 = Value//յ1LED +//#define OutputL01_12(Value) UnInfCanKGF_1.bit_data.KGF14 = Value//յ2LED +//#define OutputL01_10(Value) UnInfCanKGF_2.bit_data.KGF04 = Value;UnBCM_1Signal_1.bit_data.BCM_1SignalW21 = Value//ΣLed +//#define OutputL01_16(Value) UnInfCanKGF_1.bit_data.KGF15 = Value//ε1 202001061޸ӳΪK15 +//#define OutputL01_17(Value) UnInfCanKGF_2.bit_data.KGF06 = Value//ε2 +//#define OutputL01_07(Value) UnInfCanKGF_2.bit_data.KGF07 = Value;UnBCM_2Signal_1.bit_data.BCM_2SignalW12 = Value// +//#define OutputL01_08(Value) UnInfCanKGF_2.bit_data.KGF08 = Value;UnBCM_2Signal_1.bit_data.BCM_2SignalW10 = Value//սŵ +//#define OutputL01_09(Value) UnInfCanKGF_2.bit_data.KGF09 = Value//ǰ˪LED +//#define OutputL01_01(Value) UnInfCanKGF_2.bit_data.KGF11 = Value;UnInfCanKGF_2.bit_data.KGF12 = Value;UnBCM_2Signal_1.bit_data.BCM_2SignalW8 = Value//ǰϴ +//#define OutputL01_03(Value) UnInfCanKGF_2.bit_data.KGF10 = Value//ε1ٶȿ +//#define OutputL01_15(Value) UnInfCanKGF_2.bit_data.KGF13 = Value//20190202 +//#define OutputL01_23(Value) UnInfCanKGF_2.bit_data.KGF14 = Value//ε2ٶȿ +//#define OutputL01_24(Value) UnInfCanKGF_2.bit_data.KGF15 = Value//ǰ˪¿ +//#define OutputL01_25(Value) UnInfCanKGF_2.bit_data.KGF16 = Value//220VӴ + + + + + + + + +//wⲿ + +void powerOn(void); + + + + + + + + + + + + +#endif /* _APP_POWER_H_ */ + + + + + + diff --git a/interface_uart.c b/interface_uart.c new file mode 100644 index 0000000..5876c4e --- /dev/null +++ b/interface_uart.c @@ -0,0 +1,471 @@ + + + +#include "interface_uart.h" + + +#define BUFFER_SIZE 256 + +#define SBUS_UART UART3 + + +UartFaultInfo uart_fault_info = { + .remote_count = 0, + .remote_state = 0, +}; + + + +// 岢ʼ SBus ṹ +typedef struct { + uint16_t channels[16]; // 洢16ͨĽ + uint8_t flags; // ־ֽ +} SBusData; + + +SBusData sbus_data = { + .channels = {0}, + .flags = 0 +}; + +///* Buffer used to receive data from the console */ +typedef struct user_data { + uint8_t user_buffer[BUFFER_SIZE]; + uint8_t *buff_ptr; + uint32_t buff_remain; +} user_data_t; +static user_data_t user_data; +///* Buffer used to transmit welcome message */ + + +sdrv_uart_t uart_ctrl_ptr; + +/* uart config structures */ +//sbus ĬΪ100k +static sdrv_uart_config_t g_uart_config = { + .base = DEVICE_BASE(SBUS_UART), + .irq = DEVICE_INTR(SBUS_UART), + .baud = 100000, + .data_bits = SDRV_UART_CHAR_8_BITS, + .stop_bits = SDRV_UART_STOP_2_BIT, + .parity = SDRV_UART_EVEN_PARITY, +}; + + +// SBus ݽ +SBusData* parseSBusData(const uint8_t *input_data) +{ + + for(uint8_t i = 0; i < 25; i++) + { + test_app[i] = input_data[i]; + } + test_app[25] = 0x44; + + float sbus_temp = 0.0; + // 16ͨݣÿͨΪ11λ 0xFΪ֡ͷ + sbus_data.channels[0] = (input_data[1] | input_data[2] << 8) & 0x07FF; + sbus_data.channels[1] = (input_data[2] >> 3 | input_data[3] << 5) & 0x07FF; + sbus_data.channels[2] = (input_data[3] >> 6 | input_data[4] << 2 | input_data[5] << 10) & 0x07FF; + sbus_data.channels[3] = (input_data[5] >> 1 | input_data[6] << 7) & 0x07FF; + sbus_data.channels[4] = (input_data[6] >> 4 | input_data[7] << 4) & 0x07FF; + sbus_data.channels[5] = (input_data[7] >> 7 | input_data[8] << 1 | input_data[9] << 9) & 0x07FF; + sbus_data.channels[6] = (input_data[9] >> 2 | input_data[10] << 6) & 0x07FF; + sbus_data.channels[7] = (input_data[10] >> 5 | input_data[11] << 3) & 0x07FF; + sbus_data.channels[8] = (input_data[12] | input_data[13] << 8) & 0x07FF; + sbus_data.channels[9] = (input_data[13] >> 3 | input_data[14] << 5) & 0x07FF; + sbus_data.channels[10] = (input_data[14] >> 6 | input_data[15] << 2 | input_data[16] << 10) & 0x07FF; + sbus_data.channels[11] = (input_data[16] >> 1 | input_data[17] << 7) & 0x07FF; + sbus_data.channels[12] = (input_data[17] >> 4 | input_data[18] << 4) & 0x07FF; + sbus_data.channels[13] = (input_data[18] >> 7 | input_data[19] << 1 | input_data[20] << 9) & 0x07FF; + sbus_data.channels[14] = (input_data[20] >> 2 | input_data[21] << 6) & 0x07FF; + sbus_data.channels[15] = (input_data[21] >> 5 | input_data[22] << 3) & 0x07FF; + + // ־ֽ + sbus_data.flags = input_data[23]; + + //ת + sbus_temp = (float)(sbus_data.channels[0]); + sbus_temp = (ZERO_VALUE - sbus_temp)*23;//Ҫ任һ·ţΪ + remote_curvature = (uint16_t)((int16_t)sbus_temp);// + + //ת + sbus_temp = (float)(sbus_data.channels[1]); + sbus_temp = (ZERO_VALUE - sbus_temp)*2.3;//Ҫ任һ·ţΪ + remote_speed = (uint16_t)((int16_t)sbus_temp);//ת + + remote_Reserve = sbus_data.channels[7]; + + //SwA + if(sbus_data.channels[3] >= 1700)// + { + emergency_stop = 1; + } + else + { + emergency_stop = 0; + } + + //SwB + if(sbus_data.channels[4] >= 1700)// + { + remote_switch_b = 2; + } + else if(sbus_data.channels[6] <= 250)//ڶ + { + remote_switch_b = 0; + } + else//һ + { + remote_switch_b = 1; + } + + //SwC + if(sbus_data.channels[5] >= 1700)// + { + remote_switch_c = 2; + } + else if(sbus_data.channels[4] <= 250)//ڶ + { + remote_switch_c = 0; + } + else//һ + { + remote_switch_c = 1; + } + + //SwD + if(sbus_data.channels[6] >= 1700)// + { + remote_switch_d = 1; + } + else + { + remote_switch_d = 0; + } + + if(0 == sbus_data.flags)//ֱ߹ж //ͨ־жϣ Ϊ0ʾֱӣ + { + remote_enable = 1;//ֱ + } + else + { + remote_enable = 0;//ֱ + } + + ssdk_printf(SSDK_NOTICE, "sta\r\n"); + for(int i=0;i<16;i++) + { + ssdk_printf(SSDK_EMERG, "%04x\r\n",sbus_data.channels[i]); + } + ssdk_printf(SSDK_NOTICE, "end\r\n"); + return &sbus_data; +} + + +void callback(sdrv_uart_t *ctrl, sdrv_uart_callback_status_e status, + void *userData) +{ + uint8_t char_data[25] = {0};//һֽڻ + static uint8_t cnt_sbus = 0; + static uint8_t state = 0; + static uint8_t receive = 0; + static uint8_t sbus_buff[25]; + SBusData *tmp_sbus_data; + +// user_data_t *user_data = (user_data_t *)userData; + +// ssdk_printf(SSDK_NOTICE, "123456789\r\n"); + + /* Uart receive data. */ + if (SDRV_UART_RxFWF == status) + { + uart_fault_info.remote_count ++; + + size_t size = sdrv_uart_get_rxfifodata(ctrl, char_data,1); + +// for(uint8_t i = 0; i < 25; i++) +// { +// test_app[i] = char_data[i]; +// +// } + +// sdrv_uart_sync_transmit(&g_console_uart, char_data, +// 25, NULL, +// 0Xffff); + +// for(uint8_t i=0;i<25;i++) +// { +// ssdk_printf(SSDK_INFO,"char_data: %x \r\n", char_data[i]); +// } +// + +// for(int i=0;i= 24) + { + for(uint8_t i = 0; i < 25; i++) + { + test_app[i] = sbus_buff[i]; + } + test_app[25] = 0x55; +// ssdk_printf(SSDK_INFO,"char_data24: %x \r\n", char_data[0]); + if(0x0 == char_data[0]) + { + cnt_sbus = 0; + receive = 0; + sbus_buff[cnt_sbus] = char_data[0]; + state = 0; + tmp_sbus_data = parseSBusData(sbus_buff); + +// sdrv_uart_sync_transmit(ctrl, "AA\r\n", strlen("AA\r\n"), NULL, 0xFFFF); + +// sdrv_uart_sync_transmit(ctrl, ( const uint8_t *)&tmp_sbus_data->channels[0], +// 32, NULL, +// 0Xffff); + // ssdk_printf(SSDK_EMERG, "AAAr\n"); + } + } + else if(0x0f == char_data[0]) + { + receive = 0; + cnt_sbus = 0; + sbus_buff[cnt_sbus] = char_data[0]; + cnt_sbus++; + state = 1; + } + else + { + sbus_buff[cnt_sbus] = char_data[0]; + cnt_sbus ++; + state = 1; + } + break; + + default: + break; + } +// } + } + else if (SDRV_UART_RxFifoOverFlow == status) + { +// if (0 != user_data->buff_remain) { +// size_t size = sdrv_uart_get_rxfifodata(ctrl, user_data->user_buffer, +// user_data->buff_remain); +// } +// sdrv_uart_sync_transmit(ctrl, "\r\n", strlen("\r\n"), NULL, TIMES_OUT); +// sdrv_uart_sync_transmit(ctrl, user_data->user_buffer, +// BUFFER_SIZE - user_data->buff_remain, NULL, +// TIMES_OUT); +// sdrv_uart_sync_transmit(ctrl, "\r\n", strlen("\r\n"), NULL, TIMES_OUT); +// +// memset(user_data->user_buffer, '\0', BUFFER_SIZE); +// user_data->buff_ptr = user_data->user_buffer; +// user_data->buff_remain = BUFFER_SIZE; +// +// sdrv_uart_sync_transmit( +// ctrl, "\r\ncallback: SDRV_UART_RxFifoOverFlow!\r\n", +// strlen("\r\ncallback: SDRV_UART_RxFifoOverFlow!\r\n"), NULL, +// TIMES_OUT); +// +// /* User can stop or reset realtime receive if you want, when transfer +// * occur errors */ +// sdrv_uart_stop_realtime_receive(ctrl); +// sdrv_uart_start_realtime_receive(ctrl); + + } + else if (SDRV_UART_ParityError == status) + { +// sdrv_uart_sync_transmit( +// ctrl, "\r\ncallback: SDRV_UART_ParityError!\r\n", +// strlen("\r\ncallback: SDRV_UART_ParityError!\r\n"), NULL, +// TIMES_OUT); + } + else if (SDRV_UART_BaudrateError == status) + { +// sdrv_uart_sync_transmit( +// ctrl, "\r\ncallback: SDRV_UART_BaudrateError!\r\n", +// strlen("\r\ncallback: SDRV_UART_BaudrateError!\r\n"), NULL, +// TIMES_OUT); + } + else if (SDRV_UART_NoiseError == status) + { +// sdrv_uart_sync_transmit( +// ctrl, "\r\ncallback: SDRV_UART_NoiseError!\r\n", +// strlen("\r\ncallback: SDRV_UART_NoiseError!\r\n"), NULL, TIMES_OUT); + } + else if (SDRV_UART_FramingError == status) + { +// sdrv_uart_sync_transmit( +// ctrl, "\r\ncallback: SDRV_UART_FramingError!\r\n", +// strlen("\r\ncallback: SDRV_UART_FramingError!\r\n"), NULL, +// TIMES_OUT); + } + else + { +// ssdk_printf(SSDK_NOTICE, "234567891\r\n"); + } +} + + + +//ʼ +void uart_Initialize(void) +{ + /* Get uart clk Config. */ +#if ((CONFIG_E3210) || (CONFIG_E3110)) + if (g_uart_config.irq <= UART8_INTR_NUM) { + g_uart_config.clk_freq = + sdrv_ckgen_get_rate(CLK_NODE(g_ckgen_ip_uart_sf_1_to_8)); + } else { + g_uart_config.clk_freq = + sdrv_ckgen_get_rate(CLK_NODE(g_ckgen_ip_uart_sf_9_to_16)); + } +#else + if (g_uart_config.irq <= UART6_INTR_NUM) { + g_uart_config.clk_freq = + sdrv_ckgen_get_rate(CLK_NODE(g_ckgen_ip_uart_sf_1_to_6)); + } else { + g_uart_config.clk_freq = + sdrv_ckgen_get_rate(CLK_NODE(g_ckgen_ip_uart_sf_7_to_12)); + } +#endif + + /* Initializes sdrv uart controller. */ + + user_data.buff_ptr = user_data.user_buffer; + user_data.buff_remain = BUFFER_SIZE; + + ssdk_printf(SSDK_NOTICE, "uart3 init\r\n"); + sdrv_uart_controller_init(&uart_ctrl_ptr, &g_uart_config, callback, + &user_data); + + sdrv_uart_start_realtime_receive(&uart_ctrl_ptr); + +} + + +void uartTimerProcess(void) +{ + static uint8_t uart_timer = 0; + static uint8_t uart_temp[1] = {2};//мжֵ +//---------------------------------------------------------------------------- + uart_timer ++; + if(uart_timer >= 1)//500msжһ + { + uart_timer = 0; + if(uart_fault_info.remote_count == uart_temp[0])//һʾ + { + uart_fault_info.remote_state = FAULT; + } + else + { + uart_fault_info.remote_state = NORMAL; + uart_temp[0] = uart_fault_info.remote_count;//ݸ + } + + if(uart_fault_info.remote_state != uart_temp[1]) + { +// publishMessage(&uart_fault_info.remote_state, 1);// ״̬仯 ź + uart_temp[1] = uart_fault_info.remote_state; + } + } +} + + + + + + + +//Timer uart_timer_interface; + +// APPģijʼ +void uartAppInit(void) +{ + // ʼʱʹ brake_timer ĵַΪźID +// timerInit(&uart_timer_interface, 100); +// subscribe(&uart_timer_interface, uartTimerProcess); + +// timerStart(&uart_timer_interface); +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/interface_uart.h b/interface_uart.h new file mode 100644 index 0000000..8b48eea --- /dev/null +++ b/interface_uart.h @@ -0,0 +1,157 @@ +#ifndef _INTERFACE_UART_H_ +#define _INTERFACE_UART_H_ + +#include "interface_config.h" +#include + + +#define ZERO_VALUE 1022//ң0ʱΪ1022 + +#define remote_speed un_remote_control_input.bit_data.speed//ٶ +#define remote_curvature un_remote_control_input.bit_data.curvature//ת +#define remote_Reserve un_remote_control_input.bit_data.reserve1// +#define emergency_stop un_remote_control_input.bit_data.switch_a//DF_SwA +#define remote_switch_b un_remote_control_input.bit_data.switch_b//DF_SwB +#define remote_switch_c un_remote_control_input.bit_data.switch_c//DF_SwC +#define remote_switch_d un_remote_control_input.bit_data.switch_d//DF_SwD +#define remote_enable un_remote_control_input.bit_data.enable//ֱʹ״̬ + + + + + + + + + + + + + + + + + + + + + + + +typedef struct +{ + uint16_t remote_count; //ңؼ + uint8_t remote_state; //ң״̬ +}UartFaultInfo; + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +void uart_Initialize(void); + +void callback(sdrv_uart_t *ctrl, sdrv_uart_callback_status_e status, + void *userData); + +#endif /* _INTERFACE_H_ */ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/interface_wdt.c b/interface_wdt.c new file mode 100644 index 0000000..9f377fe --- /dev/null +++ b/interface_wdt.c @@ -0,0 +1,48 @@ +#include +#include +#include +#include +#include +#include "interface_config.h" + +sdrv_wdt_t *wdt_type = (sdrv_wdt_t *)(uintptr_t)APB_WDT1_BASE; + +static bool internal_reset_switch = true; +static bool external_reset_switch = false; + +//Źʼ +void wdt_init(uint32_t timeoutms) +{ + static sdrv_wdt_config_t wdt_cfg; + int ret = 0; +//-------------------------------------------------- + sdrv_wdt_get_default_config(&wdt_cfg); + + if (internal_reset_switch) { + wdt_cfg.int_rst.reset_en = true; + sdrv_rstgen_wdt_reset_enable(RESET_WDT1, true); + } + + if (external_reset_switch) { + /* config GPIO_H12 as WDT1_RST_N function */ + wdt_cfg.ext_rst.reset_en = true; + wdt_cfg.ext_rst.reset_mode = 0; /* low level active */ + } + + ret = sdrv_wdt_init(wdt_type, &wdt_cfg); + ASSERT(ret == 0); + +// irq_attach(wdt_irq, wdt_irq_handler, NULL); +// irq_enable(wdt_irq); + + sdrv_wdt_set_timeout(wdt_type, timeoutms); + + ret = sdrv_wdt_enable(wdt_type); +} + + + + + + + diff --git a/interface_wdt.h b/interface_wdt.h new file mode 100644 index 0000000..d4be720 --- /dev/null +++ b/interface_wdt.h @@ -0,0 +1,64 @@ +#ifndef _INTERFACE_WDT_H_ +#define _INTERFACE_WDT_H_ + +#include + + + +#define WDT_OUTms 1600 + + + + +//ⲿ +void wdt_init(uint32_t timeoutms); + + + + +void External_wdt_refresh(void); + +//ⲿ +extern sdrv_wdt_t *wdt_type; + +// + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +#endif /* _INTERFACE_WDT_H_*/ \ No newline at end of file diff --git a/main.c b/main.c new file mode 100644 index 0000000..3e4f93a --- /dev/null +++ b/main.c @@ -0,0 +1,135 @@ +#include +#include "board.h" +#include "debug.h" +#include "regs_base.h" +#include "irq_num.h" + +#include "clock_cfg.h" +#include "reset_cfg.h" +#include "pinmux_cfg.h" +#include "eth_cfg.h" +#include "lwip/err.h" +#include "lwip/inet.h" +#include "lwip/tcp.h" +#include "lwip/timeouts.h" +#include + +#include "lwip/udp.h" +#include "flexcan_cfg.h" + +#include "interface_config.h" +#include "irq.h" +//app包含 +#include "app/app_config.h" +#include "app/app_param_manage.h" +#include "app/app_power.h" +#include "app/app_brake.h" +#include "app/app_differential_drive.h" +#include +#include +#include +#include +#include +#include +void testAppInit(void); + +#define IP_ADDR0 10 +#define IP_ADDR1 18 +#define IP_ADDR2 252 +#define IP_ADDR3 34 + + +extern int btm_init(void); +extern struct tcp_pcb *netdemo_connect_server(char *ip, int port); +extern bool netdemo_tcp_connected(void); +extern bool netdemo_tcp_send_done(void); +extern void netdemo_clear_tcp_send_done(void); + + + + + +uint8_t mainCnt = 0; +uint8_t mainCntarr[8] = {0x56,0x88,0x98,0x79,0x23}; +uint8_t desIp[4] = {192,168,17,3}; + +uint8_t test_app[26] = {0}; + +int main(void) +{ + initFramework(); + + uint8_t BOOT_Arr[2] = {0x01,0x02};//上电发送app帧给上位机用 + + // 释放MCU各模块reset信号 + board_reset_init(); + + // 配置时钟 + int ret = sdrv_ckgen_init(&g_clock_config); + ASSERT(ret == 0); + + // VIC初始化 + irq_initialize(VIC1_BASE, IRQ_MAX_INTR_NUM); + + // 配置pinmux + sdrv_pinctrl_init(NUM_OF_CONFIGURED_PINS, g_pin_init_config); + + // Timer初始化，用于提供LWIP sys_now接口获取当前时间 + btm_init(); + + // 使能打印 + board_debug_console_init(); + printf("debug_console_init initial OK %d\n",getCurrentTime()); + //初始化所有can + initialization_All_Flexcan(); + printf("initialization_All_Flexcan initial OK %d\n",getCurrentTime()); + // ETH初始化 + board_eth_init(); + + initSpi();//初始化SPI + + //以太网端口初始化 + UDP_Echo_Init(UDPCB_1, udp_Callback_1, COMMUNICATION_PORT); + UDP_Echo_Init(UDPCB_2, udp_Callback_2, UPPER_PORT); + UDP_Echo_Init(UDPCB_3, udp_Callback_3, PARAM_PORT); + UDP_Echo_Init(UDPCB_4, udp_Callback_4, DOWNLOAR_PORT); + UDP_Echo_Init(UDPCB_5, udp_Callback_5, WDT_PORT); + printf("board_eth_init initial OK %d\n",getCurrentTime()); + //读取重启标志 + g_systemDataRecord.canBootloaderUpgrade = rdbyte_24c02(0x00); + printf("rdbyte_24c02 OK %d\n",getCurrentTime()); + + ethernet_parameter.download_ip[0] = rdbyte_24c02(BOOT_DES_IP);//读取下载的IP + ethernet_parameter.download_ip[1] = rdbyte_24c02(BOOT_DES_IP+1);//读取下载的IP + ethernet_parameter.download_ip[2] = rdbyte_24c02(BOOT_DES_IP+2);//读取下载的IP + ethernet_parameter.download_ip[3] = rdbyte_24c02(BOOT_DES_IP+3);//读取下载的IP + + //发送重启后的第一帧给上位机 + 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.72 \n"); + // 初始化框架 放在最前面，解决电机can发送信号累积不处理的问题。 + canInterfaceInit(); + ethernetInterfaceInit(); + bootInterfaceInit(); + + testAppInit(); + paramAppInit(); + diffAppInit(); + brakeAppInit(); + powerAppInit(); //电源管理 + turnAppInit(); //舵机转向 +// tempAppInit(); //温度 + lightAppInit(); //灯光 + baseAppInit(); //基站 +// gpioInterfaceInit(); + requestAppInit(); + + printf("All init OK ------ %d\n",getCurrentTime()); + for (;;) + { + // 处理信号 + processMessages(); + } +}