From 35ceb50ca7d04cc3db319d0bb1535e25057103b4 Mon Sep 17 00:00:00 2001 From: neozng Date: Sat, 29 Apr 2023 22:33:16 +0800 Subject: [PATCH] =?UTF-8?q?=E5=B0=86=E8=A7=86=E8=A7=89=E9=80=9A=E4=BF=A1?= =?UTF-8?q?=E8=BD=AC=E7=A7=BB=E5=88=B0INStask=E4=B9=8B=E5=90=8E=EF=BC=8C?= =?UTF-8?q?=E9=A2=91=E7=8E=87=E6=94=B9=E4=B8=BA1kHz=EF=BC=8Ccmd=E4=BB=85?= =?UTF-8?q?=E4=BF=9D=E7=95=99=E9=A2=9C=E8=89=B2=E5=92=8C=E6=A8=A1=E5=BC=8F?= =?UTF-8?q?=E8=AE=BE=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Src/freertos.c | 2 ++ application/cmd/robot_cmd.c | 7 +---- application/gimbal/gimbal.c | 37 ------------------------- modules/imu/ins_task.c | 4 +++ modules/master_machine/master_process.c | 32 +++++++++++++-------- modules/master_machine/master_process.h | 30 ++++++++++++++------ 6 files changed, 49 insertions(+), 63 deletions(-) diff --git a/Src/freertos.c b/Src/freertos.c index 18bb45c..0ecf8a9 100644 --- a/Src/freertos.c +++ b/Src/freertos.c @@ -29,6 +29,7 @@ #include "motor_task.h" #include "led_task.h" #include "referee_task.h" +#include "master_process.h" #include "daemon.h" #include "robot.h" /* USER CODE END Includes */ @@ -169,6 +170,7 @@ void StartINSTASK(void const *argument) { // 1kHz INS_Task(); + VisionSend(); // 解算完成后发送视觉数据 osDelay(1); } } diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 3b2cfa8..e49c951 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -316,12 +316,7 @@ void RobotCMDTask() EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 // 设置视觉发送数据,还需增加加速度和角速度数据 - vision_send_data.bullet_speed = 15; - vision_send_data.enemy_color = 0; - vision_send_data.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; - vision_send_data.yaw = gimbal_fetch_data.gimbal_imu_data.Yaw; - vision_send_data.roll = gimbal_fetch_data.gimbal_imu_data.Roll; - ; + // VisionSetFlag(chassis_fetch_data.enemy_color,,chassis_fetch_data.bullet_speed) // 推送消息,双板通信,视觉通信等 // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置 diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index f95c3db..e1a070f 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -15,45 +15,8 @@ static Subscriber_t *gimbal_sub; // cmd控制消息订阅者 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息 -BMI088Instance *imu; void GimbalInit() { - BMI088_Init_Config_s imu_config = { - .spi_acc_config = { - .GPIOx = CS1_ACCEL_GPIO_Port, - .cs_pin = CS1_ACCEL_Pin, - .spi_handle = &hspi1, - }, - .spi_gyro_config = { - .GPIOx = CS1_GYRO_GPIO_Port, - .cs_pin = CS1_GYRO_Pin, - .spi_handle = &hspi1, - }, - .acc_int_config = { - .exti_mode = EXTI_TRIGGER_FALLING, - .GPIO_Pin = INT_ACC_Pin, - .GPIOx = INT_ACC_GPIO_Port, - }, - .gyro_int_config = { - .exti_mode = EXTI_TRIGGER_FALLING, - .GPIO_Pin = INT_GYRO_Pin, - .GPIOx = INT_GYRO_GPIO_Port, - }, - .heat_pid_config = { - .Kp = 0.0f, - .Kd = 0.0f, - .Ki = 0.0f, - .MaxOut = 0.0f, - .DeadBand = 0.0f, - }, - .heat_pwm_config = { - .channel = TIM_CHANNEL_1, - .htim = &htim1, - }, - .cali_mode = BMI088_CALIBRATE_ONLINE_MODE, - .work_mode = BMI088_BLOCK_PERIODIC_MODE, - }; - // imu = BMI088Register(&imu_config); gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 // YAW Motor_Init_Config_s yaw_config = { diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index 8fd18d7..07a356e 100644 --- a/modules/imu/ins_task.c +++ b/modules/imu/ins_task.c @@ -18,6 +18,8 @@ #include "spi.h" #include "user_lib.h" #include "general_def.h" +#include "master_process.h" + static INS_t INS; static IMU_Param_t IMU_Param; static PIDInstance TempCtrl = {0}; @@ -152,6 +154,8 @@ void INS_Task(void) INS.Pitch = QEKF_INS.Pitch; INS.Roll = QEKF_INS.Roll; INS.YawTotalAngle = QEKF_INS.YawTotalAngle; + + VisionSetAltitude(INS.Yaw,INS.Pitch); } // temperature control diff --git a/modules/master_machine/master_process.c b/modules/master_machine/master_process.c index f5ec0a7..3efe96e 100644 --- a/modules/master_machine/master_process.c +++ b/modules/master_machine/master_process.c @@ -16,9 +16,7 @@ #include "bsp_log.h" static Vision_Recv_s recv_data; -// @todo:由于后续需要进行IMU-Cam的硬件触发采集控制,因此可能需要将发送设置为定时任务,或由IMU采集完成产生的中断唤醒的任务, -// 后者显然更nice,使得时间戳对齐. 因此,在send_data中设定其他的标志位数据,让ins_task填充姿态值. -// static Vision_Send_s send_data; +static Vision_Send_s send_data; static USARTInstance *vision_usart_instance; static DaemonInstance *vision_daemon_instance; @@ -69,26 +67,36 @@ Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle) /** * @brief 发送函数 - * @todo 1.提高可读性,将get_protocol_send_data的第6个参数增加一个float buffer - * 2.添加标志位解码 - * 3.由于后续需要进行IMU-Cam的硬件触发采集控制,因此可能需要将发送设置为定时任务 - * 或由IMU采集完成产生的中断唤醒的任务,使得时间戳对齐. - * + * * @param send 待发送数据 + * */ -void VisionSend(Vision_Send_s *send) +void VisionSend() { // buff和txlen必须为static,才能保证在函数退出后不被释放,使得DMA正确完成发送 // 析构后的陷阱需要特别注意! static uint16_t flag_register; - static uint8_t send_buff[VISION_SEND_SIZE]; + static uint8_t send_buff[VISION_SEND_SIZE]; static uint16_t tx_len; // TODO: code to set flag_register flag_register = 30 << 8 | 0b00000001; // 将数据转化为seasky协议的数据包 - get_protocol_send_data(0x02, flag_register, &send->yaw, 3, send_buff, &tx_len); + get_protocol_send_data(0x02, flag_register, &send_data.yaw, 3, send_buff, &tx_len); USARTSend(vision_usart_instance, send_buff, tx_len, USART_TRANSFER_DMA); // 和视觉通信使用IT,防止和接收使用的DMA冲突 // 此处为HAL设计的缺陷,DMASTOP会停止发送和接收,导致再也无法进入接收中断. // 也可在发送完成中断中重新启动DMA接收,但较为复杂.因此,此处使用IT发送. // 若使用了daemon,则也可以使用DMA发送. -} \ No newline at end of file +} + +void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed) +{ + send_data.enemy_color = enemy_color; + send_data.work_mode = work_mode; + send_data.bullet_speed = bullet_speed; +} + +void VisionSetAltitude(float yaw, float pitch) +{ + send_data.yaw = yaw; + send_data.pitch = pitch; +} diff --git a/modules/master_machine/master_process.h b/modules/master_machine/master_process.h index e23cf7a..953ca62 100644 --- a/modules/master_machine/master_process.h +++ b/modules/master_machine/master_process.h @@ -47,8 +47,9 @@ typedef struct typedef enum { - BLUE = 0, - RED = 1 + COLOR_NONE = 0, + COLOR_BLUE = 1, + COLOR_RED = 2, } Enemy_Color_e; typedef enum @@ -76,9 +77,6 @@ typedef struct float yaw; float pitch; - float roll; - - // uint32_t time_stamp; // @todo 用于和相机的时间戳对齐 } Vision_Send_s; #pragma pack() @@ -90,10 +88,26 @@ typedef struct Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle); /** - * @brief 发送视觉视觉 + * @brief 发送视觉数据 * - * @param send 视觉需要的数据 */ -void VisionSend(Vision_Send_s *send); +void VisionSend(); + +/** + * @brief 设置视觉发送标志位 + * + * @param enemy_color + * @param work_mode + * @param bullet_speed + */ +void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed); + +/** + * @brief 设置发送数据的姿态部分 + * + * @param yaw + * @param pitch + */ +void VisionSetAltitude(float yaw, float pitch); #endif // !MASTER_PROCESS_H \ No newline at end of file