From bc1f928e02876286333af82f088e44191cf64027 Mon Sep 17 00:00:00 2001 From: NeoZng Date: Sat, 3 Dec 2022 15:20:17 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86hardfault=E8=B0=83?= =?UTF-8?q?=E8=AF=95=E6=94=AF=E6=8C=81,=E4=BF=AE=E5=A4=8D=E4=BA=86?= =?UTF-8?q?=E6=B6=88=E6=81=AF=E9=98=9F=E5=88=97=E5=9C=A8=E5=8F=91=E5=B8=83?= =?UTF-8?q?=E8=80=85=E5=85=88=E4=BA=8E=E8=AE=A2=E9=98=85=E8=80=85=E5=88=9B?= =?UTF-8?q?=E5=BB=BA=E8=AF=9D=E9=A2=98=E6=97=B6=E9=87=8E=E6=8C=87=E9=92=88?= =?UTF-8?q?=E7=9A=84=E9=94=99=E8=AF=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- HAL_N_Middlewares/Src/stm32f4xx_it.c | 2 + README.md | 8 +- application/chassis/chassis.c | 138 ++++++++++++++++++++++-- application/cmd/chassis_cmd.c | 22 +++- application/cmd/gimbal_cmd.c | 37 +++++-- application/gimbal/gimbal.c | 69 +++++++++++- application/robot.c | 18 +++- application/shoot/shoot.c | 101 ++++++++++++++++- modules/imu/ins_task.c | 7 +- modules/imu/ins_task.h | 9 +- modules/message_center/message_center.c | 43 ++++---- modules/referee/referee.c | 22 ++-- modules/referee/referee.h | 18 ++-- modules/remote/remote_control.h | 1 + 14 files changed, 401 insertions(+), 94 deletions(-) diff --git a/HAL_N_Middlewares/Src/stm32f4xx_it.c b/HAL_N_Middlewares/Src/stm32f4xx_it.c index 46fafde..1616e15 100644 --- a/HAL_N_Middlewares/Src/stm32f4xx_it.c +++ b/HAL_N_Middlewares/Src/stm32f4xx_it.c @@ -100,6 +100,8 @@ void HardFault_Handler(void) /* USER CODE END HardFault_IRQn 0 */ while (1) { + // 发生hardfault,点击step over会自动跳转回出错的指令,方便调试 + asm("bx lr"); /* USER CODE BEGIN W1_HardFault_IRQn 0 */ /* USER CODE END W1_HardFault_IRQn 0 */ } diff --git a/README.md b/README.md index ccd108a..182d7da 100644 --- a/README.md +++ b/README.md @@ -163,11 +163,11 @@ Module层主要存放的是类型定义和实例指针数组，在该层没有 4. 增加机器人整车控制应用 - 主要功能：实现机器人的控制 - 在完成BSP层和Module层后，如果在APP层没有控制代码，则代码并无实际功能。换言之，BSP层与Module层的存在是为了APP层更简单、更合理、更易于扩展和移植。本框架的初始目标即是实现：在APP层仅需思考逻辑并用无关硬件的C语言代码实现即可完成整个机器人的控制。所有需要使用的模块和算法都在Module层提供。 + 在完成BSP层和Module层后，如果在APP层没有控制代码，则代码并无实际功能。换言之，BSP层与Module层的存在是为了APP层更简单、更合理、更易于扩展和移植。本框架的初始目标即是实现：在APP层仅需思考逻辑并用无关硬件的C语言代码实现即可完成整个机器人的控制。所有需要使用的模块和算法都在Module层提供，硬件的抽象在bsp层完成。 - - APP层按照模块（如云台、发射、底盘）建立对应的子文件夹，在其中完成初始化和相关逻辑功能的编写。还有用于发布指令的云台指令应用和底盘指令应用，前者应该包含一个遥控器模块和一个视觉通信模块，后者包含裁判系统模块。它们包含的模块都会处理一些指令和控制信息，因此将这两个应用从云台和底盘应用中隔离出来。这样还可以方便兼容双板。 + - APP层按照机械设计结构（如云台、发射、底盘）建立对应的子文件夹，在其中完成初始化和相关逻辑功能的编写。还有用于发布指令的云台指令应用和底盘指令应用，前者应该包含一个遥控器模块和一个视觉通信模块，后者包含裁判系统模块。它们包含的模块都会处理一些指令和控制信息，因此将这两个应用从云台和底盘应用中隔离出来。这样还可以方便兼容双板。 - - 单双板切换在application的`robot_def.h`中进行，修改宏定义可以切换开发板的设定模式。当设定为单板的时候，在`robot.c`中会对gimbal，chassis，shoot，gimbal_cmd，chassis_cmd五个应用都进行初始化。对于双板的情况，需要将上板配置为gimbal board，下板配置为chassis board，它们会分别初始化gimbal/shoot/gimbal_cmd和chassis/chassis_cmd。 + - 单双板切换在application的`robot_def.h`中进行，**修改宏定义可以切换开发板的设定模式**。当设定为单板的时候，在`robot.c`中会对gimbal，chassis，shoot，gimbal_cmd，chassis_cmd五个应用都进行初始化。对于双板的情况，需要将上板配置为gimbal board，下板配置为chassis board，它们会分别初始化gimbal/shoot/gimbal_cmd和chassis/chassis_cmd。 - 对于单板的情况，所有应用之间的信息交互通过message center完成。而使用双板时，需要通过板间通信传递控制信息（默认遥控器接收机和pc在云台板，裁判系统在底盘板，因此需要互发信息）。当前通过**条件编译**来控制信息的去向（发往message center/接收，还是通过can comm发送/接收），后续考虑将双板通信纳入message center的实现中，根据`robot_def.h`的开发板定义自动处理通信，降低应用层级的逻辑复杂度。 @@ -357,7 +357,7 @@ HAL库初始化 --> BSP初始化 --> Application初始化 --> app调用其拥有 APP会调用其所有的模块的初始化函数（注册函数），这是因为本框架的设计思想是任何模块在被注册（构造/初始化）之前，都是不存在的，当且仅当定义了一个模块结构体（也称实例）的时候，才有一个实体的概念。 -main函数唯一需要的函数是app层的`robot.c`中的`RobotInitia()`函数，它首先会调用BSP初始化，然后进行所有应用的初始化；每个应用会调用对应模块的初始化；一些依赖通信外设的模块会将通信支持相关的bsp进行初始化。初始化结束之后实时系统启动。 +main函数唯一需要的函数是app层的`robot.c`中的`RobotInit()`函数，它首先会调用BSP初始化，然后进行所有应用的初始化；每个应用会调用对应模块的初始化；一些依赖通信外设的模块会将通信支持相关的bsp进行初始化。初始化结束之后实时系统启动。 diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index e09a3ab..2b19b39 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -2,33 +2,149 @@ #include "robot_def.h" #include "dji_motor.h" #include "super_cap.h" +#include "message_center.h" -#ifdef CHASSIS_BOARD //使用板载IMU获取底盘转动角速度 +/* 底盘应用包含的模块和信息存储 */ +#ifdef CHASSIS_BOARD // 使用板载IMU获取底盘转动角速度 #include "ins_task.h" -IMU_Data_t* Chassis_IMU_data; +IMU_Data_t *Chassis_IMU_data; #endif // CHASSIS_BOARD +// static SuperCAP cap; +static dji_motor_instance *lf; // left right forward back +static dji_motor_instance *rf; +static dji_motor_instance *lb; +static dji_motor_instance *rb; -// SuperCAP cap; -static dji_motor_instance* lf; //left right forward back -static dji_motor_instance* rf; -static dji_motor_instance* lb; -static dji_motor_instance* rb; -static Chassis_Ctrl_Cmd_s chassis_cmd_recv; +static Publisher_t* chassis_pub; static Chassis_Upload_Data_s chassis_feedback_data; +static Subscriber_t* chassis_sub; +static Chassis_Ctrl_Cmd_s chassis_cmd_recv; + static void mecanum_calculate() { } - - void ChassisInit() { + // 左前轮 + Motor_Init_Config_s left_foward_config = { + .can_init_config = { + .can_handle = &hcan2, + .tx_id = 1, + }, + .controller_param_init_config = { + .angle_PID = { + .Kd = 10, + .Ki = 1, + .Kd = 2, + }, + .speed_PID = { + }, + .current_PID = { + + }, + }, + .controller_setting_init_config = { + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = MOTOR_FEED, + .close_loop_type = SPEED_LOOP | CURRENT_LOOP, + .reverse_flag = MOTOR_DIRECTION_REVERSE, + }, + .motor_type = M3508}; + // 右前轮 + Motor_Init_Config_s right_foward_config = { + .can_init_config = { + .can_handle = &hcan2, + .tx_id = 2, + }, + .controller_param_init_config = { + .angle_PID = { + .Kd = 10, + .Ki = 1, + .Kd = 2, + }, + .speed_PID = { + + }, + .current_PID = { + + }, + }, + .controller_setting_init_config = { + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = MOTOR_FEED, + .close_loop_type = SPEED_LOOP | CURRENT_LOOP, + .reverse_flag = MOTOR_DIRECTION_REVERSE, + }, + .motor_type = M3508}; + // 左后轮 + Motor_Init_Config_s left_back_config = { + .can_init_config = { + .can_handle = &hcan2, + .tx_id = 3, + }, + .controller_param_init_config = { + .angle_PID = { + .Kd = 10, + .Ki = 1, + .Kd = 2, + }, + .speed_PID = { + + }, + .current_PID = { + + }, + }, + .controller_setting_init_config = { + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = MOTOR_FEED, + .close_loop_type = SPEED_LOOP | CURRENT_LOOP, + .reverse_flag = MOTOR_DIRECTION_REVERSE, + }, + .motor_type = M3508}; + // 右后轮 + Motor_Init_Config_s right_back_config = { + .can_init_config = { + .can_handle = &hcan2, + .tx_id = 4, + }, + .controller_param_init_config = { + .angle_PID = { + .Kd = 10, + .Ki = 1, + .Kd = 2, + }, + .speed_PID = { + + }, + .current_PID = { + + }, + }, + .controller_setting_init_config = { + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = MOTOR_FEED, + .close_loop_type = SPEED_LOOP | CURRENT_LOOP, + .reverse_flag = MOTOR_DIRECTION_REVERSE, + }, + .motor_type = M3508}; + + lf = DJIMotorInit(&left_foward_config); + rf = DJIMotorInit(&right_foward_config); + lb = DJIMotorInit(&left_back_config); + rb = DJIMotorInit(&right_back_config); + + // SupercapInit(); + + chassis_sub=SubRegister("chassis_cmd",sizeof(Chassis_Ctrl_Cmd_s)); + chassis_pub=PubRegister("chassis_feed",sizeof(Chassis_Upload_Data_s)); } -void ChassisTask() +void ChassisTask() { } \ No newline at end of file diff --git a/application/cmd/chassis_cmd.c b/application/cmd/chassis_cmd.c index 8c7d871..c208f8c 100644 --- a/application/cmd/chassis_cmd.c +++ b/application/cmd/chassis_cmd.c @@ -1,22 +1,38 @@ #include "robot_def.h" #include "chassis_cmd.h" #include "referee.h" +#include "message_center.h" + +/* chassis_cmd应用包含的模块和信息存储 */ +static referee_info_t *referee_data; // 裁判系统的数据 #ifndef ONE_BOARD #include "can_comm.h" static CANCommInstance *chasiss_can_comm; // 双板通信CAN comm -#endif // !ONE_BOARD - +#endif // !ONE_BOARD +//和chassis应用的交互 +static Publisher_t* chassis_cmd_pub; static Chassis_Ctrl_Cmd_s chassis_cmd; // 发送给chassis应用的控制信息 +static Subscriber_t* chassis_feed_sub; static Chassis_Upload_Data_s chassis_fetch_data; // chassis反馈的数据 +//和gimbal_cmd的交互 +static Publisher_t* chassis2gimbal_pub; static Chassis2Gimbal_Data_s data_to_gimbal_cmd; // 发送给gimbal_cmd的数据,包括热量限制,底盘功率等 +static Subscriber_t* gimbal2chassis_sub; static Gimbal2Chassis_Data_s data_from_gimbal_cmd; // 来自gimbal_cmd的数据,主要是底盘控制量 -static referee_info_t *referee_data; // 裁判系统的数据 + void ChassisCMDInit() { + referee_data= RefereeInit(&huart6); + + chassis_cmd_pub=PubRegister("chassis_cmd",sizeof(Chassis_Ctrl_Cmd_s)); + chassis_feed_sub=SubRegister("chassis_feed",sizeof(Chassis_Upload_Data_s)); + chassis2gimbal_pub=PubRegister("chassis2gimbal",sizeof(Chassis2Gimbal_Data_s)); + gimbal2chassis_sub=SubRegister("gimbal2chassis",sizeof(Gimbal2Chassis_Data_s)); } void ChassisCMDTask() { + } diff --git a/application/cmd/gimbal_cmd.c b/application/cmd/gimbal_cmd.c index edbf924..02eec61 100644 --- a/application/cmd/gimbal_cmd.c +++ b/application/cmd/gimbal_cmd.c @@ -3,22 +3,34 @@ #include "remote_control.h" #include "ins_task.h" #include "master_process.h" +#include "message_center.h" + +/* gimbal_cmd应用包含的模块实例指针和交互信息存储*/ #ifndef ONE_BOARD #include "can_comm.h" -static CANCommInstance *chasiss_can_comm; +static CANCommInstance *chasiss_can_comm; //双板通信 #endif // !ONE_BOARD - -static Gimbal_Ctrl_Cmd_s gimbal_cmd_send; // 传递给云台的控制信息 -static Shoot_Ctrl_Cmd_s shoot_cmd_send; // 传递给发射的控制信息 -static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息 -static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息 -static Gimbal2Chassis_Data_s data_to_chassis_cmd; // 发送给底盘CMD应用的控制信息,主要是遥控器和UI绘制相关 -static Chassis2Gimbal_Data_s data_from_chassis_cmd; // 从底盘CMD应用接收的控制信息,底盘功率枪口热量等 static RC_ctrl_t *remote_control_data; // 遥控器数据 static Vision_Recv_s *vision_recv_data; // 视觉接收数据 static Vision_Send_s *vision_send_data; // 视觉发送数据 +static Publisher_t* gimbal_cmd_pub; +static Gimbal_Ctrl_Cmd_s gimbal_cmd_send; // 传递给云台的控制信息 +static Subscriber_t* gimbal_cmd_feed_sub; +static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息 + +static Publisher_t* shoot_cmd_pub; +static Shoot_Ctrl_Cmd_s shoot_cmd_send; // 传递给发射的控制信息 +static Subscriber_t* shoot_cmd_feed_sub; +static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息 + +static Publisher_t* gimbal2chassis_pub; +static Gimbal2Chassis_Data_s data_to_chassis_cmd; // 发送给底盘CMD应用的控制信息,主要是遥控器和UI绘制相关 +static Subscriber_t* chassis2gimbal_sub; +static Chassis2Gimbal_Data_s data_from_chassis_cmd; // 从底盘CMD应用接收的控制信息,底盘功率枪口热量等 + + static void CalcOffsetAngle() { } @@ -37,6 +49,15 @@ static void SetCtrlMessage() void GimbalCMDInit() { + remote_control_data=RC_init(&huart3); + vision_recv_data=VisionInit(&huart1); + + gimbal_cmd_pub=PubRegister("gimbal_cmd",sizeof(Gimbal_Ctrl_Cmd_s)); + gimbal_cmd_feed_sub=SubRegister("gimbal_feed",sizeof(Gimbal_Upload_Data_s)); + shoot_cmd_pub=PubRegister("shoot_cmd",sizeof(Shoot_Ctrl_Cmd_s)); + shoot_cmd_feed_sub=SubRegister("shoot_feed",sizeof(Shoot_Upload_Data_s)); + gimbal2chassis_pub=PubRegister("gimbal2chassis",sizeof(Gimbal2Chassis_Data_s)); + chassis2gimbal_sub=SubRegister("chassis2gimbal",sizeof(Chassis2Gimbal_Data_s)); } void GimbalCMDTask() diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index a1fb2d8..411b586 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -2,15 +2,76 @@ #include "robot_def.h" #include "dji_motor.h" #include "ins_task.h" +#include "message_center.h" -static IMU_Data_t Gimbal_IMU_data; // 云台IMU数据 -static dji_motor_instance yaw; // yaw电机 -static dji_motor_instance pitch; // pitch电机 -static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自gimbal_cmd的控制信息 + +static attitude_t* Gimbal_IMU_data; // 云台IMU数据 +static dji_motor_instance* yaw_motor; // yaw电机 +static dji_motor_instance *pitch_motor; // pitch电机 + +static Publisher_t* gimbal_pub; static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给gimbal_cmd的云台状态信息 +static Subscriber_t* gimbal_sub; +static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自gimbal_cmd的控制信息 + void GimbalInit() { + Gimbal_IMU_data=INS_Init(); + + // YAW + Motor_Init_Config_s yaw_config = { + .can_init_config = { + .can_handle = &hcan1, + .tx_id = 1, + }, + .controller_param_init_config = { + .angle_PID = { + .Kd = 10, + .Ki = 1, + .Kd = 2, + }, + .speed_PID = { + + }, + .other_angle_feedback_ptr=&Gimbal_IMU_data->YawTotalAngle, + }, + .controller_setting_init_config = { + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = MOTOR_FEED, + .close_loop_type = ANGLE_LOOP | SPEED_LOOP, + .reverse_flag = MOTOR_DIRECTION_REVERSE, + }, + .motor_type = GM6020}; + // PITCH + Motor_Init_Config_s pitch_config = { + .can_init_config = { + .can_handle = &hcan1, + .tx_id = 2, + }, + .controller_param_init_config = { + .angle_PID = { + .Kd = 10, + .Ki = 1, + .Kd = 2, + }, + .speed_PID = { + + }, + }, + .controller_setting_init_config = { + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = MOTOR_FEED, + .close_loop_type = ANGLE_LOOP | SPEED_LOOP, + .reverse_flag = MOTOR_DIRECTION_REVERSE, + }, + .motor_type = GM6020}; + + yaw_motor=DJIMotorInit(&yaw_config); + pitch_motor=DJIMotorInit(&pitch_config); + + gimbal_pub=PubRegister("gimbal_feed",sizeof(Gimbal_Upload_Data_s)); + gimbal_sub=SubRegister("gimbal_cmd",sizeof(Gimbal_Ctrl_Cmd_s)); } void GimbalTask() diff --git a/application/robot.c b/application/robot.c index 59615d3..2a6ec05 100644 --- a/application/robot.c +++ b/application/robot.c @@ -3,32 +3,40 @@ #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) #include "chassis.h" +#include "chassis_cmd.h" #endif #if defined(ONE_BOARD) || defined(GIMBAL_BOARD) #include "gimbal.h" #include "shoot.h" +#include "gimbal_cmd.h" #endif + void RobotInit() { - #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) - + ChassisInit(); + ChassisCMDInit(); #endif #if defined(ONE_BOARD) || defined(GIMBAL_BOARD) - + GimbalCMDInit(); + GimbalInit(); + ShootInit(); #endif } void RobotTask() { #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) - + ChassisTask(); + ChassisCMDTask(); #endif #if defined(ONE_BOARD) || defined(GIMBAL_BOARD) - + GimbalCMDTask(); + GimbalTask(); + ShootTask(); #endif } diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 1104d25..a60e3f5 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -1,15 +1,106 @@ #include "shoot.h" #include "robot_def.h" #include "dji_motor.h" +#include "message_center.h" -static dji_motor_instance *friction_l; // 左摩擦轮 -static dji_motor_instance *friction_r; // 右摩擦轮 -static dji_motor_instance *loader; // 拨盘电机 -static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自gimbal_cmd的发射控制信息 -static Shoot_Upload_Data_s shoot_feedback_data; // 反馈回gimbal_cmd的发射状态信息 +/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 + */ +static dji_motor_instance *friction_l; // 左摩擦轮 +static dji_motor_instance *friction_r; // 右摩擦轮 +static dji_motor_instance *loader; // 拨盘电机 + +static Publisher_t *shoot_pub; +static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自gimbal_cmd的发射控制信息 +static Subscriber_t *shoot_sub; +static Shoot_Upload_Data_s shoot_feedback_data; // 来自gimbal_cmd的发射控制信息 void ShootInit() { + // 左摩擦轮 + Motor_Init_Config_s left_friction_config = { + .can_init_config = { + .can_handle = &hcan1, + .tx_id = 1, + }, + .controller_param_init_config = { + .angle_PID = { + .Kd = 10, + .Ki = 1, + .Kd = 2, + }, + .speed_PID = { + + }, + .current_PID = { + + }, + }, + .controller_setting_init_config = { + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = MOTOR_FEED, + .close_loop_type = SPEED_LOOP | CURRENT_LOOP, + .reverse_flag = MOTOR_DIRECTION_REVERSE, + }, + .motor_type = M3508}; + // 右摩擦轮 + Motor_Init_Config_s right_friction_config = { + .can_init_config = { + .can_handle = &hcan1, + .tx_id = 2, + }, + .controller_param_init_config = { + .angle_PID = { + .Kd = 10, + .Ki = 1, + .Kd = 2, + }, + .speed_PID = { + + }, + .current_PID = { + + }, + }, + .controller_setting_init_config = { + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = MOTOR_FEED, + .close_loop_type = SPEED_LOOP | CURRENT_LOOP, + .reverse_flag = MOTOR_DIRECTION_REVERSE, + }, + .motor_type = M3508}; + // 拨盘电机 + Motor_Init_Config_s loader_config = { + .can_init_config = { + .can_handle = &hcan1, + .tx_id = 3, + }, + .controller_param_init_config = { + .angle_PID = { + .Kd = 10, + .Ki = 1, + .Kd = 2, + }, + .speed_PID = { + + }, + .current_PID = { + + }, + }, + .controller_setting_init_config = { + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = MOTOR_FEED, + .close_loop_type = SPEED_LOOP | CURRENT_LOOP, + .reverse_flag = MOTOR_DIRECTION_REVERSE, + }, + .motor_type = M2006}; + + friction_l = DJIMotorInit(&left_friction_config); + friction_r = DJIMotorInit(&right_friction_config); + loader = DJIMotorInit(&loader_config); + + shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s)); + shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s)); } void ShootTask() diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index 33f6b45..073d972 100644 --- a/modules/imu/ins_task.c +++ b/modules/imu/ins_task.c @@ -33,11 +33,6 @@ float RefTemp = 40; // 恒温设定温度 static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]); -/* 调用此函数获得姿态数据指针 */ -attitude_t *GetINSptr() -{ - return (attitude_t *)&INS.Roll; -} /** * @brief 温度控制 @@ -49,7 +44,7 @@ static void IMU_Temperature_Ctrl(void) imu_pwm_set(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX)); } -void INS_Init(void) +attitude_t* INS_Init(void) { while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR); IMU_Param.scale[X] = 1; diff --git a/modules/imu/ins_task.h b/modules/imu/ins_task.h index 56ab9b2..f8c9212 100644 --- a/modules/imu/ins_task.h +++ b/modules/imu/ins_task.h @@ -71,18 +71,11 @@ typedef struct float Roll; } IMU_Param_t; -/** - * @brief 反馈位姿结构体指针,apps通过初始化时保存该指针以实现数据访问 - * - * @return attitude_t* - */ -attitude_t *GetINSptr(); - /** * @brief 初始化惯导解算系统 * */ -void INS_Init(void); +attitude_t *INS_Init(void); /** * @brief 此函数放入实时系统中,以1kHz频率运行 diff --git a/modules/message_center/message_center.c b/modules/message_center/message_center.c index 3f610a3..43e6399 100644 --- a/modules/message_center/message_center.c +++ b/modules/message_center/message_center.c @@ -72,19 +72,21 @@ static Publisher_t message_center = { .first_subs = NULL, .next_event_node = NULL}; -static void CheckName(char* name) +static void CheckName(char *name) { - if(strnlen(name,MAX_EVENT_NAME_LEN+1)>=MAX_EVENT_NAME_LEN) + if (strnlen(name, MAX_EVENT_NAME_LEN + 1) >= MAX_EVENT_NAME_LEN) { - while (1); // 进入这里说明事件名超出长度限制 + while (1) + ; // 进入这里说明事件名超出长度限制 } } -static void CheckLen(uint8_t len1,uint8_t len2) +static void CheckLen(uint8_t len1, uint8_t len2) { - if(len1!=len2) + if (len1 != len2) { - while(1); // 相同事件的消息长度不同 + while (1) + ; // 相同事件的消息长度不同 } } @@ -97,14 +99,8 @@ Subscriber_t *SubRegister(char *name, uint8_t data_len) node = node->next_event_node; // 指向下一个发布者(发布者发布的事件) if (strcmp(name, node->event_name) == 0) // 如果事件名相同就订阅这个事件 { - CheckLen(data_len,node->data_len); - Subscriber_t *sub = node->first_subs; // 指向订阅了该事件的第一个订阅者 - while (sub->next_subs_queue) // 遍历订阅了该事件的链表 - { - sub = sub->next_subs_queue; // 移动到下一个订阅者 - } // 遇到空指针停下,说明到了链表尾部,创建新的订阅节点 - - // 申请内存,注意要memset因为新空间不一定是空的,可能有之前留存的垃圾值 + CheckLen(data_len, node->data_len); + // 创建新的订阅者结点,申请内存,注意要memset因为新空间不一定是空的,可能有之前留存的垃圾值 Subscriber_t *ret = (Subscriber_t *)malloc(sizeof(Subscriber_t)); memset(ret, 0, sizeof(Subscriber_t)); // 对新建的Subscriber进行初始化 @@ -113,11 +109,22 @@ Subscriber_t *SubRegister(char *name, uint8_t data_len) { // 给消息队列的每一个元素分配空间,queue里保存的实际上是数据执指针,这样可以兼容不同的数据长度 ret->queue[i] = malloc(sizeof(data_len)); } - - sub->next_subs_queue = ret; // 接到链表尾部,延长该订阅该事件的subs链表 + //如果之前没有订阅者,特殊处理一下 + if(node->first_subs==NULL) + { + node->first_subs=ret; + return ret; + } + // 遍历订阅者链表,直到到达尾部 + Subscriber_t *sub = node->first_subs; // iterator + while (sub->next_subs_queue) // 遍历订阅了该事件的订阅者链表 + { + sub = sub->next_subs_queue; // 移动到下一个订阅者,遇到空指针停下,说明到了链表尾部 + } + sub->next_subs_queue = ret; // 把刚刚创建的订阅者接上 return ret; } - // 时间名不同,在下一轮循环访问下一个结点 + // 事件名不同,在下一轮循环访问下一个结点 } // 遍历完,发现尚未注册事件(还没有发布者);那么创建一个事件,此时node是publisher链表的最后一个结点 node->next_event_node = (Publisher_t *)malloc(sizeof(Publisher_t)); @@ -146,7 +153,7 @@ Publisher_t *PubRegister(char *name, uint8_t data_len) node = node->next_event_node; // 切换到下一个发布者(事件)结点 if (strcmp(node->event_name, name) == 0) // 如果已经注册了相同的事件,直接返回结点指针 { - CheckLen(data_len,node->data_len); + CheckLen(data_len, node->data_len); return node; } } // 遍历完发现尚未创建name对应的事件 diff --git a/modules/referee/referee.c b/modules/referee/referee.c index f9ed9ac..5ad72b3 100644 --- a/modules/referee/referee.c +++ b/modules/referee/referee.c @@ -11,22 +11,23 @@ static usart_instance* referee_usart_instance; /**************裁判系统数据******************/ -referee_info_t referee_info; -uint8_t Judge_Self_ID; // 当前机器人的ID -uint16_t Judge_SelfClient_ID; // 发送者机器人对应的客户端ID +static referee_info_t referee_info; +static uint8_t Judge_Self_ID; // 当前机器人的ID +static uint16_t Judge_SelfClient_ID; // 发送者机器人对应的客户端ID static void ReceiveCallback() { JudgeReadData(referee_usart_instance->recv_buff); } -void RefereeInit(UART_HandleTypeDef *referee_usart_handle) +referee_info_t* RefereeInit(UART_HandleTypeDef *referee_usart_handle) { USART_Init_Config_s conf; conf.module_callback = ReceiveCallback; conf.usart_handle = referee_usart_handle; conf.recv_buff_size = RE_RX_BUFFER_SIZE; referee_usart_instance=USARTRegister(&conf); + return &referee_info; } /** @@ -120,15 +121,4 @@ void JudgeReadData(uint8_t *ReadFromUsart) JudgeReadData(ReadFromUsart + sizeof(xFrameHeader) + LEN_CMDID + referee_info.FrameHeader.DataLength + LEN_TAIL); } } -} - -/** - * @brief 读取瞬时功率 - * @param void - * @retval 实时功率值 - * @attention - */ -float JudgeGetChassisPower(void) -{ - return (referee_info.PowerHeatData.chassis_power); -} +} \ No newline at end of file diff --git a/modules/referee/referee.h b/modules/referee/referee.h index 5c3b256..4152e89 100644 --- a/modules/referee/referee.h +++ b/modules/referee/referee.h @@ -340,11 +340,17 @@ typedef struct ext_CommunatianData_t CommuData; // 队友通信信息 } referee_info_t; -extern referee_info_t referee_info; - -void RefereeInit(UART_HandleTypeDef *referee_usart_handle); -void JudgeReadData(uint8_t *ReadFromUsart); -float JudgeGetChassisPower(void); - #pragma pack() + +/** + * @brief + * + * @param referee_usart_handle + * @return referee_info_t* + */ +referee_info_t* RefereeInit(UART_HandleTypeDef *referee_usart_handle); + + + + #endif // !REFEREE_H diff --git a/modules/remote/remote_control.h b/modules/remote/remote_control.h index 895c246..e30ab3b 100644 --- a/modules/remote/remote_control.h +++ b/modules/remote/remote_control.h @@ -15,6 +15,7 @@ #include #include "main.h" +#include "usart.h" #define RC_CH_VALUE_MIN ((uint16_t)364) #define RC_CH_VALUE_OFFSET ((uint16_t)1024)