robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加上层

Browse Source
This commit is contained in:
Robofish 2025-10-06 17:37:53 +08:00
parent 9b5d806e5f
commit 39108dec77
25 changed files with 1912 additions and 486 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
CMakeLists.txt
View File

@ -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
)

381
User/device/bmi088.c Normal file
View File

@ -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;
}

81
User/device/bmi088.h Normal file
View File

@ -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

4
User/device/device.h
View File

@ -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 */

8
User/device/device_config.yaml
View File

@ -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

270
User/device/dm_imu.c
View File

@ -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);
}

90
User/device/dm_imu.h
View File

@ -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

1
User/device/motor_lz.c
View File

@ -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;

2
User/device/motor_rm.c
View File

@ -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;

153
User/module/config.c
View File

@ -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 = {

6
User/module/config.h
View File

@ -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;

246
User/module/gimbal.c Normal file
View File

@ -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);
}

180
User/module/gimbal.h Normal file
View File

@ -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

305
User/module/shoot.c Normal file
View File

@ -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;
}

200
User/module/shoot.h Normal file
View File

@ -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

44
User/task/ai.c Normal file
View File

@ -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); /* 运行结束，等待下一次唤醒 */
}
}

78
User/task/atti_esti.c
View File

@ -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); /* 运行结束，等待下一次唤醒 */
}
}

21
User/task/config.yaml
View File

@ -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

79
User/task/ctrl_gimbal.c
View File

@ -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); /* 运行结束，等待下一次唤醒 */
}

48
User/task/ctrl_shoot.c Normal file
View File

@ -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); /* 运行结束，等待下一次唤醒 */
}
}

14
User/task/init.c
View File

@ -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(); // 解锁内核

44
User/task/monitor.c Normal file
View File

@ -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); /* 运行结束，等待下一次唤醒 */
}
}

89
User/task/rc.c
View File

@ -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); /* 运行结束，等待下一次唤醒 */
}
}

15
User/task/user_task.c
View File

@ -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,
};

32
User/task/user_task.h
View File

@ -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
}