From 39108dec771278df7a86a1ac3b0d9cdb52cb4f21 Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Mon, 6 Oct 2025 17:37:53 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=B8=8A=E5=B1=82?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 CMakeLists.txt                 |   7 +-
 User/device/bmi088.c           | 381 +++++++++++++++++++++++++++++++++
 User/device/bmi088.h           |  81 +++++++
 User/device/device.h           |   4 +
 User/device/device_config.yaml |   8 +
 User/device/dm_imu.c           | 270 -----------------------
 User/device/dm_imu.h           |  90 --------
 User/device/motor_lz.c         |   1 +
 User/device/motor_rm.c         |   2 +-
 User/module/config.c           | 153 ++++++++++++-
 User/module/config.h           |   6 +-
 User/module/gimbal.c           | 246 +++++++++++++++++++++
 User/module/gimbal.h           | 180 ++++++++++++++++
 User/module/shoot.c            | 305 ++++++++++++++++++++++++++
 User/module/shoot.h            | 200 +++++++++++++++++
 User/task/ai.c                 |  44 ++++
 User/task/atti_esti.c          |  78 ++++---
 User/task/config.yaml          |  21 ++
 User/task/ctrl_gimbal.c        |  79 ++-----
 User/task/ctrl_shoot.c         |  48 +++++
 User/task/init.c               |  14 +-
 User/task/monitor.c            |  44 ++++
 User/task/rc.c                 |  89 +++++---
 User/task/user_task.c          |  15 ++
 User/task/user_task.h          |  32 ++-
 25 files changed, 1912 insertions(+), 486 deletions(-)
 create mode 100644 User/device/bmi088.c
 create mode 100644 User/device/bmi088.h
 delete mode 100644 User/device/dm_imu.c
 delete mode 100644 User/device/dm_imu.h
 create mode 100644 User/module/gimbal.c
 create mode 100644 User/module/gimbal.h
 create mode 100644 User/module/shoot.c
 create mode 100644 User/module/shoot.h
 create mode 100644 User/task/ai.c
 create mode 100644 User/task/ctrl_shoot.c
 create mode 100644 User/task/monitor.c

diff --git a/CMakeLists.txt b/CMakeLists.txt
index be7b042..b18eb1e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -67,8 +67,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/component/vmc.c
 
     # User/device sources
+    User/device/bmi088.c
     User/device/buzzer.c
-    User/device/dm_imu.c
     User/device/dr16.c
     User/device/motor.c
     User/device/motor_dm.c
@@ -80,13 +80,18 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     # User/module sources
     User/module/balance_chassis.c
     User/module/config.c
+    User/module/gimbal.c
+    User/module/shoot.c
 
     # User/task sources
+    User/task/ai.c
     User/task/atti_esti.c
     User/task/blink.c
     User/task/ctrl_chassis.c
     User/task/ctrl_gimbal.c
+    User/task/ctrl_shoot.c
     User/task/init.c
+    User/task/monitor.c
     User/task/rc.c
     User/task/user_task.c
 )
diff --git a/User/device/bmi088.c b/User/device/bmi088.c
new file mode 100644
index 0000000..0f4c154
--- /dev/null
+++ b/User/device/bmi088.c
@@ -0,0 +1,381 @@
+/*
+     BMI088 陀螺仪+加速度计传感器。
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "bmi088.h"
+
+#include <cmsis_os2.h>
+#include <gpio.h>
+#include <stdbool.h>
+#include <string.h>
+
+#include "bsp/time.h"
+#include "bsp/gpio.h"
+#include "bsp/spi.h"
+#include "component/user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+#define BMI088_REG_ACCL_CHIP_ID (0x00)
+#define BMI088_REG_ACCL_ERR (0x02)
+#define BMI088_REG_ACCL_STATUS (0x03)
+#define BMI088_REG_ACCL_X_LSB (0x12)
+#define BMI088_REG_ACCL_X_MSB (0x13)
+#define BMI088_REG_ACCL_Y_LSB (0x14)
+#define BMI088_REG_ACCL_Y_MSB (0x15)
+#define BMI088_REG_ACCL_Z_LSB (0x16)
+#define BMI088_REG_ACCL_Z_MSB (0x17)
+#define BMI088_REG_ACCL_SENSORTIME_0 (0x18)
+#define BMI088_REG_ACCL_SENSORTIME_1 (0x19)
+#define BMI088_REG_ACCL_SENSORTIME_2 (0x1A)
+#define BMI088_REG_ACCL_INT_STAT_1 (0x1D)
+#define BMI088_REG_ACCL_TEMP_MSB (0x22)
+#define BMI088_REG_ACCL_TEMP_LSB (0x23)
+#define BMI088_REG_ACCL_CONF (0x40)
+#define BMI088_REG_ACCL_RANGE (0x41)
+#define BMI088_REG_ACCL_INT1_IO_CONF (0x53)
+#define BMI088_REG_ACCL_INT2_IO_CONF (0x54)
+#define BMI088_REG_ACCL_INT1_INT2_MAP_DATA (0x58)
+#define BMI088_REG_ACCL_SELF_TEST (0x6D)
+#define BMI088_REG_ACCL_PWR_CONF (0x7C)
+#define BMI088_REG_ACCL_PWR_CTRL (0x7D)
+#define BMI088_REG_ACCL_SOFTRESET (0x7E)
+
+#define BMI088_REG_GYRO_CHIP_ID (0x00)
+#define BMI088_REG_GYRO_X_LSB (0x02)
+#define BMI088_REG_GYRO_X_MSB (0x03)
+#define BMI088_REG_GYRO_Y_LSB (0x04)
+#define BMI088_REG_GYRO_Y_MSB (0x05)
+#define BMI088_REG_GYRO_Z_LSB (0x06)
+#define BMI088_REG_GYRO_Z_MSB (0x07)
+#define BMI088_REG_GYRO_INT_STAT_1 (0x0A)
+#define BMI088_REG_GYRO_RANGE (0x0F)
+#define BMI088_REG_GYRO_BANDWIDTH (0x10)
+#define BMI088_REG_GYRO_LPM1 (0x11)
+#define BMI088_REG_GYRO_SOFTRESET (0x14)
+#define BMI088_REG_GYRO_INT_CTRL (0x15)
+#define BMI088_REG_GYRO_INT3_INT4_IO_CONF (0x16)
+#define BMI088_REG_GYRO_INT3_INT4_IO_MAP (0x18)
+#define BMI088_REG_GYRO_SELF_TEST (0x3C)
+
+#define BMI088_CHIP_ID_ACCL (0x1E)
+#define BMI088_CHIP_ID_GYRO (0x0F)
+
+#define BMI088_LEN_RX_BUFF (19)
+/* Private macro ------------------------------------------------------------ */
+#define BMI088_ACCL_NSS_SET() \
+  BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_SET)
+#define BMI088_ACCL_NSS_RESET() \
+  BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_RESET)
+
+#define BMI088_GYRO_NSS_SET() \
+  BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_SET)
+#define BMI088_GYRO_NSS_RESET() \
+  BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_RESET)
+
+/* Private typedef ---------------------------------------------------------- */
+typedef enum {
+  BMI_ACCL,
+  BMI_GYRO,
+} BMI_Device_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+static uint8_t buffer[2];
+static uint8_t bmi088_rxbuf[BMI088_LEN_RX_BUFF];
+
+static osThreadId_t thread_alert;
+static bool inited = false;
+
+/* Private function  -------------------------------------------------------- */
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+static void BMI_WriteSingle(BMI_Device_t dv, uint8_t reg, uint8_t data) {
+  buffer[0] = (reg & 0x7f);
+  buffer[1] = data;
+
+  BSP_TIME_Delay(1);
+  switch (dv) {
+    case BMI_ACCL:
+      BMI088_ACCL_NSS_RESET();
+      break;
+
+    case BMI_GYRO:
+      BMI088_GYRO_NSS_RESET();
+      break;
+  }
+
+  BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 2u, false);
+
+  switch (dv) {
+    case BMI_ACCL:
+      BMI088_ACCL_NSS_SET();
+      break;
+
+    case BMI_GYRO:
+      BMI088_GYRO_NSS_SET();
+      break;
+  }
+}
+
+static uint8_t BMI_ReadSingle(BMI_Device_t dv, uint8_t reg) {
+  BSP_TIME_Delay(1);
+  switch (dv) {
+    case BMI_ACCL:
+      BMI088_ACCL_NSS_RESET();
+      break;
+
+    case BMI_GYRO:
+      BMI088_GYRO_NSS_RESET();
+      break;
+  }
+  buffer[0] = (uint8_t)(reg | 0x80);
+  BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false);
+  BSP_SPI_Receive(BSP_SPI_BMI088, buffer, 2u, false);
+
+  switch (dv) {
+    case BMI_ACCL:
+      BMI088_ACCL_NSS_SET();
+      return buffer[1];
+
+    case BMI_GYRO:
+      BMI088_GYRO_NSS_SET();
+      return buffer[0];
+  }
+}
+
+static void BMI_Read(BMI_Device_t dv, uint8_t reg, uint8_t *data, uint8_t len) {
+  if (data == NULL) return;
+
+  switch (dv) {
+    case BMI_ACCL:
+      BMI088_ACCL_NSS_RESET();
+      break;
+
+    case BMI_GYRO:
+      BMI088_GYRO_NSS_RESET();
+      break;
+  }
+  buffer[0] = (uint8_t)(reg | 0x80);
+  BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false); 
+  BSP_SPI_Receive(BSP_SPI_BMI088, data, len, true);
+}
+
+static void BMI088_RxCpltCallback(void) {
+  if (BSP_GPIO_ReadPin(BSP_GPIO_ACCL_CS) == GPIO_PIN_RESET) {
+    BMI088_ACCL_NSS_SET();
+    osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_RAW_REDY);
+  }
+  if (BSP_GPIO_ReadPin(BSP_GPIO_GYRO_CS) == GPIO_PIN_RESET) {
+    BMI088_GYRO_NSS_SET();
+    osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_RAW_REDY);
+  }
+}
+
+static void BMI088_AcclIntCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_NEW_DATA);
+}
+
+static void BMI088_GyroIntCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_NEW_DATA);
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali) {
+  if (bmi088 == NULL) return DEVICE_ERR_NULL;
+  if (cali == NULL) return DEVICE_ERR_NULL;
+  if (inited) return DEVICE_ERR_INITED;
+  if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
+
+  bmi088->cali = cali;
+
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_SOFTRESET, 0xB6);
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_SOFTRESET, 0xB6);
+  BSP_TIME_Delay(30);
+
+  /* Switch accl to SPI mode. */
+  BMI_ReadSingle(BMI_ACCL, BMI088_CHIP_ID_ACCL);
+
+  if (BMI_ReadSingle(BMI_ACCL, BMI088_REG_ACCL_CHIP_ID) != BMI088_CHIP_ID_ACCL)
+    return DEVICE_ERR_NO_DEV;
+
+  if (BMI_ReadSingle(BMI_GYRO, BMI088_REG_GYRO_CHIP_ID) != BMI088_CHIP_ID_GYRO)
+    return DEVICE_ERR_NO_DEV;
+
+  BSP_GPIO_DisableIRQ(BSP_GPIO_ACCL_INT);
+  BSP_GPIO_DisableIRQ(BSP_GPIO_GYRO_INT);
+
+  BSP_SPI_RegisterCallback(BSP_SPI_BMI088, BSP_SPI_RX_CPLT_CB,
+                           BMI088_RxCpltCallback);
+  BSP_GPIO_RegisterCallback(BSP_GPIO_ACCL_INT, BMI088_AcclIntCallback);
+  BSP_GPIO_RegisterCallback(BSP_GPIO_GYRO_INT, BMI088_GyroIntCallback);
+
+  /* Accl init. */
+  /* Filter setting: Normal. */
+  /* ODR: 0xAB: 800Hz. 0xAA: 400Hz. 0xA9: 200Hz. 0xA8: 100Hz. 0xA6: 25Hz. */
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_CONF, 0xAA);
+
+  /* 0x00: +-3G. 0x01: +-6G. 0x02: +-12G. 0x03: +-24G. */
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_RANGE, 0x01);
+
+  /* INT1 as output. Push-pull. Active low. Output. */
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_IO_CONF, 0x08);
+
+  /* Map data ready interrupt to INT1. */
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_INT2_MAP_DATA, 0x04);
+
+  /* Turn on accl. Now we can read data. */
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_PWR_CTRL, 0x04);
+  BSP_TIME_Delay(50);
+
+  /* Gyro init. */
+  /* 0x00: +-2000. 0x01: +-1000. 0x02: +-500. 0x03: +-250. 0x04: +-125. */
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_RANGE, 0x01);
+
+  /* Filter bw: 47Hz. */
+  /* ODR: 0x02: 1000Hz. 0x03: 400Hz. 0x06: 200Hz. 0x07: 100Hz. */
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_BANDWIDTH, 0x03);
+
+  /* INT3 and INT4 as output. Push-pull. Active low. */
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_CONF, 0x00);
+
+  /* Map data ready interrupt to INT3. */
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_MAP, 0x01);
+
+  /* Enable new data interrupt. */
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT_CTRL, 0x80);
+
+  BSP_TIME_Delay(10);
+
+  inited = true;
+
+  BSP_GPIO_EnableIRQ(BSP_GPIO_ACCL_INT);
+  BSP_GPIO_EnableIRQ(BSP_GPIO_GYRO_INT);
+  return DEVICE_OK;
+}
+
+bool BMI088_GyroStable(AHRS_Gyro_t *gyro) {
+  return ((gyro->x < 0.03f) && (gyro->y < 0.03f) && (gyro->z < 0.03f));
+}
+
+uint32_t BMI088_WaitNew() {
+  return osThreadFlagsWait(
+      SIGNAL_BMI088_ACCL_NEW_DATA | SIGNAL_BMI088_GYRO_NEW_DATA, osFlagsWaitAll,
+      osWaitForever);
+}
+
+int8_t BMI088_AcclStartDmaRecv() {
+  BMI_Read(BMI_ACCL, BMI088_REG_ACCL_X_LSB, bmi088_rxbuf, BMI088_LEN_RX_BUFF);
+  return DEVICE_OK;
+}
+
+uint32_t BMI088_AcclWaitDmaCplt() {
+  return osThreadFlagsWait(SIGNAL_BMI088_ACCL_RAW_REDY, osFlagsWaitAll,
+                           osWaitForever);
+}
+
+int8_t BMI088_GyroStartDmaRecv() {
+  BMI_Read(BMI_GYRO, BMI088_REG_GYRO_X_LSB, bmi088_rxbuf + 7, 6u);
+  return DEVICE_OK;
+}
+
+uint32_t BMI088_GyroWaitDmaCplt() {
+  return osThreadFlagsWait(SIGNAL_BMI088_GYRO_RAW_REDY, osFlagsWaitAll,
+                           osWaitForever);
+}
+
+int8_t BMI088_ParseAccl(BMI088_t *bmi088) {
+  if (bmi088 == NULL) return DEVICE_ERR_NULL;
+
+#if 1
+  int16_t raw_x, raw_y, raw_z;
+  memcpy(&raw_x, bmi088_rxbuf + 1, sizeof(raw_x));
+  memcpy(&raw_y, bmi088_rxbuf + 3, sizeof(raw_y));
+  memcpy(&raw_z, bmi088_rxbuf + 5, sizeof(raw_z));
+
+  bmi088->accl.x = (float)raw_x;
+  bmi088->accl.y = (float)raw_y;
+  bmi088->accl.z = (float)raw_z;
+
+#else
+  const int16_t *praw_x = (int16_t *)(bmi088_rxbuf + 1);
+  const int16_t *praw_y = (int16_t *)(bmi088_rxbuf + 3);
+  const int16_t *praw_z = (int16_t *)(bmi088_rxbuf + 5);
+
+  bmi088->accl.x = (float)*praw_x;
+  bmi088->accl.y = (float)*praw_y;
+  bmi088->accl.z = (float)*praw_z;
+
+#endif
+
+  /* 3G: 10920. 6G: 5460. 12G: 2730. 24G: 1365. */
+  bmi088->accl.x /= 5460.0f;
+  bmi088->accl.y /= 5460.0f;
+  bmi088->accl.z /= 5460.0f;
+
+  int16_t raw_temp =
+      (uint16_t)((bmi088_rxbuf[17] << 3) | (bmi088_rxbuf[18] >> 5));
+
+  if (raw_temp > 1023) raw_temp -= 2048;
+
+  bmi088->temp = (float)raw_temp * 0.125f + 23.0f;
+
+  return DEVICE_OK;
+}
+
+int8_t BMI088_ParseGyro(BMI088_t *bmi088) {
+  if (bmi088 == NULL) return DEVICE_ERR_NULL;
+
+#if 1
+  /* Gyroscope imu_raw -> degrees/sec -> radians/sec */
+  int16_t raw_x, raw_y, raw_z;
+  memcpy(&raw_x, bmi088_rxbuf + 7, sizeof(raw_x));
+  memcpy(&raw_y, bmi088_rxbuf + 9, sizeof(raw_y));
+  memcpy(&raw_z, bmi088_rxbuf + 11, sizeof(raw_z));
+
+  bmi088->gyro.x = (float)raw_x;
+  bmi088->gyro.y = (float)raw_y;
+  bmi088->gyro.z = (float)raw_z;
+
+#else
+  /* Gyroscope imu_raw -> degrees/sec -> radians/sec */
+  const int16_t *raw_x = (int16_t *)(bmi088_rxbuf + 7);
+  const int16_t *raw_y = (int16_t *)(bmi088_rxbuf + 9);
+  const int16_t *raw_z = (int16_t *)(bmi088_rxbuf + 11);
+
+  bmi088->gyro.x = (float)*raw_x;
+  bmi088->gyro.y = (float)*raw_y;
+  bmi088->gyro.z = (float)*raw_z;
+#endif
+
+  /* FS125: 262.144. FS250: 131.072. FS500: 65.536. FS1000: 32.768.
+   * FS2000: 16.384.*/
+  bmi088->gyro.x /= 32.768f;
+  bmi088->gyro.y /= 32.768f;
+  bmi088->gyro.z /= 32.768f;
+
+  bmi088->gyro.x *= M_DEG2RAD_MULT;
+  bmi088->gyro.y *= M_DEG2RAD_MULT;
+  bmi088->gyro.z *= M_DEG2RAD_MULT;
+
+  bmi088->gyro.x -= bmi088->cali->gyro_offset.x;
+  bmi088->gyro.y -= bmi088->cali->gyro_offset.y;
+  bmi088->gyro.z -= bmi088->cali->gyro_offset.z;
+
+  return DEVICE_ERR_NULL;
+}
+
+float BMI088_GetUpdateFreq(BMI088_t *bmi088) {
+  (void)bmi088;
+  return 400.0f;
+}
diff --git a/User/device/bmi088.h b/User/device/bmi088.h
new file mode 100644
index 0000000..eb44e0c
--- /dev/null
+++ b/User/device/bmi088.h
@@ -0,0 +1,81 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "component/ahrs.h"
+#include "device/device.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef struct {
+  struct {
+    float x;
+    float y;
+    float z;
+  } gyro_offset; /* 陀螺仪偏置 */
+} BMI088_Cali_t; /* BMI088校准数据 */
+
+typedef struct {
+  DEVICE_Header_t header;
+  AHRS_Accl_t accl;
+  AHRS_Gyro_t gyro;
+
+  float temp; /* 温度 */
+
+  const BMI088_Cali_t *cali;
+} BMI088_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Exported functions prototypes -------------------------------------------- */
+int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali);
+int8_t BMI088_Restart(void);
+
+bool BMI088_GyroStable(AHRS_Gyro_t *gyro);
+
+/* Sensor use right-handed coordinate system. */
+/*
+                x < R(logo)
+                        y
+                UP is z
+        All implementation should follow this rule.
+ */
+uint32_t BMI088_WaitNew();
+
+/*
+  BMI088的Accl和Gyro共用同一个DMA通道，所以一次只能读一个传感器。
+  即BMI088_AcclStartDmaRecv() 和 BMI088_AcclWaitDmaCplt() 中间不能
+  出现 BMI088_GyroStartDmaRecv()。
+*/
+int8_t BMI088_AcclStartDmaRecv();
+uint32_t BMI088_AcclWaitDmaCplt();
+int8_t BMI088_GyroStartDmaRecv();
+uint32_t BMI088_GyroWaitDmaCplt();
+int8_t BMI088_ParseAccl(BMI088_t *bmi088);
+int8_t BMI088_ParseGyro(BMI088_t *bmi088);
+float BMI088_GetUpdateFreq(BMI088_t *bmi088);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/User/device/device.h b/User/device/device.h
index 9230af5..18ffc73 100644
--- a/User/device/device.h
+++ b/User/device/device.h
@@ -23,6 +23,10 @@ extern "C" {
 
 /* AUTO GENERATED SIGNALS BEGIN */
 #define SIGNAL_DR16_RAW_REDY (1u << 0)
+#define SIGNAL_BMI088_ACCL_RAW_REDY (1u << 1)
+#define SIGNAL_BMI088_GYRO_RAW_REDY (1u << 2)
+#define SIGNAL_BMI088_ACCL_NEW_DATA (1u << 3)
+#define SIGNAL_BMI088_GYRO_NEW_DATA (1u << 4)
 /* AUTO GENERATED SIGNALS END */
 
 /* USER SIGNALS BEGIN */
diff --git a/User/device/device_config.yaml b/User/device/device_config.yaml
index a538a7f..c48634b 100644
--- a/User/device/device_config.yaml
+++ b/User/device/device_config.yaml
@@ -1,3 +1,11 @@
+bmi088:
+  bsp_config:
+    BSP_GPIO_ACCL_CS: BSP_GPIO_ACCL_CS
+    BSP_GPIO_ACCL_INT: BSP_GPIO_ACCL_INT
+    BSP_GPIO_GYRO_CS: BSP_GPIO_GYRO_CS
+    BSP_GPIO_GYRO_INT: BSP_GPIO_GYRO_INT
+    BSP_SPI_BMI088: BSP_SPI_BMI088
+  enabled: true
 buzzer:
   bsp_config:
     BSP_PWM_BUZZER: BSP_PWM_TIM8_CH1
diff --git a/User/device/dm_imu.c b/User/device/dm_imu.c
deleted file mode 100644
index 18b05e5..0000000
--- a/User/device/dm_imu.c
+++ /dev/null
@@ -1,270 +0,0 @@
-/*
-    DM_IMU数据获取（CAN）
-*/
-
-/* Includes ----------------------------------------------------------------- */
-#include "dm_imu.h"
-
-#include "bsp/can.h"
-#include "bsp/time.h"
-#include "component/user_math.h"
-#include <string.h>
-
-/* Private define ----------------------------------------------------------- */
-#define DM_IMU_OFFLINE_TIMEOUT      1000    // 设备离线判定时间1000ms
-
-#define ACCEL_CAN_MAX (58.8f)
-#define ACCEL_CAN_MIN	(-58.8f)
-#define GYRO_CAN_MAX	(34.88f)
-#define GYRO_CAN_MIN	(-34.88f)
-#define PITCH_CAN_MAX	(90.0f)
-#define PITCH_CAN_MIN	(-90.0f)
-#define ROLL_CAN_MAX	(180.0f)
-#define ROLL_CAN_MIN	(-180.0f)
-#define YAW_CAN_MAX		(180.0f)
-#define YAW_CAN_MIN 	(-180.0f)
-#define TEMP_MIN			(0.0f)
-#define TEMP_MAX			(60.0f)
-#define Quaternion_MIN	(-1.0f)
-#define Quaternion_MAX	(1.0f)
-
-
-/* Private macro ------------------------------------------------------------ */
-/* Private typedef ---------------------------------------------------------- */
-/* Private variables -------------------------------------------------------- */
-/* Private function --------------------------------------------------------- */
-
-static uint8_t count = 0; // 计数器，用于判断设备是否离线
-/**
- * @brief: 无符号整数转换为浮点数函数
- */
-static float uint_to_float(int x_int, float x_min, float x_max, int bits)
-{
-    float span = x_max - x_min;
-    float offset = x_min;
-    return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
-}
-
-/**
- * @brief 解析加速度计数据
- */
-static int8_t DM_IMU_ParseAccelData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
-    if (imu == NULL || data == NULL || len < 8) {
-        return DEVICE_ERR;
-    }
-    int8_t temp = data[1];
-    uint16_t acc_x_raw = (data[3] << 8) | data[2];
-    uint16_t acc_y_raw = (data[5] << 8) | data[4];
-    uint16_t acc_z_raw = (data[7] << 8) | data[6];
-    imu->data.temp = (float)temp;
-    imu->data.accl.x = uint_to_float(acc_x_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
-    imu->data.accl.y = uint_to_float(acc_y_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
-    imu->data.accl.z = uint_to_float(acc_z_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
-    return DEVICE_OK;
-}
-/**
- * @brief 解析陀螺仪数据
- */
-static int8_t DM_IMU_ParseGyroData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
-    if (imu == NULL || data == NULL || len < 8) {
-        return DEVICE_ERR;
-    }
-    uint16_t gyro_x_raw = (data[3] << 8) | data[2];
-    uint16_t gyro_y_raw = (data[5] << 8) | data[4];
-    uint16_t gyro_z_raw = (data[7] << 8) | data[6];
-    imu->data.gyro.x = uint_to_float(gyro_x_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
-    imu->data.gyro.y = uint_to_float(gyro_y_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
-    imu->data.gyro.z = uint_to_float(gyro_z_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
-    return DEVICE_OK;
-}
-/**
- * @brief 解析欧拉角数据
- */
-static int8_t DM_IMU_ParseEulerData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
-    if (imu == NULL || data == NULL || len < 8) {
-        return DEVICE_ERR;
-    }
-    uint16_t pit_raw = (data[3] << 8) | data[2];
-    uint16_t yaw_raw = (data[5] << 8) | data[4];
-    uint16_t rol_raw = (data[7] << 8) | data[6];
-    imu->data.euler.pit = uint_to_float(pit_raw, PITCH_CAN_MIN, PITCH_CAN_MAX, 16) * M_DEG2RAD_MULT;
-    imu->data.euler.yaw = uint_to_float(yaw_raw, YAW_CAN_MIN, YAW_CAN_MAX, 16) * M_DEG2RAD_MULT;
-    imu->data.euler.rol = uint_to_float(rol_raw, ROLL_CAN_MIN, ROLL_CAN_MAX, 16) * M_DEG2RAD_MULT;
-    return DEVICE_OK;
-}
-
-/**
- * @brief 解析四元数数据
- */
-static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
-    if (imu == NULL || data == NULL || len < 8) {
-        return DEVICE_ERR;
-    }
-    int w = (data[1] << 6) | ((data[2] & 0xF8) >> 2);
-    int x = ((data[2] & 0x03) << 12) | (data[3] << 4) | ((data[4] & 0xF0) >> 4);
-    int y = ((data[4] & 0x0F) << 10) | (data[5] << 2) | ((data[6] & 0xC0) >> 6);
-    int z = ((data[6] & 0x3F) << 8) | data[7];
-    imu->data.quat.q0 = uint_to_float(w, Quaternion_MIN, Quaternion_MAX, 14);
-    imu->data.quat.q1 = uint_to_float(x, Quaternion_MIN, Quaternion_MAX, 14);
-    imu->data.quat.q2 = uint_to_float(y, Quaternion_MIN, Quaternion_MAX, 14);
-    imu->data.quat.q3 = uint_to_float(z, Quaternion_MIN, Quaternion_MAX, 14);
-    return DEVICE_OK;
-}
-
-
-/* Exported functions ------------------------------------------------------- */
-
-/**
- * @brief 初始化DM IMU设备
- */
-int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param) {
-    if (imu == NULL || param == NULL) {
-        return DEVICE_ERR_NULL;
-    }
-    
-    // 初始化设备头部
-    imu->header.online = false;
-    imu->header.last_online_time = 0;
-    
-    // 配置参数
-    imu->param.can = param->can;
-    imu->param.can_id = param->can_id;
-    imu->param.device_id = param->device_id;
-    imu->param.master_id = param->master_id;
-    
-    // 清零数据
-    memset(&imu->data, 0, sizeof(DM_IMU_Data_t));
-    
-    // 注册CAN接收队列，用于接收回复报文
-    int8_t result = BSP_CAN_RegisterId(imu->param.can, imu->param.master_id, 10);
-    if (result != BSP_OK) {
-        return DEVICE_ERR;
-    }
-
-    return DEVICE_OK;
-}
-
-/**
- * @brief 请求IMU数据
- */
-int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) {
-    if (imu == NULL) {
-        return DEVICE_ERR_NULL;
-    }
-    
-    // 构造发送数据：id_L, id_H(DM_IMU_ID), RID, 0xcc
-    uint8_t tx_data[4] = {
-        imu->param.device_id & 0xFF,        // id_L
-        (imu->param.device_id >> 8) & 0xFF, // id_H
-        (uint8_t)rid,                       // RID
-        0xCC                                // 固定值
-    };
-    
-    // 发送标准数据帧
-    BSP_CAN_StdDataFrame_t frame = {
-        .id = imu->param.can_id,
-        .dlc = 4,
-    };
-    memcpy(frame.data, tx_data, 4);
-    int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame);
-    return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR;
-}
-
-/**
- * @brief 更新IMU数据（从CAN中获取所有数据并解析）
- */
-int8_t DM_IMU_Update(DM_IMU_t *imu) {
-    if (imu == NULL) {
-        return DEVICE_ERR_NULL;
-    }
-    
-    BSP_CAN_Message_t msg;
-    int8_t result;
-    bool data_received = false;
-    
-    // 持续接收所有可用消息
-    while ((result = BSP_CAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) {
-        // 验证回复数据格式（至少检查数据长度）
-        if (msg.dlc < 3) {
-            continue; // 跳过无效消息
-        }
-        
-        // 根据数据位的第0位确定反馈报文类型
-        uint8_t rid = msg.data[0] & 0x0F; // 取第0位的低4位作为RID
-        
-        // 根据RID类型解析数据
-        int8_t parse_result = DEVICE_ERR;
-        switch (rid) {
-            case 0x01: // RID_ACCL
-                parse_result = DM_IMU_ParseAccelData(imu, msg.data, msg.dlc);
-                break;
-            case 0x02: // RID_GYRO
-                parse_result = DM_IMU_ParseGyroData(imu, msg.data, msg.dlc);
-                break;
-            case 0x03: // RID_EULER
-                parse_result = DM_IMU_ParseEulerData(imu, msg.data, msg.dlc);
-                break;
-            case 0x04: // RID_QUATERNION
-                parse_result = DM_IMU_ParseQuaternionData(imu, msg.data, msg.dlc);
-                break;
-            default:
-                continue; // 跳过未知类型的消息
-        }
-        
-        // 如果解析成功，标记为收到数据
-        if (parse_result == DEVICE_OK) {
-            data_received = true;
-        }
-    }
-    
-    // 如果收到任何有效数据，更新设备状态
-    if (data_received) {
-        imu->header.online = true;
-        imu->header.last_online_time = BSP_TIME_Get_ms();
-        return DEVICE_OK;
-    }
-    
-    return DEVICE_ERR;
-}
-
-/** 
- * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz）
- */ 
-int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){
-    if (imu == NULL) {
-        return DEVICE_ERR_NULL;
-    }
-    switch (count) {
-        case 0:
-            DM_IMU_Request(imu, RID_ACCL);
-            break;
-        case 1:
-            DM_IMU_Request(imu, RID_GYRO);
-            break;
-        case 2:
-            DM_IMU_Request(imu, RID_EULER);
-            break;
-        case 3:
-            DM_IMU_Request(imu, RID_QUATERNION);
-            DM_IMU_Update(imu); // 更新所有数据
-            break;
-    }
-    count++;
-    if (count >= 4) {
-        count = 0; // 重置计数器
-    }
-    return DEVICE_OK;
-}
-
-/**
- * @brief 检查设备是否在线
- */
-bool DM_IMU_IsOnline(DM_IMU_t *imu) {
-    if (imu == NULL) {
-        return false;
-    }
-    
-    uint32_t current_time = BSP_TIME_Get_ms();
-    return imu->header.online && 
-           (current_time - imu->header.last_online_time < DM_IMU_OFFLINE_TIMEOUT);
-}
diff --git a/User/device/dm_imu.h b/User/device/dm_imu.h
deleted file mode 100644
index 3965980..0000000
--- a/User/device/dm_imu.h
+++ /dev/null
@@ -1,90 +0,0 @@
-#pragma once
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* Includes ----------------------------------------------------------------- */
-#include "device/device.h"
-#include "component/ahrs.h"
-#include "bsp/can.h"
-/* Exported constants ------------------------------------------------------- */
-
-#define DM_IMU_CAN_ID_DEFAULT 0x6FF
-#define DM_IMU_ID_DEFAULT 0x42
-#define DM_IMU_MST_ID_DEFAULT 0x43
-
-/* Exported macro ----------------------------------------------------------- */
-/* Exported types ----------------------------------------------------------- */
-
-typedef struct {
-  BSP_CAN_t can;       // CAN总线句柄
-  uint16_t can_id;     // CAN通信ID
-  uint8_t device_id;   // 设备ID
-  uint8_t master_id;   // 主机ID
-} DM_IMU_Param_t;
-typedef enum {
-  RID_ACCL = 0x01, // 加速度计
-  RID_GYRO = 0x02, // 陀螺仪
-  RID_EULER = 0x03, // 欧拉角
-  RID_QUATERNION = 0x04, // 四元数
-} DM_IMU_RID_t;
-
-typedef struct {
-  AHRS_Accl_t accl; // 加速度计
-  AHRS_Gyro_t gyro; // 陀螺仪
-  AHRS_Eulr_t euler; // 欧拉角
-  AHRS_Quaternion_t quat; // 四元数
-  float temp; // 温度
-} DM_IMU_Data_t;
-
-typedef struct {
-  DEVICE_Header_t header;
-  DM_IMU_Param_t param; // IMU参数配置
-  DM_IMU_Data_t data; // IMU数据
-} DM_IMU_t;
-
-/* Exported functions prototypes -------------------------------------------- */
-
-/**
- * @brief 初始化DM IMU设备
- * @param imu DM IMU设备结构体指针
- * @param param IMU参数配置指针
- * @return DEVICE_OK 成功，其他值失败
- */
-int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param);
-
-/**
- * @brief 请求IMU数据
- * @param imu DM IMU设备结构体指针
- * @param rid 请求的数据类型
- * @return DEVICE_OK 成功，其他值失败
- */
-int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid);
-
-
-/**
- * @brief 更新IMU数据（从CAN中获取所有数据并解析）
- * @param imu DM IMU设备结构体指针
- * @return DEVICE_OK 成功，其他值失败
- */
-int8_t DM_IMU_Update(DM_IMU_t *imu);
-
-/**
- * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz，运行4次才有完整数据）
- * @param imu DM IMU设备结构体指针
- * @return DEVICE_OK 成功，其他值失败
- */
-int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu);
-
-/**
- * @brief 检查设备是否在线
- * @param imu DM IMU设备结构体指针
- * @return true 在线，false 离线
- */
-bool DM_IMU_IsOnline(DM_IMU_t *imu);
-
-
-#ifdef __cplusplus
-}
-#endif
diff --git a/User/device/motor_lz.c b/User/device/motor_lz.c
index 0553530..7ed7344 100644
--- a/User/device/motor_lz.c
+++ b/User/device/motor_lz.c
@@ -243,6 +243,7 @@ int8_t MOTOR_LZ_Init(void) {
     return BSP_CAN_RegisterIdParser(MOTOR_LZ_IdParser) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
 }
 
+
 int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) {
     if (param == NULL) return DEVICE_ERR_NULL;
     
diff --git a/User/device/motor_rm.c b/User/device/motor_rm.c
index ba10434..9b5ea44 100644
--- a/User/device/motor_rm.c
+++ b/User/device/motor_rm.c
@@ -139,13 +139,13 @@ static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) {
         motor->feedback.rotor_speed = rotor_speed;
         motor->feedback.torque_current = torque_current;
     }
-    if (motor->motor.reverse) {
         while (motor->feedback.rotor_abs_angle < 0) {
             motor->feedback.rotor_abs_angle += M_2PI;
         }
         while (motor->feedback.rotor_abs_angle >= M_2PI) {
             motor->feedback.rotor_abs_angle -= M_2PI;
         }
+    if (motor->motor.reverse) {
         motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle;
         motor->feedback.rotor_speed = -motor->feedback.rotor_speed;
         motor->feedback.torque_current = -motor->feedback.torque_current;
diff --git a/User/module/config.c b/User/module/config.c
index 3de3599..279458c 100644
--- a/User/module/config.c
+++ b/User/module/config.c
@@ -17,12 +17,153 @@
 
 // 机器人参数配置
 Config_RobotParam_t robot_config = {
-
-    .imu_param = {
-        .can = BSP_CAN_2,
-        .can_id = 0x6FF,      
-        .device_id = 0x42,   
-        .master_id = 0x43,   
+    .shoot_param = {
+            .trig_step_angle=M_2PI/8,
+            .shot_delay_time=0.05f,
+            .shot_burst_num=1,
+            .fric_motor_param[0] = {
+                .can = BSP_CAN_2,
+                .id = 0x201,
+                .module = MOTOR_M3508,
+                .reverse = true,
+                .gear=false,
+            },
+            .fric_motor_param[1] = {
+                .can = BSP_CAN_2,
+                .id = 0x202,
+                .module = MOTOR_M3508,
+                .reverse = false,
+                .gear=false,
+            },
+            .trig_motor_param = {
+                .can = BSP_CAN_1,
+                .id = 0x201,
+                .module = MOTOR_M2006,
+                .reverse = true,
+                .gear=true,
+            },
+            .fric_follow = {
+                .k=1.0f,
+                .p=1.8f,
+                .i=0.0f,
+                .d=0.0f,
+                .i_limit=0.0f,
+                .out_limit=0.9f, 
+                .d_cutoff_freq=30.0f,
+                .range=-1.0f,
+            },
+            
+            .fric_err = {
+                .k=1.0f,
+                .p=4.0f,
+                .i=0.4f,
+                .d=0.04f,
+                .i_limit=0.25f,
+                .out_limit=0.25f, 
+                .d_cutoff_freq=40.0f,
+                .range=-1.0f, 
+            },
+            .trig_omg = {
+            .k=1.0f,
+            .p=1.5f,
+            .i=0.3f,
+            .d=0.5f,
+            .i_limit=0.2f,
+            .out_limit=0.9f,
+            .d_cutoff_freq=-1.0f,
+            .range=-1.0f,
+            },
+            .trig = {
+                .k=1.0f,
+                .p=1.0f,
+                .i=0.1f,
+                .d=0.05f,
+                .i_limit=0.8f,
+                .out_limit=0.5f,
+                .d_cutoff_freq=-1.0f,
+                .range=M_2PI,
+            },
+            .filter.fric = {
+                .in = 30.0f,
+                .out = 30.0f,
+            },
+            .filter.trig = {
+                .in = 30.0f,
+                .out = 30.0f,
+            },
+            
+        
+        },
+        
+    .gimbal_param = {
+        .pid = {
+            .yaw_omega = {
+                .k = 1.0f,
+                .p = 1.0f,
+                .i = 0.5f,
+                .d = 0.0f,
+                .i_limit = 1.0f,
+                .out_limit = 1.0f,
+                .d_cutoff_freq = -1.0f,
+                .range = -1.0f,
+            },
+            .yaw_angle = {
+                .k = 10.0f,
+                .p = 3.0f,
+                .i = 0.0f,
+                .d = 0.0f,
+                .i_limit = 0.0f,
+                .out_limit = 10.0f,
+                .d_cutoff_freq = -1.0f,
+                .range = M_2PI,
+            },
+            .pit_omega = {
+                .k = 0.25f,
+                .p = 1.0f,
+                .i = 0.0f,
+                .d = 0.0f,
+                .i_limit = 1.0f,
+                .out_limit = 1.0f,
+                .d_cutoff_freq = -1.0f,
+                .range = -1.0f,
+            },
+            .pit_angle = {
+                .k = 5.0f,
+                .p = 5.0f,
+                .i = 2.5f,
+                .d = 0.0f,
+                .i_limit = 0.0f,
+                .out_limit = 10.0f,
+                .d_cutoff_freq = -1.0f,
+                .range = M_2PI,
+            },
+        },
+        .mech_zero = {
+            .yaw = 0.0f,
+            .pit = 1.77f,
+        },
+        .travel = {
+            .yaw = -1.0f,
+            .pit = 0.8f,
+        },
+        .low_pass_cutoff_freq = {
+            .out = -1.0f,
+            .gyro = 1000.0f,
+        },
+        .pit_motor ={
+            .can = BSP_CAN_2,
+            .id = 0x206,
+            .gear = false,
+            .module = MOTOR_GM6020,
+            .reverse = true,
+        },
+        .yaw_motor = {
+            .can = BSP_CAN_1,
+            .can_id = 0x1,
+            .master_id = 0x11,
+            .module = MOTOR_DM_J4310,
+            .reverse = false,
+        }
     },
 
     .chassis_param = {
diff --git a/User/module/config.h b/User/module/config.h
index 9b6e362..102c329 100644
--- a/User/module/config.h
+++ b/User/module/config.h
@@ -9,15 +9,17 @@ extern "C" {
 #endif
 
 #include <stdint.h>
-#include "device/dm_imu.h"
 #include "device/motor_lz.h"
 #include "device/motor_lk.h"
 #include "module/balance_chassis.h"
+#include "module/gimbal.h"
+#include "module/shoot.h"
 #include "device/virtual_chassis.h"
 
 typedef struct {
-    DM_IMU_Param_t imu_param;
+    Shoot_Params_t shoot_param;
     Chassis_Params_t chassis_param;
+    Gimbal_Params_t gimbal_param;
     Virtual_Chassis_Param_t virtual_chassis_param;  // 虚拟底盘参数
 } Config_RobotParam_t;
 
diff --git a/User/module/gimbal.c b/User/module/gimbal.c
new file mode 100644
index 0000000..bce9c48
--- /dev/null
+++ b/User/module/gimbal.c
@@ -0,0 +1,246 @@
+/*
+ * 云台模组
+ */
+
+/* Includes ----------------------------------------------------------------- */
+#include "gimbal.h"
+#include "bsp/can.h"
+#include "bsp/time.h"
+#include <math.h> 
+#include "component/filter.h"
+#include "component/pid.h"
+#include "device/motor_dm.h"
+#include "device/motor_rm.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* Private function  -------------------------------------------------------- */
+
+/**
+ * \brief 设置云台模式
+ *
+ * \param c 包含云台数据的结构体
+ * \param mode 要设置的模式
+ *
+ * \return 函数运行结果
+ */
+static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
+  if (g == NULL)
+    return -1;
+  if (mode == g->mode)
+    return GIMBAL_OK;
+
+  PID_Reset(&g->pid.yaw_angle);
+  PID_Reset(&g->pid.yaw_omega);
+  PID_Reset(&g->pid.pit_angle);
+  PID_Reset(&g->pid.pit_omega);
+  LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f);
+  LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
+
+  MOTOR_DM_Enable(&(g->param->yaw_motor));
+  
+  AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
+  // if (g->mode == GIMBAL_MODE_RELAX) {
+  //   if (mode == GIMBAL_MODE_ABSOLUTE) {
+  //     g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
+  //   } else if (mode == GIMBAL_MODE_RELATIVE) {
+  //     g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
+  //   }
+  // }
+  g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
+  g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
+
+  g->mode = mode;
+  return 0;
+}
+
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * \brief 初始化云台
+ *
+ * \param g 包含云台数据的结构体
+ * \param param 包含云台参数的结构体指针
+ * \param target_freq 任务预期的运行频率
+ *
+ * \return 函数运行结果
+ */
+int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
+                   float target_freq) {
+  if (g == NULL)
+    return -1;
+
+  g->param = param;
+  g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */
+
+  /* 初始化云台电机控制PID和LPF */
+  PID_Init(&(g->pid.yaw_angle), KPID_MODE_NO_D, target_freq,
+           &(g->param->pid.yaw_angle));
+  PID_Init(&(g->pid.yaw_omega), KPID_MODE_CALC_D, target_freq,
+           &(g->param->pid.yaw_omega));
+  PID_Init(&(g->pid.pit_angle), KPID_MODE_NO_D, target_freq,
+           &(g->param->pid.pit_angle));
+  PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq,
+           &(g->param->pid.pit_omega));
+
+  LowPassFilter2p_Init(&g->filter_out.yaw, target_freq,
+                       g->param->low_pass_cutoff_freq.out);
+  LowPassFilter2p_Init(&g->filter_out.pit, target_freq,
+                       g->param->low_pass_cutoff_freq.out);
+  g->limit.yaw.max = g->param->mech_zero.yaw + g->param->travel.yaw;
+  g->limit.yaw.min = g->param->mech_zero.yaw;
+  g->limit.pit.max = g->param->mech_zero.pit + g->param->travel.pit;
+  g->limit.pit.min = g->param->mech_zero.pit;
+  BSP_CAN_Init();
+    
+  MOTOR_RM_Register(&(g->param->pit_motor));
+  MOTOR_DM_Register(&(g->param->yaw_motor));
+
+  MOTOR_DM_Enable(&(g->param->yaw_motor));
+  return 0;
+}
+
+/**
+ * \brief 通过CAN设备更新云台反馈信息
+ *
+ * \param gimbal 云台
+ * \param can CAN设备
+ *
+ * \return 函数运行结果
+ */
+int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) {
+  if (gimbal == NULL)
+    return -1;
+
+  /* 更新RM电机反馈数据（pitch轴） */
+  MOTOR_RM_Update(&(gimbal->param->pit_motor));
+  MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&(gimbal->param->pit_motor));
+  if (rm_motor != NULL) {
+    gimbal->feedback.motor.pit = rm_motor->feedback;
+  }
+
+  /* 更新DM电机反馈数据（yaw轴） */
+  MOTOR_DM_Update(&(gimbal->param->yaw_motor));
+  MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->yaw_motor));
+  if (dm_motor != NULL) {
+    gimbal->feedback.motor.yaw = dm_motor->motor.feedback;
+  }
+
+  return 0;
+}
+
+int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
+
+  if (gimbal == NULL) {
+    return -1;
+  }
+  gimbal->feedback.imu.gyro = imu->gyro;
+  gimbal->feedback.imu.eulr = imu->eulr;
+}
+
+/**
+ * \brief 运行云台控制逻辑
+ *
+ * \param g 包含云台数据的结构体
+ * \param g_cmd 云台控制指令
+ *
+ * \return 函数运行结果
+ */
+int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
+  if (g == NULL || g_cmd == NULL) {
+    return -1;
+  }
+
+  g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f;
+  g->lask_wakeup = BSP_TIME_Get_us();
+
+  Gimbal_SetMode(g, g_cmd->mode);
+
+  /* 处理yaw控制命令，软件限位 - 使用电机绝对角度 */
+  float delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f;
+  if (g->param->travel.yaw > 0) {
+    /* 计算当前电机角度与IMU角度的偏差 */
+    float motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw;
+    /* 处理跨越±π的情况 */
+    if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
+    if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
+    
+    /* 计算到限位边界的距离 */
+    const float delta_max = CircleError(g->limit.yaw.max,
+        (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
+    const float delta_min = CircleError(g->limit.yaw.min,
+        (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
+    
+    /* 限制控制命令 */
+    if (delta_yaw > delta_max) delta_yaw = delta_max;
+    if (delta_yaw < delta_min) delta_yaw = delta_min;
+  }
+  CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw, M_2PI);
+
+  /* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
+  float delta_pit = g_cmd->delta_pit * g->dt;
+  if (g->param->travel.pit > 0) {
+    /* 计算当前电机角度与IMU角度的偏差 */
+    float motor_imu_offset = g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.rol;
+    /* 处理跨越±π的情况 */
+    if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
+    if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
+    
+    /* 计算到限位边界的距离 */
+    const float delta_max = CircleError(g->limit.pit.max,
+        (g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
+    const float delta_min = CircleError(g->limit.pit.min,
+        (g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
+    
+    /* 限制控制命令 */
+    if (delta_pit > delta_max) delta_pit = delta_max;
+    if (delta_pit < delta_min) delta_pit = delta_min;
+  }
+  CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
+
+  /* 控制相关逻辑 */
+  float yaw_omega_set_point, pit_omega_set_point;
+  switch (g->mode) {
+  case GIMBAL_MODE_RELAX:
+    g->out.yaw = 0.0f;
+    g->out.pit = 0.0f;
+    break;
+
+  case GIMBAL_MODE_ABSOLUTE:
+    yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
+                                   g->feedback.imu.eulr.yaw, 0.0f, g->dt);
+    g->out.yaw = PID_Calc(&(g->pid.pit_omega), yaw_omega_set_point,
+                          g->feedback.imu.gyro.z, 0.f, g->dt);
+
+    pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
+                                   g->feedback.imu.eulr.rol, 0.0f, g->dt);
+    g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
+                          g->feedback.imu.gyro.y, 0.f, g->dt);
+
+              
+    break;
+
+    /* 输出滤波 */
+    g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
+    g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
+
+    return 0;
+  }
+}
+
+  /**
+   * \brief 云台输出
+   *
+   * \param s 包含云台数据的结构体
+   * \param out CAN设备云台输出结构体
+   */
+void Gimbal_Output(Gimbal_t *g){
+    MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit);
+    MOTOR_MIT_Output_t output = {0};
+    output.torque = g->out.yaw * 5.0f; // 乘以减速比
+    output.kd = 0.5f;
+    MOTOR_RM_Ctrl(&g->param->pit_motor);
+    MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output);
+  }
diff --git a/User/module/gimbal.h b/User/module/gimbal.h
new file mode 100644
index 0000000..4d85414
--- /dev/null
+++ b/User/module/gimbal.h
@@ -0,0 +1,180 @@
+/*
+ * 云台模组
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "component/ahrs.h"
+#include "component/filter.h"
+#include "component/pid.h"
+#include "device/motor.h"
+#include "device/motor_dm.h"
+#include "device/motor_rm.h"
+
+/* Exported constants ------------------------------------------------------- */
+#define GIMBAL_OK (0)        /* 运行正常 */
+#define GIMBAL_ERR (-1)      /* 运行时发现了其他错误 */
+#define GIMBAL_ERR_NULL (-2) /* 运行时发现NULL指针 */
+#define GIMBAL_ERR_MODE (-3) /* 运行时配置了错误的CMD_GimbalMode_t */
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+typedef enum {
+  GIMBAL_MODE_RELAX,    /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
+  GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
+  GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
+} Gimbal_Mode_t;
+
+typedef struct {
+  Gimbal_Mode_t mode;
+  float delta_yaw;
+  float delta_pit;
+} Gimbal_CMD_t;
+
+/* 软件限位 */
+typedef struct {
+  float max;
+  float min;
+} Gimbal_Limit_t;
+
+/* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
+typedef struct {
+  MOTOR_RM_Param_t pit_motor; /* pitch轴电机参数 */
+  MOTOR_DM_Param_t yaw_motor; /* yaw轴电机参数 */
+  struct {
+    KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */
+    KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */
+    KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */
+    KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */
+  } pid;
+
+  /* 低通滤波器截止频率 */
+  struct {
+    float out;  /* 电机输出 */
+    float gyro; /* 陀螺仪数据 */
+  } low_pass_cutoff_freq;
+
+  struct {
+    float yaw; /* yaw轴机械限位 */
+    float pit; /* pitch轴机械限位 */
+  } mech_zero;
+
+  struct {
+    float yaw; /* yaw轴机械限位行程 -1表示无限位 */
+    float pit; /* pitch轴机械限位行程 -1表示无限位*/
+  } travel;
+
+} Gimbal_Params_t;
+
+typedef struct {
+  AHRS_Gyro_t gyro;
+  AHRS_Eulr_t eulr;
+} Gimbal_IMU_t;
+/* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */
+typedef struct {
+  Gimbal_IMU_t imu;
+  struct {
+    MOTOR_Feedback_t yaw; /* yaw轴电机反馈 */
+    MOTOR_Feedback_t pit; /* pitch轴电机反馈 */
+  } motor;
+} Gimbal_Feedback_t;
+
+/* 云台输出数据的结构体*/
+typedef struct {
+  float yaw; /* yaw轴电机输出 */
+  float pit; /* pitch轴电机输出 */
+} Gimbal_Output_t;
+/*
+ * 运行的主结构体，所有这个文件里的函数都在操作这个结构体。
+ * 包含了初始化参数，中间变量，输出变量。
+ */
+typedef struct {
+  uint64_t lask_wakeup;
+  float dt;
+
+  Gimbal_Params_t *param; /* 云台的参数，用Gimbal_Init设定 */
+
+  /* 模块通用 */
+  Gimbal_Mode_t mode; /* 云台模式 */
+
+  /* PID计算的目标值 */
+  struct {
+    AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */
+  } setpoint;
+
+  struct {
+    KPID_t yaw_angle; /* yaw轴角位置环PID */
+    KPID_t yaw_omega; /* yaw轴角速度环PID */
+    KPID_t pit_angle; /* pitch轴角位置环PID */
+    KPID_t pit_omega; /* pitch轴角速度环PID */
+  } pid;
+
+  struct {
+    Gimbal_Limit_t yaw;
+    Gimbal_Limit_t pit;
+  } limit;
+
+  struct {
+    LowPassFilter2p_t yaw;
+    LowPassFilter2p_t pit;
+  } filter_out;
+
+  Gimbal_Output_t out;        /* 云台输出 */
+  Gimbal_Feedback_t feedback; /* 反馈 */
+
+} Gimbal_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * \brief 初始化云台
+ *
+ * \param g 包含云台数据的结构体
+ * \param param 包含云台参数的结构体指针
+ * \param target_freq 任务预期的运行频率
+ *
+ * \return 函数运行结果
+ */
+int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
+                   float target_freq);
+
+/**
+ * \brief 通过CAN设备更新云台反馈信息
+ *
+ * \param gimbal 云台
+ * \param can CAN设备
+ *
+ * \return 函数运行结果
+ */
+int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal);
+
+int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu);
+/**
+ * \brief 运行云台控制逻辑
+ *
+ * \param g 包含云台数据的结构体
+ * \param fb 云台反馈信息
+ * \param g_cmd 云台控制指令
+ * \param dt_sec 两次调用的时间间隔
+ *
+ * \return 函数运行结果
+ */
+int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd);
+
+/**
+ * \brief 云台输出
+ *
+ * \param s 包含云台数据的结构体
+ * \param out CAN设备云台输出结构体
+ */
+void Gimbal_Output(Gimbal_t *g);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/User/module/shoot.c b/User/module/shoot.c
new file mode 100644
index 0000000..5c51976
--- /dev/null
+++ b/User/module/shoot.c
@@ -0,0 +1,305 @@
+
+#include "shoot.h"
+#include <string.h>
+#include "can.h"
+#include "component/filter.h"
+#include "component/user_math.h"
+#include <math.h>
+#include "bsp/time.h"
+
+static bool last_firecmd;
+
+static inline void ScaleSumTo1(float *a, float *b) {
+    float sum = *a + *b;
+    if (sum > 1.0f) {
+        float scale = 1.0f / sum;
+        *a *= scale;
+        *b *= scale;
+    }
+}
+
+
+int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode)
+{
+    if (s == NULL) {
+        return -1; // 参数错误
+    }
+    s->mode=mode;
+    return 0;
+}
+
+int8_t Shoot_ResetIntegral(Shoot_t *s)
+{
+    if (s == NULL) {
+        return -1; // 参数错误
+    }
+    for(int i=0;i<SHOOT_FRIC_NUM;i++)
+    {	
+        PID_ResetIntegral(&s->pid.fric_follow[i]);
+        PID_ResetIntegral(&s->pid.fric_err[i]);
+    }
+    PID_ResetIntegral(&s->pid.trig);
+    PID_ResetIntegral(&s->pid.trig_omg);
+    return 0;
+}
+
+int8_t Shoot_ResetCalu(Shoot_t *s)
+{
+    if (s == NULL) {
+        return -1; // 参数错误
+    }
+    for(int i=0;i<SHOOT_FRIC_NUM;i++)
+    {	
+        PID_Reset(&s->pid.fric_follow[i]);
+        PID_Reset(&s->pid.fric_err[i]);
+        LowPassFilter2p_Reset(&s->filter.fric.in[i], 0.0f);
+        LowPassFilter2p_Reset(&s->filter.fric.out[i], 0.0f);
+    }
+    PID_Reset(&s->pid.trig);
+    PID_Reset(&s->pid.trig_omg);
+    LowPassFilter2p_Reset(&s->filter.trig.in, 0.0f);
+    LowPassFilter2p_Reset(&s->filter.trig.out, 0.0f);
+    return 0;
+}
+
+int8_t Shoot_ResetOutput(Shoot_t *s)
+{
+    if (s == NULL) {
+        return -1; // 参数错误
+    }
+    for(int i=0;i<SHOOT_FRIC_NUM;i++)
+    {	
+        s->output.out_follow[i]=0.0f;
+        s->output.out_err[i]=0.0f;
+        s->output.out_fric[i]=0.0f;
+        s->output.lpfout_fric[i]=0.0f;
+    }
+	s->output.outagl_trig=0.0f;
+	s->output.outomg_trig=0.0f;
+    s->output.outlpf_trig=0.0f;
+    return 0;
+}
+
+int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
+{
+    if (s == NULL) {
+        return -1; // 参数错误
+    }
+    s->target_variable.target_rpm=4000.0f/MAX_FRIC_RPM;
+    return 0;
+}
+/**
+ * \brief 根据发射弹丸数量及发射频率计算拨弹电机目标角度
+ *
+ * \param s 包含发射数据的结构体
+ * \param num 需要发射的弹丸数量
+ */
+int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
+{
+    if (s == NULL  || s->shoot_Anglecalu.num_to_shoot == 0) {
+        return -1; 
+    }
+    if(s->now - s->shoot_Anglecalu.time_last_shoot >= s->param->shot_delay_time && cmd->firecmd)
+    {
+        s->shoot_Anglecalu.time_last_shoot=s->now;
+        s->target_variable.target_angle += s->param->trig_step_angle;
+		if(s->target_variable.target_angle>M_PI)s->target_variable.target_angle-=M_2PI;
+		else if((s->target_variable.target_angle<-M_PI))s->target_variable.target_angle+=M_2PI;
+		s->shoot_Anglecalu.num_to_shoot--;
+    }
+    return 0;
+}
+
+int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
+{
+    if (s == NULL || param == NULL || target_freq <= 0.0f) {
+        return -1; // 参数错误
+    }
+    s->param=param;
+
+    BSP_CAN_Init();
+    for(int i=0;i<SHOOT_FRIC_NUM;i++){
+	MOTOR_RM_Register(&param->fric_motor_param[i]);
+    PID_Init(&s->pid.fric_follow[i], KPID_MODE_CALC_D, target_freq,&param->fric_follow);
+    LowPassFilter2p_Init(&s->filter.fric.in[i], target_freq, s->param->filter.fric.in);
+    LowPassFilter2p_Init(&s->filter.fric.out[i], target_freq, s->param->filter.fric.out);
+    }
+	MOTOR_RM_Register(&param->trig_motor_param);
+    PID_Init(&s->pid.trig, KPID_MODE_CALC_D, target_freq,&param->trig);
+    PID_Init(&s->pid.trig_omg, KPID_MODE_CALC_D, target_freq,&param->trig_omg);
+
+    LowPassFilter2p_Init(&s->filter.trig.in, target_freq, s->param->filter.trig.in);
+    LowPassFilter2p_Init(&s->filter.trig.out, target_freq, s->param->filter.trig.out);
+	
+    memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
+    memset(&s->output,0,sizeof(s->output));
+    s->target_variable.target_rpm=4000.0f;
+
+	return 0;
+}
+
+int8_t Shoot_UpdateFeedback(Shoot_t *s)
+{
+    if (s == NULL) {
+        return -1; // 参数错误
+    }
+    float rpm_sum=0.0f;
+    for(int i = 0; i < SHOOT_FRIC_NUM; i++) {
+        /* 更新摩擦电机反馈 */
+        MOTOR_RM_Update(&s->param->fric_motor_param[i]);
+		MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->fric_motor_param[i]);
+		if(motor_fed!=NULL)
+		{
+			s->feedback.fric[i]=motor_fed->motor.feedback;
+		}
+        /* 滤波反馈rpm */
+        s->feedback.fil_fric_rpm[i] = LowPassFilter2p_Apply(&s->filter.fric.in[i], s->feedback.fric[i].rotor_speed);
+        /* 归一化rpm */
+        s->feedback.fric_rpm[i] = s->feedback.fil_fric_rpm[i] / MAX_FRIC_RPM;
+        if(s->feedback.fric_rpm[i]>1.0f)s->feedback.fric_rpm[i]=1.0f;
+        if(s->feedback.fric_rpm[i]<-1.0f)s->feedback.fric_rpm[i]=-1.0f;
+        /* 计算平均rpm */
+        rpm_sum+=s->feedback.fric_rpm[i];
+    }
+    s->feedback.fric_avgrpm=rpm_sum/SHOOT_FRIC_NUM;
+	/* 更新拨弹电机反馈 */
+    MOTOR_RM_Update(&s->param->trig_motor_param);
+	MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->trig_motor_param);
+		if(motor_fed!=NULL)
+		{
+			s->feedback.trig=motor_fed->feedback;
+		}
+    /* 将多圈角度归化到单圈 (-M_PI, M_PI) */
+		s->feedback.trig_angle_cicle = s->feedback.trig.rotor_abs_angle;
+		s->feedback.trig_angle_cicle = fmod(s->feedback.trig_angle_cicle, M_2PI); // 将角度限制在 [-2π, 2π]
+		if (s->feedback.trig_angle_cicle > M_PI) {
+		s->feedback.trig_angle_cicle -= M_2PI; // 调整到 [-π, π]
+		}else if (s->feedback.trig_angle_cicle < -M_PI) {
+		s->feedback.trig_angle_cicle += M_2PI; // 调整到 [-π, π]
+		}
+    
+    s->feedback.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.rotor_speed);
+    s->feedback.trig_rpm = s->feedback.trig.rotor_speed / MAX_TRIG_RPM;
+    if(s->feedback.trig_rpm>1.0f)s->feedback.trig_rpm=1.0f;  //如果单环效果好就删
+    if(s->feedback.trig_rpm<-1.0f)s->feedback.trig_rpm=-1.0f;
+    
+    s->errtosee = s->feedback.fric[0].rotor_speed - s->feedback.fric[1].rotor_speed;
+	return 0;
+}
+
+int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
+{
+    if (s == NULL || cmd == NULL) {
+        return -1; // 参数错误
+    }
+    s->now = BSP_TIME_Get_us() / 1000000.0f;
+    s->dt = (BSP_TIME_Get_us() - s->lask_wakeup) / 1000000.0f;
+    s->lask_wakeup = BSP_TIME_Get_us();
+	s->online = cmd->online;
+    if(!s->online /*|| s->mode==SHOOT_MODE_SAFE*/){
+        for(int i=0;i<SHOOT_FRIC_NUM;i++)
+        {	
+            MOTOR_RM_Relax(&s->param->fric_motor_param[i]);
+        }
+        MOTOR_RM_Relax(&s->param->trig_motor_param);
+    }
+    else{
+        switch(s->running_state)
+        {
+            case SHOOT_STATE_IDLE:/*熄火等待*/
+                for(int i=0;i<SHOOT_FRIC_NUM;i++)
+                {	
+                    PID_ResetIntegral(&s->pid.fric_follow[i]);
+                    s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],0.0f,s->feedback.fric_rpm[i],0,s->dt);
+                    s->output.out_fric[i]=s->output.out_follow[i];
+                    s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
+                    MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
+                }
+                s->output.outagl_trig   =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
+                s->output.outomg_trig  =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
+                s->output.outlpf_trig   =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
+                MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
+                if(cmd->ready)
+                    {   
+                        Shoot_ResetCalu(s);
+                        Shoot_ResetIntegral(s);
+                        Shoot_ResetOutput(s);
+                        s->running_state=SHOOT_STATE_READY;
+                    }
+                break;
+            case SHOOT_STATE_READY:/*准备射击*/
+                
+                for(int i=0;i<SHOOT_FRIC_NUM;i++)
+                {	/* 计算跟随输出、计算修正输出 */ 
+                    s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],s->target_variable.target_rpm/MAX_FRIC_RPM,s->feedback.fric_rpm[i],0,s->dt);
+                    s->output.out_err[i]=PID_Calc(&s->pid.fric_err[i],s->feedback.fric_avgrpm,s->feedback.fric_rpm[i],0,s->dt);
+                    /* 按比例缩放并加和输出 */
+                    ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]);                    
+                    s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[i];
+                    /* 滤波 */
+                    s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
+                    /* 输出 */
+                    MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
+                }
+                /* 拨弹电机输出 */
+                s->output.outagl_trig   =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
+                s->output.outomg_trig  =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
+                s->output.outlpf_trig   =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
+                MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
+                
+                /* 检查状态机 */
+                if(!cmd->ready)
+                    {
+                        Shoot_ResetCalu(s);
+                        Shoot_ResetOutput(s);
+                        s->running_state=SHOOT_STATE_IDLE;
+                    }
+                    else if(last_firecmd==false&&cmd->firecmd==true)
+                    {
+                        Shoot_ResetCalu(s);
+                        Shoot_ResetOutput(s);
+                        s->running_state=SHOOT_STATE_FIRE;
+                        s->shoot_Anglecalu.num_to_shoot+=s->param->shot_burst_num;
+					
+                    }
+                break;
+            case SHOOT_STATE_FIRE:
+				Shoot_CaluTargetAngle(s, cmd);
+                for(int i=0;i<SHOOT_FRIC_NUM;i++)
+                {	
+                    s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],s->target_variable.target_rpm/MAX_FRIC_RPM,s->feedback.fric_rpm[i],0,s->dt);
+                    s->output.out_err[i]=PID_Calc(&s->pid.fric_err[i],s->feedback.fric_avgrpm,s->feedback.fric_rpm[i],0,s->dt);
+                    ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]);
+                    s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[i];
+                    s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
+                    MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
+                }
+                s->output.outagl_trig   =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
+                s->output.outomg_trig  =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
+                s->output.outlpf_trig   =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
+                MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
+                if(!cmd->firecmd)
+                    {
+                        Shoot_ResetCalu(s);
+                        Shoot_ResetOutput(s);
+                        s->running_state=SHOOT_STATE_READY;
+                    }
+                break;
+            default:
+                s->running_state=SHOOT_STATE_IDLE;
+                break;
+        }
+    }
+	MOTOR_RM_Ctrl(&s->param->fric_motor_param[0]);
+    MOTOR_RM_Ctrl(&s->param->trig_motor_param);
+    last_firecmd = cmd->firecmd;
+    return 0;
+}
+
+
+
+
+
+
+
+
diff --git a/User/module/shoot.h b/User/module/shoot.h
new file mode 100644
index 0000000..57f1f94
--- /dev/null
+++ b/User/module/shoot.h
@@ -0,0 +1,200 @@
+/*
+ * far蛇模组
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "main.h"
+#include <stdbool.h>
+#include "component/pid.h"
+#include "device/motor_rm.h"
+
+
+/* Exported constants ------------------------------------------------------- */
+#define SHOOT_OK (0)        /* 运行正常 */
+#define SHOOT_ERR (-1)      /* 运行时发现了其他错误 */
+#define SHOOT_ERR_NULL (-2) /* 运行时发现NULL指针 */
+#define SHOOT_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */
+#define SHOOT_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */
+
+#define SHOOT_FRIC_NUM (2) /* 摩擦轮数量 */
+#define MAX_FRIC_RPM 7000.0f
+#define MAX_TRIG_RPM 5000.0f
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+typedef enum {
+    SHOOT_STATE_IDLE = 0,      // 熄火
+    SHOOT_STATE_READY,         // 准备射击
+    SHOOT_STATE_FIRE           // 射击
+} Shoot_State_t;
+
+typedef enum {
+    SHOOT_MODE_SAFE = 0,      // 安全模式
+    SHOOT_MODE_SINGLE,         // 单发模式
+    SHOOT_MODE_BURST,          // 多发模式
+    SHOOT_MODE_CONTINUE         // 连发模式
+} Shoot_Mode_t;
+typedef struct {
+  bool online;	
+	
+  bool ready;                    /* 准备射击 */
+  bool firecmd;                  /* 射击指令 */
+
+} Shoot_CMD_t;
+typedef struct {
+  MOTOR_Feedback_t fric[SHOOT_FRIC_NUM]; /* 摩擦轮电机反馈 */
+  MOTOR_Feedback_t trig; /* 拨弹电机反馈 */
+
+  float fil_fric_rpm[SHOOT_FRIC_NUM]; /* 滤波后的摩擦轮转速 */
+  float fil_trig_rpm; /* 滤波后的拨弹电机转速*/
+
+  float trig_angle_cicle; /* 拨弹电机减速输出轴单圈角度（0~M_2PI） */
+
+  float fric_rpm[SHOOT_FRIC_NUM]; /* 归一化摩擦轮转速 */
+  float fric_avgrpm; /* 归一化摩擦轮平均转速*/
+  float trig_rpm; /* 归一化拨弹电机转速*/
+
+}Shoot_Feedback_t;
+
+typedef struct{
+  float   time_last_shoot;
+  uint8_t num_to_shoot;
+  uint8_t num_shooted;
+}Shoot_AngleCalu_t;
+
+typedef struct {
+  float out_follow[SHOOT_FRIC_NUM];
+  float out_err[SHOOT_FRIC_NUM];
+  float out_fric[SHOOT_FRIC_NUM];
+  float lpfout_fric[SHOOT_FRIC_NUM];
+
+  
+  float outagl_trig;
+  float outomg_trig;
+  float outlpf_trig;
+}Shoot_Output_t;
+
+
+/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
+typedef struct {
+  float trig_step_angle; /* 每发弹丸拨弹电机转动的角度 */
+  float shot_delay_time; /* 射击间隔时间，单位秒 */
+  uint8_t shot_burst_num; /* 多发模式下一次射击的发数 */
+
+  MOTOR_RM_Param_t fric_motor_param[SHOOT_FRIC_NUM];
+  MOTOR_RM_Param_t trig_motor_param;
+  
+
+  KPID_Params_t fric_follow;               /* 摩擦轮电机PID控制参数，用于跟随目标速度 */
+  KPID_Params_t fric_err;               /* 摩擦轮电机PID控制参数，用于消除转速误差 */
+  KPID_Params_t trig;              /* 拨弹电机PID控制参数 */
+  KPID_Params_t trig_omg;              /* 拨弹电机PID控制参数 */
+
+  /* 低通滤波器截止频率 */
+  struct {
+    struct{
+    float in;  /* 反馈值滤波器 */
+    float out; /* 输出值滤波器 */
+    }fric;
+    struct{
+    float in;  /* 反馈值滤波器 */
+    float out; /* 输出值滤波器 */
+    }trig;
+  } filter;
+} Shoot_Params_t;
+
+/*
+ * 运行的主结构体，所有这个文件里的函数都在操作这个结构体
+ * 包含了初始化参数，中间变量，输出变量
+ */
+typedef struct {
+  bool online;
+
+  float    now;
+  uint64_t lask_wakeup;
+  float    dt;
+
+  Shoot_Params_t *param; /* */
+  /* 模块通用 */
+  Shoot_State_t running_state; /* 运行状态机 */
+  Shoot_Mode_t  mode;
+  /* 反馈信息 */
+  Shoot_Feedback_t feedback;
+  /* 控制信息*/
+  Shoot_AngleCalu_t shoot_Anglecalu;
+  Shoot_Output_t output;
+  /* 目标控制量 */
+  struct {
+    float target_rpm; /* 目标摩擦轮转速 */
+    float target_angle; /* 目标拨弹位置 */
+  }target_variable;
+
+  /* 反馈控制用的PID */
+  struct {
+    KPID_t fric_follow[SHOOT_FRIC_NUM]; /* */
+    KPID_t fric_err[SHOOT_FRIC_NUM];    /*  */
+    KPID_t trig;
+    KPID_t trig_omg;
+  } pid;
+
+  /* 滤波器 */
+  struct {
+        struct{
+    LowPassFilter2p_t in[SHOOT_FRIC_NUM];  /* 反馈值滤波器 */
+    LowPassFilter2p_t out[SHOOT_FRIC_NUM]; /* 输出值滤波器 */
+    }fric;
+    struct{
+    LowPassFilter2p_t in;  /* 反馈值滤波器 */
+    LowPassFilter2p_t out; /* 输出值滤波器 */
+    }trig;
+  } filter;
+
+  float errtosee; /*调试用*/
+
+} Shoot_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * \brief 初始化发射
+ *
+ * \param s 包含发射数据的结构体
+ * \param param 包含发射参数的结构体指针
+ * \param target_freq 任务预期的运行频率
+ *
+ * \return 函数运行结果
+ */
+int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq);
+
+/**
+ * \brief 更新反馈
+ *
+ * \param s 包含发射数据的结构体
+ *
+ * \return 函数运行结果
+ */
+int8_t Shoot_UpdateFeedback(Shoot_t *s);
+
+/**
+ * \brief 初始化发射
+ *
+ * \param s 包含发射数据的结构体
+ * \param cmd 包含发射命令的结构体
+ *
+ * \return 函数运行结果
+ */
+int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+
diff --git a/User/task/ai.c b/User/task/ai.c
new file mode 100644
index 0000000..ae90c59
--- /dev/null
+++ b/User/task/ai.c
@@ -0,0 +1,44 @@
+/*
+    ai Task
+    
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task/user_task.h"
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+void Task_ai(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / AI_FREQ;
+
+  osDelay(AI_INIT_DELAY); /* 延时一段时间再开启任务 */
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  /* USER CODE INIT BEGIN */
+
+  /* USER CODE INIT END */
+  
+  while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    /* USER CODE BEGIN */
+
+    /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}
\ No newline at end of file
diff --git a/User/task/atti_esti.c b/User/task/atti_esti.c
index 4c666b3..65e5fa7 100644
--- a/User/task/atti_esti.c
+++ b/User/task/atti_esti.c
@@ -1,15 +1,17 @@
 /*
     atti_esti Task
-    
+    陀螺仪解算任务
 */
 
 /* Includes ----------------------------------------------------------------- */
+#include "cmsis_os2.h"
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
-#include "bsp/can.h"
-#include "device/dm_imu.h"
-#include "module/config.h"
-#include "module/balance_chassis.h"
+#include "bsp/pwm.h"
+#include "component/ahrs.h"
+#include "component/pid.h"
+#include "device/bmi088.h"
+#include "module/gimbal.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -17,8 +19,21 @@
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
-DM_IMU_t dm_imu;
-Chassis_IMU_t chassis_imu_send;
+BMI088_t bmi088;
+
+AHRS_t gimbal_ahrs;
+AHRS_Magn_t magn;
+AHRS_Eulr_t eulr_to_send;
+
+KPID_t imu_temp_ctrl_pid;
+
+Gimbal_IMU_t gimbal_to_send;
+
+BMI088_Cali_t cali_bmi088= {
+  .gyro_offset = {0.0f,0.0f,0.0f},
+};
+
+
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -27,31 +42,44 @@ void Task_atti_esti(void *argument) {
   (void)argument; /* 未使用argument，消除警告 */
 
   
-  /* 计算任务运行到指定频率需要等待的tick数 */
-  const uint32_t delay_tick = osKernelGetTickFreq() / ATTI_ESTI_FREQ;
-
   osDelay(ATTI_ESTI_INIT_DELAY); /* 延时一段时间再开启任务 */
 
-  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
-  BSP_CAN_Init();
-  // 初始化DM IMU设备
-  DM_IMU_Init(&dm_imu, &Config_GetRobotParam()->imu_param);
+  BMI088_Init(&bmi088,&cali_bmi088);
+  AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
+
   /* USER CODE INIT END */
-  
+
   while (1) {
-    tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
-    
-    if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) {
-        osMessageQueueReset(task_runtime.msgq.chassis_imu); // 重置消息队列，防止阻塞
-        chassis_imu_send.accl = dm_imu.data.accl;
-        chassis_imu_send.gyro = dm_imu.data.gyro;
-        chassis_imu_send.euler = dm_imu.data.euler;
-        osMessageQueuePut(task_runtime.msgq.chassis_imu, &chassis_imu_send, 0, 0); // 非阻塞发送IMU数据
-    }
+    BMI088_WaitNew();
+    BMI088_AcclStartDmaRecv();
+    BMI088_AcclWaitDmaCplt();
+
+    BMI088_GyroStartDmaRecv();
+    BMI088_GyroWaitDmaCplt();
+
+    /* 锁住RTOS内核防止数据解析过程中断，造成错误 */
+    osKernelLock();
+    /* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */
+    BMI088_ParseAccl(&bmi088);
+    BMI088_ParseGyro(&bmi088);
+    // IST8310_Parse(&ist8310);
+
+    /* 根据设备接收到的数据进行姿态解析 */
+    AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
+
+    /* 根据解析出来的四元数计算欧拉角 */
+    AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
+    osKernelUnlock();
+
+    gimbal_to_send.eulr = eulr_to_send;
+    gimbal_to_send.gyro = bmi088.gyro;
+
+    osMessageQueueReset(task_runtime.msgq.gimbal.imu);
+    osMessageQueuePut(task_runtime.msgq.gimbal.imu, &gimbal_to_send, 0, 0);
+
     /* USER CODE END */
-    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }
   
 }
\ No newline at end of file
diff --git a/User/task/config.yaml b/User/task/config.yaml
index 0295ca8..8184c9d 100644
--- a/User/task/config.yaml
+++ b/User/task/config.yaml
@@ -33,3 +33,24 @@
   function: Task_ctrl_gimbal
   name: ctrl_gimbal
   stack: 256
+- delay: 0
+  description: ''
+  freq_control: true
+  frequency: 500.0
+  function: Task_ctrl_shoot
+  name: ctrl_shoot
+  stack: 256
+- delay: 0
+  description: ''
+  freq_control: true
+  frequency: 500.0
+  function: Task_monitor
+  name: monitor
+  stack: 256
+- delay: 0
+  description: ''
+  freq_control: true
+  frequency: 500.0
+  function: Task_ai
+  name: ai
+  stack: 256
diff --git a/User/task/ctrl_gimbal.c b/User/task/ctrl_gimbal.c
index 8054f87..05de732 100644
--- a/User/task/ctrl_gimbal.c
+++ b/User/task/ctrl_gimbal.c
@@ -4,16 +4,11 @@
 */
 
 /* Includes ----------------------------------------------------------------- */
-#include "device/motor_rm.h"
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
-#include "bsp/can.h"
-#include "device/motor_dm.h"
-#include "device/motor_rm.h"
-#include "device/motor.h"
-#include "component/pid.h"
-#include <stdbool.h>
-
+#include "component/ahrs.h"
+#include "module/gimbal.h"
+#include "module/config.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -21,45 +16,9 @@
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
-MOTOR_DM_Param_t dm_4310_param = {
-    .can = BSP_CAN_1,
-    .master_id = 0x11,    // 主站ID
-    .can_id = 0x01,       // 反馈ID 10进制是85
-    .module = MOTOR_DM_J4310,
-    .reverse = false,
-};
-
-// MOTOR_RM_Param_t m2006 = {
-//     .can = BSP_CAN_1,
-//     .id = 0x201,
-//     .module = MOTOR_M2006,
-//     .reverse = false,
-//     .gear = true,
-// };
-
-MOTOR_MIT_Output_t gimbal_output = {
-    .angle = 0.0f,
-    .velocity = 0.0f,
-    .torque = 0.0f,
-    .kp = 0.0f,
-    .kd = 0.0f,
-};
-MOTOR_DM_t *dm_4310_motor = NULL;
-
-// MOTOR_Feedback_t m2006_fb;
-MOTOR_Feedback_t dm_4310_fb;
-
-// KPID_Params_t gimbal_angle_pid = {
-//   .leg_theta={
-//       .k=1.0f,
-//       .p=5.0f,           /* 摆角比例系数 */
-//       .i=0.0f,           /* 摆角积分系数 */
-//       .d=0.0f,           /* 摆角微分系数 */
-//       .i_limit=0.0f,     /* 积分限幅 */
-//       .out_limit=0.05f,  /* 输出限幅，腿长差值限制 */
-//       .d_cutoff_freq=30.0f, /* 微分截止频率 */
-//       .range=-1.0f,      /* 不使用循环误差处理 */
-// };
+Gimbal_t gimbal;
+Gimbal_IMU_t gimbal_imu;
+Gimbal_CMD_t gimbal_cmd;
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -75,24 +34,22 @@ void Task_ctrl_gimbal(void *argument) {
 
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
-  BSP_CAN_Init();
-  // MOTOR_DM_Register(&dm_4310_param);
-
+  Gimbal_Init(&gimbal, &Config_GetRobotParam()->gimbal_param, CTRL_GIMBAL_FREQ);
+  /* USER CODE INIT END */
+  
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
-    // MOTOR_DM_Update(&dm_4310_param);
-    // MOTOR_DM_t *dm_4310_motor = MOTOR_DM_GetMotor(&dm_4310_param);
-    // if (dm_4310_motor) {
-    //     dm_4310_fb = dm_4310_motor->motor.feedback;
-    // }
-    // if (dm_4310_motor->motor.header.online == false) {
-    //   MOTOR_DM_Enable(&dm_4310_param); // 使能电机
-    // } else {
-    //   MOTOR_DM_MITCtrl(&dm_4310_param, &gimbal_output);
-    // }
-    
+    if (osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu, NULL, 0) == osOK) {
+      Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
+    }
+    osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
 
+    Gimbal_UpdateFeedback(&gimbal);
+
+    Gimbal_Control(&gimbal, &gimbal_cmd);
+
+    Gimbal_Output(&gimbal);
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }
diff --git a/User/task/ctrl_shoot.c b/User/task/ctrl_shoot.c
new file mode 100644
index 0000000..98010f9
--- /dev/null
+++ b/User/task/ctrl_shoot.c
@@ -0,0 +1,48 @@
+/*
+    ctrl_shoot Task
+    
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task/user_task.h"
+/* USER INCLUDE BEGIN */
+#include "module/shoot.h"
+#include "module/config.h"
+/* USER INCLUDE END */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+Shoot_t shoot;
+Shoot_CMD_t shoot_cmd;
+/* USER STRUCT END */
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+void Task_ctrl_shoot(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_SHOOT_FREQ;
+
+  osDelay(CTRL_SHOOT_INIT_DELAY); /* 延时一段时间再开启任务 */
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  /* USER CODE INIT BEGIN */
+  Shoot_Init(&shoot,&Config_GetRobotParam()->shoot_param,CTRL_SHOOT_FREQ);
+  /* USER CODE INIT END */
+  
+  while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    /* USER CODE BEGIN */
+    osMessageQueueGet(task_runtime.msgq.shoot.shoot_cmd, &shoot_cmd, NULL, 0);
+    Shoot_UpdateFeedback(&shoot);
+    Shoot_Control(&shoot,&shoot_cmd);
+    /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}
\ No newline at end of file
diff --git a/User/task/init.c b/User/task/init.c
index 5de16ab..d6baa6f 100644
--- a/User/task/init.c
+++ b/User/task/init.c
@@ -7,10 +7,10 @@
 #include "task/user_task.h"
 
 /* USER INCLUDE BEGIN */
-#include "component/ahrs.h"
-#include "module/config.h"
 #include "module/balance_chassis.h"
-/* USER INCLUDE BEGIN */
+#include "module/gimbal.h"
+#include "module/shoot.h"
+/* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
 /* Private define ----------------------------------------------------------- */
@@ -36,13 +36,19 @@ void Task_Init(void *argument) {
   task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
   task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti);
   task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
-  // task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal);
+  task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal);
+  task_runtime.thread.ctrl_shoot = osThreadNew(Task_ctrl_shoot, NULL, &attr_ctrl_shoot);
+  task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor);
+  task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
 
   // 创建消息队列
   /* USER MESSAGE BEGIN */
   task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
   task_runtime.msgq.chassis_imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL);
   task_runtime.msgq.Chassis_cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
+  task_runtime.msgq.gimbal.imu= osMessageQueueNew(2u, sizeof(Gimbal_IMU_t), NULL);
+  task_runtime.msgq.gimbal.cmd= osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
+  task_runtime.msgq.shoot.shoot_cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL);
   /* USER MESSAGE END */
 
   osKernelUnlock(); // 解锁内核
diff --git a/User/task/monitor.c b/User/task/monitor.c
new file mode 100644
index 0000000..55d5bd3
--- /dev/null
+++ b/User/task/monitor.c
@@ -0,0 +1,44 @@
+/*
+    monitor Task
+    
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task/user_task.h"
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+void Task_monitor(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / MONITOR_FREQ;
+
+  osDelay(MONITOR_INIT_DELAY); /* 延时一段时间再开启任务 */
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  /* USER CODE INIT BEGIN */
+
+  /* USER CODE INIT END */
+  
+  while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    /* USER CODE BEGIN */
+
+    /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}
\ No newline at end of file
diff --git a/User/task/rc.c b/User/task/rc.c
index d9d6ee8..6989659 100644
--- a/User/task/rc.c
+++ b/User/task/rc.c
@@ -1,17 +1,16 @@
 /*
     rc Task
-
+    
 */
 
 /* Includes ----------------------------------------------------------------- */
-#include "cmsis_os2.h"
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
 #include "device/dr16.h"
 #include "module/config.h"
 #include "module/balance_chassis.h"
-// #include "module/shoot.h"
-// #include "module/gimbal.h"
+#include "module/gimbal.h"
+#include "module/shoot.h"
 #include <stdbool.h>
 // #include
 /* USER INCLUDE END */
@@ -22,7 +21,9 @@
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
 DR16_t dr16;
+Shoot_CMD_t for_shoot;
 Chassis_CMD_t cmd_for_chassis;
+Gimbal_CMD_t cmd_for_gimbal;
 // RC_CAN_t rc_can;
 // Shoot_CMD_t for_shoot;
 // Gimbal_CMD_t cmd_for_gimbal;
@@ -33,13 +34,19 @@ Chassis_CMD_t cmd_for_chassis;
 void Task_rc(void *argument) {
   (void)argument; /* 未使用argument，消除警告 */
 
+  
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / RC_FREQ;
+
   osDelay(RC_INIT_DELAY); /* 延时一段时间再开启任务 */
 
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
   DR16_Init(&dr16);
   /* USER CODE INIT END */
-
+  
   while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
     DR16_StartDmaRecv(&dr16);
     if (DR16_WaitDmaCplt(20)) {
@@ -48,8 +55,9 @@ void Task_rc(void *argument) {
     } else {
       DR16_Offline(&dr16);
     }
-    switch (dr16.data.sw_l) {
 
+
+    switch (dr16.data.sw_l) {
     case DR16_SW_UP:
         cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
         break;
@@ -72,26 +80,57 @@ void Task_rc(void *argument) {
     osMessageQueuePut(task_runtime.msgq.Chassis_cmd, &cmd_for_chassis, 0, 0); // 非阻塞发送底盘控制命令
     
 
-    // for_shoot.online = dr16.header.online;
-    // switch (dr16.data.sw_r) {
-    // case DR16_SW_UP:
-    //   for_shoot.ready = false;
-    //   for_shoot.firecmd = false;
-    //   break;
-    // case DR16_SW_MID:
-    //   for_shoot.ready = true;
-    //   for_shoot.firecmd = false;
-    //   break;
-    // case DR16_SW_DOWN:
-    //   for_shoot.ready = true;
-    //   for_shoot.firecmd = true;
-    //   break;
-    // default:
-    //   for_shoot.ready = false;
-    //   for_shoot.firecmd = false;
-    //   break;
-    // }
+        switch (dr16.data.sw_l) {
+    case DR16_SW_UP:
+      cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
+      cmd_for_gimbal.delta_yaw = 0.0f;
+      cmd_for_gimbal.delta_pit = 0.0f;
+      break;
+    case DR16_SW_MID:
+      cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
+      cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x*5.0f;
+      cmd_for_gimbal.delta_pit = dr16.data.ch_r_y*5.0f;
+      break;
+    case DR16_SW_DOWN:
+      cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
+      cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x* 5.0f;
+      cmd_for_gimbal.delta_pit = dr16.data.ch_r_y*5.0f;
+      break;
+    default:
+      cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
+      cmd_for_gimbal.delta_yaw = 0.0f;
+      cmd_for_gimbal.delta_pit = 0.0f;
+      break;
+    }
+
+    osMessageQueueReset(task_runtime.msgq.gimbal.cmd);
+    osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0);
+
+
+    for_shoot.online = dr16.header.online;
+    switch (dr16.data.sw_r) {
+    case DR16_SW_UP:
+      for_shoot.ready = false;
+      for_shoot.firecmd = false;
+      break;
+    case DR16_SW_MID:
+      for_shoot.ready = true;
+      for_shoot.firecmd = false;
+      break;
+    case DR16_SW_DOWN:
+      for_shoot.ready = true;
+      for_shoot.firecmd = true;
+      break;
+    default:
+      for_shoot.ready = false;
+      for_shoot.firecmd = false;
+      break;
+    }
+    osMessageQueueReset(task_runtime.msgq.shoot.shoot_cmd);
+    osMessageQueuePut(task_runtime.msgq.shoot.shoot_cmd, &for_shoot, 0, 0);
 
     /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }
+  
 }
\ No newline at end of file
diff --git a/User/task/user_task.c b/User/task/user_task.c
index 49fe884..02e9e51 100644
--- a/User/task/user_task.c
+++ b/User/task/user_task.c
@@ -33,4 +33,19 @@ const osThreadAttr_t attr_ctrl_gimbal = {
     .name = "ctrl_gimbal",
     .priority = osPriorityNormal,
     .stack_size = 256 * 4,
+};
+const osThreadAttr_t attr_ctrl_shoot = {
+    .name = "ctrl_shoot",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 4,
+};
+const osThreadAttr_t attr_monitor = {
+    .name = "monitor",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 4,
+};
+const osThreadAttr_t attr_ai = {
+    .name = "ai",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 4,
 };
\ No newline at end of file
diff --git a/User/task/user_task.h b/User/task/user_task.h
index c73494a..d25e52e 100644
--- a/User/task/user_task.h
+++ b/User/task/user_task.h
@@ -18,6 +18,9 @@ extern "C" {
 #define ATTI_ESTI_FREQ (1000.0)
 #define CTRL_CHASSIS_FREQ (500.0)
 #define CTRL_GIMBAL_FREQ (500.0)
+#define CTRL_SHOOT_FREQ (500.0)
+#define MONITOR_FREQ (500.0)
+#define AI_FREQ (500.0)
 
 /* 任务初始化延时ms */
 #define TASK_INIT_DELAY (100u)
@@ -26,6 +29,9 @@ extern "C" {
 #define ATTI_ESTI_INIT_DELAY (0)
 #define CTRL_CHASSIS_INIT_DELAY (500)
 #define CTRL_GIMBAL_INIT_DELAY (0)
+#define CTRL_SHOOT_INIT_DELAY (0)
+#define MONITOR_INIT_DELAY (0)
+#define AI_INIT_DELAY (0)
 
 /* Exported defines --------------------------------------------------------- */
 /* Exported macro ----------------------------------------------------------- */
@@ -40,6 +46,9 @@ typedef struct {
         osThreadId_t atti_esti;
         osThreadId_t ctrl_chassis;
         osThreadId_t ctrl_gimbal;
+        osThreadId_t ctrl_shoot;
+        osThreadId_t monitor;
+        osThreadId_t ai;
     } thread;
 
     /* USER MESSAGE BEGIN */
@@ -48,7 +57,13 @@ typedef struct {
 
         osMessageQueueId_t chassis_imu;
         osMessageQueueId_t Chassis_cmd;
-
+        struct {
+            osMessageQueueId_t imu;
+            osMessageQueueId_t cmd;
+        }gimbal;
+        struct {
+            osMessageQueueId_t shoot_cmd; /* 发射命令队列 */
+        }shoot;
     } msgq;
     /* USER MESSAGE END */
 
@@ -70,6 +85,9 @@ typedef struct {
         UBaseType_t atti_esti;
         UBaseType_t ctrl_chassis;
         UBaseType_t ctrl_gimbal;
+        UBaseType_t ctrl_shoot;
+        UBaseType_t monitor;
+        UBaseType_t ai;
     } stack_water_mark;
 
     /* 各任务运行频率 */
@@ -79,6 +97,9 @@ typedef struct {
         float atti_esti;
         float ctrl_chassis;
         float ctrl_gimbal;
+        float ctrl_shoot;
+        float monitor;
+        float ai;
     } freq;
 
     /* 任务最近运行时间 */
@@ -88,6 +109,9 @@ typedef struct {
         float atti_esti;
         float ctrl_chassis;
         float ctrl_gimbal;
+        float ctrl_shoot;
+        float monitor;
+        float ai;
     } last_up_time;
 
 } Task_Runtime_t;
@@ -102,6 +126,9 @@ extern const osThreadAttr_t attr_rc;
 extern const osThreadAttr_t attr_atti_esti;
 extern const osThreadAttr_t attr_ctrl_chassis;
 extern const osThreadAttr_t attr_ctrl_gimbal;
+extern const osThreadAttr_t attr_ctrl_shoot;
+extern const osThreadAttr_t attr_monitor;
+extern const osThreadAttr_t attr_ai;
 
 /* 任务函数声明 */
 void Task_Init(void *argument);
@@ -110,6 +137,9 @@ void Task_rc(void *argument);
 void Task_atti_esti(void *argument);
 void Task_ctrl_chassis(void *argument);
 void Task_ctrl_gimbal(void *argument);
+void Task_ctrl_shoot(void *argument);
+void Task_monitor(void *argument);
+void Task_ai(void *argument);
 
 #ifdef __cplusplus
 }