commit 2acba0caf091368a0e1475dfcb51820c4b3d41ac Author: liumin Date: Sat Oct 4 16:06:55 2025 +0800 第一次提交 diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..c097e3a9 --- /dev/null +++ b/.gitignore @@ -0,0 +1,9 @@ +# 忽略整个目录及其所有子文件和子目录 +/configs/ +/IAR/ +/IAR_Norflash/ +/SES/ +/configs/** +/IAR/** +/IAR_Norflash/** +/SES/** \ No newline at end of file diff --git a/MFC2AF5DEDB.tmp b/MFC2AF5DEDB.tmp new file mode 100644 index 00000000..f2942d86 --- /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_brake.c b/app/app_brake.c new file mode 100644 index 00000000..eb882c4e --- /dev/null +++ b/app/app_brake.c @@ -0,0 +1,238 @@ +#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 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 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_02 = setBrakeOff(); + un_h_bridge_output1.bit_data.channel_01 = setBrakeOn(); + un_h_bridge_output1.bit_data.channel_02 = setBrakeOff(); + printf("Brake: Motor forward\n"); + break; + + case 2: // 电机后退状态 + un_h_bridge_output.bit_data.channel_01 = setBrakeOff(); + un_h_bridge_output.bit_data.channel_02 = setBrakeOn(); + un_h_bridge_output1.bit_data.channel_01 = setBrakeOff(); + un_h_bridge_output1.bit_data.channel_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_02 = setBrakeOff(); + un_h_bridge_output1.bit_data.channel_01 = setBrakeOff(); + un_h_bridge_output1.bit_data.channel_02 = setBrakeOff(); + printf("Brake: Motor off\n"); + break; + } + + publishMessage(&un_h_bridge_output, 1); + publishMessage(&un_inf_can_kgf_output1, 1); +} + + + +// 修改刹车定时器处理函数 +static void brakeTimerProcess(void *signal_id) +{ + (void)signal_id; + +//#ifdef OIL_BRAKE + switch (brake_data.state) + { + case BRAKE_STATE_IDLE: + 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; + + printf("writeE2 brake_position = %d\n",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 00000000..6c0a7b0e --- /dev/null +++ b/app/app_brake.h @@ -0,0 +1,60 @@ +#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" + +#define OIL_BRAKE 0 +#define ELECTROMAGNETIC_BRAKE 1 + + +// 定义刹车状态机状态 +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 00000000..91fa977f --- /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.h b/app/app_dependence.h new file mode 100644 index 00000000..47cd146c --- /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.c b/app/app_differential_drive.c new file mode 100644 index 00000000..b3cb677e --- /dev/null +++ b/app/app_differential_drive.c @@ -0,0 +1,1068 @@ +#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; + +// 定义全局变量 +DiffData diff_data; + +PID_t speed_pid; +PID_t yaw_rate_pid; + +PID_t Acc_front_speed_pid; +PID_t Dec_front_speed_pid; + + +/** + * @brief 根据挡位和输入转矩计算输出转矩值 + * @param gear 挡位状态（STATE_FORWARD/STATE_BACKWARD/其他） + * @param input_torque 输入转矩值 + * @return 处理后的转矩值（已包含偏移量和系数） + */ +float calculateTorqueOutput(uint8_t gear, float input_torque) +{ + const float OFFSET = 300.0f; // 偏移量常量 + const float SCALE_FACTOR = 100.0f; // 缩放系数 + const float DEFAULT_VALUE = 30000.0f; // 默认输出值 + + float output_torque; + + if (gear == STATE_FORWARD) + { + output_torque = (input_torque + OFFSET) * SCALE_FACTOR; + } + else if (gear == STATE_BACKWARD) + { + output_torque = (-input_torque + OFFSET) * SCALE_FACTOR; + } + else + { + output_torque = DEFAULT_VALUE; + } + + return output_torque; +} + +/** + * @brief 车辆状态控制状态机 + * @note 根据车速和扭矩方向切换前进/后退状态，带扭矩回滞保护 + * @param ctx 状态机上下文，包含当前状态(STATE_INIT/FORWARD/BACKWARD) + * @param speed 当前车速（单位：km/h），0表示静止状态 + * @param torque 当前扭矩（单位：Nm），正数表示前进方向，负数表示后退方向 + */ +void handleVehicleState(MotorState *ctx, float speed, float torque) +{ + switch (*ctx) + { + // 初始状态：根据扭矩方向初始化 + case STATE_INIT: + { + if (torque >= 0.0f) + { + *ctx = STATE_FORWARD; // 正扭矩进前进档 + } + else + { + *ctx = STATE_BACKWARD; // 负扭矩进倒档 + } + break; + } + + // 前进状态：零速且反向扭矩超阈值切倒档 + case STATE_FORWARD: + { + if ( (speed == 0.0f) && (torque <= -TORQUE_HYSTERESIS_THRESHOLD) ) + { + *ctx = STATE_BACKWARD; // 满足条件切换 + } + else + { + *ctx = STATE_FORWARD; // 否则保持 + } + break; + } + + // 倒车状态：零速且正向扭矩超阈值切前进 + case STATE_BACKWARD: + { + if ( (speed == 0.0f) && (torque >= TORQUE_HYSTERESIS_THRESHOLD) ) + { + *ctx = STATE_FORWARD; // 满足条件切换 + } + else + { + *ctx = STATE_BACKWARD; // 否则保持 + } + break; + } + + default:; // 异常处理 + } +} + + +// 设置电机输出 +void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint16_t discharge_power) +{ + + float abs_left_front_speed = 0; + float abs_right_front_speed = 0; + float abs_left_rear_speed = 0; + float abs_right_rear_speed = 0; + + // 档位 + abs_left_front_speed = calculateTorqueOutput(diff_data.motor_state[0], out_torq[0]); //根据挡位增加转矩方向 + abs_right_front_speed = calculateTorqueOutput(diff_data.motor_state[1], out_torq[1]); + abs_left_rear_speed = calculateTorqueOutput(diff_data.motor_state[2], out_torq[2]); + abs_right_rear_speed = calculateTorqueOutput(diff_data.motor_state[3], out_torq[3]); + + un_motor_output1.bit_data.gear = diff_data.motor_state[0]; + un_motor_output2.bit_data.gear = diff_data.motor_state[1]; + + if(STATE_FORWARD == diff_data.motor_state[2])//把后两台电机反相 + { + un_motor_output3.bit_data.gear = STATE_BACKWARD; + } + else if(STATE_BACKWARD == diff_data.motor_state[2]) + { + un_motor_output3.bit_data.gear = STATE_FORWARD; + } + else + { + un_motor_output3.bit_data.gear = STATE_INIT; + } + + if(STATE_FORWARD == diff_data.motor_state[3]) + { + un_motor_output4.bit_data.gear = STATE_BACKWARD; + } + else if(STATE_BACKWARD == diff_data.motor_state[3]) + { + un_motor_output4.bit_data.gear = STATE_FORWARD; + } + else + { + un_motor_output4.bit_data.gear = STATE_INIT; + } + + + // 设置左右电机期望转速 +// un_motor_output1.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_left_speed) + 30000); // 20240921 增加偏移量 30000 +// un_motor_output2.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_right_speed) + 30000); // 20240921 增加偏移量 30000 + + // 设置模式为扭矩模式 + un_motor_output1.bit_data.mode = MOTOR_MODE; + un_motor_output2.bit_data.mode = MOTOR_MODE; + un_motor_output3.bit_data.mode = MOTOR_MODE; + un_motor_output4.bit_data.mode = MOTOR_MODE; + + // 设置扭矩 + un_motor_output1.bit_data.set_torque = (uint16_t)( (int16_t)abs_left_front_speed ); + un_motor_output2.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_front_speed ); + un_motor_output3.bit_data.set_torque = (uint16_t)( (int16_t)abs_left_rear_speed ); + un_motor_output4.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_rear_speed ); + + //设定转速 + un_motor_output1.bit_data.set_rotation_speed = 30000; + un_motor_output2.bit_data.set_rotation_speed = 30000; + un_motor_output3.bit_data.set_rotation_speed = 30000; + un_motor_output4.bit_data.set_rotation_speed = 30000; + + // 设置馈电功率 + un_motor_output1.bit_data.feed_power = feed_power; + un_motor_output2.bit_data.feed_power = feed_power; + un_motor_output3.bit_data.feed_power = feed_power; + un_motor_output4.bit_data.feed_power = feed_power; + + + // 设置放电功率 + un_motor_output1.bit_data.discharge_power = discharge_power; + un_motor_output2.bit_data.discharge_power = discharge_power; + un_motor_output3.bit_data.discharge_power = discharge_power; + un_motor_output4.bit_data.discharge_power = discharge_power; + + +} + + +// 限制值在最小值和最大值之间 +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); + +// float left_speed_mps = 0; +// float right_speed_mps = 0; + +// float left_speed_mps = 0; +// float right_speed_mps = 0; + + // 计算当前速度 + diff_data.speed = (left_speed_mps + right_speed_mps) / 2.0f; + // 计算速度差 + 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 rpm1 电机1当前转速（单位：rpm） + * @param rpm2 电机2当前转速（单位：rpm） + * @param total_torque 系统总需求扭矩（单位：Nm） + * @param torque1 [out] 电机1分配到的扭矩（单位：Nm） + * @param torque2 [out] 电机2分配到的扭矩（单位：Nm） + * @note 分配原则：转速越高的电机分配扭矩越小，确保负载均衡 + */ +void distributeTorque(float rpm1, float rpm2, float total_torque, float* torque1, float* torque2, float max_torque, float min_torque) +{ + + // 总扭矩为0时快速返回 + if (fabs(total_torque) < 0.001f) { + *torque1 = 0.0f; + *torque2 = 0.0f; + return; + } + +// // 保护条件：当两电机均静止时采用平均分配策略 +// if (fabs(rpm1) < 0.001f && fabs(rpm2) < 0.001f) { +// *torque1 = total_torque / 2.0f; +// *torque2 = total_torque / 2.0f; +// return; +// } + + // 计算权重因子（与转速成反比关系） + // 注：添加0.001f防止零转速时除零错误，fabs确保负转速正确处理 + float weight1 = 1.0f / (fabs(rpm1) + 0.001f); + float weight2 = 1.0f / (fabs(rpm2) + 0.001f); + + // 归一化计算分配比例 + float total_weight = weight1 + weight2; + *torque1 = total_torque * (weight1 / total_weight); + *torque2 = total_torque * (weight2 / total_weight); + + // 独立限制单侧扭矩（修改核心逻辑） + if (fabs(*torque1) > max_torque) { + *torque1 = copysignf(max_torque, *torque1); + } + if (fabs(*torque2) > max_torque) { + *torque2 = copysignf(max_torque, *torque2); + } + + // 仅对非零扭矩应用下限限制 + if (fabs(*torque1) < min_torque) { + *torque1 = copysignf(min_torque, *torque1); + } + if ( fabs(*torque2) < min_torque) { + *torque2 = copysignf(min_torque, *torque2); + } + +} + + + +/** + * @brief 根据轮速差动态调整电机扭矩（带非负限制） + * @param speed_left 左轮速度（单位：rpm或自定义） + * @param speed_right 右轮速度（单位：rpm或自定义） + * @param torque_left 左轮扭矩指针（单位：Nm或自定义） + * @param torque_right 右轮扭矩指针（单位：Nm或自定义） + * @param threshold 触发调整的速差阈值（单位同轮速） + * @param k 扭矩调整系数（无量纲，建议0 threshold) { + // 计算需要减少的扭矩量（速差超出阈值部分×系数） + float torque_reduction = (speed_diff - threshold) * k; + + if (speed_left > speed_right) { + // 左轮过快时减少左扭矩，并限制最小值为0 + *torque_left = fmaxf(*torque_left - torque_reduction, 0.0f); + } else { + // 右轮过快时减少右扭矩，并限制最小值为0 + *torque_right = fmaxf(*torque_right - torque_reduction, 0.0f); + } + } +} +// 计算左右电机速度 +void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max_speed, float *motor_speed) +{ + // 防止速度过低导致不必要的计算 + 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 left_speed_mps = 0.0f; + float right_speed_mps = 0.0f; + + float max_torque = diff_data.max_Torq;//不需要限制，PID输出已经限制了 +// linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque); +// yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque); + + if( diff_data.min_Torq > fabs(linear_velocity_x) )//20250728 增加死区 解决手柄回中，不停车问题 + { + linear_velocity_x = 0; + } + + left_speed_mps = linear_velocity_x + yaw_rate; + right_speed_mps = linear_velocity_x - yaw_rate; + +//扭矩分配 + if(max_torque < left_speed_mps) + { + right_speed_mps = right_speed_mps - (left_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差 + left_speed_mps = max_torque; + } + else if(-max_torque > left_speed_mps) + { + right_speed_mps = right_speed_mps - (left_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差 + left_speed_mps = -max_torque; + } + else if(max_torque < right_speed_mps) + { + left_speed_mps = left_speed_mps - (right_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差 + right_speed_mps = max_torque; + } + else if(-max_torque > right_speed_mps) + { + left_speed_mps = left_speed_mps - (right_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差 + right_speed_mps = -max_torque; + } + else{} + +// printf("input_torq: left=%.1f right=%.1f yaw_rate=%.1f\n", left_speed_mps, right_speed_mps, yaw_rate); +// adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],100, 5); +// adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],100, 5); +// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed); + + motor_speed[0] = left_speed_mps;//加速状态，没有负扭矩，要么前进加速要么后退加速 + motor_speed[2] = left_speed_mps; + + motor_speed[1] = right_speed_mps; + motor_speed[3] = right_speed_mps; + + handleVehicleState(&diff_data.motor_state[0], diff_data.left_front_motor_speed, motor_speed[0]); //通过扭矩以及速度来判断挡位 + handleVehicleState(&diff_data.motor_state[1], diff_data.right_front_motor_speed, motor_speed[1]); + handleVehicleState(&diff_data.motor_state[2], diff_data.left_rear_motor_speed, motor_speed[2]); + handleVehicleState(&diff_data.motor_state[3], diff_data.right_rear_motor_speed, motor_speed[3]); + + +// distributeTorque(diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed,2*left_speed_mps,&motor_speed[0],&motor_speed[2],diff_data.max_Torq,diff_data.min_Torq); +// distributeTorque(diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed,2*right_speed_mps,&motor_speed[1],&motor_speed[3],diff_data.max_Torq,diff_data.min_Torq); + +#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; //20250316 为解决原地转向和直行转向相同，所以把左右输出的速度交换 + 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_rpm = -left_motor_rpm; + // 返回计算结果 + *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); +//diff_data.desired_speed, 0.1, 2, 10, 1, 5 + +// diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5); + + 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);// 0.2 + （3 - 0.5）* （15-0.2） / （17 - 0.5） + } + 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时，设定为原地转向 20250321 修改为考虑负号 + if( (diff_data.desired_speed >= 0) && (diff_data.desired_speed <= 1.0f) ) + { + diff_data.desired_yaw_rate = diff_data.desired_curvature * 1.0f; + } + else if( (diff_data.desired_speed < 0) && (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; + } + +// printf("desired_speed: %f, desired_yaw: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate); + // 使用 PID 控制器计算输出速度和曲率 + float output_speed = calculatePidOutput(&speed_pid, diff_data.desired_speed, diff_data.speed, 0.0f, dt); + float output_yaw_rate = calculatePidOutput(&yaw_rate_pid, diff_data.desired_yaw_rate, diff_data.yaw_rate, 0.0f, dt); + + // 计算最大加速度，用函数计算 + float max_acceleration = calculateMaxAcceleration(); + // 限制输出速度在当前速度和最大加速度计算出来的速度之间 + // output_speed = constrain(output_speed, diff_data.speed - max_acceleration * dt, diff_data.speed + max_acceleration * dt); + + if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) && ( 10 > fabs(diff_data.left_motor_speed) ) && ( 10 > fabs(diff_data.right_motor_speed) ) )//手柄回中，速度小的时候清0 + { + resetPidIntegral(&speed_pid); + resetPidIntegral(&yaw_rate_pid); + output_speed = 0; + output_yaw_rate = 0; + } + + // 使用差速车辆动力学模型计算左右电机的期望速度 + float out_torque[4] = {0,0,0,0}; + // 使用差速车辆动力学模型计算左右电机的期望速度 + computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, out_torque); + + if( fabs(diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed) >= diff_data.diff_dead_zone )//如果超过系数 + { + diff_data.left_speed_diff = diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed; + diff_data.left_diff_touue = calculatePidOutput(&Acc_front_speed_pid, 0.0f, diff_data.left_speed_diff, 0.0f, dt); //左侧转速差PID + } + else + { + diff_data.left_speed_diff = 0; + Acc_front_speed_pid.integral = 0; + diff_data.left_diff_touue = 0; + } + + + if( fabs(diff_data.right_front_motor_speed - diff_data.right_rear_motor_speed) >= diff_data.diff_dead_zone )//如果超过系数 + { + diff_data.right_speed_diff = diff_data.right_front_motor_speed - diff_data.right_rear_motor_speed; + diff_data.right_diff_touue = calculatePidOutput(&Dec_front_speed_pid, 0.0f, diff_data.right_speed_diff, 0.0f, dt); //左侧转速差PID + } + else + { + diff_data.right_speed_diff = 0; + Dec_front_speed_pid.integral = 0; + diff_data.right_diff_touue = 0; + } + + + if(out_torque[0] > 0)//根据大小来限定值为分配扭矩。最小就是0扭矩。 + { + diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, -out_torque[0], out_torque[0]); + } + else + { + diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, out_torque[0], -out_torque[0]); + } + + + if(out_torque[1] > 0) + { + diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, -out_torque[1], out_torque[1]); + } + else + { + diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, out_torque[1], -out_torque[1]); + } + + + diff_data.out_torq[0] = (out_torque[0] + diff_data.left_diff_touue);//因为每一个电机都是相同的扭矩，所以扭矩和为2倍。 + diff_data.out_torq[2] = (out_torque[0] - diff_data.left_diff_touue); + + diff_data.out_torq[1] = (out_torque[1] + diff_data.right_diff_touue); + diff_data.out_torq[3] = (out_torque[1] - diff_data.right_diff_touue); + + out_torque[0] = constrain(out_torque[0], -diff_data.max_Torq, diff_data.max_Torq); //限定最大扭矩 + out_torque[1] = constrain(out_torque[1], -diff_data.max_Torq, diff_data.max_Torq); + out_torque[2] = constrain(out_torque[2], -diff_data.max_Torq, diff_data.max_Torq); + out_torque[3] = constrain(out_torque[3], -diff_data.max_Torq, diff_data.max_Torq); + +// printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral); + + + + // 设置电机输出 + setMotorOutput(&diff_data.out_torq[0], + diff_data.max_Torq,// + (uint16_t)getParam("feedPwr"), + (uint16_t)getParam("dispPwr")); + // 发布左右电机期望转速,电源在工作状态才能发送 + if (power_data.current_state == POWER_WORKING) + { + publishMessage(&un_motor_output1, 1); + publishMessage(&un_motor_output2, 1); + publishMessage(&un_motor_output3, 1); + publishMessage(&un_motor_output4, 1); + + } + + + un_can_debug_output.bit_data.speed = (uint8_t)(int8_t)(diff_data.speed*10); + un_can_debug_output.bit_data.desired_speed = (uint8_t)(int8_t)(diff_data.desired_speed*10); + + un_can_debug_output.bit_data.curvature = (uint8_t)(int8_t)(diff_data.yaw_rate*10); + un_can_debug_output.bit_data.desired_curvature = (uint8_t)(int8_t)(diff_data.desired_yaw_rate*10); + + un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(diff_data.left_speed_diff); + un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(diff_data.right_speed_diff); + + publishMessage(&diff_data, 1); + +} + +/****************************************************************************** + Filter(); N个数中取两个 +******************************************************************************/ +int16_t Filter(int16_t *s,uint8_t Len) +{ + uint8_t i,j; + int16_t temp; +//降序排序 + for(i=0;i*(s+j)) + { + *(s+i)=*(s+i)^*(s+j); + *(s+j)=*(s+j)^*(s+i); + *(s+i)=*(s+i)^*(s+j); + } + } + temp=(*(s+Len/2)+*(s+(Len/2-1)))/2;//20210225修改为除以2，负数不能够右移 + return(temp); +} + + + + + + +// 差速输入处理函数 +static void diffInput(void *signal_id) +{ + float motor_speed_temp = 0.0f; + + if (signal_id == &un_sw_sample) + { + diff_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch; + } + 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, 1, 20, 5, 5, 0.5); + diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0.1, 2, 2, 1, 0.5); + + diff_data.desired_curvature = -diff_data.desired_curvature; + + if(diff_data.desired_speed >= 0)//20250320 增加根据速度大小来决定方向，解决后退时转弯反向的问题 + { + diff_data.desired_curvature = diff_data.desired_curvature; + } + else + { + 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, 2, 10, 1, 5);//20250320 修改死区为0.2解决停不住的问题 + 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) )// 处理第一个电机速度信号（左电机） + { + diff_data.left_front_motor_speed = (float)((int16_t)(un_motor_input1.bit_data.speed - 30000));//20240921 增加偏移量 + diff_data.left_rear_motor_speed = (float)((int16_t)(un_motor_input3.bit_data.speed - 30000));//20240921 增加偏移量 + diff_data.left_rear_motor_speed = - diff_data.left_rear_motor_speed;//20250708 增加反相 + + if(fabs(diff_data.left_rear_motor_speed) > fabs(diff_data.left_front_motor_speed))//取速度较小的轮速 + { + motor_speed_temp = diff_data.left_front_motor_speed; + } + else + { + motor_speed_temp = diff_data.left_rear_motor_speed; + } + diff_data.left_motor_speed = motor_speed_temp; + } + else if( (signal_id == &un_motor_input2) || (signal_id == &un_motor_input4) )// 处理第二个电机速度信号（右电机） + { + diff_data.right_front_motor_speed = (float)((int16_t)(un_motor_input2.bit_data.speed - 30000)); // 20250502 1号控制器增加反相 + diff_data.right_rear_motor_speed = (float)((int16_t)(un_motor_input4.bit_data.speed - 30000)); + + diff_data.right_rear_motor_speed = - diff_data.right_rear_motor_speed;//20250708 增加反相 + + if(fabs(diff_data.right_front_motor_speed) > fabs(diff_data.right_rear_motor_speed))//取速度较小的轮速 + { + motor_speed_temp = diff_data.right_rear_motor_speed; + } + else + { + motor_speed_temp = diff_data.right_front_motor_speed; + } + + diff_data.right_motor_speed = motor_speed_temp; + } + + // 急停开关 + diff_data.emergency_stop_state = (uint8_t)(diff_data.emergency_stop_switch == app_close() || diff_data.remote_emergency_stop == app_close()); + + // 如果急停被激活,强制设定速度为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 = 5;//停车 就为0 20250425 修改为5，解决手柄回中，震荡问题 +// } +// else +// { +// diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩 +// } + if((power_data.current_state == POWER_WORKING))//电机上电才运行 + { + diffProcess(&diff_data);//计算左右电机期望转速 + } + else + { + resetPidIntegral(&speed_pid); + resetPidIntegral(&yaw_rate_pid); + diff_data.motor_state[0] = STATE_INIT; + diff_data.motor_state[1] = STATE_INIT; + diff_data.motor_state[2] = STATE_INIT; + diff_data.motor_state[3] = STATE_INIT; + } + +} + + + +// 预充完成处理函数 +void preChargeFinish(void *signal_id) +{ + (void)signal_id; + + float out_torq[4] = {0.0f,0.0f,0.0f,0.0f}; + + setMotorOutput(out_torq, (uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr")); + // 档位 + un_motor_output1.bit_data.gear = 0; // 0表示空挡 + un_motor_output2.bit_data.gear = 0; + un_motor_output3.bit_data.gear = 0; // 0表示空挡 + un_motor_output4.bit_data.gear = 0; + + publishMessage(&un_motor_output1, 1); + publishMessage(&un_motor_output2, 1); + publishMessage(&un_motor_output3, 1); + publishMessage(&un_motor_output4, 1); +} + + +void diffParametersInit(void *signal_id) +{ + (void)signal_id; // 标记变量为已使用，避免编译器警告 + + if(diff_data.mode == MODE_AUTO)//20250504 自动模式PID + { + setPidParameters(&speed_pid, + getParam("Ospd_kp"), + getParam("Ospd_ki"), + getParam("Ospd_kd"), + getParam("Ospd_il"), + getParam("Ospd_ol") + ); + + setPidParameters(&yaw_rate_pid, + getParam("Ocrv_kp"), + getParam("Ocrv_ki"), + getParam("Ocrv_kd"), + getParam("Ocrv_il"), + getParam("Ocrv_ol") + ); + } + else//手动模式 + { + setPidParameters(&speed_pid, + getParam("spd_kp"), + getParam("spd_ki"), + getParam("spd_kd"), + getParam("spd_il"), + getParam("spd_ol") + ); + + setPidParameters(&yaw_rate_pid, + getParam("crv_kp"), + getParam("crv_ki"), + getParam("crv_kd"), + getParam("crv_il"), + getParam("crv_ol") + ); + } + + // 设置曲率 PID 控制器的参数 + setPidParameters(&Dec_front_speed_pid, + getParam("mot_kp"), + getParam("mot_ki"), + getParam("mot_kd"), + getParam("mot_il"), + getParam("mot_ol") + ); + + // 设置曲率 PID 控制器的参数 + setPidParameters(&Acc_front_speed_pid, + Dec_front_speed_pid.kp, + Dec_front_speed_pid.ki, + Dec_front_speed_pid.kd, + Dec_front_speed_pid.integral_limit, + Dec_front_speed_pid.output_limit + ); + + diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩 + diff_data.max_Torq = (float)getParam("maxTorq"); + + if(0 == (float)getParam("diff_sp"))//20250711 防止参数为0，影响计算。 + { + diff_data.diff_dead_zone = 2; + } + else + { + diff_data.diff_dead_zone = (float)getParam("diff_sp");//参数读取设定最大扭矩 + } + + + + printf("desired_speed: %f, desired_yaw_rate: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate); + printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate); +// printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate); + + printf("LF_speed = %f,RF_speed = %f,LR_speed = %f,RR_speed = %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("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed); + printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", diff_data.out_torq[0], diff_data.out_torq[1], diff_data.out_torq[2], diff_data.out_torq[3]); + + float deffspeed = (float)((int16_t)(un_remote_control_input.bit_data.speed)); + float deffcurvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature)); + // 单位转换 + deffspeed = deffspeed * 0.01f; + deffcurvature = deffcurvature * 0.0001f; + + printf("remote_speed: %f, remote_yaw_rate: %f\n", deffspeed, deffcurvature); + + printf(" car state = %d,%d,%d,%d\n", diff_data.motor_state[0],diff_data.motor_state[1],diff_data.motor_state[2],diff_data.motor_state[3]); + + 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调用一次 + + printf("diffControl: diffAppInit OK \n"); +} diff --git a/app/app_differential_drive.h b/app/app_differential_drive.h new file mode 100644 index 00000000..ad816075 --- /dev/null +++ b/app/app_differential_drive.h @@ -0,0 +1,96 @@ +#ifndef APP_DIFFERENTIAL_DRIVE_H +#define APP_DIFFERENTIAL_DRIVE_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "app_config.h" + +#define SPEED_FITER_NUM 6 + + +#define SPEED_PID_MODE 0 +#define THROTTLE_PID_MODE 1 + +#define TURN_MIN_TOUQUE 1 //n*m + +#define SPEED_MODE 0x01 +#define TORQUE_MODE 0x02 + + +#define TORQUE_HYSTERESIS_THRESHOLD 0.3f + + +#define MOTOR_MODE TORQUE_MODE + + + +#define ALPHA 0.1f // 滤波系数α∈[0.01,0.3]，0.2对应截止频率约10Hz（假设采样周期10ms） +#define LOWPASS_FILTER(speed, prev) (ALPHA * (speed) + (1 - ALPHA) * (prev)) + +// 状态机内部状态 +typedef enum +{ + STATE_INIT, ///< 初始状态（转速为0且等待扭矩方向判定） + STATE_FORWARD, ///< 前进 + STATE_BACKWARD, ///< 后退 +} MotorState; + +typedef enum +{ + MODE_MANUAL, // 手动模式 + MODE_AUTO // 自动模式 +} ControlMode; + +typedef struct DiffData +{ + ControlMode mode ; // 控制模式 + MotorState motor_state[4]; //当前车辆状态 + 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; // 当前右后电机速度 + 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; // 输出右电机速度 + float out_torq[4]; //4个电机扭矩 + float max_Torq; // 最大扭矩限制 + float min_Torq; // 最小扭矩限制 + + float left_speed_diff; // 左侧转速差 + float right_speed_diff; // 右侧转速差 + + float left_diff_touue; // 左侧扭矩差 + float right_diff_touue; // 右侧扭矩差 + float diff_dead_zone; // 差速速度死区 + +} 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 00000000..96f9d12e --- /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 00000000..00dc8e60 --- /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 00000000..e8e4bac1 --- /dev/null +++ b/app/app_frm_signal.c @@ -0,0 +1,324 @@ +#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 (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("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 00000000..f598bfa0 --- /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 50u // 不同信号的订阅者数量 +#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 00000000..bedf8825 --- /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 00000000..3e76cc27 --- /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 00000000..14084c4b --- /dev/null +++ b/app/app_light.c @@ -0,0 +1,240 @@ +#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_output1.bit_data.KGF05 = state_value; + un_inf_can_kgf_output1.bit_data.KGF06 = state_value; + un_inf_can_kgf_output1.bit_data.KGF10 = state_value; + un_inf_can_kgf_output1.bit_data.KGF11 = state_value; + + break; + case LIGHT_TAIL://尾灯，后面4个灯 + un_inf_can_kgf_output2.bit_data.KGF12 = state_value; + un_inf_can_kgf_output2.bit_data.KGF13 = state_value; + un_inf_can_kgf_output2.bit_data.KGF14 = state_value; + un_inf_can_kgf_output2.bit_data.KGF15 = state_value; + break; + case LIGHT_LEFT_TURN://左转向，左边4个灯 + un_inf_can_kgf_output1.bit_data.KGF06 = state_value; + un_inf_can_kgf_output1.bit_data.KGF11 = state_value; + un_inf_can_kgf_output2.bit_data.KGF12 = state_value; + un_inf_can_kgf_output2.bit_data.KGF13 = state_value; + break; + case LIGHT_RIGHT_TURN://右转向灯，右边4个灯 + un_inf_can_kgf_output1.bit_data.KGF05 = state_value; + un_inf_can_kgf_output1.bit_data.KGF10 = state_value; + un_inf_can_kgf_output2.bit_data.KGF14 = state_value; + un_inf_can_kgf_output2.bit_data.KGF15 = state_value; + break; + case LIGHT_BRAKE://刹车灯，四个黄灯 + un_inf_can_kgf_output1.bit_data.KGF10 = state_value; + un_inf_can_kgf_output1.bit_data.KGF11 = state_value; + un_inf_can_kgf_output2.bit_data.KGF13 = state_value; + un_inf_can_kgf_output2.bit_data.KGF15 = state_value; + break; + case LIGHT_ALARM://报警灯，四个红灯 + un_inf_can_kgf_output1.bit_data.KGF05 = state_value; + un_inf_can_kgf_output1.bit_data.KGF06 = state_value; + un_inf_can_kgf_output2.bit_data.KGF12 = state_value; + un_inf_can_kgf_output2.bit_data.KGF14 = state_value; + 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_output1, 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 00000000..fb77356f --- /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 00000000..542e18e3 --- /dev/null +++ b/app/app_param_manage.c @@ -0,0 +1,493 @@ +#include "app_config.h" +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" +#include "app_param_manage.h" + + +#define E2_RESERVE_COUNT 0x20 //增加IP地址 修改为0x20 20250110 + +// 待发送的参数请求信号 +UnParamRequest un_param_request1 ; +UnParamRequest un_param_request2; + +RequestContext request_send ; +RequestContext request_context ; + +uint8_t read_write_e2_finished = 0; + +// 定义全局信号实例，读写信号现在包括 offset 和 size +ParamSignal param_signal = { + .param_ptr = NULL, // 参数指针初始化为 NULL + .type = READ_OPERATION, // 操作类型设置为读操作 + .offset = 0, // 整个数据块的偏移 + .size = sizeof(param_manager.arr) // 整个数据块的大小 +}; + + +// 全局变量：初始化参数名称结构体 +ParamNames param_names = { +#define X(name) .name = #name, + PARAM_LIST +#undef X +}; + + +UnParamManager param_manager ; + +static uint8_t is_param_initialized = 0; + +// 打印所有参数的名称和值, 每行 4 个参数 +void printParams() +{ + unsigned int param_count = 0; + + #define X(name) \ + printf("%-8s: %-8.2f", param_names.name, param_manager.bit_data.name); \ + param_count++; \ + if (param_count % 4 == 0) { \ + printf("\n"); \ + } else { \ + printf(" "); \ + } + PARAM_LIST + #undef X + + // 如果最后一行不足 4 个参数, 打印换行 + if (param_count % 4 != 0) + { + printf("\n"); + } +} + + + +void writeByte24c02(uint16_t addr, uint8_t data) +{ + if(0 != wrbyte_24c02(addr,data)) + { + printf("E2PROM write error!\n"); + } +} + +uint8_t readByte24c02(uint16_t addr) +{ + return rdbyte_24c02(addr); +} + +// 定义一个通用的 EEPROM 访问函数 +uint8_t accessEeprom(size_t offset, void *data, size_t size, OperationType type) +{ + if (data == NULL || size == 0) + { + return 1; // 返回错误状态，表示无效的参数 + } + + uint8_t *byte_data = (uint8_t *)data; // 将 void* 转换为 uint8_t*，方便逐字节操作 + size_t index; + + if (type == WRITE_OPERATION) + { + // 写入操作 + for (index = 0; index < size; index++) + { + writeByte24c02((uint16_t)(offset + index + E2_RESERVE_COUNT), byte_data[index]); + udelay(4000);//写入一个字节延时4ms + } + // 校验 + for (index = 0; index < size; index++) + { + if (readByte24c02((uint16_t)(offset + index + E2_RESERVE_COUNT)) != byte_data[index]) + { + return 2; // 返回错误状态，表示写入验证失败 + } + } + } + else + { + // 读取操作 + for (index = 0; index < size; index++) + { + byte_data[index] = readByte24c02((uint16_t)(offset + index + E2_RESERVE_COUNT)); + } + } + return 0; // 返回状态，表示成功 +} + +void handleParamOp(void *data) +{ + ParamSignal *signal = (ParamSignal *)data; + + if (signal->param_ptr == NULL) + { + // 操作整个参数管理器 + if (accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), signal->type) == 0) + { + read_write_e2_finished = 1; + publishMessage(&read_write_e2_finished, 1); // 读写成功 + } + else + { + read_write_e2_finished = 2; + publishMessage(&read_write_e2_finished, 1); // 读写失败 + } + } + else + { + // 根据信号中的偏移和大小操作单个参数 + if (accessEeprom(signal->offset, signal->param_ptr, signal->size, signal->type) == 0) + { + read_write_e2_finished = 1; + publishMessage(&read_write_e2_finished, 1); // 读写成功 + } + else + { + read_write_e2_finished = 2; + publishMessage(&read_write_e2_finished, 1); // 读写失败 + } + } +} + + +uint8_t calculateCRC(const uint8_t* data, uint32_t length) { + uint8_t crc = 0; + for (uint32_t i = 0; i < length; ++i) { + crc += data[i]; // 简单的校验和，按字节累加 + } + return crc; +} + + +float readParameter(const char *param_name) { + float float_value = 0; + unsigned int offset = 0; + + #define X(name) \ + if (strcmp(param_name, param_names.name) == 0) { \ + accessEeprom(offset, ¶m_manager.bit_data.name,sizeof(param_manager.bit_data.name), READ_OPERATION);\ + memcpy(&float_value, ¶m_manager.bit_data.name, sizeof(param_manager.bit_data.name)); \ + return float_value; \ + } \ + offset += 4; + + PARAM_LIST + #undef X + + printf("Parameter not found: %s\n", param_name); + return 0; +} + + + +void writeParameter(const char *param_name, const uint8_t *data) { + unsigned int offset = 0; + + #define X(name) \ + if (strcmp(param_name, param_names.name) == 0) { \ + memcpy(¶m_manager.bit_data.name, data, sizeof(param_manager.bit_data.name)); \ + accessEeprom(offset, ¶m_manager.bit_data.name,sizeof(param_manager.bit_data.name), WRITE_OPERATION);\ + return; \ + } \ + offset += 4; + + PARAM_LIST + #undef X + + printf("Parameter not found: %s\n", param_name); +} + + + +void sendParamRequestResponse(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port, uint8_t isWriteOperation) { + // 准备响应帧 + paramRequest->bit_data.frame_header = 0xFF80; + paramRequest->bit_data.frame_type = 0x002B; + paramRequest->bit_data.frame_length = sizeof(StrParamRequest); + paramRequest->bit_data.accumulated = 0; + paramRequest->bit_data.request_id = isWriteOperation ? 98 : 99; + + paramRequest->bit_data.crc = calculateCRC(paramRequest->arr, sizeof(paramRequest->arr) - 1); + + request_send.param_request = paramRequest; + request_send.sender_ip = sender_ip; + request_send.sender_port = sender_port; + // 发送信号，从UDP发送 + publishMessage(&request_send, 1); +} + + +void processReadAllParams(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) { + uint8_t allParams[256][4]; // Size based on E2 size + unsigned int i = 0; + float param_value; + uint8_t exceeded_max = 0; // 新增标志变量 + + // 清零 paramRequest + memset(paramRequest, 0, sizeof(UnParamRequest)); + + accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION); + + printf("Sending parameter data:\n"); + + #define X(name) \ + if (!exceeded_max) { \ + if (i < 256) { \ + strncpy((char *)paramRequest->bit_data.param_name[i], #name, sizeof(paramRequest->bit_data.param_name[i]) - 1); \ + paramRequest->bit_data.param_name[i][sizeof(paramRequest->bit_data.param_name[i]) - 1] = '\0'; \ + memcpy(allParams[i], ¶m_manager.bit_data.name, sizeof(param_manager.bit_data.name)); \ + memcpy(¶m_value, allParams[i], sizeof(float)); \ + printf("Parameter name: %-20s Value: %f\n", #name, param_value); \ + i++; \ + } else { \ + printf("Warning: Exceeded maximum number of parameters\n"); \ + exceeded_max = 1; \ + } \ + } + + PARAM_LIST + #undef X + + // Pack all parameter data into paramRequest + memcpy(paramRequest->bit_data.data, allParams, sizeof(allParams)); + + printf("Total parameters sent: %d\n", i); + + // Send response + sendParamRequestResponse(paramRequest, sender_ip, sender_port, 0); +} + + +void processWriteRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) { + float value; + printf("Processing write request.\n"); + + // 先发送信号，然后从结构体读数 + for (int i = 0; i < 256; ++i) { + if (strlen((char *)paramRequest->bit_data.param_name[i]) > 0) { + writeParameter(paramRequest->bit_data.param_name[i], paramRequest->bit_data.data[i]); + printf("paramRequest->bit_data.param_name[i]:%s \n",paramRequest->bit_data.param_name[i]); + memcpy(&value, paramRequest->bit_data.data[i], sizeof(float)); + printf("paramRequest->bit_data.data[i]:%f \n", value); + } + } + + // 发送响应，发送所有参数 + processReadAllParams(paramRequest, sender_ip, sender_port); +} + + + +void processReadRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) { + printf("Processing read request.\n"); + + // 先备份原始请求数据 + UnParamRequest originalRequest; + memcpy(&originalRequest, paramRequest, sizeof(UnParamRequest)); + + // 清零响应数据 + memset(paramRequest, 0, sizeof(UnParamRequest)); + + // 处理客户端请求的参数 + for (int i = 0; i < 256; ++i) { + if (strlen((char *)originalRequest.bit_data.param_name[i]) > 0) { + // 复制参数名到响应 + strcpy((char *)paramRequest->bit_data.param_name[i], + (char *)originalRequest.bit_data.param_name[i]); + // 读取参数值 + float readData = readParameter(originalRequest.bit_data.param_name[i]); + memcpy(paramRequest->bit_data.data[i], &readData, sizeof(float)); + + printf("Read parameter: %s = %f\n", + originalRequest.bit_data.param_name[i], readData); + } + } + + // 发送响应 - 直接传递 paramRequest + sendParamRequestResponse(paramRequest, sender_ip, sender_port, 0); +} + + + + + + +void OnParamSignal(void *data) +{ + RequestContext *signal = (RequestContext *)data; + + uint8_t *datagram = (uint8_t *)signal->param_request->arr; + uint16_t request_id = ((uint16_t)datagram[7] << 8) | (uint16_t)datagram[8];// 大端模式 + + // 调试输出 + printf("Received request ID: 0x%04X\n", request_id); + + // 计算CRC + uint8_t calculatedCrc = calculateCRC(datagram, sizeof(UnParamRequest) - 1); + uint8_t receivedCrc = datagram[sizeof(UnParamRequest) - 1]; + + // 比较CRC + if (calculatedCrc != receivedCrc) + { + printf("CRC check failed, discarding data\n"); + printf("Calculated CRC: 0x%02X, Received CRC: 0x%02X\n", calculatedCrc, receivedCrc); + return; + } + + printf("CRC check passed\n"); + + if (request_id == 100) + { // 读请求 + processReadRequestFrame(signal->param_request, signal->sender_ip, signal->sender_port); + } + else if (request_id == 101) + { // 写请求 + processWriteRequestFrame(signal->param_request, signal->sender_ip, signal->sender_port); + } + else if (request_id == 102) + { // 读取所有参数 + processReadAllParams(signal->param_request, signal->sender_ip, signal->sender_port); + } + else + { + printf("Unknown request ID.\n"); + return; + } +} + +float getParam(const char *param_name) +{ + // 检查是否已初始化 + if (!is_param_initialized) + { + printf("Parameters not initialized, reinitializing\n"); + accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION);//Read all parameters from E2 + is_param_initialized = 1; // Mark as initialized + printParams(); + return 0.0f; + } + + // 检查参数名是否为空 + if (param_name == NULL) + { + printf("Error: Parameter name is empty\n"); + return 0.0f; + } + + // 遍历所有参数 + #define X(name) \ + if (strcmp(param_name, #name) == 0) \ + { \ + return param_manager.bit_data.name; \ + } + PARAM_LIST + #undef X + + // 如果没有找到匹配的参数名 + printf("Error: Parameter %s not found\n", param_name); + return 0.0f; +} + +// setParam 函数 +uint8_t setParam(const char *param_name, float value) +{ + // 检查参数名是否为空 + if (param_name == NULL) + { + printf("Error: Parameter name is empty\n"); + return 2; // 返回错误码 + } + + // 参数名和值写入EEPROM，先转成字节数组 + uint8_t data[sizeof(float)]; + memcpy(data, &value, sizeof(float)); + writeParameter(param_name, data); + + // 更新参数 + #define X(name) \ + if (strcmp(param_name, #name) == 0) \ + { \ + memcpy(¶m_manager.bit_data.name, data, sizeof(param_manager.bit_data.name)); \ + } + PARAM_LIST + #undef X + + return 0; +} + +void paramAppInit(void) +{ + // 初始化全局变量 + memset(&un_param_request1, 0, sizeof(UnParamRequest)); + memset(&un_param_request2, 0, sizeof(UnParamRequest)); + + // 正确初始化 RequestContext 结构体 + request_send.param_request = &un_param_request1; + request_send.sender_ip = 0; + request_send.sender_port = 0; + + request_context.param_request = &un_param_request2; + request_context.sender_ip = 0; + request_context.sender_port = 0; + + // 上电读取所有参数 + memset(param_manager.arr, 0, sizeof(param_manager.arr)); + accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION); +// whl_bas 轮胎直径<单位：m> +// max_rpm 最大转速<单位：rpm/min> +// whl_dia 轴距<单位：m> +// max_acc 最大加速度 +// spd_kp 遥控速度P +// spd_ki 遥控速度I +// spd_kd 遥控速度D +// spd_il 遥控速度积分限制 +// spd_ol 遥控速度PID输出限制 +// crv_kp 遥控转弯P +// crv_ki 遥控转弯I +// crv_kd 遥控转弯D +// crv_il 遥控转弯积分限制 +// crv_ol 遥控转弯PID输出限制 +// brk_on 刹车刹紧时间 单位：ms +// brk_off 刹车释放时间 单位：ms +// maxTorq 最大扭矩 单位：n*m +// feedPwr 最大馈电功率 单位：w +// dispPwr 最大放电功率 单位：w +// VehMass 车重 单位：kg +// gRatio 减速比 +// prCTime 预充电时间 单位：s +// brk_pos 刹车位置 +// pwr_sta 电源状态 +// high_sw 高压开关状态 +// stop_sw 急停开关状态 +// lightSt 灯光状态 +// pwr_btn 电源开关状态 +// sleepTm 休眠时间 单位：min +// wakeTm 唤醒时间 单位：min +// Ospd_kp 自主速度P +// Ospd_ki 自主速度I +// Ospd_kd 自主速度D +// Ospd_il 自主速度积分限制 +// Ospd_ol 自主速度PID输出限制 +// Ocrv_kp 自主转弯P +// Ocrv_ki 自主转弯I +// Ocrv_kd 自主转弯D +// Ocrv_il 自主转弯积分限制 +// Ocrv_ol 自主转弯PID输出限制 +// minTorq 输出扭矩死区 单位：n*m +// brk_rev 刹车方向 +// mot_kp 同侧扭矩P参数 +// mot_ki 同侧扭矩I参数 +// mot_kd 同侧扭矩d参数 +// mot_il 同侧扭矩积分限制 +// mot_ol 同侧扭矩输出限制 +// diff_sp 同侧扭矩速度差阈值 +// test 初始化测试参数 + // 订阅信号 + subscribe(¶m_signal, handleParamOp); + subscribe(&request_context, OnParamSignal);// 接收到上位机读写参数信号 + + printParams();//打印所有参数 + + is_param_initialized = 1; // 标记初始化完成 + + printf("paramAPP init OK! %d\n",getCurrentTime()); +} diff --git a/app/app_param_manage.h b/app/app_param_manage.h new file mode 100644 index 00000000..1c71921b --- /dev/null +++ b/app/app_param_manage.h @@ -0,0 +1,152 @@ +#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(high_sw) \ + X(stop_sw) \ + X(lightSt) \ + X(pwr_btn) \ + X(sleepTm) \ + X(wakeTm) \ + X(Ospd_kp) \ + X(Ospd_ki) \ + X(Ospd_kd) \ + X(Ospd_il) \ + X(Ospd_ol) \ + X(Ocrv_kp) \ + X(Ocrv_ki) \ + X(Ocrv_kd) \ + X(Ocrv_il) \ + X(Ocrv_ol) \ + X(minTorq) \ + X(brk_rev) \ + 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 00000000..d347b4b1 --- /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 00000000..25a79620 --- /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 00000000..5a158c96 --- /dev/null +++ b/app/app_power.c @@ -0,0 +1,429 @@ +#include "app_config.h" +#include "interface.h" +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" +#include "app_param_manage.h" + +#include "app_power.h" + + +// 定义按钮状态枚举 +typedef enum { + BUTTON_STATE_INITIAL, + BUTTON_STATE_SHORT_PRESS, + BUTTON_STATE_SHORT_PRESS_DETECTED, + BUTTON_STATE_WAIT_FOR_LONG_PRESS, + BUTTON_STATE_LONG_PRESS, + BUTTON_STATE_LONG_PRESS_WAIT +} ButtonState; + +// 定义按钮结构体 +typedef struct { + ButtonState state; + uint32_t press_start_time; + uint32_t release_start_time; + uint8_t is_power_on; + Timer timer; + uint8_t old_is_power_on; + Timer timer1; + +} PowerButton; + +// 全局变量 +PowerSystem power_data; +static PowerButton power_button = {BUTTON_STATE_INITIAL, 0, 0, 0, {0},0}; + +// 电源按钮处理函数 +static void handlePowerButton(void) +{ + switch (power_button.state) + { + case BUTTON_STATE_INITIAL: + if (power_data.remote_power_switch == app_close()) + { + power_button.state = BUTTON_STATE_SHORT_PRESS_DETECTED; + timerStart(&power_button.timer, 500, 0); // 启动短按定时器，500ms + } + break; + + case BUTTON_STATE_SHORT_PRESS_DETECTED: + if (power_data.remote_power_switch == app_open()) + { + if (power_button.timer.active) // 定时器未到期,短按完成，启动等待长按定时器 + { + power_button.state = BUTTON_STATE_WAIT_FOR_LONG_PRESS; + timerStart(&power_button.timer, 500, 0); // 启动等待长按定时器，500ms + } + } + else if (!power_button.timer.active)// 短按定时器到期，按键仍被按下，视为无效，重置为初始状态 + { + power_button.state = BUTTON_STATE_INITIAL; + } + break; + + case BUTTON_STATE_WAIT_FOR_LONG_PRESS: + if (power_data.remote_power_switch == app_close())// 检测是否在等待时间内进行长按 + { + power_button.state = BUTTON_STATE_LONG_PRESS; + timerStart(&power_button.timer, 1000, 0); // 启动长按定时器，1000ms + } + else if (!power_button.timer.active) // 等待长按超时，重置为初始状态 + { + power_button.state = BUTTON_STATE_INITIAL; + } + break; + + case BUTTON_STATE_LONG_PRESS: + + if (!power_button.timer.active)// 长按完成，切换电源状态 20250423 修改不需要判断松开按键就打开控制器 + { + power_button.is_power_on = !power_button.is_power_on; + printf("PowerButton: is_power_on = %d\n", power_button.is_power_on); + power_button.state = BUTTON_STATE_LONG_PRESS_WAIT; + } + else if(power_data.remote_power_switch == app_open()) + { + power_button.state = BUTTON_STATE_INITIAL; + printf("Long press for short duration"); + } + else + break; + + case BUTTON_STATE_LONG_PRESS_WAIT: + if (power_data.remote_power_switch == app_open())// 检测按键释放 + { + power_button.state = BUTTON_STATE_INITIAL; + printf("Release the button"); + } + default: + power_button.state = BUTTON_STATE_INITIAL; + break; + } +} + + +// 输出处理函数 +static void powerOutput(void *signal_id) +{ + (void)signal_id; + + // 根据当前状态,控制各个设备的电源 + switch (power_data.current_state) + { + case POWER_PRE_CHARGE: + publishMessage(&power_data.pre_charge_finish, 1);//发布预充完成信号，100ms发送一次，直到预充完成 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOn(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + break; + + case POWER_NEUTRAL: + publishMessage(&power_data.pre_charge_finish, 1);//20250316增加，发送空挡信号，保证电机控制器高压上电后，发送空挡信号 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOn(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOn(); // 高压继电器 + un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + break; + + + case POWER_STANDBY: + // 初始状态,只开启基本设备 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOff(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOff(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + break; + + case POWER_WORKING: + // 工作状态,除预充继电器外所有设备开启 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOn(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOn(); // 高压继电器 + un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + break; + + case POWER_EMERGENCY: + // 急停状态,断开高压 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + break; + + case POWER_SLEEP: + // 休眠状态,关闭所有设备 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOff(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOff(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + break; + + default: + break; + } + publishMessage(&power_data, 1); + publishMessage(&un_inf_can_kgf_output1, 1); + publishMessage(&un_inf_can_kgf_output2, 1); +} + +static void wakeupProcess(void *signal_id) +{ + (void)signal_id; + + un_gather_output.bit_data.sleep_duration = (uint16_t)getParam("sleepTm"); + un_gather_output.bit_data.wakeup_interval = (uint16_t)getParam("wakeTm"); + + if(un_gather_output.bit_data.sleep_duration < 5)//最小值限定 + { + un_gather_output.bit_data.sleep_duration = 5; + } + + if(un_gather_output.bit_data.wakeup_interval < 5)//最小值限定 + { + un_gather_output.bit_data.wakeup_interval = 5; + } + + un_gather_output.bit_data.vehicle_mode = power_data.current_state; + + publishMessage(&un_gather_output, 1); + timerStart(&power_data.timer1, 500, 1); //周期调用 +} + + +// 定时器处理函数 +static void powerTimerProcess(void *signal_id) +{ + (void)signal_id; + + // 调用电源按钮处理函数 + handlePowerButton(); + // 电源按钮状态有变化，保存到参数 + if (power_button.is_power_on != power_button.old_is_power_on) + { + setParam("pwr_btn", (float)power_button.is_power_on); + power_button.old_is_power_on = power_button.is_power_on; + } + + // 状态转换逻辑 + switch (power_data.current_state) + { + case POWER_PRE_CHARGE: + if (!power_data.timer_pre_charge.active) // 预充时间到 + { + power_data.current_state = POWER_NEUTRAL; // 工作 + power_data.pre_charge_finish = 1; // 预充完成 + printf("Power: Transitioning from PRE_CHARGE to POWER_NEUTRAL state\n"); + } + break; + case POWER_NEUTRAL://20250316增加，发送空挡信号 + if (power_data.neutral_cnt >= 5) // 运行5次 + { + power_data.neutral_cnt = 0; + power_data.current_state = POWER_WORKING; // 工作 + power_data.pre_charge_finish = 1; // 预充完成 + printf("Power: Transitioning from POWER_NEUTRAL to WORKING state\n"); + } + else + { + power_data.neutral_cnt ++; + power_data.current_state = POWER_NEUTRAL; // 空挡 + power_data.pre_charge_finish = 1; // 预充完成 + } + break; + + case POWER_STANDBY: + if (power_data.high_voltage_switch == app_open()) // 高压开关断开 + { + power_data.current_state = POWER_SLEEP; // 休眠 + printf("Power: Transitioning from STANDBY to SLEEP state\n"); + } + else if (power_button.is_power_on == app_close() && power_data.emergency_stop == app_close()) // 遥控器电源开关闭合且急停开关闭合 + { + power_data.current_state = POWER_EMERGENCY; // 急停 + printf("Power: Transitioning from STANDBY to EMERGENCY state\n"); + } + break; + + case POWER_WORKING: + if (power_data.high_voltage_switch == app_open()) // 高压开关断开 + { + power_data.current_state = POWER_SLEEP; // 休眠 + printf("Power: Transitioning from STANDBY to SLEEP state\n"); + } + else if (power_data.emergency_stop == app_close()) // 急停开关闭合 + { + power_data.current_state = POWER_EMERGENCY; // 急停 + printf("Power: Transitioning from WORKING to EMERGENCY state\n"); + + printf("emergency_stop_switch: %d, remote_emergency_stop: %d\n", power_data.emergency_stop_switch, power_data.remote_emergency_stop); //打印状态 + printf("remote_stop: %d\n", un_remote_control_input.bit_data.switch_b); + } + break; + case POWER_EMERGENCY: + if (power_data.high_voltage_switch == app_open()) // 高压开关断开 + { + power_data.current_state = POWER_SLEEP; // 休眠 + printf("Power: Transitioning from EMERGENCY to SLEEP state\n"); + } + else if (power_button.is_power_on == app_open()) // 遥控器电源开关断开 + { + power_data.current_state = POWER_STANDBY; // 待机 + printf("Power: Transitioning from EMERGENCY to STANDBY state\n"); + } + else if (power_data.emergency_stop == app_open()) // 急停断开 + { + power_data.current_state = POWER_PRE_CHARGE; // 预充 + timerStart(&power_data.timer_pre_charge, (uint32_t)(getParam("prCTime") * 1000), 1); // 启动预充定时器 + printf("Power: Transitioning from EMERGENCY to PRE_CHARGE state\n"); + } + break; + case POWER_SLEEP: + if (power_data.high_voltage_switch == app_close()) // 高压开关闭合 + { + power_data.current_state = POWER_STANDBY; // 待机 + printf("Power: Transitioning from SLEEP to STANDBY state\n"); + } + break; + default: + power_data.current_state = POWER_STANDBY; // 待机 + break; + } + + powerOutput(NULL); // 输出 + + // 电源状态有变化，记录到参数 + if (power_data.old_state != power_data.current_state) + { + power_data.old_state = power_data.current_state; + setParam("pwr_sta", (float)power_data.current_state); + } + + timerStart(&power_data.timer, 100, 1); //周期调用 +} + + + + +// 处理所有输入信号的函数 +static void powerInput(void *signal_id) +{ + //不能直接赋值，用memcpy + PowerSystem old_data; + memcpy(&old_data, &power_data, sizeof(PowerSystem)); + + // 填充数据 + if (signal_id == &un_sw_sample) + { + power_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch;//急停开关 + power_data.high_voltage_switch = (uint8_t)un_sw_sample.bit_data.High_voltage_switch;//高压开关 + } + else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据 + { + power_data.remote_power_switch = (uint8_t)un_remote_control_input.bit_data.switch_d; // 遥控器电源开关 + power_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1;// 遥控器急停开关 + } + // 急停开关 + power_data.emergency_stop = (uint8_t)( (power_data.emergency_stop_switch == app_close()) || (power_data.remote_emergency_stop == app_close()) ); + + // 急停开关有变化，记录到参数 + if (power_data.old_emergency_stop != power_data.emergency_stop) + { + power_data.old_emergency_stop = power_data.emergency_stop; + setParam("stop_sw", (float)power_data.emergency_stop); + } + + // 高压开关状态有变化，记录到参数 + if (power_data.old_high_voltage_switch != power_data.high_voltage_switch) + { + power_data.old_high_voltage_switch = power_data.high_voltage_switch; + setParam("high_sw", (float)power_data.high_voltage_switch); + } +} + + +// 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; + //恢复高压开关状态 + power_data.high_voltage_switch = (uint8_t)getParam("high_sw"); + power_data.old_high_voltage_switch = power_data.high_voltage_switch; + //恢复急停开关状态 + power_data.emergency_stop = (uint8_t)getParam("stop_sw"); + power_data.old_emergency_stop = power_data.emergency_stop; + + // 订阅输入信号 + subscribe(&un_sw_sample, powerInput); // 急停开关、高压开关 + 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 00000000..adb981ae --- /dev/null +++ b/app/app_power.h @@ -0,0 +1,57 @@ +#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_NEUTRAL, + 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 old_high_voltage_switch; // 上一次高压开关 + uint8_t remote_power_switch; // 遥控器电源开关 + uint8_t remote_emergency_stop; // 遥控器急停开关 + uint8_t emergency_stop; // 急停状态 + uint8_t old_emergency_stop; // 上一次急停开关 + uint8_t pre_charge_finish; // 预充完成标志位 + uint8_t old_state; // 上一次状态 + uint8_t neutral_cnt; +} 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 00000000..8b73d225 --- /dev/null +++ b/app/app_request.c @@ -0,0 +1,396 @@ +#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.desired_curvature * 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.curvature * 10000.0); + un_vehicle_Info_output.bit_data.curvature = ((request16_temp << 8) | (request16_temp >> 8));//当前曲率 + + request16_temp = (uint16_t)(int16_t)(diff_data.left_motor_speed *6); + un_vehicle_Info_output.bit_data.set_left_speed = ((request16_temp << 8) | (request16_temp >> 8));//当前速度 + + request16_temp = (uint16_t)(int16_t)(diff_data.right_motor_speed *6); + un_vehicle_Info_output.bit_data.set_right_speed = ((request16_temp << 8) | (request16_temp >> 8));//当前曲率 + + } + else if(signal_id == &un_auto_computer_input) + { + 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 = SWAP_ENDIAN_16( (uint16_t)((int16_t)(un_motor_input1.bit_data.MotCon_1Signal4) + 30000) ); + +// un_motor_status_output.bit_data.left_torque = ((un_motor_input1.bit_data.torque << 8) | (un_motor_input1.bit_data.torque >> 8));//左侧扭矩 +// un_motor_status_output.bit_data.left_voltage = ((un_motor_input1.bit_data.bus_voltage << 8) | (un_motor_input1.bit_data.bus_voltage >> 8));//左侧电压 +// un_motor_status_output.bit_data.left_fault_code = un_motor_input1.bit_data.fault_code;//左侧故障码 + + } + else if(signal_id == &un_motor_input2) + { +// un_motor_status_output.bit_data.right_wheel_speed = SWAP_ENDIAN_16 ( (uint16_t)((int16_t)(un_motor_input1.bit_data.MotCon_1Signal3) + 30000) );//左侧轮速 +// un_motor_status_output.bit_data.right_torque = ((un_motor_input2.bit_data.torque << 8) | (un_motor_input2.bit_data.torque >> 8));//右侧扭矩 +// un_motor_status_output.bit_data.right_fault_code = un_motor_input2.bit_data.fault_code;//右侧故障码 +// un_motor_status_output.bit_data.right_voltage = ((un_motor_input2.bit_data.bus_voltage << 8) | (un_motor_input2.bit_data.bus_voltage >> 8));//右侧电压 + + } + 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 00000000..13ac8b75 --- /dev/null +++ b/app/app_request.h @@ -0,0 +1,89 @@ +#ifndef APP_REQUEST_H +#define APP_REQUEST_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "app_config.h" +#include "app_differential_drive.h" + + + +#define SWAP_ENDIAN_16(x) ((((x) & 0xFF) << 8) | (((x) >> 8) & 0xFF)) +#define SWAP_ENDIAN_32(x) (((x) << 24) | (((x) & 0xFF00) << 8) | (((x) >> 8) & 0xFF00) | ((x) >> 24)) + + + + +//typedef enum +//{ +// MODE_MANUAL, // 手动模式 +// 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 00000000..db6b6c8d --- /dev/null +++ b/app/app_temp.c @@ -0,0 +1,291 @@ +#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 handleTemperatureAlarm(int16_t current_temp, float alarm_temp, + float critical_temp, float threshold_temp, + TempState *state) +{ + switch (*state) + { + case TEMP_NORMAL: + // 从正常状态进入警告状态的条件 + if (current_temp > (alarm_temp + threshold_temp)) + { + *state = TEMP_WARNING; + printf("Temperature Warning: Activated! Current temp: %d°C\n", current_temp); + } + // 从正常状态直接进入严重状态的条件 + else if (current_temp > (critical_temp + threshold_temp)) + { + *state = TEMP_CRITICAL; + printf("Temperature Critical: Activated! Current temp: %d°C\n", current_temp); + } + else + { + *state = TEMP_NORMAL; + } + break; + + case TEMP_WARNING: + // 从警告状态返回正常状态的条件 + if (current_temp < (alarm_temp - threshold_temp)) + { + *state = TEMP_NORMAL; + printf("Temperature Warning: Deactivated! Current temp: %d°C\n", current_temp); + } + // 从警告状态进入严重状态的条件 + else if (current_temp > (critical_temp + threshold_temp)) + { + *state = TEMP_CRITICAL; + printf("Temperature Critical: Activated! Current temp: %d°C\n", current_temp); + } + else + { + *state = TEMP_WARNING; + } + break; + + case TEMP_CRITICAL: + // 从严重状态返回警告状态的条件 + if (current_temp < (critical_temp - threshold_temp)) + { + *state = TEMP_WARNING; + printf("Temperature Critical: Deactivated! Current temp: %d°C\n", current_temp); + } + // 从严重状态直接返回正常状态的条件 + else if (current_temp < (alarm_temp - threshold_temp)) + { + *state = TEMP_NORMAL; + printf("Temperature Warning: Deactivated! Current temp: %d°C\n", current_temp); + } + else + { + *state = TEMP_CRITICAL; + } + break; + + default: + *state = TEMP_NORMAL; + break; + } +} + + +// 温度输出处理函数 +static void tempOutput(void *signal_id) +{ + (void)signal_id; + + // 电机1风扇 左前 + switch (temp_data.state[0]) + { + case TEMP_NORMAL: + un_inf_can_kgf_output1.bit_data.KGF02 = setFanOff();//电机控制器风扇 + un_inf_can_kgf_output1.bit_data.pwm_02 = 0; + break; + case TEMP_WARNING: + un_inf_can_kgf_output1.bit_data.KGF02 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output1.bit_data.pwm_02 = 5; + break; + case TEMP_CRITICAL: + un_inf_can_kgf_output1.bit_data.KGF02 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output1.bit_data.pwm_02 = 0; + break; + } + + // 电机2风扇 右前 + switch (temp_data.state[1]) + { + case TEMP_NORMAL: + un_inf_can_kgf_output1.bit_data.KGF01 = setFanOff();//电机控制器风扇 + un_inf_can_kgf_output1.bit_data.pwm_01 = 0; + break; + case TEMP_WARNING: + un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output1.bit_data.pwm_01 = 5; + break; + case TEMP_CRITICAL: + un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output1.bit_data.pwm_01 = 0; + break; + } + + + // 电机3风扇 左后 + switch (temp_data.state[2]) + { + case TEMP_NORMAL: + un_inf_can_kgf_output2.bit_data.KGF08 = setFanOff();//电机控制器风扇 + un_inf_can_kgf_output2.bit_data.pwm_08 = 0; + break; + case TEMP_WARNING: + un_inf_can_kgf_output2.bit_data.KGF08 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output2.bit_data.pwm_08 = 5; + break; + case TEMP_CRITICAL: + un_inf_can_kgf_output2.bit_data.KGF08 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output2.bit_data.pwm_08 = 0; + break; + } + + // 电机4风扇 右后 + switch (temp_data.state[3]) + { + case TEMP_NORMAL: + un_inf_can_kgf_output2.bit_data.KGF07 = setFanOff();//电机控制器风扇 + un_inf_can_kgf_output2.bit_data.pwm_07 = 0; + break; + case TEMP_WARNING: + un_inf_can_kgf_output2.bit_data.KGF07 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output2.bit_data.pwm_07 = 5; + break; + case TEMP_CRITICAL: + un_inf_can_kgf_output2.bit_data.KGF07 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output2.bit_data.pwm_07 = 0; + break; + } + +// // 电机3风扇 +// switch (temp_data.state[2]) +// { +// case TEMP_NORMAL: +// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOff();//电机控制器风扇 +// un_inf_can_kgf_output1.bit_data.pwm_01 = 0; +// break; +// case TEMP_WARNING: +// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇 +// un_inf_can_kgf_output1.bit_data.pwm_01 = 5; +// break; +// case TEMP_CRITICAL: +// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇 +// un_inf_can_kgf_output1.bit_data.pwm_01 = 0; +// break; +// } + + publishMessage(&un_inf_can_kgf_output1, 1); + publishMessage(&un_inf_can_kgf_output2, 1); +} + +// 温度状态处理函数 +static void tempProcess(void *signal_id) +{ + (void)signal_id; + int16_t max_temp[4] = {0,0,0,0}; + + // 调用按钮处理函数 + max_temp[0] = temp_data.current_temp[0]; + max_temp[1] = temp_data.current_temp[1]; + max_temp[2] = temp_data.current_temp[2]; + max_temp[3] = temp_data.current_temp[3]; + + // printf("motor1 temp: %d, motor2 temp: %d\n", max_temp[0], max_temp[1]); + handleTemperatureAlarm(max_temp[0], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[0]); + + handleTemperatureAlarm(max_temp[1], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[1]); + + handleTemperatureAlarm(max_temp[2], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[2]); + + handleTemperatureAlarm(max_temp[3], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[3]); + + + + + + +// if (max_temp[0] >= 60) // 假设60度为危险温度 +// { +// temp_data.state[0] = TEMP_CRITICAL; +// } +// else if (max_temp[0] >= 40) // 假设40度为警告温度 +// { +// temp_data.state[0] = TEMP_WARNING; +// } +// else +// { +// temp_data.state[0] = TEMP_NORMAL; +// } +// +// +// max_temp[1] = temp_data.current_temp[1]; +// if (max_temp[1] >= 60) // 假设60度为危险温度 +// { +// temp_data.state[1] = TEMP_CRITICAL; +// } +// else if (max_temp[1] >= 40) // 假设40度为警告温度 +// { +// temp_data.state[1] = TEMP_WARNING; +// } +// else +// { +// temp_data.state[1] = TEMP_NORMAL; +// } +// +//// printf("motor1 temp: %d, motor2 temp: %d\n", max_temp[0], max_temp[1]); +//// printf("motor1 state: %d, motor2 state: %d\n", temp_data.state[0], temp_data.state[1]); + + tempOutput(NULL); + + timerStart(&temp_data.timer, 1000, 1); //1s +} + +// 处理输入信号的函数 +static void tempInput(void *signal_id) +{ + (void)signal_id; + // 填充数据 + if (signal_id == &un_motor_temp1) + { + temp_data.current_temp[0] = ( (int16_t)(un_motor_temp1.bit_data.controller_temp) - 40);//40偏移量 + } + else if(signal_id == &un_motor_temp2) + { + temp_data.current_temp[1] = ( (int16_t)(un_motor_temp2.bit_data.controller_temp) - 40); + } + else if(signal_id == &un_motor_temp3) + { + temp_data.current_temp[2] = ( (int16_t)(un_motor_temp3.bit_data.controller_temp) - 40); + } + else if(signal_id == &un_motor_temp4) + { + temp_data.current_temp[3] = ( (int16_t)(un_motor_temp4.bit_data.controller_temp) - 40); + } + else{} +} + + +// APP模块的初始化 +void tempAppInit(void) +{ + // 初始化 + timerInit(&temp_data.timer); + + memset(&temp_data, 0, sizeof(TempSystem)); + temp_data.state[0] = TEMP_NORMAL; + temp_data.state[1] = TEMP_NORMAL; + temp_data.mode = TEMP_MODE_AUTO; + temp_data.target_temp = 25; // 默认目标温度25度 + + // 订阅输入信号,处理温度逻辑 + subscribe(&un_motor_temp1, tempInput); + subscribe(&un_motor_temp2, tempInput); + subscribe(&un_motor_temp3, tempInput); + subscribe(&un_motor_temp4, 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 00000000..aefb8319 --- /dev/null +++ b/app/app_temp.h @@ -0,0 +1,58 @@ +#ifndef APP_TEMP_H +#define APP_TEMP_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#define MOTOR_WARNING_TEMP 40 +#define MOTOR_CRITICAL_TEMP 60 +#define MOTOR_THRESHOLD_TEMP 5//回滞值 + + + + + +// 定义温度状态枚举 +typedef enum +{ + TEMP_NORMAL, + TEMP_WARNING, + TEMP_CRITICAL +} TempState; + +// 定义温度控制模式枚举 +typedef enum +{ + TEMP_MODE_AUTO, + TEMP_MODE_MANUAL +} TempMode; + +typedef struct +{ + TempState state[8]; + TempMode mode; + int16_t current_temp[8]; // 当前温度,8个不同的温度 + int16_t target_temp; // 目标温度 + uint8_t fan_speed[8]; // 风扇速度 8个通道 + 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 00000000..60bfe6de --- /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 00000000..9bdf00e5 --- /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_ultrasonic.c b/app/app_ultrasonic.c new file mode 100644 index 00000000..29a4d5ee --- /dev/null +++ b/app/app_ultrasonic.c @@ -0,0 +1,83 @@ +#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_ultrasonic.h" + + +// 定时器结构体 +Timer ultrasonic_timer; +Timer ultrasonic_timer1; + +uint16_t fornt_distance = 0;//mm + +// 定时器信号处理函数 +void ultrasonicTimer1Process(void *signal_id) +{ + (void)signal_id; // 标记变量为已使用，避免编译器警告 + + printf("fornt distance = %d\n",fornt_distance); + + + timerStart(&ultrasonic_timer1, 1000,1); +} + + + + + +// 定时器信号处理函数 +void ultrasonicTimerProcess(void *signal_id) +{ + (void)signal_id; // 标记变量为已使用，避免编译器警告 + + un_ultrasonic_output1.bit_data.node = ULTRASONIC_FRONT_NODE; + un_ultrasonic_output1.bit_data.function_code = ULTRASONIC_READ; + un_ultrasonic_output1.bit_data.register_address = REALTIME_DISTANCE_REG; + un_ultrasonic_output1.bit_data.data = 0x100; + publishMessage(&un_ultrasonic_output1, 1); + + timerStart(&ultrasonic_timer, 200,1); +} + +void ultrasonicInput(void *signal_id) +{ + if(signal_id == &un_ultrasonic_input1) + { + fornt_distance = SWAP_ENDIAN_16(un_ultrasonic_input1.bit_data.data);//高低字节交换 + } +} + + + + +// APP模块的初始化 +void ultrasonicAppInit(void) +{ + // 初始化定时器 + timerInit(&ultrasonic_timer); + timerInit(&ultrasonic_timer1); + // 订阅定时器信号 + subscribe(&ultrasonic_timer, ultrasonicTimerProcess); + subscribe(&ultrasonic_timer1, ultrasonicTimer1Process); + + subscribe(&un_ultrasonic_input1, ultrasonicInput); + printf("ultrasonicAPP: initial OK %d\n",getCurrentTime()); + timerStart(&ultrasonic_timer, 200,1); + timerStart(&ultrasonic_timer1, 1000,1); +} + + + + + + + + + + diff --git a/app/app_ultrasonic.h b/app/app_ultrasonic.h new file mode 100644 index 00000000..924c13a2 --- /dev/null +++ b/app/app_ultrasonic.h @@ -0,0 +1,75 @@ +#ifndef ULTRASONIC_H +#define ULTRASONIC_H + +#ifdef __cplusplus +extern "C" { +#endif + + +#define ULTRASONIC_FRONT_NODE 0x1 +#define ULTRASONIC_BACK_NODE 0x2 + +#define ULTRASONIC_ID_1 0x520 + ULTRASONIC_FRONT_NODE +#define ULTRASONIC_ID_2 0x520 + ULTRASONIC_BACK_NODE + +#define ULTRASONIC_READ 0x03 +#define ULTRASONIC_WRITE 0x06 + + + + +#define SOFTWARE_VERSION_REG 0x0000 // ģ汾(ʮֵ), : uint16_t + + +#define PROCESSED_DISTANCE_REG 0x0100 /* + * ľֵ(mm), : uint16_t + * ָ࣬㷨 + * Ӧʱ䣺190-750ms(̶) + */ + +#define REALTIME_DISTANCE_REG 0x0101 /* + * ʵʱβֵ(mm), : uint16_t + * ָ󴥷β + * Ӧʱ䣺15-140ms(̶) + */ + +#define TEMPERATURE_REG 0x0102 /* + * ¶ȴֵ, : int16_t + * λ0.1 (ԭʼֵ/10), ֱʣ0.5 + * Ӧʱ䣺15-140ms(̶) + */ + +#define ECHO_TIME_REG 0x010A /* + * زʱԭʼֵ(s), : uint16_t + * 뻻㣺ֵ/5.75 = ׾ + * Ӧʱ䣺15-140ms(̶) + */ + + + +#define SWAP_ENDIAN_16(x) ((((x) & 0xFF) << 8) | (((x) >> 8) & 0xFF)) +#define SWAP_ENDIAN_32(x) (((x) << 24) | (((x) & 0xFF00) << 8) | (((x) >> 8) & 0xFF00) | ((x) >> 24)) + + + + + + + + + + + + void ultrasonicAppInit(void); + + + + + + + +#ifdef __cplusplus +} +#endif + +#endif // ULTRASONIC_H diff --git a/app_brake.c b/app_brake.c new file mode 100644 index 00000000..a7b42c1b --- /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 00000000..34f2250b --- /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/app_turntable.c b/app_turntable.c new file mode 100644 index 00000000..d4c42366 --- /dev/null +++ b/app_turntable.c @@ -0,0 +1,460 @@ +#include "app_config.h" +#include "app_dependence.h" +#include "interface.h" +#include "app_turntable.h" +#include "app_pid.h" +#include "app_param_manage.h" + +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" + +#include "sdrv_vic.h" + +PID_t turnable_speed_pid; +PID_t turnable_position_pid; + +TurnableData turnable_data = {0}; + + + + + +/** + * @brief 带死区的原始数据到物理量转换函数（简单版） + * @param raw_value 原始16位无符号整数值 [0, 65535] + * @param min 物理量最小值（如 -10.0） + * @param max 物理量最大值（如 +10.0） + * @param deadzone 死区范围（物理量单位，如 1.0 表示 ±1.0 内为死区） + * @return 转换后的物理量值（若在死区内返回0，否则返回实际值） + */ +static float convertPhysical(uint16_t raw_value, float min, float max, float deadzone) +{ + // 1. 计算实际物理量值 + float physical_value = min + ((float)raw_value / 65535.0f) * (max - min); + + // 2. 判断是否在死区内（绝对值 ≤ deadzone） + if (fabs(physical_value) <= deadzone) + { + return 0.0f; // 死区内返回0 + } + else + { + return physical_value; // 死区外返回实际值 + } +} + + + +/** + * @brief 将浮点数转换为uint32_t（按小端序存储） + * @param num 输入的浮点数 + * @return 转换后的uint32_t值（直接内存拷贝结果） + * @note 此函数通过内存直接拷贝实现转换，不进行数值计算，结果受平台字节序影响 + */ +uint32_t floatToUint32(float num) +{ + uint32_t result; + // 将浮点数的内存数据直接拷贝到uint32_t变量 + memcpy(&result, &num, sizeof(num)); + return result; +} + + +/** + * @brief 电机失能函数（停止电机运行） + * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255) + * @param master_id 主控制器ID (用于标识发送方) + * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据 + * @return 0: 成功, -1: 参数无效 + * @note 此函数会修改unsdodata指向的结构体内容，调用后需及时发送CAN报文 + */ +int8_t motorDisable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata) +{ + /* 参数有效性检查 */ + if (unsdodata == NULL) { + return -1; + } + + /* 设置CAN报文ID域 */ + unsdodata->tx_can_id.bits.mode = 3; /* 通信模式3：电机失能 */ + unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */ + unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */ + unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */ + + /* 清零数据域 */ + unsdodata->tx_can_data.bit_data.data = 0; + unsdodata->tx_can_data.bit_data.index = 0; + unsdodata->tx_can_data.bit_data.object_index = 0; + + return 0; +} + +/** + * @brief 电机使能函数（启动电机运行） + * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255) + * @param master_id 主控制器ID (用于标识发送方) + * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据 + * @return 0: 成功, -1: 参数无效 + * @note 通信模式4：电机使能 + */ +int8_t motorEnable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata) +{ + /* 参数有效性检查 */ + if (unsdodata == NULL) { + return -1; + } + + /* 设置CAN报文ID域 */ + unsdodata->tx_can_id.bits.mode = 3; /* 通信模式4：电机使能 */ + unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */ + unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */ + unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */ + + /* 清零数据域 */ + unsdodata->tx_can_data.bit_data.data = 0; + unsdodata->tx_can_data.bit_data.index = 0; + unsdodata->tx_can_data.bit_data.object_index = 0; + + return 0; +} + +/** + * @brief 设置电机运行模式 + * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255) + * @param master_id 主控制器ID (用于标识发送方) + * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据 + * @param mode 要设置的模式值 (具体含义需参考电机协议文档) + * @return 0: 成功, -1: 参数无效 + * @note RUM_MODE应为预定义的宏，表示运行模式索引 + */ +int8_t setMotorMode(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint8_t mode) +{ + /* 参数有效性检查 */ + if (unsdodata == NULL) { + return -1; + } + + /* 设置CAN报文ID域 */ + unsdodata->tx_can_id.bits.mode = 0x12; /* 通信模式0x12：参数写入 */ + unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */ + unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */ + unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */ + + /* 设置数据域 */ + unsdodata->tx_can_data.bit_data.index = RUM_MODE; /* 运行模式索引 */ + unsdodata->tx_can_data.bit_data.object_index = 0; /* 子索引通常为0 */ + unsdodata->tx_can_data.bit_data.data = mode; /* 模式值 */ + + return 0; +} + +/** + * @brief 写入电机参数 + * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255) + * @param master_id 主控制器ID (用于标识发送方) + * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据 + * @param index 要写入的参数索引 (具体含义需参考电机协议文档) + * @param ref 要写入的参数值 (浮点数，会自动转换为uint32_t) + * @return 0: 成功, -1: 参数无效 + * @note 使用floatToUint32函数转换浮点参数 + */ +int8_t setMotorWrite(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint16_t index, float ref) +{ + /* 参数有效性检查 */ + if (unsdodata == NULL) { + return -1; + } + + /* 设置CAN报文ID域 */ + unsdodata->tx_can_id.bits.mode = 0x12; /* 通信模式0x12：参数写入 */ + unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */ + unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */ + unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */ + + /* 设置数据域 */ + unsdodata->tx_can_data.bit_data.index = index; /* 参数索引 */ + unsdodata->tx_can_data.bit_data.object_index = 0; /* 子索引通常为0 */ + unsdodata->tx_can_data.bit_data.data = floatToUint32(ref); /* 转换并写入参数值 */ + + return 0; +} + + +/** + * @brief 动态斜率限制（支持变时间间隔） + * @param last_command 上一次的电流指令值 + * @param target_current 本次目标电流指令 + * @param delta_time 距离上一次调用的时间间隔 (s) + * @return 限制后的安全电流指令 + */ +float dynamic_current_limit(float *last_command, float target_current, float delta_time) +{ + // 计算期望的变化量 + float desired_change = target_current - *last_command; + + // 计算两种限制 + float step_limit = MAX_STEP; + float time_limit = MAX_DI_DT * delta_time; // 动态计算时间限制 + + // 选择更严格的限制 + float max_allowed_change = (step_limit < time_limit) ? step_limit : time_limit; + + // 应用限制并返回新指令 + float actual_change = constrain(desired_change, -max_allowed_change, max_allowed_change); + *last_command = *last_command + actual_change;//更新过去值 + + return *last_command + actual_change; +} + +static void setTurnableMotorOutput() +{ + static float previous_time2 = 0.0f; + + float time1 = (float)getCurrentTime(); + float dt = (time1 - previous_time2) / PERIOD_TICK; + previous_time2 = time1; + + turnable_data.out_pitch_motor_ampere_limit = dynamic_current_limit(&turnable_data.out_pitch_motor_ampere_last,turnable_data.out_pitch_motor_ampere,dt); + turnable_data.out_right_motor_ampere_limit = dynamic_current_limit(&turnable_data.out_right_motor_ampere_last,turnable_data.out_right_motor_ampere,dt); + turnable_data.out_left_motor_ampere_limit = dynamic_current_limit(&turnable_data.out_left_motor_ampere_last ,turnable_data.out_left_motor_ampere ,dt); + + + + + setMotorWrite(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1, IQ_REF_INDEX,turnable_data.out_pitch_motor_ampere_limit); + setMotorWrite(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2, IQ_REF_INDEX,turnable_data.out_right_motor_ampere_limit); + setMotorWrite(MASTER_CANID, TURN_MOTOR_CANID, &un_sdo_output3, IQ_REF_INDEX,turnable_data.out_left_motor_ampere_limit); + + + un_can_debug_output.bit_data.speed = (uint8_t)(int8_t)(turnable_data.speed*10); + un_can_debug_output.bit_data.desired_speed = (uint8_t)(int8_t)(turnable_data.desired_speed*10); + +// un_can_debug_output.bit_data.curvature = (uint8_t)(int8_t)(diff_data.yaw_rate*10); +// un_can_debug_output.bit_data.desired_curvature = (uint8_t)(int8_t)(diff_data.desired_yaw_rate*10); + + un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(turnable_data.out_left_motor_ampere_limit * 100); + un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(turnable_data.out_right_motor_ampere_limit*100); + + + + + + publishMessage(&un_sdo_output1, 1); + publishMessage(&un_sdo_output2, 1); + publishMessage(&un_sdo_output3, 1); +} + + + + + +// 转台 +static void turnableProcess(void *signal_id) +{ + + static float previous_time1 = 0.0f; + + float time1 = (float)getCurrentTime(); + float dt = (time1 - previous_time1) / PERIOD_TICK; + previous_time1 = time1; +// if((turnable_data.current_state == POWER_WORKING))//高压上电才运行 +// { + switch(turnable_data.turnable_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据 + { + case 0: + timerStart(&turnable_data.turnable_timer, 1000, 0); // 启动定时器，1s + turnable_data.turnable_state = 1; + break; + + case 1: + if (!turnable_data.turnable_timer.active)// 1s定时 + { + turnable_data.turnable_state = 2; + } + else + { + turnable_data.turnable_state = 1; + } + break; + + case 2://模式设置 + if(turnable_data.turnable_cnt >= 5)//发送5次 + { + turnable_data.turnable_cnt = 0; + turnable_data.turnable_state = 3; + } + else + { + turnable_data.turnable_cnt ++; + turnable_data.turnable_state = 2; + + setMotorMode(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1, POSITION_MODE_CSP); + setMotorMode(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2, CURRENT_MODE); + setMotorMode(MASTER_CANID, TURN_MOTOR_CANID, &un_sdo_output3, CURRENT_MODE); + + publishMessage(&un_sdo_output1, 1); + publishMessage(&un_sdo_output2, 1); + publishMessage(&un_sdo_output3, 1); + + } + break; + +//------------------------------------------------------------------------------ + case 3: + if(turnable_data.turnable_cnt >= 5)//发送5次 + { + turnable_data.turnable_cnt = 0; + turnable_data.turnable_state = 4; + } + else + { + turnable_data.turnable_cnt ++; + turnable_data.turnable_state = 3; + + motorEnable(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1); + motorEnable(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2); + motorEnable(MASTER_CANID, TURN_MOTOR_CANID, &un_sdo_output3); + + publishMessage(&un_sdo_output1, 1); + publishMessage(&un_sdo_output2, 1); + publishMessage(&un_sdo_output3, 1); + } + break; + + case 4: + turnable_data.out_left_motor_ampere = calculatePidOutput(&turnable_speed_pid, turnable_data.desired_speed, turnable_data.speed, 0.0f, dt); + + turnable_data.turnable_cnt = 0; + turnable_data.turnable_state = 4; + + setTurnableMotorOutput();//输出函数 + break; + + default:break; + } +// } +// else +// { +// turnable_data.turnable_cnt ++; +// turnable_data.turnable_state = 0; +// } +} + + + + +void turnableParametersInit(void *signal_id) +{ + (void)signal_id; // 标记变量为已使用，避免编译器警告 + + setPidParameters(&turnable_speed_pid, + getParam("spd_kp"), + getParam("spd_ki"), + getParam("spd_kd"), + getParam("spd_il"), + getParam("spd_ol") + ); + + printf( "turnable left A %f\n",turnable_data.out_left_motor_ampere); + printf( "turnable right A %f\n",turnable_data.out_right_motor_ampere); + printf( "turnable pitch A %f\n",turnable_data.out_pitch_motor_ampere); + printf( "desired speed %f\n",turnable_data.desired_speed); + printf( "speed %f\n",turnable_data.speed); + printf( "turnable state %d\n",turnable_data.turnable_state); + + timerStart(&turnable_data.turnable_timer1,1000,1);//100ms调用一次 + + +} + + + + + + + + + + + + + +// 差速输入处理函数 +static void turnableInput(void *signal_id) +{ + if(signal_id == &power_data)//电机上电 + { + turnable_data.current_state = power_data.current_state; + } + else if(signal_id == &un_computer_turnable_Input) + { + turnable_data.desired_speed = (float)( SWAP_ENDIAN_32(un_computer_turnable_Input.bit_data.position_x) ); + + } + else{} + + turnable_data.right_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_right_intput.rx_can_data.bit_data.current_velocity),ANGULAR_VELOCITY_MIN,ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE ); + turnable_data.left_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_turn_intput.rx_can_data.bit_data.current_velocity) ,ANGULAR_VELOCITY_MIN,ANGULAR_VELOCITY_MAX, MOTOR_VELOCITY_DEADZONE ); + turnable_data.speed = (turnable_data.right_motor_speed + turnable_data.left_motor_speed)/2.0f; + + turnableProcess(signal_id);//处理映射 + timerStart(&turnable_data.turnable_timer2,100,1);//100ms调用一次 +} + + +void turnableInit() +{ + // 初始化速度 PID 控制器 + initializePid(&turnable_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); + + // 设置速度 PID 控制器的参数 + setPidParameters(&turnable_speed_pid, + getParam("spd_kp"), + getParam("spd_ki"), + getParam("spd_kd"), + getParam("spd_il"), + getParam("spd_ol") + ); + + subscribe(&un_computer_turnable_Input, turnableInput); + + timerInit(&turnable_data.turnable_timer); + timerInit(&turnable_data.turnable_timer1); + + timerInit(&turnable_data.turnable_timer2); + subscribe(&turnable_data.turnable_timer2, turnableInput); + + timerStart(&turnable_data.turnable_timer2,100,1);//100ms调用一次 + + subscribe(&turnable_data.turnable_timer1, turnableParametersInit); + timerStart(&turnable_data.turnable_timer1,1000,1);//100ms调用一次 + + turnable_data.turnable_state = 0; + un_right_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE; + un_right_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE; + un_right_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE; + + un_turn_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE; + un_turn_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE; + un_turn_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE; + + un_pitch_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE; + un_pitch_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE; + un_pitch_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE; + + printf( "turnable: initial OK %d\n",getCurrentTime()); +} + + + + + + + + + + + + + diff --git a/app_turntable.h b/app_turntable.h new file mode 100644 index 00000000..5a2093b0 --- /dev/null +++ b/app_turntable.h @@ -0,0 +1,119 @@ +#ifndef TURNTAABLE_H +#define TURNTAABLE_H + + +#include "app_power.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define MASTER_CANID 0xFD + +#define PITCH_MOTOR_CANID 0x7D +#define RIGHT_MOTOR_CANID 0x7E +#define TURN_MOTOR_CANID 0x7F + +#define PITCH_MOTOR_RxCANID (0x20000FD + (PITCH_MOTOR_CANID << 8)) // 0x2007DFD +#define RIGHT_MOTOR_RxCANID (0x20000FD + (RIGHT_MOTOR_CANID << 8)) // 0x2007EFD +#define TURN_MOTOR_RxCANID (0x20000FD + (TURN_MOTOR_CANID << 8)) // 0x2007FFD + +#define MOTOR_RxCAN_Mask 0x1F00FFFF //CAN Ƴλ + +#define LIMIT_SPEED_INDEX 0x7017//CSPٶ +#define LOC_REF_INDEX 0x7016//CSPλ +#define IQ_REF_INDEX 0x7006//ģʽ + +#define RUM_MODE 0x7005//modeģʽ + +#define OPERATION_MODE 0 // ˿ģʽ +#define POSITION_MODE_PP 1 // λģʽ (PP - Profile Position) +#define VELOCITY_MODE 2 // ٶģʽ +#define CURRENT_MODE 3 // ģʽ +#define POSITION_MODE_CSP 5 // λģʽ (CSP - Cyclic Synchronous Position) + + +// ĵ仯 (A) +#define MAX_STEP 1.0f // 仯 (A)5Aʼ +#define MAX_DI_DT 1000.0f // 仯 (A/s)5000A/sʼ + + +// ѧ +#define PI 3.14159f // ߾Ȧֵ + +// Ƕ̶ (ӦByte0~1: ǰǶ) +#define ANGLE_RANGE_MIN (-4.0f * PI) // СǶ: -4 +#define ANGLE_RANGE_MAX (4.0f * PI) // Ƕ: 4 + +// ٶ̶ (ӦByte2~3: ǰٶ) +#define ANGULAR_VELOCITY_MIN -15.0f // Сٶ: -15 rad/s +#define ANGULAR_VELOCITY_MAX 15.0f // ٶ: 15 rad/s + + +#define MOTOR_VELOCITY_DEADZONE 1.0f// : 120 Nm + +// ̶ (ӦByte4~5: ǰ) +#define TORQUE_MIN -120.0f // С: -120 Nm +#define TORQUE_MAX 120.0f // : 120 Nm + +#define ZERO_VAULE 0x0080 // 32768,Ҫλǰ + + + + +#define SWAP_ENDIAN_16(x) ((((x) & 0xFF) << 8) | (((x) >> 8) & 0xFF)) +#define SWAP_ENDIAN_32(x) (((x) << 24) | (((x) & 0xFF00) << 8) | (((x) >> 8) & 0xFF00) | ((x) >> 24)) + + + +typedef struct TurnableData +{ + uint8_t turnable_state; + PowerState current_state; // ǰԴ״̬ + + float position_x; //ת̨λx + float position_y; //ת̨λy + float position_z; //ת̨λz + + float desired_speed; // ת̨ٶ + float desired_pitch_position; // λ + float desired_horizontal_position; // ˮƽλ + + float left_motor_speed; // ǰٶ + float right_motor_speed; // ǰҵٶ + float speed; // ǰתٶ + float pitch_position; // ǰλ + float horizontal_position; // ǰˮƽλ + + float max_speed; // ٶ + float out_left_motor_ampere; // + float out_right_motor_ampere; // ҵ + float out_pitch_motor_ampere; // ҵ + + float out_left_motor_ampere_last; // + float out_right_motor_ampere_last; // ҵ + float out_pitch_motor_ampere_last; // ҵ + + float out_left_motor_ampere_limit; // ֵ + float out_right_motor_ampere_limit; // ҵֵ + float out_pitch_motor_ampere_limit; // ҵֵ + + + Timer turnable_timer; // ʱ + Timer turnable_timer1; // ʱ + Timer turnable_timer2; // ʱ + uint8_t turnable_cnt; + + float max_ampere; // +} TurnableData; + + + +void turnableInit(); + + +#ifdef __cplusplus +} +#endif + +#endif // TURNTAABLE_H diff --git a/crc16.h b/crc16.h new file mode 100644 index 00000000..d1f27ea2 --- /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 00000000..6ff4f7f2 --- /dev/null +++ b/interface.c @@ -0,0 +1,72 @@ + + +#include "interface_config.h" + +UnMotorInput un_motor_input1 ;//电机控制器1 左前侧 +UnMotorInput un_motor_input2 ;//电机控制器2 右前侧 +UnMotorInput un_motor_input3 ;//电机控制器1 左后侧 +UnMotorInput un_motor_input4 ;//电机控制器2 右后侧 + + +UnMotorTempInput un_motor_temp1 ;//电机控制器1 左前侧 +UnMotorTempInput un_motor_temp2 ;//电机控制器2 右前侧 +UnMotorTempInput un_motor_temp3 ;//电机控制器3 左后侧 +UnMotorTempInput un_motor_temp4 ;//电机控制器4 右后侧 + + + +UnBmsInput un_bms_input ;//BMS接收数据 +UnTempModuleInput un_temp_module_input ;//温度采集模块 + +UnMotorOutput un_motor_output1 ;//电机输出 +UnMotorOutput un_motor_output2 ;//电机输出 +UnMotorOutput un_motor_output3 ;//电机输出 +UnMotorOutput un_motor_output4 ;//电机输出 + +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 ;//遥控器输入 + +UnHBridgeOutput un_h_bridge_output ;//H桥输出 +UnHBridgeOutput un_h_bridge_output1 ;//H桥输出 + + +UnGatherOutput un_gather_output ;//采集模块输出 + +UnUltrasonicInput un_ultrasonic_input1 ;//超声波传感器输入1 + +UnUltrasonicOutput un_ultrasonic_output1 ;//超声波传感器输出 + + +//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 ;// 自动控制数据输出，返回给请求者 + + + + +UnCanDebugOutput un_can_debug_output;//调试输出 + + + + + diff --git a/interface.h b/interface.h new file mode 100644 index 00000000..d9e18a00 --- /dev/null +++ b/interface.h @@ -0,0 +1,985 @@ +#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 +{ + //-----接收数据0x101或者0x201---------------------------------------------- + unsigned int speed : 16; // 转速 偏移量 -30000 + unsigned int bus_voltage : 16; // 母线电压 系数 0.1 偏移量 -3000 + unsigned int torque : 16; // 扭矩 系数 0.01 偏移量 -300 实际物理量＝数据×系数＋偏移量 + unsigned int fault_code : 8; // 故障码 + unsigned int heartbeat : 8; // 心跳 + +} StrMotorInput; +typedef union _UnMotorInput +{ + StrMotorInput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrMotorInput)]; // 通过结构体类型确定大小 +} UnMotorInput; + + + +// 接收电机控制器输入 +typedef struct _StrCanDebugOutput +{ + uint8_t speed; + uint8_t desired_speed; + uint8_t curvature; + uint8_t desired_curvature; + uint16_t set_left_out; + uint16_t set_right_out; +} StrCanDebugOutput; + +typedef union _UnCanDebugOutput +{ + StrCanDebugOutput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrCanDebugOutput)]; // 通过结构体类型确定大小 +} UnCanDebugOutput; + + + + + + + + + + +typedef struct _StrMotorTempInput +{ + //-----接收数据0x103或者0x104---------------------------------------------- + unsigned int controller_temp : 8; // 控制器温度 偏移量 -40 + unsigned int motor_temp : 8; // 电机温度 偏移量 -40 + unsigned int motor_max_torque: 16; // 电机最大扭矩 系数 0.01 偏移量 -300 实际物理量＝数据×系数＋偏移量 + unsigned int reserved_1 : 8; // 保留1 + unsigned int reserved_2 : 8; // 保留2 + unsigned int reserved_3 : 8; // 保留3 + unsigned int heartbeat2 : 8; // 心跳 + +} StrMotorTempInput; + +typedef union _UnMotorTempInput +{ + StrMotorTempInput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrMotorInput)]; // 通过结构体类型确定大小 +} UnMotorTempInput; + + + + + + + + + + + +// 接收BMS输入 +typedef struct _StrBmsInput +{ +//----接收0x100---------------------------------- + // 多字节数据，高位在前，低位在后 + unsigned int bus_voltage : 16; // 母线电压 单位为10mV + unsigned int bus_current : 16; // 母线电流 单位为10mA + unsigned int remainder_capacity : 16; // 剩余容量 单位为10mAh 充电为正，放电为负 + unsigned int crc1 : 16; //crc +//----接收0x101---------------------------------- + // 多字节数据，高位在前，低位在后 + unsigned int full_capacity : 16; // 充满容量 单位为10mAh + unsigned int Discharge_times : 16; // 放电循环次数 单位为1次 + unsigned int soc : 16; // soc 1% + unsigned int crc2 : 16; //crc +} 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口输入 + +//20250116 增加温度采集字节 +typedef struct _StrSwSample +{ + unsigned int emergency_stop_switch : 1; // 通道1，急停开关 + unsigned int High_voltage_switch : 1; // 通道2，高压开关 + unsigned int flg_wake_ecu : 1; // 通道3 + unsigned int CH04 : 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; + unsigned int temperature[2]; // 温度 + +} StrSwSample; + +typedef union _UnSwSample +{ + StrSwSample bit_data; // 使用定义的结构体变量名 + unsigned char 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; + + +// 输出到采集模块 +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; + + + + + + + + + +// canoe协议输出 +typedef struct _StrSdoOutput +{ +//-----发送数据0x601---------------------------------------------- + unsigned int cmd : 8; // 命令 + unsigned int object_index : 16; // 索引 + unsigned int sub_index : 8; // 从索引 + unsigned int data : 32; // 数据 +} StrSdoOutput; + +typedef union _UnSdoOutput +{ + StrSdoOutput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrSdoOutput)]; // 通过结构体类型确定大小 +} UnSdoOutput; + + + +// 超声波数据发送 +typedef struct _StrUltrasonicOutput +{ +//-----发送数据0x521---------------------------------------------- + uint8_t node; // 节点 + uint8_t function_code; // 功能码 + uint16_t register_address; // 寄存器地址 + uint16_t data; // 数据 + uint16_t reserve;// 保留 +} StrUltrasonicOutput; + +typedef union _UnUltrasonicOutput +{ + StrUltrasonicOutput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrUltrasonicOutput)]; // 通过结构体类型确定大小 +} UnUltrasonicOutput; + + +// 超声波数据读取 +typedef struct _StrUltrasonicIntput +{ +//-----发送数据0x521---------------------------------------------- + uint8_t node; // 节点 + uint8_t function_code; // 功能码 + uint8_t len; // 长度 + uint16_t data; // 数据 + uint8_t reserve1;// 保留 + uint16_t reserve2;// 保留 +} StrUltrasonicInput; + +typedef union _UnUltrasonicInput +{ + StrUltrasonicInput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrUltrasonicInput)]; // 通过结构体类型确定大小 +} UnUltrasonicInput; + + + + + + + + + + + + + + + + +//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; // 帧头 固定值0xAACC + unsigned int frame_type : 16; // 帧类型 固定值0x0020 + unsigned int frame_length : 16; // 帧长 固定值41 + unsigned int accumulated : 8; // 累加值 按帧累加 + unsigned int desired_speed : 16; // 期望速度 系数0.01， 正为前进，负为后退 单位m/s + unsigned int desired_curvature : 16; // 期望曲率 系数0.0001，正为左转，负为右转 + unsigned int set_left_speed : 16; // 设定左转速度 电机1设定转速 正为正传，负为反转 + unsigned int set_right_speed : 16; // 设定右转速度 电机2设定转速 正为正传，负为反转 + unsigned int longitude : 32; // 纬度 系数 1e-7 值域（-180°，180°） + unsigned int latitude : 32; // 经度 系数 1e-7 值域（-90°，90°） + unsigned int altitude : 32; // 高度 + unsigned int heading_angle : 16; // 航向角 系数 0.01 值域（0-360°） + unsigned int speed : 16; // 当前车速 系数 0.01 正为前进，负为后退 单位m/s + unsigned int curvature : 16; // 当前曲率 系数 0.0001正为左转，负为右转 + unsigned int battery_voltage : 16; // 电池电压 系数 0.1 单位V + unsigned int battery_soc : 16; // 电池SOC 系数 0.1 单位：% + unsigned int battery_current : 16; // 电池电流 系数 0.1 偏移量 -30000 单位A + unsigned int vehicle_fault_state : 8; // 整车故障状态 0 故障 1正常 + unsigned int crc : 8; // CRC 按字节累加之和 取低8位 +} 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; // 左侧电机扭矩限制 系数 0.01 偏移量 -300 单位N/s + unsigned int right_torque_limit : 16; // 右侧电机扭矩限制 系数 0.01 偏移量 -300 单位N/s + unsigned int left_power_in : 16; // 左侧电机馈电功率 系数 1 偏移量 0 单位W + unsigned int right_power_in : 16; // 右侧电机馈电功率 系数 1 偏移量 0 单位W + unsigned int left_power_out : 16; // 左侧电机放电功率 系数 1 偏移量 0 单位W + unsigned int right_power_out : 16; // 右侧电机放电功率 系数 1 偏移量 0 单位W + unsigned int left_voltage : 16; // 左侧电机电压 系数0.1 偏移量-3000 单位V + unsigned int right_voltage : 16; // 右侧电机电压 系数0.1 偏移量-3000 单位V + 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; // 帧头 固定值0xAACC + unsigned int frame_type : 16; // 帧类型 固定值0x0022 + unsigned int frame_length : 16; // 帧长 固定值56 + unsigned int accumulated : 8; // 累加值 按帧累加 + unsigned int rc_straight_p : 32; // 遥控直行P参数 系数 0.001 + unsigned int rc_straight_i : 32; // 遥控直行I参数 系数 0.001 + unsigned int rc_straight_d : 32; // 遥控直行D参数 系数 0.001 + unsigned int auto_straight_p : 32; // 自主直行P参数 系数 0.001 + unsigned int auto_straight_i : 32; // 自主直行I参数 系数 0.001 + unsigned int auto_straight_d : 32; // 自主直行D参数 系数 0.001 + unsigned int rc_turn_p : 32; // 遥控转弯P参数 系数 0.001 + unsigned int rc_turn_i : 32; // 遥控转弯I参数 系数 0.001 + unsigned int rc_turn_d : 32; // 遥控转弯D参数 系数 0.001 + unsigned int auto_turn_p : 32; // 自主转弯P参数 系数 0.001 + unsigned int auto_turn_i : 32; // 自主转弯I参数 系数 0.001 + unsigned int auto_turn_d : 32; // 自主转弯D参数 系数 0.001 + unsigned int checksum : 8; // 校验和 按字节累加之和 取低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 UnUltrasonicInput un_ultrasonic_input1;//超声波传感器输入1 + +extern UnUltrasonicOutput un_ultrasonic_output1;//超声波传感器输出 + + + +extern UnMotorOutput un_motor_output1; //电机输出 +extern UnMotorOutput un_motor_output2; //电机输出 +extern UnMotorOutput un_motor_output3; //电机输出 +extern UnMotorOutput un_motor_output4; //电机输出 +extern UnMotorTempInput un_motor_temp1; //电机控制器1 温度 +extern UnMotorTempInput un_motor_temp2; //电机控制器2 温度 +extern UnMotorTempInput un_motor_temp3; //电机控制器3 左后侧 +extern UnMotorTempInput un_motor_temp4; //电机控制器4 右后侧 + + +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;//H桥输出 + +extern UnWheelSpeedOutput un_wheel_wpeed_output; +extern UnGatherOutput un_gather_output;//采集模块输出 + + +//串口 +extern UnRemoteControlInput un_remote_control_input; //遥控器输入 + +//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]; + +extern UnCanDebugOutput un_can_debug_output;//调试输出 + + +//函数 +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 00000000..999152ca --- /dev/null +++ b/interface_24c02.c @@ -0,0 +1,428 @@ + + +#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 + +// 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();//ι + } + } + + 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 00000000..98354964 --- /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 00000000..14f5390a --- /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 00000000..f96aa52c --- /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 00000000..39b82fc0 --- /dev/null +++ b/interface_can.c @@ -0,0 +1,1430 @@ + +#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" +#include "app/app_ultrasonic.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; + +bool ecu_online = 0;//0ߣ1 + + +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))//¶ + { + 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; +//-------------------------------------------------------------- + switch (status) + { + case FLEXCAN_RX_IDLE: + break; + + case FLEXCAN_RX_FIFO_IDLE: + if(LEFT_FRONT_MOTOR_INPUT1 == (buf->id)) + { + can_fault_info.bit_data.motor1_count ++; + for(i = 0; i < (buf->length); i++) + { + un_motor_input1.arr[i] = buf->dataBuffer[i]; + } + //ź + publishMessage(&un_motor_input1, 1); + } + else if( LEFT_FRONT_MOTOR_INPUT2 == (buf->id) ) + { + for(i = 0; i < (buf->length); i++) + { + un_motor_temp1.arr[i] = buf->dataBuffer[i]; + } + //ź + publishMessage(&un_motor_temp1, 1);//ź + } + else if( LEFT_REAR_MOTOR2_INPUT1 == (buf->id) )// + { + can_fault_info.bit_data.motor3_count ++; + for(i = 0; i < (buf->length); i++) + { + un_motor_input3.arr[i] = buf->dataBuffer[i]; + } + //ź +// publishMessage(&un_motor_input3, 1); //޸Ϊ + } + else if( LEFT_REAR_MOTOR2_INPUT2 == (buf->id) ) + { + for(i = 0; i < (buf->length); i++) + { + un_motor_temp3.arr[i] = buf->dataBuffer[i]; + } + //ź + publishMessage(&un_motor_temp3, 1);//ź + } + else{} + 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; +//-------------------------------------------------------------- + switch (status) + { + case FLEXCAN_RX_IDLE: + break; + + case FLEXCAN_RX_FIFO_IDLE: + if(RIGHT_FRONT_MOTOR_INPUT1 == (buf->id)) + { + can_fault_info.bit_data.motor2_count ++; + for(i = 0; i < (buf->length); i++) + { + un_motor_input2.arr[i] = buf->dataBuffer[i]; + } + //ź + publishMessage(&un_motor_input2, 1); + } + else if( RIGHT_FRONT_MOTOR_INPUT2 == (buf->id) ) + { + for(i = 0; i < (buf->length); i++) + { + un_motor_temp2.arr[i] = buf->dataBuffer[i]; + } + //ź + publishMessage(&un_motor_temp2, 1);//ź + } + else if( RIGHT_REAR_MOTOR_INPUT1 == (buf->id) )// + { + can_fault_info.bit_data.motor4_count ++; + for(i = 0; i < (buf->length); i++) + { + un_motor_input4.arr[i] = buf->dataBuffer[i]; + } + //ź + publishMessage(&un_motor_input4, 1); + } + else if( RIGHT_REAR_MOTOR_INPUT2 == (buf->id) ) + { + for(i = 0; i < (buf->length); i++) + { + un_motor_temp4.arr[i] = buf->dataBuffer[i]; + } + //ź + publishMessage(&un_motor_temp4, 1);//ź + } + else{} + 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{} + + 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; + uint8_t i = 0; +//-------------------------------------------------------------- + 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 if(ULTRASONIC_ID_1 == (buf->id)) + { + for(i = 0; i < (buf->length); i++) + { + un_ultrasonic_input1.arr[i] = buf->dataBuffer[i]; + } + publishMessage(&un_ultrasonic_input1, 1); + } + 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(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); + +// printf("ori_remote_stop: %d\n", un_remote_control_input.bit_data.switch_b); + } + 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, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output1, 8, 15);// +//} +// +//static void processSdoOutput2(void *signal_id) +//{ +// (void)signal_id; // DZΪʹã +// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output2, 8, 16); +//} +// +//static void processSdoOutput3(void *signal_id) +//{ +// (void)signal_id; // DZΪʹã +// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output3, 8, 17); +//} +// +//static void processSdoOutput4(void *signal_id) +//{ +// (void)signal_id; // DZΪʹã +// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output4, 8, 18); +//} +// +//static void processSdoOutput5(void *signal_id) +//{ +// (void)signal_id; // DZΪʹã +// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output5, 8, 19); +//} +// +// +//static void processSdoOutput7(void *signal_id) +//{ +// (void)signal_id; // DZΪʹã +// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output7, 8, 20); +//} + + + +static void processMotorOutput1(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_1, LEFT_FRONT_MOTOR_OUTPUT1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1, 8, 15);//1Ťغת + CAN_Send_Msg(&can_handle_1, LEFT_REAR_MOTOR_OUTPUT1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output3, 8, 16);//1Ťغת +} + +static void processMotorOutput2(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_2, RIGHT_FRONT_MOTOR_OUTPUT1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2, 8, 15);//2Ťغת + CAN_Send_Msg(&can_handle_2, RIGHT_REAR_MOTOR_OUTPUT1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output4, 8, 16);//2Ťغת +} + +static void processMotorOutput3(void *signal_id) +{ + (void)signal_id; // DZΪʹã + + CAN_Send_Msg(&can_handle_1, LEFT_FRONT_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1.arr[8], 8, 17);//1 + CAN_Send_Msg(&can_handle_1, LEFT_REAR_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output3.arr[8], 8, 18);//1 + +} + +static void processMotorOutput4(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_2, RIGHT_FRONT_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2.arr[8], 8, 17);//2 + CAN_Send_Msg(&can_handle_2, RIGHT_REAR_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output4.arr[8], 8, 18);//2 +} + +static void processKgfOutput1(void *signal_id) +{ + (void)signal_id; // DZΪʹã + + if(1 == ecu_online)//20250318 ޸Ϊ߲ܷͣɼģE3ͬʱ + { + un_inf_can_kgf_output1.bit_data.can_rx5 = 0xE3; + CAN_Send_Msg(&can_handle_0, 0x11000002, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_inf_can_kgf_output1, 8, 15);//KGF1 + } +} + +static void processKgfOutput2(void *signal_id) +{ + (void)signal_id; // DZΪʹã + if(1 == ecu_online)//20250318 ޸Ϊ߲ܷͣɼģE3ͬʱ + { + un_inf_can_kgf_output2.bit_data.can_rx5 = 0xE3; + CAN_Send_Msg(&can_handle_0, 0x14000002, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_inf_can_kgf_output2, 8, 16);//KGF2 + } + +} + +//static void processKgfOutput2(void *signal_id) +//{ +// (void)signal_id; // DZΪʹã +// if(1 == ecu_online)//20250318 ޸Ϊ߲ܷͣɼģE3ͬʱ +// { +// un_inf_can_kgf_output2.bit_data.can_rx5 = 0xE3; +// CAN_Send_Msg(&can_handle_0, 0x14000002, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_inf_can_kgf_output2, 8, 16);//KGF2 +// } +// +//} + + + +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_5, 0x7F2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_h_bridge_output, 8, 17);//H + CAN_Send_Msg(&can_handle_5, 0x722, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_h_bridge_output1, 8, 19);//H +} + +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);//ɼ +} + + + +static void processUltrasonicOutput(void *signal_id) +{ + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_6, ULTRASONIC_ID_1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_ultrasonic_output1, 8, 15);// +} + +//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);//Ƹ +//} + + + + +//can 100msһ +void canSendAll(void *signal_id) +{ + static uint16_t wheel_speed_cnt = 0; +// static uint8_t kgf_cnt = 0; + static uint16_t bms_cnt1 = 0; + static uint16_t bms_cnt2 = 0; +// static uint16_t motor_speed_cnt = 0; + static uint16_t motor_power_cnt = 0; +// static uint8_t h_bridge_cnt = 0; + uint8_t CanData[8] = {0,0,0,0,0,0,0,0}; +//------------------------------------------------------------------------- + (void)signal_id; // DZΪʹã + + bms_cnt1 ++; + if(bms_cnt1 >= 1000)//1s + { + bms_cnt1 = 0; + + CanData[0] = 0x5A; + + CAN_Send_Msg(&can_handle_3, 0x100, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 15);//BMS + } + + bms_cnt2 ++; + if(bms_cnt2 >= 1100)//1s + { + bms_cnt2 = 0; + + CanData[0] = 0x5A; + + CAN_Send_Msg(&can_handle_3, 0x101, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 16);//BMS + } + + + motor_power_cnt ++; + if(motor_power_cnt >= 1000)//1sһ + { + motor_power_cnt = 0; + processMotorOutput3(CanData); + processMotorOutput4(CanData); + } + +// motor_power_cnt ++; +// if(motor_power_cnt >= 10)//ȡת 20ms +// { +// motor_power_cnt = 0; +// +// un_motor_output1.bit_data.MotCon_1Signal6 = 0x84; +// un_motor_output1.bit_data.MotCon_1Signal7 = 0x02; +// un_motor_output2.bit_data.MotCon_1Signal6 = 0x84; +// un_motor_output2.bit_data.MotCon_1Signal7 = 0x02; +// +// processMotorOutput3(CanData); +// processMotorOutput4(CanData); +// } + +// kgf_cnt ++; +// if(kgf_cnt >= 5)//20210312 CANͨѶģ 20210710޸Ϊ50ms +// { +// kgf_cnt = 0; +// +// processKgfOutput1(CanData); +// processKgfOutput2(CanData); +// } +// + //ٷ + wheel_speed_cnt ++; + if(wheel_speed_cnt >= 100) + { + wheel_speed_cnt = 0; + + processWheelSpeedOutput(CanData); + + CAN_Send_Msg(&can_handle_0, 0x120, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS + CAN_Send_Msg(&can_handle_1, 0x121, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS + CAN_Send_Msg(&can_handle_2, 0x122, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS + CAN_Send_Msg(&can_handle_3, 0x123, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS + CAN_Send_Msg(&can_handle_4, 0x124, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS + CAN_Send_Msg(&can_handle_5, 0x125, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS + CAN_Send_Msg(&can_handle_6, 0x126, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, &un_can_debug_output.arr[0], 8, 16);//BMS + CAN_Send_Msg(&can_handle_7, 0x127, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS + } +// + + //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˶೤ʱ +} + +static void canInterfaceInput(void *signal_id) +{ + if (signal_id == &un_sw_sample) + { + ecu_online = un_sw_sample.bit_data.flg_wake_ecu; + } +} + + + +// 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)); + + un_inf_can_kgf_output2.bit_data.KGF04 = 1;//ɱǿԼϵͨ + + + // ʼʱʹ 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_gather_output, processUnGatherOutput); + subscribe(&un_sw_sample, canInterfaceInput); // ͣءѹ ѱ־ + subscribe(&un_ultrasonic_output1, processUltrasonicOutput); // + + + + + + + + +// subscribe(&un_h_bridge_output2, processHBridgeOutput2); +// subscribe(&un_lifter_output, processLifterOutput); + +// subscribe(&un_sdo_output1, processSdoOutput1); +// subscribe(&un_sdo_output2, processSdoOutput2); +// subscribe(&un_sdo_output3, processSdoOutput3); +// subscribe(&un_sdo_output4, processSdoOutput4); +// subscribe(&un_sdo_output5, processSdoOutput5); +// subscribe(&un_sdo_output7, processSdoOutput7); + + 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 00000000..4e5c237d --- /dev/null +++ b/interface_can.h @@ -0,0 +1,204 @@ + +#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 LEFT_FRONT_MOTOR_INPUT1 0x101 +#define LEFT_FRONT_MOTOR_INPUT2 0x103//¶ + + +#define LEFT_REAR_MOTOR2_INPUT1 0x102 +#define LEFT_REAR_MOTOR2_INPUT2 0x104//¶ + + +#define RIGHT_FRONT_MOTOR_INPUT1 0x101 +#define RIGHT_FRONT_MOTOR_INPUT2 0x103//¶ + + +#define RIGHT_REAR_MOTOR_INPUT1 0x102 +#define RIGHT_REAR_MOTOR_INPUT2 0x104//¶ + + +#define LEFT_FRONT_MOTOR_OUTPUT1 0x201 +#define LEFT_FRONT_MOTOR_OUTPUT2 0x401 + +#define LEFT_REAR_MOTOR_OUTPUT1 0x202 +#define LEFT_REAR_MOTOR_OUTPUT2 0x402 + +#define RIGHT_FRONT_MOTOR_OUTPUT1 0x201 +#define RIGHT_FRONT_MOTOR_OUTPUT2 0x401 + +#define RIGHT_REAR_MOTOR_OUTPUT1 0x202 +#define RIGHT_REAR_MOTOR_OUTPUT2 0x402 + + + + +//#define MOTOR_INPUT_ID_1 0x15000003//ɼģ +//#define MOTOR_INPUT_ID_2 0x10F92708//ǰ +//#define MOTOR_INPUT_ID_3 0x15000003//ɼģ +//#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 0x15000003 + + + +#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 motor3_count; //ֶݼ + uint8_t motor4_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 motor3_state; //1״̬ + uint8_t motor4_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 00000000..2e3bd1e3 --- /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 00000000..76df6e1c --- /dev/null +++ b/interface_ethernet.c @@ -0,0 +1,660 @@ +#include +#include +#include "debug.h" +#include +#include +#include "interface_ethernet.h" +#include "eth_cfg.h" +#include "lwip/timeouts.h" +#include "sdrv_eth.h" +#include "app/app_param_manage.h" +#include "app/app_differential_drive.h" + + +// +#define MAX_BUFFER_SIZE (sizeof(((RequestContext *)0)->param_request->arr)) +#define MAX_PACKET_SIZE 1024 // СΪ1K + +Timer ethernet_timer_interface; +Timer ethernet_timer_interface1; + +struct udp_pcb *udpcb_1; +struct udp_pcb *udpcb_2; +struct udp_pcb *udpcb_3; +struct udp_pcb *udpcb_4; +struct udp_pcb *udpcb_5; + +bool eth_pool_rx = false; + + +UnEthernetFault ethernet_fault_Info = { + .bit_data.auto_count = 0, + .bit_data.manual_count = 0, + .bit_data.auto_state = 0, + .bit_data.manual_state = 0, +}; + + + +StrEthernetParameter ethernet_parameter = { + .local_ip = {192,168,17,20}, + .mask = {255,255,255,0}, + .computer_ip = {192,168,17,3}, + .local_communication_port = 8011, + .Local_upper_port = 8000, + .Local_download_port = 7811, + .upper_ip = {192,168,17,3}, + .download_ip = {192,168,17,183}, + .target_upper_port = 8000, + .target_download_port = 7811, + .target_communication_port = 8011, +}; + + + +static bool g_eth1_rx_pkt; + + + +static void eth_dma_rx_int_cb(struct net_driver_s *dev) +{ + if (dev == &g_eth1_dev) + g_eth1_rx_pkt = true; + +// printf("eth_dma_rx_int_cb\n"); +} + + + + +static void lwip_Ethernet(void *signal_id) +{ +// uint32_t time_boot = getCurrentTime();//¼ǰʱ + (void)signal_id; // DZΪʹã + // ETH֡ݸLWIPЭջ + // ʹܽжϣΪISRdisableжϣˣ + // жϽձ־(g_eth1_rx_pkt)Ĵв + // µĽжϱӦԣﲻԭ⡣ + if ((!eth_pool_rx && g_eth1_rx_pkt) || eth_pool_rx) + { + g_eth1_rx_pkt = false; + dwc_eth_rx(&g_eth1_dev, eth_pool_rx); + } + + // LWIPЭջӿڣLWIPʱ + sys_check_timeouts(); + + timerStart(ðernet_timer_interface1, 10,1); //10msһ + + // printf("lwipInterface spend time:%d\n",getCurrentTime() - time_boot);//app˶೤ʱ +} + + +//err_t +//udp_bind(struct udp_pcb *pcb, const ip_addr_t *ipaddr, u16_t port) + +int8_t UdpSendToData(uint8_t udpcbID, uint8_t *buf, uint16_t len, uint8_t *sip, uint16_t port) +{ + struct pbuf *DesPuff = NULL; + ip4_addr_t Desaddr; + int8_t SendState = 0; +//---------------------------------------------------------------------------------------------------- + IP4_ADDR(&Desaddr,sip[0],sip[1],sip[2],sip[3]);//ֵַת + + DesPuff = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_POOL); /* ڴ */ + + pbuf_take(DesPuff, buf, len); /* g_lwip_demo_sendbufеݴpbufṹ */ + + if(UDPCB_1 == udpcbID) + { + SendState = udp_sendto(udpcb_1, DesPuff, &Desaddr, port); + } + else if(UDPCB_2 == udpcbID) + { + SendState = udp_sendto(udpcb_2, DesPuff, &Desaddr, port); + } + else if(UDPCB_3 == udpcbID) + { + SendState = udp_sendto(udpcb_3, DesPuff, &Desaddr, port); + } + else if(UDPCB_3 == udpcbID) + { + SendState = udp_sendto(udpcb_3, DesPuff, &Desaddr, port); + } + else if(UDPCB_4 == udpcbID) + { + SendState = udp_sendto(udpcb_4, DesPuff, &Desaddr, port); + } + else if(UDPCB_5 == udpcbID) + { + SendState = udp_sendto(udpcb_5, DesPuff, &Desaddr, port); + } + else + { + } + pbuf_free(DesPuff); /* ͷڴ */ + + return SendState; +} + +// udpcbID UDP +//recv ص +//port 󶨶˿ +void UDP_Echo_Init(uint8_t udpcbID, udp_recv_fn recv, uint16_t port) +{ +//------------------------------------------ + if(UDPCB_1 == udpcbID) + { + /* ½һƿ*/ + udpcb_1 = udp_new(); + ASSERT(udpcb_1); + /* 󶨶˿ں */ + udp_bind(udpcb_1, IP_ADDR_ANY, port); + /* עݻص */ + udp_recv(udpcb_1, recv, (void *)udpcbID); + } + else if(UDPCB_2 == udpcbID) + { + /* ½һƿ*/ + udpcb_2 = udp_new(); + ASSERT(udpcb_2); + /* 󶨶˿ں */ + udp_bind(udpcb_2, IP_ADDR_ANY, port); + /* עݻص */ + udp_recv(udpcb_2, recv, (void *)udpcbID); + } + else if(UDPCB_3 == udpcbID) + { + /* ½һƿ*/ + udpcb_3 = udp_new(); + ASSERT(udpcb_3); + /* 󶨶˿ں */ + udp_bind(udpcb_3, IP_ADDR_ANY, port); + /* עݻص */ + udp_recv(udpcb_3, recv, (void *)udpcbID); + } + else if(UDPCB_4 == udpcbID) + { + /* ½һƿ*/ + udpcb_4 = udp_new(); + ASSERT(udpcb_4); + /* 󶨶˿ں */ + udp_bind(udpcb_4, IP_ADDR_ANY, port); + /* עݻص */ + udp_recv(udpcb_4, recv, (void *)udpcbID); + } + else if(UDPCB_5 == udpcbID) + { + /* ½һƿ*/ + udpcb_5 = udp_new(); + ASSERT(udpcb_5); + /* 󶨶˿ں */ + udp_bind(udpcb_5, IP_ADDR_ANY, port); + /* עݻص */ + udp_recv(udpcb_5, recv, (void *)udpcbID); + } + else + { + } + +} + + + +void udp_Callback_1(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) +{ + uint8_t *buf = (uint8_t *)(p->payload); + uint16_t i = 0; + uint16_t len = 0; + uint16_t udp_temp = 0; +//------------------------------------------------------------------------------ + + if( (0xFF == buf[0] ) && ( 0xBB == buf[1] ) )//ֶңԶң + { + ethernet_fault_Info.bit_data.manual_count ++; + + if( (p->len) >= sizeof(UnManualComputerInput) )//ȡȳȡС + { + len = sizeof(UnManualComputerInput); + } + else + { + len = p->len; + } + + for(i = 0; i < len; i++) + { + un_manual_computer_input.arr[i] = buf[i]; + } + publishMessage(&un_manual_computer_input, 1); + +// p->len = len; +// printf("Manualrecive len:%d\n",len); +// udp_sendto(upcb, p, addr, port); + } + else if( (0xFF == buf[0] ) && (0xCC == buf[1] ) )// Զ + { + ethernet_fault_Info.bit_data.auto_count ++; + + if( (p->len) >= sizeof(UnAutoComputerInput) )//ȡȳȡС + { + len = sizeof(UnAutoComputerInput); + } + else + { + len = p->len; + } + + +// unsigned int frame_header : 16; // ֡ͷ ̶ֵ0xFFCC +// unsigned int frame_type : 16; // ֡ ̶ֵ0x0001 +// unsigned int frame_length : 8; // ֡ ̶ֵ0x19 +// unsigned int heartbeat : 8; // ֡ۼ +// unsigned int set_speed : 16; // 趨ٶ ϵ0.01ΪǰΪ λm/s +// unsigned int set_curvature : 16; // 趨 ϵ0.0001ΪתΪת +// unsigned int latitude : 32; // γ ϵ10^-7431234567ʾ43.1234567 +// unsigned int longitude : 32; // ϵ10^-7431234567ʾ43.1234567 +// unsigned int altitude : 32; // ߶ λmm +// unsigned int heading : 16; // 35999ʾ359.99 +// unsigned int crc : 8; // CRC ֽۼ֮ͣȡ8λ + for(i = 0; i < len; i++) + { + un_auto_computer_input.arr[i] = buf[i]; + } + + udp_temp = ((un_auto_computer_input.bit_data.set_speed << 8) | (un_auto_computer_input.bit_data.set_speed >> 8));//趨ٶ 20240928 ޸ĸߵλ + un_auto_computer_input.bit_data.set_speed = udp_temp; + + udp_temp = ((un_auto_computer_input.bit_data.set_curvature << 8) | (un_auto_computer_input.bit_data.set_curvature >> 8));//趨 + un_auto_computer_input.bit_data.set_curvature = udp_temp; + publishMessage(&un_auto_computer_input, 1); + +// p->len = len; +// printf("Autorecive len:%d\n",len); +// udp_sendto(upcb, p, addr, port); + } + else + { + } + + pbuf_free(p); +} + + + +void udp_Callback_2(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) +{ + uint8_t *buf = (uint8_t *)(p->payload); + uint8_t ip_addr[4] = {0,0,0,0}; + +//------------------------------------------------------------------------------------------------ + + ip_addr[0] = addr->addr & 0xff; /* IADDR4 */ + ip_addr[1] = (addr->addr >> 8) & 0xff; /* IADDR3 */ + ip_addr[2] = (addr->addr >> 16) & 0xff; /* IADDR2 */ + ip_addr[3] = (addr->addr >> 24) & 0xff; /* IADDR1 */ + + ethernet_parameter.target_upper_port = port;//յĶ˿ںIP´ηͶӦIPͶ˿ + + ethernet_parameter.upper_ip[0] = ip_addr[0]; + ethernet_parameter.upper_ip[1] = ip_addr[1]; + ethernet_parameter.upper_ip[2] = ip_addr[2]; + ethernet_parameter.upper_ip[3] = ip_addr[3]; + + if( (0x80 == buf[0]) && (0x00 == buf[1]) ) + { + memcpy((uint8_t *)&(un_request_frame.arr[0]),buf, sizeof(UnRequestFrame)); + publishMessage(&un_request_frame, 1); + } + + pbuf_free(p); +} + + +void udp_Callback_4(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) +{ + uint8_t *buf = (uint8_t *)(p->payload); +//------------------------------------------------------------------------------------------------ + + boot_eth_flag = true; + FrameHeader = ( (buf[0] << 8) | (buf[1]) ); + printf("FrameHeader %d\n",FrameHeader); + + pbuf_free(p); +} + + +void udp_Callback_5(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) +{ + uint8_t *buf = (uint8_t *)(p->payload); +//------------------------------------------------------------------------------------------------ + + if(FEEDDOG_HEADER == ( (buf[0] << 8) | (buf[1]) )) + { + WDTReFresh_flag = true;//ι־ + printf("Feed dog flag received %d\n",getCurrentTime()); + } + + + pbuf_free(p); +} + + +//100ms һ +void ethernetTimerProcess(void) +{ + static uint8_t ethernet_timer = 0; + static uint8_t ethernet_temp[4] = {0,0,0,0};//мжֵ + uint8_t temp = 0; + + ethernet_timer ++; + if(ethernet_timer >= 5)//500msжһ + { + ethernet_timer = 0; + +//ж------------------------------------------------------------ + if(ethernet_fault_Info.bit_data.auto_count == ethernet_temp[0])//һʾ + { + ethernet_fault_Info.bit_data.auto_state = FAULT; + } + else + { + ethernet_fault_Info.bit_data.auto_state = NORMAL; + ethernet_temp[0] = ethernet_fault_Info.bit_data.auto_count;//ݸ + } + + if(ethernet_fault_Info.bit_data.auto_state != ethernet_temp[1]) + { + ethernet_temp[1] = ethernet_fault_Info.bit_data.auto_state; + temp ++; + } + + +//ֶж------------------------------------------------------------ + if(ethernet_fault_Info.bit_data.manual_count == ethernet_temp[2])//һʾ + { + ethernet_fault_Info.bit_data.manual_state = FAULT; + } + else + { + ethernet_fault_Info.bit_data.manual_state = NORMAL; + ethernet_temp[2] = ethernet_fault_Info.bit_data.manual_count;//ݸ + } + + if(ethernet_fault_Info.bit_data.manual_state != ethernet_temp[3]) + { + ethernet_temp[3] = ethernet_fault_Info.bit_data.manual_state; + temp ++; + } + } + + publishMessage(ðernet_fault_Info, 1);// ״̬仯 ź +} + + +static void vehicleInfOoutput(void *signal_id) +{ + (void)signal_id; // DZΪʹã + + UdpSendToData(UDPCB_2,(uint8_t *)(&un_vehicle_Info_output), sizeof(un_vehicle_Info_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); +} + +static void motorStatusOutput(void *signal_id) +{ + (void)signal_id; // DZΪʹã + + UdpSendToData(UDPCB_2,(uint8_t *)(&un_motor_status_output), sizeof(un_motor_status_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); +} + +static void pidOutput(void *signal_id) +{ + (void)signal_id; // DZΪʹã + + UdpSendToData(UDPCB_2,(uint8_t *)(&un_pid_output), sizeof(un_pid_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); +} + +static void remoteControlOutput(void *signal_id) +{ + (void)signal_id; // DZΪʹã + + UdpSendToData(UDPCB_2,(uint8_t *)(&un_remote_control_output), sizeof(un_remote_control_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); +} + +static void manualControlOutput(void *signal_id) +{ + (void)signal_id; // DZΪʹã + + UdpSendToData(UDPCB_2,(uint8_t *)(&un_manual_control_output),sizeof(un_manual_control_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); + +} + +static void autoControlOutput(void *signal_id) +{ + (void)signal_id; // DZΪʹã + + UdpSendToData(UDPCB_2,(uint8_t *)(&un_auto_control_output), sizeof(un_auto_control_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port); +} + + + + + + +//̫ +void ethernetSendAll(void *signal_id) +{ + static uint8_t computer_output_cnt = 0; + uint32_t crc_temp = 0; + uint8_t i = 0; + uint16_t Rg_Tmp = 0; + float Rg_FloatTmp = 0; + static uint8_t eth_cnt = 0; + static uint8_t app_output_cnt = 0; + uint8_t BOOT_Arr[2] = {0x01,0x02};//ϵ緢app֡λ +//------------------------------------------------------------------------- + //̫50ms 20210811޸Ϊ100ms + //20210827޸Ϊλǰλٺһ + computer_output_cnt ++; + if(computer_output_cnt >= 1) + { + computer_output_cnt = 0; + //ֽۼӺͣעⲻź 20210826޸İֽۼ + +// printf("left_motor_speed = %f\n",diff_data.left_motor_speed); +// printf("right_motor_speed = %f\n",diff_data.right_motor_speed); + + un_computer_output.bit_data.frame_header = 0xCCFF; + un_computer_output.bit_data.frame_type = 0x1100; + un_computer_output.bit_data.frame_length = 0x0B; + un_computer_output.bit_data.accumulated = eth_cnt ++; + + Rg_FloatTmp = (diff_data.left_motor_speed*(float)getParam("whl_dia") * M_PI*60)/1000.0/(float)getParam("gRatio");// ת/ӡkm/h һȦ1.8 + Rg_FloatTmp = Rg_FloatTmp*100;//,Ҫ100 ,ϵ0.01 + Rg_Tmp = (uint16_t)((int16_t)(Rg_FloatTmp));//תΪ޷); //20231012޸ٷ 20231226-3ų޸ķ + un_computer_output.bit_data.speed = ((Rg_Tmp << 8) | (Rg_Tmp >> 8));//λǰλֺ + + Rg_FloatTmp = (diff_data.right_motor_speed*(float)getParam("whl_dia") * M_PI*60)/1000.0/(float)getParam("gRatio");// ת/ӡkm/h һȦ1.8 + Rg_FloatTmp = Rg_FloatTmp*100;//,Ҫ100 ,ϵ0.01 + Rg_Tmp = (uint16_t)((int16_t)(Rg_FloatTmp));//תΪ޷); //20231012޸ٷ 20231226-3ų޸ķ + un_computer_output.bit_data.curvature = ((Rg_Tmp << 8) | (Rg_Tmp >> 8));//λǰλֺ + + crc_temp = 0; + for(i = 0; i<10 ;i++)//ȹ̶ + { + crc_temp = crc_temp + (un_computer_output.arr[i]); + } + un_computer_output.bit_data.crc = (uint8_t)crc_temp; + + UdpSendToData(UDPCB_1,(uint8_t *)(&un_computer_output.arr[0]), 11, (uint8_t *)ðernet_parameter.computer_ip[0], COMMUNICATION_PORT); + UdpSendToData(UDPCB_4,(uint8_t *)(&un_computer_output.arr[0]), 11, (uint8_t *)ðernet_parameter.computer_ip[0], COMMUNICATION_PORT); + } + + + if(app_output_cnt < 10)//20250109 100ms 10֡app̫֡ûгʼɾͷ͵Ķ֡ + { + app_output_cnt ++; + UdpSendToData(UDPCB_4, BOOT_Arr, 2, (uint8_t *)ðernet_parameter.download_ip[0], BOOT_TX_PORT); + } +} + +// ̫ʱźŴ +static void ethernetInterfaceTimerProcess(void *signal_id) +{ + uint32_t time_boot = getCurrentTime();//¼ǰʱ + (void)signal_id; // DZΪʹã + + ethernetSendAll(signal_id); + + ethernetTimerProcess();//ж + + timerStart(ðernet_timer_interface, 100,1); //100msһ + +// printf("ethernetInterface spend time:%d\n",getCurrentTime() - time_boot);//app˶೤ʱ +} + + + + + + + + +static unsigned char receive_buffer[MAX_BUFFER_SIZE]; +//ص +void udp_Callback_3(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) +{ + static unsigned int received_size = 0; + uint8_t *buf = (uint8_t *)(p->payload); + + // Ƿµ֡ͷ + if ((0x80 == buf[1]) && (0xFF == buf[0])) + { + // ýչ + received_size = 0; + printf("⵽µ֡ͷ,ýչ\n"); + } + + // Ƿ㹻Ŀռ洢 + if (received_size + p->len <= MAX_BUFFER_SIZE) + { + printf("p->len %d :\n" , p->len); + + memcpy(receive_buffer + received_size, buf, p->len); + received_size += p->len; + + // Ƿյ + if (received_size >= sizeof(request_context.param_request->arr)) + { + printf("received_size : %d :\n" , received_size); + + for(u16_t i = 0; i< sizeof(request_context.param_request->arr); i++) + { + request_context.param_request->arr[i] = receive_buffer[i]; + } + + memcpy(request_context.param_request->arr, receive_buffer, sizeof(request_context.param_request->arr)); + request_context.sender_ip = (unsigned int)addr->addr; + request_context.sender_port = port; + + // ӡ request_context.param_request->arr +// printf("յ param_request->arr :\n"); +// for (size_t i = 0; i < sizeof(request_context.param_request->arr); ++i) +// { +// printf("%02X ", request_context.param_request->arr[i]); +// if ((i + 1) % 1024 == 0 || i == sizeof(request_context.param_request->arr) - 1) +// { +// printf("\n"); +// feedWatchdog(); +// } +// } + + publishMessage(&request_context, 1); + + printf("received_size%d \n" , received_size); + // ýջ + received_size = 0; + printf("յ,\n"); + } + } + else + { + // ,ý + received_size = 0; + printf("ջ,ݶ\n"); + } + + unsigned int tmp_count = pbuf_free(p); + printf("pbuf_freeѾͷţtmp_count :%d \n" , tmp_count); + p = NULL; +} + +void OnParamSend(void *data) +{ + uint8_t ip_addr[4] = {0,0,0,0}; + RequestContext *signal = (RequestContext *)(data); + + ip_addr[0] = signal->sender_ip & 0xff; /* IADDR4 */ + ip_addr[1] = (signal->sender_ip >> 8) & 0xff; /* IADDR3 */ + ip_addr[2] = (signal->sender_ip >> 16) & 0xff; /* IADDR2 */ + ip_addr[3] = (signal->sender_ip >> 24) & 0xff; /* IADDR1 */ + + uint32_t total_size = sizeof(signal->param_request->arr); + uint32_t sent_size = 0; + + + while (sent_size < total_size) + { + uint32_t remaining = total_size - sent_size; + uint32_t packet_size = (remaining > MAX_PACKET_SIZE) ? MAX_PACKET_SIZE : remaining; + + UdpSendToData(UDPCB_3, + (uint8_t *)(signal->param_request->arr) + sent_size, + packet_size, + ip_addr, + signal->sender_port); + + sent_size += packet_size; + // Сӳ,ʧ + udelay(1000); + } + printf("ݷɣ%d \n", sent_size); +} + + + +// APPģijʼ +void ethernetInterfaceInit(void) +{ +//----------------------------------- + if (!eth_pool_rx) { + // ʹDMA RxжϡTxδʹжϣΪETHʺܸߣ + // Txжϴʱٶȡ + dwc_eth_enable_dma_rx_int(&g_eth1_dev, eth_dma_rx_int_cb); + } + + timerInit(ðernet_timer_interface1); + timerInit(ðernet_timer_interface); + + // Ķʱźţڶʱɼ + subscribe(ðernet_timer_interface, ethernetInterfaceTimerProcess); + subscribe(ðernet_timer_interface1, lwip_Ethernet); + + + subscribe(&un_vehicle_Info_output, vehicleInfOoutput); + subscribe(&un_motor_status_output, motorStatusOutput); + subscribe(&un_pid_output, pidOutput); + subscribe(&un_remote_control_output, remoteControlOutput); + subscribe(&un_manual_control_output, manualControlOutput); + subscribe(&un_auto_control_output, autoControlOutput); + + // Ϣʹþ̬ԱΪص + subscribe(&request_send, OnParamSend); + + + timerStart(ðernet_timer_interface, 100,1); //100msһ + timerStart(ðernet_timer_interface1, 10,1); //10msһ + + printf( "ethernetInterface: initial OK %d\n",getCurrentTime()); +} + + \ No newline at end of file diff --git a/interface_ethernet.h b/interface_ethernet.h new file mode 100644 index 00000000..e6abef43 --- /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 00000000..109675c7 --- /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 00000000..029f4abb --- /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 00000000..d5225a67 --- /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 00000000..9aec95bc --- /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 00000000..5876c4e4 --- /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 00000000..8b48eea4 --- /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 00000000..9f377fed --- /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 00000000..d4be7206 --- /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 00000000..2d192de1 --- /dev/null +++ b/main.c @@ -0,0 +1,141 @@ +#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 "app/app_request.h" +#include "app/app_ultrasonic.h" + +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 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.75 \n"); + // 初始化框架 放在最前面，解决电机can发送信号累积不处理的问题。 + + testAppInit(); + paramAppInit(); + diffAppInit(); + brakeAppInit(); + powerAppInit(); //电源管理 + tempAppInit(); //温度 + lightAppInit(); //灯光 + ethernetInterfaceInit(); //以太网先初始化 + requestAppInit(); + canInterfaceInit(); + bootInterfaceInit(); +// ultrasonicAppInit(); + + printf("All init OK ------ %d\n",getCurrentTime()); + + for (;;) + { + // 处理信号 + processMessages(); + } +} diff --git a/teset.c b/teset.c new file mode 100644 index 00000000..9b002b8e --- /dev/null +++ b/teset.c @@ -0,0 +1,8 @@ +ljkasdl;asld;fla;ksdfl;ajsdfjasf;d + + +askljdfl;asjdfljas;ldfjasdf + + + +asdfasdfklj \ No newline at end of file