robofish/MRobot
1
0
Fork 0
mirror of https://github.com/goldenfishs/MRobot.git synced 2026-04-01 05:17:13 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

修复

Browse Source
This commit is contained in:
Robofish
2025-11-25 21:25:41 +08:00
parent 09c8ef7be8
commit e2e275b6e4
117 changed files with 12755 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
assets/User_code/device/.DS_Store vendored Normal file
View File

Binary file not shown.

0
assets/User_code/device/.gitkeep Normal file
View File

142
assets/User_code/device/ai.c Normal file
View File

@@ -0,0 +1,142 @@
/*
AI
*/
/* Includes ----------------------------------------------------------------- */
#include "ai.h"
#include <stdbool.h>
#include <string.h>
#include "bsp/time.h"
#include "bsp/uart.h"
#include "component/ahrs.h"
#include "component/crc16.h"
#include "component/crc8.h"
#include "component/user_math.h"
#include "component/filter.h"
/* Private define ----------------------------------------------------------- */
#define AI_LEN_RX_BUFF (sizeof(AI_DownPackage_t))
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static uint8_t rxbuf[AI_LEN_RX_BUFF];
static bool inited = false;
static osThreadId_t thread_alert;
static uint32_t drop_message = 0;
// uint16_t crc16;
/* Private function -------------------------------------------------------- */
static void Ai_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
}
static void Ai_IdleLineCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
}
/* Exported functions ------------------------------------------------------- */
int8_t AI_Init(AI_t *ai) {
UNUSED(ai);
if (inited) return DEVICE_ERR_INITED;
thread_alert = osThreadGetId();
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
Ai_RxCpltCallback);
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB,
Ai_IdleLineCallback);
inited = true;
return 0;
}
int8_t AI_Restart(AI_t *ai) {
UNUSED(ai);
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_AI));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_AI));
return DEVICE_OK;
}
int8_t AI_StartReceiving(AI_t *ai) {
UNUSED(ai);
// if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
// AI_LEN_RX_BUFF) == HAL_OK)
if (BSP_UART_Receive(BSP_UART_AI, rxbuf,
AI_LEN_RX_BUFF, true) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
bool AI_WaitDmaCplt(void) {
return (osThreadFlagsWait(SIGNAL_AI_RAW_REDY, osFlagsWaitAll,0) ==
SIGNAL_AI_RAW_REDY);
}
int8_t AI_ParseHost(AI_t *ai) {
// crc16 = CRC16_Calc((const uint8_t *)&(rxbuf), sizeof(ai->from_host) - 2, CRC16_INIT);
if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->from_host)))
goto error;
ai->header.online = true;
ai->header.last_online_time = BSP_TIME_Get();
memcpy(&(ai->from_host), rxbuf, sizeof(ai->from_host));
memset(rxbuf, 0, AI_LEN_RX_BUFF);
return DEVICE_OK;
error:
drop_message++;
return DEVICE_ERR;
}
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *data){
if (ai == NULL || data == NULL) return DEVICE_ERR_NULL;
ai->to_host.mcu.id = AI_ID_MCU;
ai->to_host.mcu.package.quat=*data;
ai->to_host.mcu.package.notice = ai->status;
ai->to_host.mcu.crc16 = CRC16_Calc((const uint8_t *)&(ai->to_host.mcu), sizeof(AI_UpPackageMCU_t) - 2, CRC16_INIT);
return DEVICE_OK;
}
int8_t AI_PackRef(AI_t *ai, const AI_UpPackageReferee_t *data) {
if (ai == NULL || data == NULL) return DEVICE_ERR_NULL;
ai->to_host.ref = *data;
return DEVICE_OK;
}
int8_t AI_HandleOffline(AI_t *ai) {
if (ai == NULL) return DEVICE_ERR_NULL;
if (BSP_TIME_Get() - ai->header.last_online_time >
100000) {
ai->header.online = false;
}
return DEVICE_OK;
}
int8_t AI_StartSend(AI_t *ai, bool ref_online){
if (ai == NULL) return DEVICE_ERR_NULL;
if (ref_online) {
// 发送裁判系统数据和MCU数据
if (BSP_UART_Transmit(BSP_UART_AI, (uint8_t *)&(ai->to_host),
sizeof(ai->to_host.ref) + sizeof(ai->to_host.mcu), true) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
} else {
// 只发送MCU数据
if (BSP_UART_Transmit(BSP_UART_AI, (uint8_t *)&(ai->to_host.mcu),
sizeof(ai->to_host.mcu), true) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
}
}

131
assets/User_code/device/ai.h Normal file
View File

@@ -0,0 +1,131 @@
/*
AI
*/
#pragma once
#include <sys/cdefs.h>
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "component/ahrs.h"
#include "component/filter.h"
#include "component/user_math.h"
#include "device/device.h"
#include <cmsis_os2.h>
#include <stdbool.h>
#include <stdint.h>
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
#define AI_ID_MCU (0xC4)
#define AI_ID_REF (0xA8)
#define AI_ID_AI (0xA1)
/* Exported types ----------------------------------------------------------- */
typedef enum {
AI_ARMOR_HERO = 0, /*英雄机器人*/
AI_ARMOR_INFANTRY, /*步兵机器人*/
AI_ARMOR_SENTRY, /*哨兵机器人*/
AI_ARMOR_ENGINEER, /*工程机器人*/
AI_ARMOR_OUTPOST, /*前哨占*/
AI_ARMOR_BASE, /*基地*/
AI_ARMOR_NORMAL, /*由AI自动选择*/
} AI_ArmorsType_t;
typedef enum {
AI_STATUS_OFF = 0, /* 关闭 */
AI_STATUS_AUTOAIM, /* 自瞄 */
AI_STATUS_AUTOPICK, /* 自动取矿 */
AI_STATUS_AUTOPUT, /* 自动兑矿 */
AI_STATUS_AUTOHITBUFF, /* 自动打符 */
AI_STATUS_AUTONAV,
} AI_Status_t;
typedef enum {
AI_NOTICE_NONE = 0,
AI_NOTICE_SEARCH,
AI_NOTICE_FIRE,
}AI_Notice_t;
/* 电控 -> 视觉 MCU数据结构体*/
typedef struct __packed {
AHRS_Quaternion_t quat; /* 四元数 */
// struct {
// AI_ArmorsType_t armor_type;
// AI_Status_t status;
// }notice; /* 控制命令 */
uint8_t notice;
} AI_Protucol_UpDataMCU_t;
/* 电控 -> 视觉 裁判系统数据结构体*/
typedef struct __packed {
/* USER REFEREE BEGIN */
uint16_t team; /* 本身队伍 */
uint16_t time; /* 比赛开始时间 */
/* USER REFEREE END */
} AI_Protocol_UpDataReferee_t;
/* 视觉 -> 电控 数据包结构体*/
typedef struct __packed {
AHRS_Eulr_t eulr; /* 欧拉角 */
MoveVector_t move_vec; /* 运动向量 */
uint8_t notice; /* 控制命令 */
} AI_Protocol_DownData_t;
/* 电控 -> 视觉 裁判系统数据包 */
typedef struct __packed {
uint8_t id; /* 包ID */
AI_Protocol_UpDataReferee_t package; /* 数据包 */
uint16_t crc16; /* CRC16校验 */
} AI_UpPackageReferee_t;
/* 电控 -> 视觉 MUC数据包 */
typedef struct __packed {
uint8_t id;
AI_Protucol_UpDataMCU_t package;
uint16_t crc16;
} AI_UpPackageMCU_t;
/* 视觉 -> 电控 数据包 */
typedef struct __packed {
uint8_t id; /* 包ID */
AI_Protocol_DownData_t package; /* 数据包 */
uint16_t crc16; /* CRC16校验 */
} AI_DownPackage_t;
typedef struct __packed {
DEVICE_Header_t header; /* 设备通用头部 */
AI_DownPackage_t from_host;
AI_Status_t status;
struct {
AI_UpPackageReferee_t ref;
AI_UpPackageMCU_t mcu;
} to_host;
} AI_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t AI_Init(AI_t *ai);
int8_t AI_Restart(AI_t *ai);
int8_t AI_StartReceiving(AI_t *ai);
bool AI_WaitDmaCplt(void);
int8_t AI_ParseHost(AI_t *ai);
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat);
int8_t AI_PackRef(AI_t *ai, const AI_UpPackageReferee_t *data);
int8_t AI_HandleOffline(AI_t *ai);
int8_t AI_StartSend(AI_t *ai, bool ref_online);
#ifdef __cplusplus
}
#endif

381
assets/User_code/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
assets/User_code/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

171
assets/User_code/device/buzzer.c Normal file
View File

@@ -0,0 +1,171 @@
#include "device/buzzer.h"
#include "bsp/time.h"
#include <math.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define MUSIC_DEFAULT_VOLUME 0.5f
#define MUSIC_A4_FREQ 440.0f // A4音符频率
/* USER MUSIC MENU BEGIN */
// RM音乐
const Tone_t RM[] = {
{NOTE_B, 5, 200},
{NOTE_G, 4, 200},
{NOTE_B, 5, 400},
{NOTE_G, 4, 200},
{NOTE_B, 5, 400},
{NOTE_G, 4, 200},
{NOTE_D, 5, 400},
{NOTE_G, 4, 200},
{NOTE_C, 5, 200},
{NOTE_C, 5, 200},
{NOTE_G, 4, 200},
{NOTE_B, 5, 200},
{NOTE_C, 5, 200}
};
// Nokia 经典铃声音符
const Tone_t NOKIA[] = {
{NOTE_E, 5, 125}, {NOTE_D, 5, 125}, {NOTE_FS, 4, 250}, {NOTE_GS, 4, 250},
{NOTE_CS, 5, 125}, {NOTE_B, 4, 125}, {NOTE_D, 4, 250}, {NOTE_E, 4, 250},
{NOTE_B, 4, 125}, {NOTE_A, 4, 125}, {NOTE_CS, 4, 250}, {NOTE_E, 4, 250},
{NOTE_A, 4, 500}
};
/* USER MUSIC MENU END */
static void BUZZER_Update(BUZZER_t *buzzer){
buzzer->header.online = true;
buzzer->header.last_online_time = BSP_TIME_Get_ms();
}
// 根据音符和八度计算频率的辅助函数
static float BUZZER_CalcFreq(NOTE_t note, uint8_t octave) {
if (note == NOTE_REST) {
return 0.0f; // 休止符返回0频率
}
// 将音符和八度转换为MIDI音符编号
int midi_num = (int)note + (int)((octave + 1) * 12);
// 使用A4 (440Hz) 作为参考，计算频率
// 公式: freq = 440 * 2^((midi_num - 69)/12)
float freq = 440.0f * powf(2.0f, ((float)midi_num - 69.0f) / 12.0f);
return freq;
}
// 播放单个音符
static int8_t BUZZER_PlayTone(BUZZER_t *buzzer, NOTE_t note, uint8_t octave, uint16_t duration_ms) {
if (buzzer == NULL || !buzzer->header.online)
return DEVICE_ERR;
float freq = BUZZER_CalcFreq(note, octave);
if (freq > 0.0f) {
// 播放音符
if (BUZZER_Set(buzzer, freq, MUSIC_DEFAULT_VOLUME) != DEVICE_OK)
return DEVICE_ERR;
if (BUZZER_Start(buzzer) != DEVICE_OK)
return DEVICE_ERR;
} else {
// 休止符，停止播放
BUZZER_Stop(buzzer);
}
// 等待指定时间
BSP_TIME_Delay_ms(duration_ms);
// 停止当前音符，为下一个音符做准备
BUZZER_Stop(buzzer);
BSP_TIME_Delay_ms(20); // 短暂间隔
return DEVICE_OK;
}
int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel) {
if (buzzer == NULL) return DEVICE_ERR;
buzzer->channel = channel;
buzzer->header.online = true;
BUZZER_Stop(buzzer);
return DEVICE_OK ;
}
int8_t BUZZER_Start(BUZZER_t *buzzer) {
if (buzzer == NULL || !buzzer->header.online)
return DEVICE_ERR;
BUZZER_Update(buzzer);
return (BSP_PWM_Start(buzzer->channel) == BSP_OK) ?
DEVICE_OK : DEVICE_ERR;
}
int8_t BUZZER_Stop(BUZZER_t *buzzer) {
if (buzzer == NULL || !buzzer->header.online)
return DEVICE_ERR;
BUZZER_Update(buzzer);
return (BSP_PWM_Stop(buzzer->channel) == BSP_OK) ?
DEVICE_OK : DEVICE_ERR;
}
int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle) {
if (buzzer == NULL || !buzzer->header.online)
return DEVICE_ERR;
int result = DEVICE_OK ;
BUZZER_Update(buzzer);
if (BSP_PWM_SetFreq(buzzer->channel, freq) != BSP_OK)
result = DEVICE_ERR;
if (BSP_PWM_SetComp(buzzer->channel, duty_cycle) != BSP_OK)
result = DEVICE_ERR;
return result;
}
int8_t BUZZER_PlayMusic(BUZZER_t *buzzer, MUSIC_t music) {
if (buzzer == NULL || !buzzer->header.online)
return DEVICE_ERR;
const Tone_t *melody = NULL;
size_t melody_length = 0;
// 根据音乐类型选择对应的音符数组
switch (music) {
case MUSIC_RM:
melody = RM;
melody_length = sizeof(RM) / sizeof(Tone_t);
break;
case MUSIC_NOKIA:
melody = NOKIA;
melody_length = sizeof(NOKIA) / sizeof(Tone_t);
break;
default:
return DEVICE_ERR;
}
// 播放整首音乐
for (size_t i = 0; i < melody_length; i++) {
if (BUZZER_PlayTone(buzzer, melody[i].note, melody[i].octave, melody[i].duration_ms) != DEVICE_OK) {
BUZZER_Stop(buzzer); // 出错时停止播放
return DEVICE_ERR;
}
}
// 音乐播放完成后停止
BUZZER_Stop(buzzer);
return DEVICE_OK;
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

138
assets/User_code/device/buzzer.h Normal file
View File

@@ -0,0 +1,138 @@
/**
* @file buzzer.h
* @brief 蜂鸣器设备驱动头文件
* @details 提供蜂鸣器音频播放功能，支持单音符播放和预设音乐播放
* @author Generated by STM32CubeMX
* @date 2025年10月23日
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "bsp/pwm.h" // PWM底层硬件抽象层
#include "device.h" // 设备通用头文件
#include <stddef.h> // 标准定义
#include <stdint.h> // 标准整型定义
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported types ----------------------------------------------------------- */
/**
* @brief 音符枚举类型
* @details 定义标准十二平均律音符，用于音乐播放
*/
typedef enum {
NOTE_C = 0, ///< Do音符
NOTE_CS = 1, ///< Do#音符 (升Do)
NOTE_D = 2, ///< Re音符
NOTE_DS = 3, ///< Re#音符 (升Re)
NOTE_E = 4, ///< Mi音符
NOTE_F = 5, ///< Fa音符
NOTE_FS = 6, ///< Fa#音符 (升Fa)
NOTE_G = 7, ///< Sol音符
NOTE_GS = 8, ///< Sol#音符 (升Sol)
NOTE_A = 9, ///< La音符
NOTE_AS = 10, ///< La#音符 (升La)
NOTE_B = 11, ///< Si音符
NOTE_REST = 255 ///< 休止符 (无声音)
} NOTE_t;
/**
* @brief 音调结构体
* @details 定义一个完整的音调信息，包括音符、八度和持续时间
*/
typedef struct {
NOTE_t note; ///< 音符名称 (使用NOTE_t枚举)
uint8_t octave; ///< 八度 (0-8，通常使用3-7)
uint16_t duration_ms; ///< 持续时间，单位毫秒
} Tone_t;
/**
* @brief 预设音乐枚举类型
* @details 定义可播放的预设音乐类型
*/
typedef enum {
/* USER MUSIC MENU BEGIN */
MUSIC_RM, ///< RM战队音乐
MUSIC_NOKIA, ///< 诺基亚经典铃声
/* USER MUSIC MENU END */
} MUSIC_t;
/**
* @brief 蜂鸣器设备结构体
* @details 蜂鸣器设备的完整描述，包含设备头信息和PWM通道
*/
typedef struct {
DEVICE_Header_t header; ///< 设备通用头信息 (在线状态、时间戳等)
BSP_PWM_Channel_t channel; ///< PWM输出通道
} BUZZER_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化蜂鸣器设备
* @param buzzer 蜂鸣器设备结构体指针
* @param channel PWM输出通道
* @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
* @note 初始化后蜂鸣器处于停止状态
*/
int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel);
/**
* @brief 启动蜂鸣器播放
* @param buzzer 蜂鸣器设备结构体指针
* @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
* @note 需要先调用BUZZER_Set设置频率和占空比
*/
int8_t BUZZER_Start(BUZZER_t *buzzer);
/**
* @brief 停止蜂鸣器播放
* @param buzzer 蜂鸣器设备结构体指针
* @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
*/
int8_t BUZZER_Stop(BUZZER_t *buzzer);
/**
* @brief 设置蜂鸣器频率和占空比
* @param buzzer 蜂鸣器设备结构体指针
* @param freq 频率 (Hz)，通常范围20Hz-20kHz
* @param duty_cycle 占空比 (0.0-1.0)，影响音量大小
* @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
* @note 设置后需要调用BUZZER_Start才能听到声音
*/
int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle);
/**
* @brief 播放预设音乐
* @param buzzer 蜂鸣器设备结构体指针
* @param music 音乐类型 (使用MUSIC_t枚举)
* @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
* @note 这是一个阻塞函数，会播放完整首音乐后返回
*/
int8_t BUZZER_PlayMusic(BUZZER_t *buzzer, MUSIC_t music);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

227
assets/User_code/device/config.yaml Normal file
View File

@@ -0,0 +1,227 @@
devices:
dr16:
name: "DR16"
description: "大疆遥控器接收机"
dependencies:
bsp: ["uart"]
component: ["user_math"]
bsp_requirements:
- type: "uart"
var_name: "BSP_UART_DR16" # 需要替换的变量名
description: "用于接收遥控器数据"
thread_signals:
- name: "SIGNAL_DR16_RAW_REDY"
files:
header: "dr16.h"
source: "dr16.c"
ops9:
name: "OPS9"
description: "ACTION OPS9 码盘"
dependencies:
bsp: ["uart"]
component: ["user_math"]
bsp_requirements:
- type: "uart"
var_name: "BSP_UART_OPS9" # 需要替换的变量名
description: "用于接收码盘"
thread_signals:
- name: "SIGNAL_OPS9_RAW_REDY"
files:
header: "ops9.h"
source: "ops9.c"
bmi088:
name: "BMI088"
description: "BMI088 陀螺仪+加速度计传感器"
dependencies:
bsp: ["spi", "gpio"]
component: ["user_math"]
bsp_requirements:
- type: "spi"
var_name: "BSP_SPI_BMI088"
description: "用于与 BMI088 通信的 SPI 总线"
- type: "gpio"
var_name: "BSP_GPIO_ACCL_CS"
description: "加速度计片选输出引脚"
gpio_type: "output"
- type: "gpio"
var_name: "BSP_GPIO_GYRO_CS"
description: "陀螺仪片选输出引脚"
gpio_type: "output"
- type: "gpio"
var_name: "BSP_GPIO_ACCL_INT"
description: "加速度计中断输入引脚"
gpio_type: "EXTI"
- type: "gpio"
var_name: "BSP_GPIO_GYRO_INT"
description: "陀螺仪中断输入引脚"
gpio_type: "EXTI"
thread_signals:
- name: "SIGNAL_BMI088_ACCL_RAW_REDY"
- name: "SIGNAL_BMI088_GYRO_RAW_REDY"
- name: "SIGNAL_BMI088_ACCL_NEW_DATA"
- name: "SIGNAL_BMI088_GYRO_NEW_DATA"
files:
header: "bmi088.h"
source: "bmi088.c"
ist8310:
name: "IST8310"
description: "IST8310 地磁传感器"
dependencies:
bsp: ["i2c", "gpio"]
component: []
bsp_requirements:
- type: "i2c"
var_name: "BSP_I2C_COMP"
description: "用于与 IST8310 通信的 I2C 总线"
- type: "gpio"
var_name: "CMPS_RST_Pin"
description: "IST8310 复位引脚"
gpio_type: "output"
- type: "gpio"
var_name: "CMPS_INT_Pin"
description: "IST8310 数据中断引脚"
gpio_type: "EXTI"
thread_signals:
- name: "SIGNAL_IST8310_MAGN_RAW_REDY"
- name: "SIGNAL_IST8310_MAGN_NEW_DATA"
files:
header: "ist8310.h"
source: "ist8310.c"
motor_vesc:
name: "VESC 电调"
description: "VESC 电调驱动"
dependencies:
bsp: ["can", "time", "mm"]
component: ["user_math"]
files:
header: "motor_vesc.h"
source: "motor_vesc.c"
motor_odrive:
name: "ODrive 电机"
description: "ODrive 电机驱动"
dependencies:
bsp: ["can", "time", "mm"]
component: ["user_math"]
files:
header: "motor_odrive.h"
source: "motor_odrive.c"
motor_rm:
name: "RM 电机"
description: "RM 电机驱动"
dependencies:
bsp: ["can", "time", "mm"]
component: ["user_math"]
files:
header: "motor_rm.h"
source: "motor_rm.c"
motor:
name: "通用电机"
description: "通用电机驱动"
dependencies:
bsp: []
component: []
bsp_requirements: []
thread_signals: []
files:
header: "motor.h"
source: "motor.c"
ws2812:
name: "WS2812 LED 灯"
description: "WS2812 RGB LED 灯驱动"
dependencies:
bsp: ["pwm", "time"]
component: []
thread_signals: []
files:
header: "ws2812.h"
source: "ws2812.c"
buzzer:
name: "蜂鸣器"
description: "蜂鸣器驱动"
dependencies:
bsp: ["pwm"]
component: []
bsp_requirements:
- type: "pwm"
var_name: "BSP_PWM_BUZZER"
description: "用于蜂鸣器的PWM通道"
thread_signals: []
files:
header: "buzzer.h"
source: "buzzer.c"
dm_imu:
name: "DM IMU"
description: "DM IMU 传感器"
dependencies:
bsp: ["can", "time"]
component: ["user_math"]
files:
header: "dm_imu.h"
source: "dm_imu.c"
led:
name: "LED 灯"
description: "LED 灯驱动"
dependencies:
bsp: ["gpio", "pwm"]
component: []
thread_signals: []
files:
header: "led.h"
source: "led.c"
motor_lk:
name: "LK 电机"
description: "LK 电机驱动"
dependencies:
bsp: ["can", "time", "mm"]
component: ["user_math"]
files:
header: "motor_lk.h"
source: "motor_lk.c"
motor_lz:
name: "LZ 电机"
description: "LZ 电机驱动"
dependencies:
bsp: ["can", "time", "mm"]
component: ["user_math"]
files:
header: "motor_lz.h"
source: "motor_lz.c"
servo:
name: "舵机"
description: "舵机驱动"
dependencies:
bsp: ["pwm"]
component: []
thread_signals: []
files:
header: "servo.h"
source: "servo.c"
vofa:
name: "VOFA"
description: "VOFA 数据传输协议"
dependencies:
bsp: ["uart"]
component: []
bsp_requirements:
- type: "uart"
var_name: "BSP_UART_VOFA" # 需要替换的变量名
description: "用于VOFA数据传输"
thread_signals: []
files:
header: "vofa.h"
source: "vofa.c"

47
assets/User_code/device/device.h Normal file
View File

@@ -0,0 +1,47 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define DEVICE_OK (0)
#define DEVICE_ERR (-1)
#define DEVICE_ERR_NULL (-2)
#define DEVICE_ERR_INITED (-3)
#define DEVICE_ERR_NO_DEV (-4)
/* AUTO GENERATED SIGNALS BEGIN */
/* AUTO GENERATED SIGNALS END */
/* USER SIGNALS BEGIN */
/* USER SIGNALS END */
/*设备层通用Header*/
typedef struct {
bool online;
uint64_t last_online_time;
} DEVICE_Header_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

271
assets/User_code/device/dm_imu.c Normal file
View File

@@ -0,0 +1,271 @@
/*
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;
}
return DEVICE_ERR;
}
/**
* @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
assets/User_code/device/dm_imu.h Normal file
View File

@@ -0,0 +1,90 @@
#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

169
assets/User_code/device/dr16.c Normal file
View File

@@ -0,0 +1,169 @@
/*
DR16接收机
Example：
DR16_Init(&dr16);
while (1) {
DR16_StartDmaRecv(&dr16);
if (DR16_WaitDmaCplt(20)) {
DR16_ParseData(&dr16);
} else {
DR16_Offline(&dr16);
}
}
*/
/* Includes ----------------------------------------------------------------- */
#include "dr16.h"
#include "bsp/uart.h"
#include "bsp/time.h"
#include "device.h"
#include <string.h>
#include <stdbool.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */
#define DR16_CH_VALUE_MIN (364u)
#define DR16_CH_VALUE_MID (1024u)
#define DR16_CH_VALUE_MAX (1684u)
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static osThreadId_t thread_alert;
static bool inited = false;
/* Private function -------------------------------------------------------- */
static void DR16_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY);
}
static bool DR16_DataCorrupted(const DR16_t *dr16) {
if (dr16 == NULL) return DEVICE_ERR_NULL;
if ((dr16->raw_data.ch_r_x < DR16_CH_VALUE_MIN) ||
(dr16->raw_data.ch_r_x > DR16_CH_VALUE_MAX))
return DEVICE_ERR;
if ((dr16->raw_data.ch_r_y < DR16_CH_VALUE_MIN) ||
(dr16->raw_data.ch_r_y > DR16_CH_VALUE_MAX))
return DEVICE_ERR;
if ((dr16->raw_data.ch_l_x < DR16_CH_VALUE_MIN) ||
(dr16->raw_data.ch_l_x > DR16_CH_VALUE_MAX))
return DEVICE_ERR;
if ((dr16->raw_data.ch_l_y < DR16_CH_VALUE_MIN) ||
(dr16->raw_data.ch_l_y > DR16_CH_VALUE_MAX))
return DEVICE_ERR;
if (dr16->raw_data.sw_l == 0) return DEVICE_ERR;
if (dr16->raw_data.sw_r == 0) return DEVICE_ERR;
return DEVICE_OK;
}
/* Exported functions ------------------------------------------------------- */
int8_t DR16_Init(DR16_t *dr16) {
if (dr16 == NULL) return DEVICE_ERR_NULL;
if (inited) return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_DR16, BSP_UART_RX_CPLT_CB,
DR16_RxCpltCallback);
inited = true;
return DEVICE_OK;
}
int8_t DR16_Restart(void) {
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_DR16));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_DR16));
return DEVICE_OK;
}
int8_t DR16_StartDmaRecv(DR16_t *dr16) {
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_DR16),
(uint8_t *)&(dr16->raw_data),
sizeof(dr16->raw_data)) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
bool DR16_WaitDmaCplt(uint32_t timeout) {
return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
SIGNAL_DR16_RAW_REDY);
}
int8_t DR16_ParseData(DR16_t *dr16){
if (dr16 == NULL) return DEVICE_ERR_NULL;
if (DR16_DataCorrupted(dr16)) {
return DEVICE_ERR;
}
dr16->header.online = true;
dr16->header.last_online_time = BSP_TIME_Get_us();
memset(&(dr16->data), 0, sizeof(dr16->data));
float full_range = (float)(DR16_CH_VALUE_MAX - DR16_CH_VALUE_MIN);
// 解析摇杆数据
dr16->data.ch_r_x = 2.0f * ((float)dr16->raw_data.ch_r_x - DR16_CH_VALUE_MID) / full_range;
dr16->data.ch_r_y = 2.0f * ((float)dr16->raw_data.ch_r_y - DR16_CH_VALUE_MID) / full_range;
dr16->data.ch_l_x = 2.0f * ((float)dr16->raw_data.ch_l_x - DR16_CH_VALUE_MID) / full_range;
dr16->data.ch_l_y = 2.0f * ((float)dr16->raw_data.ch_l_y - DR16_CH_VALUE_MID) / full_range;
// 解析拨杆位置
dr16->data.sw_l = (DR16_SwitchPos_t)dr16->raw_data.sw_l;
dr16->data.sw_r = (DR16_SwitchPos_t)dr16->raw_data.sw_r;
// 解析鼠标数据
dr16->data.mouse.x = dr16->raw_data.x;
dr16->data.mouse.y = dr16->raw_data.y;
dr16->data.mouse.z = dr16->raw_data.z;
dr16->data.mouse.l_click = dr16->raw_data.press_l;
dr16->data.mouse.r_click = dr16->raw_data.press_r;
// 解析键盘按键 - 使用union简化代码
uint16_t key_value = dr16->raw_data.key;
// 解析键盘位映射（W-B键，位0-15）
for (int i = DR16_KEY_W; i <= DR16_KEY_B; i++) {
dr16->data.keyboard.key[i] = (key_value & (1 << i)) != 0;
}
// 解析鼠标点击
dr16->data.keyboard.key[DR16_L_CLICK] = dr16->data.mouse.l_click;
dr16->data.keyboard.key[DR16_R_CLICK] = dr16->data.mouse.r_click;
// 解析第五通道
dr16->data.ch_res = 2.0f * ((float)dr16->raw_data.res - DR16_CH_VALUE_MID) / full_range;
return DEVICE_OK;
}
int8_t DR16_Offline(DR16_t *dr16){
if (dr16 == NULL) return DEVICE_ERR_NULL;
dr16->header.online = false;
memset(&(dr16->data), 0, sizeof(dr16->data));
return DEVICE_OK;
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

117
assets/User_code/device/dr16.h Normal file
View File

@@ -0,0 +1,117 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include "component/user_math.h"
#include "device.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct __packed {
uint16_t ch_r_x : 11;
uint16_t ch_r_y : 11;
uint16_t ch_l_x : 11;
uint16_t ch_l_y : 11;
uint8_t sw_r : 2;
uint8_t sw_l : 2;
int16_t x;
int16_t y;
int16_t z;
uint8_t press_l;
uint8_t press_r;
uint16_t key;
uint16_t res;
} DR16_RawData_t;
typedef enum {
DR16_SW_ERR = 0,
DR16_SW_UP = 1,
DR16_SW_MID = 3,
DR16_SW_DOWN = 2,
} DR16_SwitchPos_t;
/* 键盘按键值 */
typedef enum {
DR16_KEY_W = 0,
DR16_KEY_S,
DR16_KEY_A,
DR16_KEY_D,
DR16_KEY_SHIFT,
DR16_KEY_CTRL,
DR16_KEY_Q,
DR16_KEY_E,
DR16_KEY_R,
DR16_KEY_F,
DR16_KEY_G,
DR16_KEY_Z,
DR16_KEY_X,
DR16_KEY_C,
DR16_KEY_V,
DR16_KEY_B,
DR16_L_CLICK,
DR16_R_CLICK,
DR16_KEY_NUM,
} DR16_Key_t;
typedef struct {
float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
float ch_res; /* 第五通道值 */
DR16_SwitchPos_t sw_r; /* 右侧拨杆位置 */
DR16_SwitchPos_t sw_l; /* 左侧拨杆位置 */
struct {
int16_t x;
int16_t y;
int16_t z;
bool l_click; /* 左键 */
bool r_click; /* 右键 */
} mouse; /* 鼠标值 */
union {
bool key[DR16_KEY_NUM]; /* 键盘按键值 */
uint16_t value; /* 键盘按键值的位映射 */
} keyboard;
uint16_t res; /* 保留，未启用 */
} DR16_Data_t;
typedef struct {
DEVICE_Header_t header;
DR16_RawData_t raw_data;
DR16_Data_t data;
} DR16_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t DR16_Init(DR16_t *dr16);
int8_t DR16_Restart(void);
int8_t DR16_StartDmaRecv(DR16_t *dr16);
bool DR16_WaitDmaCplt(uint32_t timeout);
int8_t DR16_ParseData(DR16_t *dr16);
int8_t DR16_Offline(DR16_t *dr16);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

178
assets/User_code/device/ist8310.c Normal file
View File

@@ -0,0 +1,178 @@
/*
IST8310 地磁传感器。
*/
/* Includes ----------------------------------------------------------------- */
#include "ist8310.h"
#include <gpio.h>
#include <stdbool.h>
#include <string.h>
#include "bsp/time.h"
#include "bsp/gpio.h"
#include "bsp/i2c.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */
#define IST8310_WAI (0x00)
#define IST8310_STAT1 (0x02)
#define IST8310_DATAXL (0x03)
#define IST8310_STAT2 (0x09)
#define IST8310_CNTL1 (0x0A)
#define IST8310_CNTL2 (0x0B)
#define IST8310_STR (0x0C)
#define IST8310_TEMPL (0x1C)
#define IST8310_TEMPH (0x1D)
#define IST8310_AVGCNTL (0x41)
#define IST8310_PDCNTL (0x42)
#define IST8310_CHIP_ID (0x10)
#define IST8310_IIC_ADDRESS (0x0E << 1)
#define IST8310_LEN_RX_BUFF (6)
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
#define IST8310_SET() \
BSP_GPIO_WritePin(CMPS_RST_Pin, GPIO_PIN_SET)
#define IST8310_RESET() \
BSP_GPIO_WritePin(CMPS_RST_Pin, GPIO_PIN_RESET)
/* Private typedef ---------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */
uint8_t ist8310_rxbuf[IST8310_LEN_RX_BUFF];
static osThreadId_t thread_alert;
static bool inited = false;
/* Private function -------------------------------------------------------- */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
static void IST8310_WriteSingle(uint8_t reg, uint8_t data) {
BSP_I2C_MemWriteByte(BSP_I2C_COMP, IST8310_IIC_ADDRESS, reg, data);
}
static uint8_t IST8310_ReadSingle(uint8_t reg) {
return BSP_I2C_MemReadByte(BSP_I2C_COMP, IST8310_IIC_ADDRESS, reg);
}
static void IST8310_Read(uint8_t reg, uint8_t *data, uint8_t len) {
if (data == NULL) return;
BSP_I2C_MemRead(BSP_I2C_COMP, IST8310_IIC_ADDRESS, reg, data, len, true);
}
static void IST8310_MemRxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_IST8310_MAGN_RAW_REDY);
}
static void IST8310_IntCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_IST8310_MAGN_NEW_DATA);
}
/* Exported functions ------------------------------------------------------- */
int8_t IST8310_Init(IST8310_t *ist8310, const IST8310_Cali_t *cali) {
if (ist8310 == 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;
ist8310->cali = cali;
IST8310_RESET();
BSP_TIME_Delay(50);
IST8310_SET();
BSP_TIME_Delay(50);
if (IST8310_ReadSingle(IST8310_WAI) != IST8310_CHIP_ID)
return DEVICE_ERR_NO_DEV;
BSP_GPIO_DisableIRQ(CMPS_INT_Pin);
BSP_I2C_RegisterCallback(BSP_I2C_COMP, HAL_I2C_MEM_RX_CPLT_CB,
IST8310_MemRxCpltCallback);
BSP_GPIO_RegisterCallback(CMPS_INT_Pin, IST8310_IntCallback);
/* Init. */
/* 0x00: Stand-By mode. 0x01: Single measurement mode. */
/* 0x08: Data ready function enable. DRDY signal active low*/
IST8310_WriteSingle(IST8310_CNTL2, 0x08);
IST8310_WriteSingle(IST8310_AVGCNTL, 0x09);
IST8310_WriteSingle(IST8310_PDCNTL, 0xC0);
IST8310_WriteSingle(IST8310_CNTL1, 0x0B);
BSP_TIME_Delay(10);
inited = true;
BSP_GPIO_EnableIRQ(CMPS_INT_Pin);
return DEVICE_OK;
}
bool IST8310_WaitNew(uint32_t timeout) {
return (osThreadFlagsWait(SIGNAL_IST8310_MAGN_NEW_DATA, osFlagsWaitAll,
timeout) == SIGNAL_IST8310_MAGN_NEW_DATA);
}
int8_t IST8310_StartDmaRecv() {
IST8310_Read(IST8310_DATAXL, ist8310_rxbuf, IST8310_LEN_RX_BUFF);
return DEVICE_OK;
}
uint32_t IST8310_WaitDmaCplt() {
return osThreadFlagsWait(SIGNAL_IST8310_MAGN_RAW_REDY, osFlagsWaitAll,
osWaitForever);
}
int8_t IST8310_Parse(IST8310_t *ist8310) {
if (ist8310 == NULL) return DEVICE_ERR_NULL;
#if 1
/* Magn -> T */
int16_t raw_x, raw_y, raw_z;
memcpy(&raw_x, ist8310_rxbuf + 0, sizeof(raw_x));
memcpy(&raw_y, ist8310_rxbuf + 2, sizeof(raw_y));
memcpy(&raw_z, ist8310_rxbuf + 4, sizeof(raw_z));
ist8310->magn.x = (float)raw_x;
ist8310->magn.y = (float)raw_y;
ist8310->magn.z = (float)-raw_z;
#else
const int16_t *raw_x = (int16_t *)(ist8310_rxbuf + 0);
const int16_t *raw_y = (int16_t *)(ist8310_rxbuf + 2);
const int16_t *raw_z = (int16_t *)(ist8310_rxbuf + 4);
ist8310->magn.x = (float)*raw_x;
ist8310->magn.y = (float)*raw_y;
ist8310->magn.z = -(float)*raw_z;
#endif
ist8310->magn.x *= 3.0f / 20.0f;
ist8310->magn.y *= 3.0f / 20.0f;
ist8310->magn.z *= 3.0f / 20.0f;
ist8310->magn.x = (ist8310->magn.x - ist8310->cali->magn_offset.x) *
ist8310->cali->magn_scale.x;
ist8310->magn.y = (ist8310->magn.y - ist8310->cali->magn_offset.y) *
ist8310->cali->magn_scale.y;
ist8310->magn.z = (ist8310->magn.z - ist8310->cali->magn_offset.y) *
ist8310->cali->magn_scale.z;
return DEVICE_OK;
}

49
assets/User_code/device/ist8310.h Normal file
View File

@@ -0,0 +1,49 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include <stdbool.h>
#include <stdint.h>
#include "component/ahrs.h"
#include "device/device.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct {
struct {
float x;
float y;
float z;
} magn_offset; /* 磁力计偏置 */
struct {
float x;
float y;
float z;
} magn_scale; /* 磁力计缩放 */
} IST8310_Cali_t; /* IST8310校准数据 */
typedef struct {
DEVICE_Header_t header;
AHRS_Magn_t magn;
const IST8310_Cali_t *cali;
} IST8310_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t IST8310_Init(IST8310_t *ist8310, const IST8310_Cali_t *cali);
int8_t IST8310_Restart(void);
bool IST8310_WaitNew(uint32_t timeout);
int8_t IST8310_StartDmaRecv();
uint32_t IST8310_WaitDmaCplt();
int8_t IST8310_Parse(IST8310_t *ist8310);
#ifdef __cplusplus
}
#endif

37
assets/User_code/device/led.c Normal file
View File

@@ -0,0 +1,37 @@
/*
led控制
*/
/*Includes -----------------------------------------*/
#include "device/led.h"
#include "device.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
#ifdef LED_PWM
int8_t DEVICE_LED_PWM_Set(BSP_PWM_Channel_t channel, float duty_cycle)
{
if (duty_cycle < 0.0f || duty_cycle > 1.0f) {
return DEVICE_ERR_NULL; // 错误：占空比超出范围
}
uint16_t pulse = (uint16_t)(duty_cycle * (float)UINT16_MAX);
BSP_PWM_Start(channel);
BSP_PWM_SetComp(channel, pulse);
return DEVICE_OK;
}
#endif
#ifdef LED_GPIO
int8_t DEVICE_LED_GPIO_Set(BSP_GPIO_t gpio, bool value)
{
if (value) {
BSP_GPIO_WritePin(gpio, true);
} else {
BSP_GPIO_WritePin(gpio, false);
}
return DEVICE_OK;
}
#endif

63
assets/User_code/device/led.h Normal file
View File

@@ -0,0 +1,63 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
/* USER DEFIN BEGIN */
/* USER DEFIN END */
#include <stdint.h>
#ifdef LED_GPIO
#include "bsp/gpio.h"
#endif
#ifdef LED_PWM
#include "bsp/pwm.h"
#endif
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct {
#ifdef LED_GPIO
BSP_GPIO_t gpio;
#endif
#ifdef LED_PWM
BSP_PWM_Channel_t channel;
#endif
} DEVICE_LED_t;
extern DEVICE_LED_t LED_Map;
/* Exported functions prototypes -------------------------------------------- */
#ifdef LED_PWM
int8_t DEVICE_LED_PWM_Set(BSP_PWM_Channel_t channel, float duty_cycle)
{
if (duty_cycle < 0.0f || duty_cycle > 1.0f) {
return DEVICE_ERR_NULL; // 错误：占空比超出范围
}
uint16_t pulse = (uint16_t)(duty_cycle * (float)UINT16_MAX);
BSP_PWM_Start(channel);
BSP_PWM_SetComp(channel, pulse);
return DEVICE_OK;
}
#endif
#ifdef LED_GPIO
int8_t DEVICE_LED_GPIO_Set(BSP_GPIO_t gpio, bool value)
{
if (value) {
BSP_GPIO_WritePin(gpio, true);
} else {
BSP_GPIO_WritePin(gpio, false);
}
return DEVICE_OK;
}
#endif
#ifdef __cplusplus
}
#endif

52
assets/User_code/device/motor.c Normal file
View File

@@ -0,0 +1,52 @@
/*
电机通用函数
*/
/* Includes ----------------------------------------------------------------- */
#include "motor.h"
#include <string.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
/* Exported functions ------------------------------------------------------- */
float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor) {
if (motor == NULL) return DEVICE_ERR_NULL;
return motor->feedback.rotor_abs_angle;
}
float MOTOR_GetRotorSpeed(const MOTOR_t *motor) {
if (motor == NULL) return DEVICE_ERR_NULL;
return motor->feedback.rotor_speed;
}
float MOTOR_GetTorqueCurrent(const MOTOR_t *motor) {
if (motor == NULL) return DEVICE_ERR_NULL;
return motor->feedback.torque_current;
}
float MOTOR_GetTemp(const MOTOR_t *motor) {
if (motor == NULL) return DEVICE_ERR_NULL;
return motor->feedback.temp;
}

68
assets/User_code/device/motor.h Normal file
View File

@@ -0,0 +1,68 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct {
float rotor_abs_angle; /* 转子绝对角度 */
float rotor_speed; /* 实际转子转速 */
float torque_current; /* 转矩电流 */
float temp; /* 温度 */
} MOTOR_Feedback_t;
/**
* @brief mit电机输出参数结构体
*/
typedef struct {
float torque; /* 目标力矩 */
float velocity; /* 目标速度 */
float angle; /* 目标位置 */
float kp; /* 位置环增益 */
float kd; /* 速度环增益 */
} MOTOR_MIT_Output_t;
/**
* @brief 转矩电流控制模式参数结构体
*/
typedef struct {
float current; /* 目标电流 */
} MOTOR_Current_Output_t;
typedef struct {
DEVICE_Header_t header;
bool reverse; /* 是否反装 true表示反装 */
MOTOR_Feedback_t feedback;
} MOTOR_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Exported functions prototypes -------------------------------------------- */
float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor);
float MOTOR_GetRotorSpeed(const MOTOR_t *motor);
float MOTOR_GetTorqueCurrent(const MOTOR_t *motor);
float MOTOR_GetTemp(const MOTOR_t *motor);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

496
assets/User_code/device/motor_dm.c Normal file
View File

@@ -0,0 +1,496 @@
#define MOTOR_DM_FLOAT_TO_INT_SIGNED(x, x_min, x_max, bits) \
((int32_t)roundf(((x) / ((x_max) - (x_min))) * (1 << (bits)) + (1 << ((bits) - 1))))
#define MOTOR_DM_INT_TO_FLOAT_SIGNED(x_int, x_min, x_max, bits) \
(((float)((int32_t)(x_int) - (1 << ((bits) - 1))) * ((x_max) - (x_min)) / (float)(1 << (bits))))
/* Includes ----------------------------------------------------------------- */
#include "device/motor_dm.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
#include "string.h"
#include "math.h"
/* Private constants -------------------------------------------------------- */
/* DM电机数据映射范围 */
#define DM_P_MIN (-12.56637f)
#define DM_P_MAX (12.56637f)
#define DM_V_MIN (-30.0f)
#define DM_V_MAX (30.0f)
#define DM_T_MIN (-12.0f)
#define DM_T_MAX (12.0f)
#define DM_KP_MIN (0.0f)
#define DM_KP_MAX (500.0f)
#define DM_KD_MIN (0.0f)
#define DM_KD_MAX (5.0f)
/* CAN ID偏移量 */
#define DM_CAN_ID_OFFSET_POS_VEL 0x100
#define DM_CAN_ID_OFFSET_VEL 0x200
/* Private macro ------------------------------------------------------------ */
#define FLOAT_TO_UINT(x, x_min, x_max, bits) \
(uint32_t)((x - x_min) * ((1 << bits) - 1) / (x_max - x_min))
#define UINT_TO_FLOAT(x_int, x_min, x_max, bits) \
((float)(x_int) * (x_max - x_min) / ((1 << bits) - 1) + x_min)
/* Private variables -------------------------------------------------------- */
static MOTOR_DM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
static int float_to_uint(float x_float, float x_min, float x_max, int bits)
{
/* Converts a float to an unsigned int, given range and number of bits */
float span = x_max - x_min;
float offset = x_min;
return (int) ((x_float-offset)*((float)((1<<bits)-1))/span);
}
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
/* converts unsigned int to float, given range and number of bits */
float span = x_max - x_min;
float offset = x_min;
return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}
/* Private function prototypes ---------------------------------------------- */
static int8_t MOTOR_DM_ParseFeedbackFrame(MOTOR_DM_t *motor, const uint8_t *data);
static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output);
static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel);
static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel);
static MOTOR_DM_CANManager_t* MOTOR_DM_GetCANManager(BSP_CAN_t can);
/* Private functions -------------------------------------------------------- */
/**
* @brief 解析DM电机反馈帧
* @param motor 电机实例
* @param data CAN数据
* @return 解析结果
*/
static int8_t MOTOR_DM_ParseFeedbackFrame(MOTOR_DM_t *motor, const uint8_t *data) {
if (motor == NULL || data == NULL) {
return DEVICE_ERR_NULL;
}
uint16_t p_int=(data[1]<<8)|data[2];
motor->motor.feedback.rotor_abs_angle = uint_to_float(p_int, DM_P_MIN, DM_P_MAX, 16); // (-12.5,12.5)
uint16_t v_int=(data[3]<<4)|(data[4]>>4);
motor->motor.feedback.rotor_speed = uint_to_float(v_int, DM_V_MIN, DM_V_MAX, 12); // (-30.0,30.0)
uint16_t t_int=((data[4]&0xF)<<8)|data[5];
motor->motor.feedback.torque_current = uint_to_float(t_int, DM_T_MIN, DM_T_MAX, 12); // (-12.0,12.0)
motor->motor.feedback.temp = (float)(data[6]);
while (motor->motor.feedback.rotor_abs_angle < 0) {
motor->motor.feedback.rotor_abs_angle += M_2PI;
}
while (motor->motor.feedback.rotor_abs_angle >= M_2PI) {
motor->motor.feedback.rotor_abs_angle -= M_2PI;
}
if (motor->param.reverse) {
motor->motor.feedback.rotor_abs_angle = M_2PI - motor->motor.feedback.rotor_abs_angle;
motor->motor.feedback.rotor_speed = -motor->motor.feedback.rotor_speed;
motor->motor.feedback.torque_current = -motor->motor.feedback.torque_current;
}
return DEVICE_OK;
}
/**
* @brief 发送MIT模式控制命令
* @param motor 电机实例
* @param output MIT控制参数
* @return 发送结果
*/
static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output) {
if (motor == NULL || output == NULL) {
return DEVICE_ERR_NULL;
}
uint8_t data[8];
uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp;
uint16_t id = motor->param.can_id;
pos_tmp = float_to_uint(output->angle, DM_P_MIN , DM_P_MAX, 16);
vel_tmp = float_to_uint(output->velocity, DM_V_MIN , DM_V_MAX, 12);
kp_tmp = float_to_uint(output->kp, DM_KP_MIN, DM_KP_MAX, 12);
kd_tmp = float_to_uint(output->kd, DM_KD_MIN, DM_KD_MAX, 12);
tor_tmp = float_to_uint(output->torque, DM_T_MIN , DM_T_MAX, 12);
/* 打包数据 */
data[0] = (pos_tmp >> 8);
data[1] = pos_tmp;
data[2] = (vel_tmp >> 4);
data[3] = ((vel_tmp&0xF)<<4)|(kp_tmp>>8);
data[4] = kp_tmp;
data[5] = (kd_tmp >> 4);
data[6] = ((kd_tmp&0xF)<<4)|(tor_tmp>>8);
data[7] = tor_tmp;
/* 发送CAN消息 */
BSP_CAN_StdDataFrame_t frame;
frame.id = motor->param.can_id;
frame.dlc = 8;
memcpy(frame.data, data, 8);
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 发送位置速度模式控制命令
* @param motor 电机实例
* @param pos 目标位置
* @param vel 目标速度
* @return 发送结果
*/
static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel) {
if (motor == NULL) {
return DEVICE_ERR_NULL;
}
uint8_t data[8] = {0};
/* 直接发送浮点数数据 */
memcpy(&data[0], &pos, 4); // 位置，低位在前
memcpy(&data[4], &vel, 4); // 速度，低位在前
/* 发送CAN消息，ID为原ID+0x100 */
uint32_t can_id = DM_CAN_ID_OFFSET_POS_VEL + motor->param.can_id;
BSP_CAN_StdDataFrame_t frame;
frame.id = can_id;
frame.dlc = 8;
memcpy(frame.data, data, 8);
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 发送速度模式控制命令
* @param motor 电机实例
* @param vel 目标速度
* @return 发送结果
*/
static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel) {
if (motor == NULL) {
return DEVICE_ERR_NULL;
}
uint8_t data[4] = {0};
/* 直接发送浮点数数据 */
memcpy(&data[0], &vel, 4); // 速度，低位在前
/* 发送CAN消息，ID为原ID+0x200 */
uint32_t can_id = DM_CAN_ID_OFFSET_VEL + motor->param.can_id;
BSP_CAN_StdDataFrame_t frame;
frame.id = can_id;
frame.dlc = 4;
memcpy(frame.data, data, 4);
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 获取指定CAN总线的管理器
* @param can CAN总线
* @return CAN管理器指针
*/
static MOTOR_DM_CANManager_t* MOTOR_DM_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) {
return NULL;
}
return can_managers[can];
}
/**
* @brief 创建CAN管理器
* @param can CAN总线
* @return 创建结果
*/
static int8_t MOTOR_DM_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (MOTOR_DM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_DM_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(MOTOR_DM_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
/* Exported functions ------------------------------------------------------- */
/**
* @brief 注册一个DM电机
* @param param 电机参数
* @return 注册结果
*/
int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
/* 创建CAN管理器 */
if (MOTOR_DM_CreateCANManager(param->can) != DEVICE_OK) {
return DEVICE_ERR;
}
/* 获取CAN管理器 */
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
if (manager == NULL) {
return DEVICE_ERR;
}
/* 检查是否已注册 */
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) {
return DEVICE_ERR_INITED;
}
}
/* 检查是否已达到最大数量 */
if (manager->motor_count >= MOTOR_DM_MAX_MOTORS) {
return DEVICE_ERR;
}
/* 分配内存 */
MOTOR_DM_t *motor = (MOTOR_DM_t *)BSP_Malloc(sizeof(MOTOR_DM_t));
if (motor == NULL) {
return DEVICE_ERR;
}
/* 初始化电机 */
memset(motor, 0, sizeof(MOTOR_DM_t));
memcpy(&motor->param, param, sizeof(MOTOR_DM_Param_t));
motor->motor.header.online = false;
motor->motor.reverse = param->reverse;
/* 注册CAN接收ID - DM电机使用Master ID接收反馈 */
uint16_t feedback_id = param->master_id;
if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
BSP_Free(motor);
return DEVICE_ERR;
}
/* 添加到管理器 */
manager->motors[manager->motor_count] = motor;
manager->motor_count++;
return DEVICE_OK;
}
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return 更新结果
*/
int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
if (manager == NULL) {
return DEVICE_ERR_NO_DEV;
}
/* 查找电机 */
MOTOR_DM_t *motor = NULL;
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) {
motor = manager->motors[i];
break;
}
}
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
/* 主动接收CAN消息 */
uint16_t feedback_id = param->master_id;
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
uint64_t now_time = BSP_TIME_Get();
if (now_time - motor->motor.header.last_online_time > 100000) { // 100ms超时，单位微秒
motor->motor.header.online = false;
}
return DEVICE_ERR;
}
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
MOTOR_DM_ParseFeedbackFrame(motor, rx_msg.data);
return DEVICE_OK;
}
/**
* @brief 更新所有电机数据
* @return 更新结果
*/
int8_t MOTOR_DM_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_DM_t *motor = manager->motors[i];
if (motor != NULL) {
if (MOTOR_DM_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
/**
* @brief MIT模式控制
* @param param 电机参数
* @param output MIT控制参数
* @return 控制结果
*/
int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output) {
if (param == NULL || output == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
return MOTOR_DM_SendMITCmd(motor, output);
}
/**
* @brief 位置速度模式控制
* @param param 电机参数
* @param target_pos 目标位置
* @param target_vel 目标速度
* @return 控制结果
*/
int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
return MOTOR_DM_SendPosVelCmd(motor, target_pos, target_vel);
}
/**
* @brief 速度模式控制
* @param param 电机参数
* @param target_vel 目标速度
* @return 控制结果
*/
int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
return MOTOR_DM_SendVelCmd(motor, target_vel);
}
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return 电机实例指针
*/
MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return NULL;
}
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
if (manager == NULL) {
return NULL;
}
/* 查找对应的电机 */
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_DM_t *motor = manager->motors[i];
if (motor && motor->param.can == param->can &&
motor->param.master_id == param->master_id) {
return motor;
}
}
return NULL;
}
int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param){
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
BSP_CAN_StdDataFrame_t frame;
frame.id = motor->param.can_id;
frame.dlc = 8;
frame.data[0] = 0XFF;
frame.data[1] = 0xFF;
frame.data[2] = 0xFF;
frame.data[3] = 0xFF;
frame.data[4] = 0xFF;
frame.data[5] = 0xFF;
frame.data[6] = 0xFF;
frame.data[7] = 0xFC;
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 使电机松弛（设置输出为0）
* @param param 电机参数
* @return 操作结果
*/
int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_MIT_Output_t output = {0};
return MOTOR_DM_MITCtrl(param, &output);
}
/**
* @brief 使电机离线（设置在线状态为false）
* @param param 电机参数
* @return 操作结果
*/
int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
motor->motor.header.online = false;
return DEVICE_OK;
}

98
assets/User_code/device/motor_dm.h Normal file
View File

@@ -0,0 +1,98 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define MOTOR_DM_MAX_MOTORS 32
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
MOTOR_DM_J4310,
} MOTOR_DM_Module_t;
/*每个电机需要的参数*/
typedef struct {
BSP_CAN_t can;
uint16_t master_id; /* 主站ID，用于发送控制命令 */
uint16_t can_id; /* 反馈ID，用于接收电机反馈 */
MOTOR_DM_Module_t module;
bool reverse;
} MOTOR_DM_Param_t;
/*电机实例*/
typedef struct{
MOTOR_DM_Param_t param;
MOTOR_t motor;
} MOTOR_DM_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct {
BSP_CAN_t can;
MOTOR_DM_t *motors[MOTOR_DM_MAX_MOTORS];
uint8_t motor_count;
} MOTOR_DM_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 注册一个LK电机
* @param param 电机参数
* @return
*/
int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return
*/
int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param);
/**
* @brief 更新所有电机数据
* @return
*/
int8_t MOTOR_DM_UpdateAll(void);
int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output);
int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel);
int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return
*/
MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param);
int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param);
/**
* @brief 使电机松弛（设置输出为0）
* @param param
* @return
*/
int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param);
/**
* @brief 使电机离线（设置在线状态为false）
* @param param
* @return
*/
int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param);
#ifdef __cplusplus
}
#endif

329
assets/User_code/device/motor_lk.c Normal file
View File

@@ -0,0 +1,329 @@
/*
LK电机驱动
*/
/* Includes ----------------------------------------------------------------- */
#include "motor_lk.h"
#include <stdbool.h>
#include <string.h>
#include "bsp/can.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */
#define LK_CTRL_ID_BASE (0x140)
#define LK_FB_ID_BASE (0x240)
// LK电机命令字节定义
#define LK_CMD_FEEDBACK (0x9C) // 反馈命令字节
#define LK_CMD_MOTOR_OFF (0x80) // 电机关闭命令
#define LK_CMD_MOTOR_ON (0x88) // 电机运行命令
#define LK_CMD_TORQUE_CTRL (0xA1) // 转矩闭环控制命令
// LK电机参数定义
#define LK_CURR_LSB_MF (33.0f / 4096.0f) // MF电机转矩电流分辨率 A/LSB
#define LK_CURR_LSB_MG (66.0f / 4096.0f) // MG电机转矩电流分辨率 A/LSB
#define LK_POWER_RANGE_MS (1000) // MS电机功率范围 ±1000
#define LK_TORQUE_RANGE (2048) // 转矩控制值范围 ±2048
#define LK_TORQUE_CURRENT_MF (16.5f) // MF电机最大转矩电流 A
#define LK_TORQUE_CURRENT_MG (33.0f) // MG电机最大转矩电流 A
#define MOTOR_TX_BUF_SIZE (8)
#define MOTOR_RX_BUF_SIZE (8)
// 编码器分辨率定义
#define LK_ENC_14BIT_MAX (16383) // 14位编码器最大值
#define LK_ENC_15BIT_MAX (32767) // 15位编码器最大值
#define LK_ENC_16BIT_MAX (65535) // 16位编码器最大值
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */
static MOTOR_LK_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
/* Private functions -------------------------------------------------------- */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
static float MOTOR_LK_GetCurrentLSB(MOTOR_LK_Module_t module) {
switch (module) {
case MOTOR_LK_MF9025:
case MOTOR_LK_MF9035:
return LK_CURR_LSB_MF;
default:
return LK_CURR_LSB_MG; // 默认使用MG的分辨率
}
}
static uint16_t MOTOR_LK_GetEncoderMax(MOTOR_LK_Module_t module) {
// 根据电机型号返回编码器最大值，这里假设都使用16位编码器
// 实际使用时需要根据具体电机型号配置
return LK_ENC_16BIT_MAX;
}
static MOTOR_LK_CANManager_t* MOTOR_LK_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return NULL;
return can_managers[can];
}
static int8_t MOTOR_LK_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (MOTOR_LK_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LK_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(MOTOR_LK_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) {
// 检查命令字节是否为反馈命令
if (msg->data[0] != LK_CMD_FEEDBACK) {
// 如果不是标准反馈命令，可能是其他格式的数据
// 临时跳过命令字节检查，直接解析数据
// return;
}
// 解析温度 (DATA[1])
motor->motor.feedback.temp = (int8_t)msg->data[1];
// 解析转矩电流值或功率值 (DATA[2], DATA[3])
int16_t raw_current_or_power = (int16_t)((msg->data[3] << 8) | msg->data[2]);
// 根据电机类型解析电流或功率
switch (motor->param.module) {
case MOTOR_LK_MF9025:
case MOTOR_LK_MF9035:
motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module);
break;
default:
motor->motor.feedback.torque_current = (float)raw_current_or_power;
break;
}
// 解析转速 (DATA[4], DATA[5]) - 单位：1dps/LSB
int16_t raw_speed = (int16_t)((msg->data[5] << 8) | msg->data[4]);
motor->motor.feedback.rotor_speed = motor->param.reverse ? -raw_speed : raw_speed;
// 解析编码器值 (DATA[6], DATA[7])
uint16_t raw_encoder = (uint16_t)((msg->data[7] << 8) | msg->data[6]);
uint16_t encoder_max = MOTOR_LK_GetEncoderMax(motor->param.module);
// 将编码器值转换为弧度 (0 ~ 2π)
float angle = (float)raw_encoder / (float)encoder_max * M_2PI;
motor->motor.feedback.rotor_abs_angle = motor->param.reverse ? (M_2PI - angle) : angle;
}
/* Exported functions ------------------------------------------------------- */
int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
if (MOTOR_LK_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR;
// 检查是否已注册
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
return DEVICE_ERR_INITED;
}
}
// 检查数量
if (manager->motor_count >= MOTOR_LK_MAX_MOTORS) return DEVICE_ERR;
// 创建新电机实例
MOTOR_LK_t *new_motor = (MOTOR_LK_t*)BSP_Malloc(sizeof(MOTOR_LK_t));
if (new_motor == NULL) return DEVICE_ERR;
memcpy(&new_motor->param, param, sizeof(MOTOR_LK_Param_t));
memset(&new_motor->motor, 0, sizeof(MOTOR_t));
new_motor->motor.reverse = param->reverse;
// 对于某些LK电机，反馈数据可能通过命令ID发送
// 根据实际测试，使用命令ID接收反馈数据
uint16_t feedback_id = param->id; // 使用命令ID作为反馈ID
// 注册CAN接收ID
if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
BSP_Free(new_motor);
return DEVICE_ERR;
}
manager->motors[manager->motor_count] = new_motor;
manager->motor_count++;
return DEVICE_OK;
}
int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LK_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
// 对于某些LK电机，反馈数据通过命令ID发送
uint16_t feedback_id = param->id;
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
uint64_t now_time = BSP_TIME_Get();
if (now_time - motor->motor.header.last_online_time > 1000) {
motor->motor.header.online = false;
return DEVICE_ERR_NO_DEV;
}
return DEVICE_ERR;
}
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
MOTOR_LK_Decode(motor, &rx_msg);
return DEVICE_OK;
}
}
return DEVICE_ERR_NO_DEV;
}
int8_t MOTOR_LK_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LK_t *motor = manager->motors[i];
if (motor != NULL) {
if (MOTOR_LK_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
// 限制输出值范围
if (value > 1.0f) value = 1.0f;
if (value < -1.0f) value = -1.0f;
MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param);
if (motor == NULL) return DEVICE_ERR_NO_DEV;
// 根据反转参数调整输出
float output = param->reverse ? -value : value;
// 转矩闭环控制命令 - 将输出值转换为转矩控制值
int16_t torque_control = (int16_t)(output * (float)LK_TORQUE_RANGE);
// 构建CAN帧
BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
tx_frame.data[0] = LK_CMD_TORQUE_CTRL;
tx_frame.data[1] = 0x00;
tx_frame.data[2] = 0x00;
tx_frame.data[3] = 0x00;
tx_frame.data[4] = (uint8_t)(torque_control & 0xFF);
tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
tx_frame.data[6] = 0x00;
tx_frame.data[7] = 0x00;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param) {
// 对于LK电机，每次设置输出时就直接发送控制命令
// 这个函数可以用于发送其他控制命令，如电机开启/关闭
return DEVICE_OK;
}
int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
// 电机运行命令
tx_frame.data[0] = LK_CMD_MOTOR_ON; // 命令字节
tx_frame.data[1] = 0x00;
tx_frame.data[2] = 0x00;
tx_frame.data[3] = 0x00;
tx_frame.data[4] = 0x00;
tx_frame.data[5] = 0x00;
tx_frame.data[6] = 0x00;
tx_frame.data[7] = 0x00;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
// 电机关闭命令
tx_frame.data[0] = LK_CMD_MOTOR_OFF; // 命令字节
tx_frame.data[1] = 0x00;
tx_frame.data[2] = 0x00;
tx_frame.data[3] = 0x00;
tx_frame.data[4] = 0x00;
tx_frame.data[5] = 0x00;
tx_frame.data[6] = 0x00;
tx_frame.data[7] = 0x00;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param) {
if (param == NULL) return NULL;
MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
if (manager == NULL) return NULL;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LK_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
return motor;
}
}
return NULL;
}
int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param) {
return MOTOR_LK_SetOutput(param, 0.0f);
}
int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param) {
MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param);
if (motor) {
motor->motor.header.online = false;
return DEVICE_OK;
}
return DEVICE_ERR_NO_DEV;
}

119
assets/User_code/device/motor_lk.h Normal file
View File

@@ -0,0 +1,119 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define MOTOR_LK_MAX_MOTORS 32
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
MOTOR_LK_MF9025,
MOTOR_LK_MF9035,
} MOTOR_LK_Module_t;
/*每个电机需要的参数*/
typedef struct {
BSP_CAN_t can;
uint16_t id;
MOTOR_LK_Module_t module;
bool reverse;
} MOTOR_LK_Param_t;
/*电机实例*/
typedef struct{
MOTOR_LK_Param_t param;
MOTOR_t motor;
} MOTOR_LK_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct {
BSP_CAN_t can;
MOTOR_LK_t *motors[MOTOR_LK_MAX_MOTORS];
uint8_t motor_count;
} MOTOR_LK_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 注册一个LK电机
* @param param 电机参数
* @return
*/
int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return
*/
int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param);
/**
* @brief 设置一个电机的输出
* @param param 电机参数
* @param value 输出值，范围[-1.0, 1.0]
* @return
*/
int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value);
/**
* @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可
* @param param 电机参数
* @return
*/
int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param);
/**
* @brief 发送电机开启命令
* @param param 电机参数
* @return
*/
int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param);
/**
* @brief 发送电机关闭命令
* @param param 电机参数
* @return
*/
int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return
*/
MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param);
/**
* @brief 使电机松弛（设置输出为0）
* @param param
* @return
*/
int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param);
/**
* @brief 使电机离线（设置在线状态为false）
* @param param
* @return
*/
int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param);
/**
* @brief
* @param
* @return
*/
int8_t MOTOR_LK_UpdateAll(void);
#ifdef __cplusplus
}
#endif

440
assets/User_code/device/motor_lz.c Normal file
View File

@@ -0,0 +1,440 @@
/*
灵足电机驱动
灵足电机通信协议：
- CAN 2.0通信接口，波特率1Mbps
- 采用扩展帧格式(29位ID)
- ID格式：Bit28~24(通信类型) + Bit23~8(数据区2) + Bit7~0(目标地址)
*/
/* Includes ----------------------------------------------------------------- */
#include "motor_lz.h"
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include "bsp/can.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
/* Private define ----------------------------------------------------------- */
// 灵足电机协议参数
#define LZ_ANGLE_RANGE_RAD (12.57f) /* 角度范围 ±12.57 rad */
#define LZ_VELOCITY_RANGE_RAD_S (20.0f) /* 角速度范围 ±20 rad/s */
#define LZ_TORQUE_RANGE_NM (60.0f) /* 力矩范围 ±60 Nm */
#define LZ_KP_MAX (5000.0f) /* Kp最大值 */
#define LZ_KD_MAX (100.0f) /* Kd最大值 */
#define LZ_RAW_VALUE_MAX (65535) /* 16位原始值最大值 */
#define LZ_TEMP_SCALE (10.0f) /* 温度缩放因子 */
#define LZ_MAX_RECOVER_DIFF_RAD (0.28f)
#define MOTOR_TX_BUF_SIZE (8)
#define MOTOR_RX_BUF_SIZE (8)
/* Private macro ------------------------------------------------------------ */
MOTOR_LZ_MotionParam_t lz_relax_param = {
.target_angle = 0.0f,
.target_velocity = 0.0f,
.kp = 0.0f,
.kd = 0.0f,
.torque = 0.0f,
};
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static MOTOR_LZ_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
/* Private function prototypes ---------------------------------------------- */
static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can);
static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can);
static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg);
static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id);
static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value);
static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value);
static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc);
static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
/* Private functions -------------------------------------------------------- */
/**
* @brief 获取CAN管理器
*/
static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return NULL;
return can_managers[can];
}
/**
* @brief 创建CAN管理器
*/
static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (MOTOR_LZ_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LZ_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(MOTOR_LZ_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
/**
* @brief 构建扩展ID
*/
static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id) {
uint32_t ext_id = 0;
ext_id |= ((uint32_t)cmd_type & 0x1F) << 24; // Bit28~24: 通信类型
ext_id |= ((uint32_t)data2 & 0xFFFF) << 8; // Bit23~8: 数据区2
ext_id |= ((uint32_t)target_id & 0xFF); // Bit7~0: 目标地址
return ext_id;
}
/**
* @brief 浮点值转换为原始值（对称范围：-max_value ~ +max_value）
*/
static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value) {
// 限制范围
if (value > max_value) value = max_value;
if (value < -max_value) value = -max_value;
// 转换为0~65535范围，对应-max_value~max_value
return (uint16_t)((value + max_value) / (2.0f * max_value) * (float)LZ_RAW_VALUE_MAX);
}
/**
* @brief 浮点值转换为原始值（单向范围：0 ~ +max_value）
*/
static uint16_t MOTOR_LZ_FloatToRawPositive(float value, float max_value) {
// 限制范围
if (value > max_value) value = max_value;
if (value < 0.0f) value = 0.0f;
// 转换为0~65535范围，对应0~max_value
return (uint16_t)(value / max_value * (float)LZ_RAW_VALUE_MAX);
}
/**
* @brief 原始值转换为浮点值
*/
static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value) {
// 将0~65535范围转换为-max_value~max_value
return ((float)raw_value / (float)LZ_RAW_VALUE_MAX) * (2.0f * max_value) - max_value;
}
/**
* @brief 发送扩展帧
*/
static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc) {
BSP_CAN_ExtDataFrame_t tx_frame;
tx_frame.id = ext_id;
tx_frame.dlc = dlc;
if (data != NULL) {
memcpy(tx_frame.data, data, dlc);
} else {
memset(tx_frame.data, 0, dlc);
}
return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 灵足电机ID解析器
* @param original_id 原始CAN ID（29位扩展帧）
* @param frame_type 帧类型
* @return 解析后的ID（用于队列匹配）
*
* 灵足电机扩展ID格式：
* Bit28~24: 通信类型 (0x1=运控控制, 0x2=反馈数据, 0x3=使能, 0x4=停止, 0x6=设零位)
* Bit23~8: 数据区2 (根据通信类型而定)
* Bit7~0: 目标地址 (目标电机CAN ID)
*/
static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
// 只处理扩展数据帧
if (frame_type != BSP_CAN_FRAME_EXT_DATA) {
return original_id; // 非扩展帧直接返回原始ID
}
// 解析扩展ID各个字段
uint8_t cmd_type = (original_id >> 24) & 0x1F; // Bit28~24: 通信类型
uint16_t data2 = (original_id >> 8) & 0xFFFF; // Bit23~8: 数据区2
uint8_t host_id = (uint8_t)(original_id & 0xFF); // Bit7~0: 主机CAN ID
// 对于反馈数据帧，我们使用特殊的解析规则
if (cmd_type == MOTOR_LZ_CMD_FEEDBACK) {
// 反馈数据的data2字段包含：
// Bit8~15: 当前电机CAN ID
// Bit16~21: 故障信息
// Bit22~23: 模式状态
uint8_t motor_can_id = data2 & 0xFF; // bit8~15: 当前电机CAN ID
// 返回格式化的ID，便于匹配
// 格式：0x02HHMMTT (02=反馈命令, HH=主机ID, MM=电机ID, TT=主机ID)
return (0x02000000) | (host_id << 16) | (motor_can_id << 8) | host_id;
}
// 对于其他命令类型，直接返回原始ID
return original_id;
}
/**
* @brief 解码灵足电机反馈数据
*/
static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) {
if (motor == NULL || msg == NULL) return;
uint8_t cmd_type = (msg->original_id >> 24) & 0x1F;
if (cmd_type != MOTOR_LZ_CMD_FEEDBACK) return;
uint16_t id_data2 = (msg->original_id >> 8) & 0xFFFF;
uint8_t motor_can_id = id_data2 & 0xFF;
uint8_t fault_info = (id_data2 >> 8) & 0x3F;
uint8_t mode_state = (id_data2 >> 14) & 0x03;
motor->lz_feedback.motor_can_id = motor_can_id;
motor->lz_feedback.fault.under_voltage = (fault_info & 0x01) != 0;
motor->lz_feedback.fault.driver_fault = (fault_info & 0x02) != 0;
motor->lz_feedback.fault.over_temp = (fault_info & 0x04) != 0;
motor->lz_feedback.fault.encoder_fault = (fault_info & 0x08) != 0;
motor->lz_feedback.fault.stall_overload = (fault_info & 0x10) != 0;
motor->lz_feedback.fault.uncalibrated = (fault_info & 0x20) != 0;
motor->lz_feedback.state = (MOTOR_LZ_State_t)mode_state;
// 反馈解码并自动反向
uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]);
float angle = MOTOR_LZ_RawToFloat(raw_angle, LZ_ANGLE_RANGE_RAD);
uint16_t raw_velocity = (uint16_t)((msg->data[2] << 8) | msg->data[3]);
float velocity = MOTOR_LZ_RawToFloat(raw_velocity, LZ_VELOCITY_RANGE_RAD_S);
uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]);
float torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM);
while (angle <0){
angle += M_2PI;
}
while (angle > M_2PI){
angle -= M_2PI;
}
// 自动反向
if (motor->param.reverse) {
angle = M_2PI - angle;
velocity = -velocity;
torque = -torque;
}
motor->lz_feedback.current_angle = angle;
motor->lz_feedback.current_velocity = velocity;
motor->lz_feedback.current_torque = torque;
uint16_t raw_temp = (uint16_t)((msg->data[6] << 8) | msg->data[7]);
motor->lz_feedback.temperature = (float)raw_temp / LZ_TEMP_SCALE;
motor->motor.feedback.rotor_abs_angle = angle;
motor->motor.feedback.rotor_speed = velocity;
motor->motor.feedback.torque_current = torque;
motor->motor.feedback.temp = (int8_t)motor->lz_feedback.temperature;
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
}
/* Exported functions ------------------------------------------------------- */
/**
* @brief 初始化灵足电机驱动系统
* @return 设备状态码
*/
int8_t MOTOR_LZ_Init(void) {
// 注册灵足电机专用的ID解析器
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;
if (MOTOR_LZ_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR;
// 检查是否已注册
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.motor_id == param->motor_id) {
return DEVICE_ERR; // 已注册
}
}
// 检查数量
if (manager->motor_count >= MOTOR_LZ_MAX_MOTORS) return DEVICE_ERR;
// 创建新电机实例
MOTOR_LZ_t *new_motor = (MOTOR_LZ_t*)BSP_Malloc(sizeof(MOTOR_LZ_t));
if (new_motor == NULL) return DEVICE_ERR;
memcpy(&new_motor->param, param, sizeof(MOTOR_LZ_Param_t));
memset(&new_motor->motor, 0, sizeof(MOTOR_t));
memset(&new_motor->lz_feedback, 0, sizeof(MOTOR_LZ_Feedback_t));
memset(&new_motor->motion_param, 0, sizeof(MOTOR_LZ_MotionParam_t));
new_motor->motor.reverse = param->reverse;
// 注册CAN接收ID - 使用解析后的反馈数据ID
// 构建反馈数据的原始扩展ID
// 反馈数据：data2包含电机ID(bit8~15)，target_id是主机ID
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
// 通过ID解析器得到解析后的ID
uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
if (BSP_CAN_RegisterId(param->can, parsed_feedback_id, 3) != BSP_OK) {
BSP_Free(new_motor);
return DEVICE_ERR;
}
manager->motors[manager->motor_count] = new_motor;
manager->motor_count++;
return DEVICE_OK;
}
int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LZ_t *motor = manager->motors[i];
if (motor && motor->param.motor_id == param->motor_id) {
// 获取反馈数据 - 使用解析后的ID
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
BSP_CAN_Message_t msg;
while (BSP_CAN_GetMessage(param->can, parsed_feedback_id, &msg, 0) == BSP_OK) {
MOTOR_LZ_Decode(motor, &msg);
}
return DEVICE_OK;
}
}
return DEVICE_ERR_NO_DEV;
}
int8_t MOTOR_LZ_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LZ_t *motor = manager->motors[i];
if (motor) {
if (MOTOR_LZ_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param) {
if (param == NULL || motion_param == NULL) return DEVICE_ERR_NULL;
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
if (motor == NULL) return DEVICE_ERR_NO_DEV;
// 自动反向控制
MOTOR_LZ_MotionParam_t send_param = *motion_param;
if (param->reverse) {
send_param.target_angle = -send_param.target_angle;
send_param.target_velocity = -send_param.target_velocity;
send_param.torque = -send_param.torque;
}
memcpy(&motor->motion_param, motion_param, sizeof(MOTOR_LZ_MotionParam_t));
uint16_t raw_torque = MOTOR_LZ_FloatToRaw(send_param.torque, LZ_TORQUE_RANGE_NM);
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, raw_torque, param->motor_id);
uint8_t data[8];
uint16_t raw_angle = MOTOR_LZ_FloatToRaw(send_param.target_angle, LZ_ANGLE_RANGE_RAD);
data[0] = (raw_angle >> 8) & 0xFF;
data[1] = raw_angle & 0xFF;
uint16_t raw_velocity = MOTOR_LZ_FloatToRaw(send_param.target_velocity, LZ_VELOCITY_RANGE_RAD_S);
data[2] = (raw_velocity >> 8) & 0xFF;
data[3] = raw_velocity & 0xFF;
uint16_t raw_kp = MOTOR_LZ_FloatToRawPositive(send_param.kp, LZ_KP_MAX);
data[4] = (raw_kp >> 8) & 0xFF;
data[5] = raw_kp & 0xFF;
uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(send_param.kd, LZ_KD_MAX);
data[6] = (raw_kd >> 8) & 0xFF;
data[7] = raw_kd & 0xFF;
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
// 构建扩展ID - 使能命令
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_ENABLE, param->host_id, param->motor_id);
// 数据区清零
uint8_t data[8] = {0};
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault) {
if (param == NULL) return DEVICE_ERR_NULL;
// 构建扩展ID - 停止命令
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_DISABLE, param->host_id, param->motor_id);
// 数据区
uint8_t data[8] = {0};
if (clear_fault) {
data[0] = 1; // Byte[0]=1时清故障
}
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
// 构建扩展ID - 设置零位命令
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_SET_ZERO, param->host_id, param->motor_id);
// 数据区 - Byte[0]=1
uint8_t data[8] = {1, 0, 0, 0, 0, 0, 0, 0};
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param) {
if (param == NULL) return NULL;
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
if (manager == NULL) return NULL;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LZ_t *motor = manager->motors[i];
if (motor && motor->param.motor_id == param->motor_id) {
return motor;
}
}
return NULL;
}
int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param) {
return MOTOR_LZ_MotionControl(param, &lz_relax_param);
}
int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param) {
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
if (motor) {
motor->motor.header.online = false;
return DEVICE_OK;
}
return DEVICE_ERR_NO_DEV;
}
static MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) {
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
if (motor && motor->motor.header.online) {
return &motor->lz_feedback;
}
return NULL;
}

212
assets/User_code/device/motor_lz.h Normal file
View File

@@ -0,0 +1,212 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define MOTOR_LZ_MAX_MOTORS 32
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
MOTOR_LZ_RSO0,
MOTOR_LZ_RSO1,
MOTOR_LZ_RSO2,
MOTOR_LZ_RSO3,
MOTOR_LZ_RSO4,
MOTOR_LZ_RSO5,
MOTOR_LZ_RSO6,
} MOTOR_LZ_Module_t;
/* 灵足电机控制模式 */
typedef enum {
MOTOR_LZ_MODE_MOTION = 0x1, /* 运控模式 */
MOTOR_LZ_MODE_CURRENT = 0x2, /* 电流模式 */
MOTOR_LZ_MODE_VELOCITY = 0x3, /* 速度模式 */
MOTOR_LZ_MODE_POSITION = 0x4, /* 位置模式 */
} MOTOR_LZ_ControlMode_t;
/* 灵足电机通信类型 */
typedef enum {
MOTOR_LZ_CMD_MOTION = 0x1, /* 运控模式控制 */
MOTOR_LZ_CMD_FEEDBACK = 0x2, /* 电机反馈数据 */
MOTOR_LZ_CMD_ENABLE = 0x3, /* 电机使能运行 */
MOTOR_LZ_CMD_DISABLE = 0x4, /* 电机停止运行 */
MOTOR_LZ_CMD_SET_ZERO = 0x6, /* 设置电机机械零位 */
} MOTOR_LZ_CmdType_t;
/* 灵足电机运行状态 */
typedef enum {
MOTOR_LZ_STATE_RESET = 0, /* Reset模式[复位] */
MOTOR_LZ_STATE_CALI = 1, /* Cali模式[标定] */
MOTOR_LZ_STATE_MOTOR = 2, /* Motor模式[运行] */
} MOTOR_LZ_State_t;
/* 灵足电机故障信息 */
typedef struct {
bool uncalibrated; /* bit21: 未标定 */
bool stall_overload; /* bit20: 堵转过载故障 */
bool encoder_fault; /* bit19: 磁编码故障 */
bool over_temp; /* bit18: 过温 */
bool driver_fault; /* bit17: 驱动故障 */
bool under_voltage; /* bit16: 欠压故障 */
} MOTOR_LZ_Fault_t;
/* 灵足电机运控参数 */
typedef struct {
float target_angle; /* 目标角度 (-12.57f~12.57f rad) */
float target_velocity; /* 目标角速度 (-20~20 rad/s) */
float kp; /* 位置增益 (0.0~5000.0) */
float kd; /* 微分增益 (0.0~100.0) */
float torque; /* 力矩 (-60~60 Nm) */
} MOTOR_LZ_MotionParam_t;
/*每个电机需要的参数*/
typedef struct {
BSP_CAN_t can; /* CAN总线 */
uint8_t motor_id; /* 电机CAN ID */
uint8_t host_id; /* 主机CAN ID */
MOTOR_LZ_Module_t module; /* 电机型号 */
bool reverse; /* 是否反向 */
MOTOR_LZ_ControlMode_t mode; /* 控制模式 */
} MOTOR_LZ_Param_t;
/*电机反馈信息扩展*/
typedef struct {
float current_angle; /* 当前角度 (-12.57f~12.57f rad) */
float current_velocity; /* 当前角速度 (-20~20 rad/s) */
float current_torque; /* 当前力矩 (-60~60 Nm) */
float temperature; /* 当前温度 (摄氏度) */
MOTOR_LZ_State_t state; /* 运行状态 */
MOTOR_LZ_Fault_t fault; /* 故障信息 */
uint8_t motor_can_id; /* 当前电机CAN ID */
} MOTOR_LZ_Feedback_t;
/*电机实例*/
typedef struct {
MOTOR_LZ_Param_t param;
MOTOR_t motor;
MOTOR_LZ_Feedback_t lz_feedback; /* 灵足电机特有反馈信息 */
MOTOR_LZ_MotionParam_t motion_param; /* 运控模式参数 */
} MOTOR_LZ_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct {
BSP_CAN_t can;
MOTOR_LZ_t *motors[MOTOR_LZ_MAX_MOTORS];
uint8_t motor_count;
} MOTOR_LZ_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化灵足电机驱动系统
* @return 设备状态码
*/
int8_t MOTOR_LZ_Init(void);
/**
* @brief 注册一个灵足电机
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param);
/**
* @brief 更新所有电机数据
* @return 设备状态码
*/
int8_t MOTOR_LZ_UpdateAll(void);
/**
* @brief 运控模式控制电机
* @param param 电机参数
* @param motion_param 运控参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param);
/**
* @brief 电流(力矩)模式控制电机
* @param param 电机参数
* @param torque 目标力矩 (-60~60 Nm)
* @return 设备状态码
*/
int8_t MOTOR_LZ_TorqueControl(MOTOR_LZ_Param_t *param, float torque);
/**
* @brief 位置模式控制电机
* @param param 电机参数
* @param target_angle 目标角度 (-12.57~12.57 rad)
* @param max_velocity 最大速度 (0~20 rad/s)
* @return 设备状态码
*/
int8_t MOTOR_LZ_PositionControl(MOTOR_LZ_Param_t *param, float target_angle, float max_velocity);
/**
* @brief 速度模式控制电机
* @param param 电机参数
* @param target_velocity 目标速度 (-20~20 rad/s)
* @return 设备状态码
*/
int8_t MOTOR_LZ_VelocityControl(MOTOR_LZ_Param_t *param, float target_velocity);
/**
* @brief 电机使能运行
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param);
/**
* @brief 电机停止运行
* @param param 电机参数
* @param clear_fault 是否清除故障
* @return 设备状态码
*/
int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault);
/**
* @brief 设置电机机械零位
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return 电机实例指针，失败返回NULL
*/
MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param);
/**
* @brief 使电机松弛（发送停止命令）
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param);
/**
* @brief 使电机离线（设置在线状态为false）
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param);
#ifdef __cplusplus
}
#endif

327
assets/User_code/device/motor_odrive.c Normal file
View File

@@ -0,0 +1,327 @@
/*
Odrive电机驱动
*/
/* Includes ----------------------------------------------------------------- */
#include "motor_odrive.h"
#include <stdbool.h>
#include <string.h>
#include "bsp/can.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static ODrive_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
// 获取指定CAN总线的电机管理器指针
static ODrive_CANManager_t* MOTOR_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return NULL;
return can_managers[can];
}
// 为指定CAN总线创建电机管理器
static int8_t MOTOR_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (ODrive_CANManager_t*)BSP_Malloc(sizeof(ODrive_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(ODrive_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
// 解析CAN报文，更新电机反馈信息
static void Motor_Decode(ODrive_t *motor, BSP_CAN_Message_t *msg)
{
uint8_t axis_id = (msg->original_id >> 5) & 0x3F; // 提取电机号（0~63）
uint8_t cmd_id = msg->original_id & 0x1F; // 提取命令 ID（低 5 位）
motor->param.id = axis_id; // 保存电机 ID
// 解析帧类型（数据帧或远程帧）
if (msg->frame_type == BSP_CAN_FRAME_STD_DATA) {
// 数据帧处理
switch (cmd_id)
{
case ODRIVE_HEARTBEAT_MESSAGE: // 0x001 ODrive心跳消息
// motor->motor.feedback.axis_error = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24);
// motor->motor.feedback.axis_state = msg->data[4];
// motor->motor.feedback.controller_status = msg->data[5];
break;
case ENCODER_ESTIMATES: // 0x009
{
uint32_t raw_pos = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24);
uint32_t raw_vel = (msg->data[4] | msg->data[5]<<8 | msg->data[6]<<16 | msg->data[7]<<24);
memcpy(&motor->motor.feedback.rotor_abs_angle, &raw_pos, sizeof(float));
memcpy(&motor->motor.feedback.rotor_speed, &raw_vel, sizeof(float));
}
break;
case GET_ENCODER_COUNT: // 0x014
// motor->motor.feedback.encoder_shadow = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24);
// motor->motor.feedback.encoder_cpr = (msg->data[4] | msg->data[5]<<8 | msg->data[6]<<16 | msg->data[7]<<24);
break;
case GET_BUS_VOLTAGE_CURRENT: // 0x017
{
uint32_t raw_vbus, raw_ibus;
raw_vbus = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24);
raw_ibus = (msg->data[4] | msg->data[5]<<8 | msg->data[6]<<16 | msg->data[7]<<24);
// memcpy(&motor->motor.feedback.bus_voltage, &raw_vbus, sizeof(float));
memcpy(&motor->motor.feedback.torque_current, &raw_ibus, sizeof(float));
}
break;
case GET_IQ: // 0x018
{
uint32_t raw_iq_set, raw_iq_meas;
raw_iq_set = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24);
raw_iq_meas = (msg->data[4] | msg->data[5]<<8 | msg->data[6]<<16 | msg->data[7]<<24);
// memcpy(&motor->motor.feedback.iq_setpoint, &raw_iq_set, sizeof(float));
// memcpy(&motor->motor.feedback.iq_measured, &raw_iq_meas, sizeof(float));
}
break;
default:
break;
}
}
}
/* Exported functions ------------------------------------------------------- */
// 注册一个新的电机实例到管理器
int8_t ODrive_Register(ODrive_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
if (MOTOR_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
ODrive_CANManager_t *manager = MOTOR_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR;
// 检查是否已注册
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
return DEVICE_ERR_INITED;
}
}
// 检查数量
if (manager->motor_count >= ODRIVE_MAX_MOTORS) return DEVICE_ERR;
// 创建新电机实例
ODrive_t *new_motor = (ODrive_t*)BSP_Malloc(sizeof(ODrive_t));
if (new_motor == NULL) return DEVICE_ERR;
memcpy(&new_motor->param, param, sizeof(ODrive_Param_t));
memset(&new_motor->motor, 0, sizeof(MOTOR_t));
new_motor->motor.reverse = param->reverse;
// 注册CAN接收ID
if (BSP_CAN_RegisterId(param->can, param->id, 3) != BSP_OK) {
BSP_Free(new_motor);
return DEVICE_ERR;
}
manager->motors[manager->motor_count] = new_motor;
manager->motor_count++;
return DEVICE_OK;
}
// 更新指定电机的反馈数据
int8_t ODrive_Update(ODrive_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
ODrive_CANManager_t *manager = MOTOR_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
for (int i = 0; i < manager->motor_count; i++) {
ODrive_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, param->id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
uint64_t now_time = BSP_TIME_Get();
if (now_time - motor->motor.header.last_online_time > 1000) {
motor->motor.header.online = false;
return DEVICE_ERR_NO_DEV;
}
return DEVICE_ERR;
}
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
Motor_Decode(motor, &rx_msg);
return DEVICE_OK;
}
}
return DEVICE_ERR_NO_DEV;
}
// 更新所有CAN总线下所有电机的反馈数据
int8_t ODrive_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
ODrive_CANManager_t *manager = MOTOR_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
ODrive_t *motor = manager->motors[i];
if (motor != NULL) {
if (ODrive_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
// 获取指定参数对应的电机实例指针
ODrive_t* ODrive_GetMotor(ODrive_Param_t *param) {
if (param == NULL) return NULL;
ODrive_CANManager_t *manager = MOTOR_GetCANManager(param->can);
if (manager == NULL) return NULL;
for (int i = 0; i < manager->motor_count; i++) {
ODrive_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
return motor;
}
}
return NULL;
}
// 设置指定电机的输出值
int8_t ODrive_SetOutput(ODrive_Param_t *param, float value) {
if (param == NULL) return DEVICE_ERR_NULL;
// 如果电机反转标志为 true，则反向值
if (param->reverse) {
value = -value;
}
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t command_id;
uint8_t *pVal = (uint8_t *)&value;
// 选择命令 ID 和数据打包方式
switch (param->mode) {
case POSITION_CONTROL: {
command_id = SET_INPUT_POS;
float pos = value;
int16_t vel_ff = 0; // 可扩展为参数传入 0就行
int16_t torque_ff = 0; // 可扩展为参数传入 0就行
uint8_t *pPos = (uint8_t *)&pos;
uint8_t *pVel = (uint8_t *)&vel_ff;
uint8_t *pTor = (uint8_t *)&torque_ff;
memcpy(&tx_frame.data[0], pPos, 4);
memcpy(&tx_frame.data[4], pVel, 2);
memcpy(&tx_frame.data[6], pTor, 2);
tx_frame.dlc = 8;
break;
}
case VELOCITY_CONTROL: {
command_id = SET_INPUT_VEL;
float vel = value;
float torque_ff = 0.0f; // 可扩展为参数传入
uint8_t *pVel = (uint8_t *)&vel;
uint8_t *pTor = (uint8_t *)&torque_ff;
memcpy(&tx_frame.data[0], pVel, 4);
memcpy(&tx_frame.data[4], pTor, 4);
tx_frame.dlc = 8;
break;
}
case TORQUE_CONTROL: {
command_id = SET_INPUT_TORQUE;
memcpy(&tx_frame.data[0], pVal, 4);
tx_frame.dlc = 4;
break;
}
case VOLTAGE_CONTROL:
default:
return DEVICE_ERR; // 暂不支持电压模式
}
// 组装 CAN ID（标准帧）
tx_frame.id = (param->id << 5) | command_id;
// 标准数据帧
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
// 设置加速度和减速度
int8_t ODrive_SetAccel(ODrive_Param_t *param, float accel, float decel) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t command_id = SET_TRAJ_ACCEL_LIMITS;
uint8_t *pAccel = (uint8_t *)&accel;
uint8_t *pDecel = (uint8_t *)&decel;
memcpy(&tx_frame.data[0], pAccel, 4);
memcpy(&tx_frame.data[4], pDecel, 4);
tx_frame.dlc = 8;
tx_frame.id = (param->id << 5) | command_id;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
// 获取位置和速度反馈
int8_t ODrive_RequestEncoderEstimates(ODrive_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t command_id = ENCODER_ESTIMATES; // 请求编码器估计值命令
uint8_t zero_data[8] = {0}; // 发送全 0 数据（ODrive 协议要求）
memcpy(tx_frame.data, zero_data, 8);
tx_frame.dlc = 8;
tx_frame.id = (param->id << 5) | command_id;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
// 设置轴请求状态（一般用来重启 ODrive 的某个轴）
// ODrive_SetAxisRequestedState(odrive_axis[0], CLOSED_LOOP_CONTROL);
int8_t ODrive_SetAxisRequestedState(ODrive_Param_t *param, Axis_State state) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t command_id = SET_AXIS_REQUESTED_STATE;
// 将 state 转为 4 字节
memcpy(tx_frame.data, &state, 4);
memset(&tx_frame.data[4], 0, 4);
tx_frame.dlc = 4;
// 组装 CAN ID
tx_frame.id = (param->id << 5) | command_id;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
// 清除错误
int8_t ODrive_ClearErrors(ODrive_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t command_id = CLEAR_ERRORS;
memset(tx_frame.data, 0, 8);
tx_frame.dlc = 0;
tx_frame.id = (param->id << 5) | command_id;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
// 重启 ODrive
int8_t ODrive_Reboot(ODrive_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t command_id = REBOOT_ODRIVE;
memset(tx_frame.data, 0, 8);
tx_frame.dlc = 0;
tx_frame.id = (param->id << 5) | command_id;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}

162
assets/User_code/device/motor_odrive.h Normal file
View File

@@ -0,0 +1,162 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Private define ----------------------------------------------------------- */
//ODrive型号根据实际情况调整
#define ODRIVE_MAX_MOTORS 2
//COMMAND ID
#define ODRIVE_HEARTBEAT_MESSAGE 0x001 // ODrive心跳消息
#define SET_AXIS_NODE_ID 0x006 // 设置电机节点ID
#define GET_ENCODER_ESTIMATES 0x008 // 获取编码器估计值
#define GET_ENCODER_COUNT 0x00A // 获取编码器计数
#define SET_AXIS_REQUESTED_STATE 0x007 // 设置电机请求状态
#define ENCODER_ESTIMATES 0x009 // 编码器估计值
#define GET_ENCODER_COUNT 0x00A // 获取编码器计数
#define SET_CONTROLLER_MODES 0x00B // 设置控制器模式
#define SET_INPUT_POS 0x00C // 设置输入位置
#define SET_INPUT_VEL 0x00D // 设置输入速度
#define SET_INPUT_TORQUE 0x00E // 设置输入转矩
#define SET_LIMITS 0x00F // 设置限制
#define GET_IQ 0x014 // 获取电流
#define REBOOT_ODRIVE 0x016 // 重启ODrive
#define GET_BUS_VOLTAGE_CURRENT 0x017 // 获取总线电压和电流
#define CLEAR_ERRORS 0x018 // 清除错误
#define SET_POSITION_GAIN 0x01A // 设置位置增益
#define SET_VEL_GAINS 0x01B // 设置速度增益
#define SET_TRAJ_ACCEL_LIMITS 0x012 // 设置轨迹加速度限制
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
//Axis States
typedef enum {
UNDEFINED = 0x0,
IDLE = 0x1,
STARTUP_SEQUENCE = 0x2,
FULL_CALIBRATION_SEQUENCE = 0x3,
MOTOR_CALIBRATION = 0x4,
ENCODER_INDEX_SEARCH = 0x6,
ENCODER_OFFSET_CALIBRATION = 0x7,
CLOSED_LOOP_CONTROL = 0x8,
LOCKIN_SPIN = 0x9,
ENCODER_DIR_FIND = 0xA,
HOMING = 0xB,
ENCODER_HALL_POLARITY_CALIBRATION = 0xC,
ENCODER_HALL_PHASE_CALIBRATION = 0xD
} Axis_State;
//Control Modes
typedef enum{
VOLTAGE_CONTROL = 0x0,
TORQUE_CONTROL = 0x1,
VELOCITY_CONTROL = 0x2,
POSITION_CONTROL = 0x3
} Control_Mode;
/*每个电机需要的参数*/
typedef struct {
BSP_CAN_t can;
uint16_t id;
uint16_t mode;
bool reverse;
} ODrive_Param_t;
/*电机实例*/
typedef struct ODrive_t {
ODrive_Param_t param;
MOTOR_t motor;
} ODrive_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct {
BSP_CAN_t can;
ODrive_t *motors[ODRIVE_MAX_MOTORS];
uint8_t motor_count;
} ODrive_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 注册一个odrive电机
* @param param 电机参数
* @return
*/
int8_t ODrive_Register(ODrive_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return
*/
int8_t ODrive_Update(ODrive_Param_t *param);
/** * @brief 更新所有ODrive电机状态
* @return
*/
int8_t ODrive_UpdateAll(void);
/**
* @brief 设置一个电机的输出
* @param param 电机参数
* @param value 输出值
* @return
*/
int8_t ODrive_SetOutput(ODrive_Param_t *param, float value);
/** * @brief 设置电机加速度和减速度限制
* @param param 电机参数
* @param accel 加速度
* @param decel 减速度
* @return
*/
int8_t ODrive_SetAccel(ODrive_Param_t *param, float accel, float decel);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return
*/
ODrive_t* ODrive_GetMotor(ODrive_Param_t *param);
/** * @brief 获取指定电机的编码器估计值
* @param param 电机参数
* @return
*/
int8_t ODrive_RequestEncoderEstimates(ODrive_Param_t *param);
/** * @brief 设置轴请求状态（一般用来重启 ODrive 的某个轴）
* @param param 电机参数
* @return
*/
int8_t ODrive_SetAxisRequestedState(ODrive_Param_t *param, Axis_State state);
/** * @brief 清除错误
* @param param 电机参数
* @return
*/
int8_t ODrive_ClearErrors(ODrive_Param_t *param);
/** * @brief 重启 ODrive
* @param param 电机参数
* @return
*/
int8_t ODrive_Reboot(ODrive_Param_t *param);
#ifdef __cplusplus
}
#endif

321
assets/User_code/device/motor_rm.c Normal file
View File

@@ -0,0 +1,321 @@
/*
RM电机驱动
*/
/* Includes ----------------------------------------------------------------- */
#include "motor_rm.h"
#include <stdbool.h>
#include <string.h>
#include "bsp/can.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */
#define GM6020_FB_ID_BASE (0x205)
#define GM6020_CTRL_ID_BASE (0x1ff)
#define GM6020_CTRL_ID_EXTAND (0x2ff)
#define M3508_M2006_FB_ID_BASE (0x201)
#define M3508_M2006_CTRL_ID_BASE (0x200)
#define M3508_M2006_CTRL_ID_EXTAND (0x1ff)
#define M3508_M2006_ID_SETTING_ID (0x700)
#define GM6020_MAX_ABS_LSB (30000)
#define M3508_MAX_ABS_LSB (16384)
#define M2006_MAX_ABS_LSB (10000)
#define MOTOR_TX_BUF_SIZE (8)
#define MOTOR_RX_BUF_SIZE (8)
#define MOTOR_ENC_RES (8192) /* 电机编码器分辨率 */
#define MOTOR_CUR_RES (16384) /* 电机转矩电流分辨率 */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */
static MOTOR_RM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
/* Private function -------------------------------------------------------- */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module) {
switch (module) {
case MOTOR_M2006:
case MOTOR_M3508:
if (can_id >= M3508_M2006_FB_ID_BASE && can_id < M3508_M2006_FB_ID_BASE + 7) {
return can_id - M3508_M2006_FB_ID_BASE;
}
break;
case MOTOR_GM6020:
if (can_id >= GM6020_FB_ID_BASE && can_id < GM6020_FB_ID_BASE + 6) {
return can_id - GM6020_FB_ID_BASE + 4;
}
break;
default:
break;
}
return DEVICE_ERR;
}
static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) {
switch (module) {
case MOTOR_M2006: return 36.0f;
case MOTOR_M3508: return 3591.0f / 187.0f;
case MOTOR_GM6020: return 1.0f;
default: return 1.0f;
}
}
static int16_t MOTOR_RM_GetLSB(MOTOR_RM_Module_t module) {
switch (module) {
case MOTOR_M2006: return M2006_MAX_ABS_LSB;
case MOTOR_M3508: return M3508_MAX_ABS_LSB;
case MOTOR_GM6020: return GM6020_MAX_ABS_LSB;
default: return DEVICE_ERR;
}
}
static MOTOR_RM_CANManager_t* MOTOR_RM_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return NULL;
return can_managers[can];
}
static int8_t MOTOR_RM_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (MOTOR_RM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_RM_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(MOTOR_RM_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) {
uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]);
int16_t raw_speed = (int16_t)((msg->data[2] << 8) | msg->data[3]);
int16_t raw_current = (int16_t)((msg->data[4] << 8) | msg->data[5]);
int16_t lsb = MOTOR_RM_GetLSB(motor->param.module);
float ratio = MOTOR_RM_GetRatio(motor->param.module);
float rotor_angle = raw_angle / (float)MOTOR_ENC_RES * M_2PI;
float rotor_speed = raw_speed;
float torque_current = raw_current * lsb / (float)MOTOR_CUR_RES;
if (motor->param.gear) {
// 多圈累加
int32_t delta = (int32_t)raw_angle - (int32_t)motor->last_raw_angle;
if (delta > (MOTOR_ENC_RES / 2)) {
motor->gearbox_round_count--;
} else if (delta < -(MOTOR_ENC_RES / 2)) {
motor->gearbox_round_count++;
}
motor->last_raw_angle = raw_angle;
float single_turn = rotor_angle;
motor->gearbox_total_angle = (motor->gearbox_round_count * M_2PI + single_turn) / ratio;
// 输出轴多圈绝对值
motor->feedback.rotor_abs_angle = motor->gearbox_total_angle;
motor->feedback.rotor_speed = rotor_speed / ratio;
motor->feedback.torque_current = torque_current * ratio;
} else {
// 非gear模式，直接用转子单圈
motor->gearbox_round_count = 0;
motor->last_raw_angle = raw_angle;
motor->gearbox_total_angle = 0.0f;
motor->feedback.rotor_abs_angle = rotor_angle;
motor->feedback.rotor_speed = rotor_speed;
motor->feedback.torque_current = torque_current;
}
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;
}
motor->feedback.temp = msg->data[6];
}
/* Exported functions ------------------------------------------------------- */
int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
if (MOTOR_RM_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR;
// 检查是否已注册
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
return DEVICE_ERR_INITED;
}
}
// 检查数量
if (manager->motor_count >= MOTOR_RM_MAX_MOTORS) return DEVICE_ERR;
// 创建新电机实例
MOTOR_RM_t *new_motor = (MOTOR_RM_t*)BSP_Malloc(sizeof(MOTOR_RM_t));
if (new_motor == NULL) return DEVICE_ERR;
memcpy(&new_motor->param, param, sizeof(MOTOR_RM_Param_t));
memset(&new_motor->motor, 0, sizeof(MOTOR_t));
new_motor->motor.reverse = param->reverse;
// 注册CAN接收ID
if (BSP_CAN_RegisterId(param->can, param->id, 3) != BSP_OK) {
BSP_Free(new_motor);
return DEVICE_ERR;
}
manager->motors[manager->motor_count] = new_motor;
manager->motor_count++;
return DEVICE_OK;
}
int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_RM_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, param->id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
uint64_t now_time = BSP_TIME_Get();
if (now_time - motor->motor.header.last_online_time > 1000) {
motor->motor.header.online = false;
return DEVICE_ERR_NO_DEV;
}
return DEVICE_ERR;
}
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
Motor_RM_Decode(motor, &rx_msg);
motor->motor.feedback = motor->feedback;
return DEVICE_OK;
}
}
return DEVICE_ERR_NO_DEV;
}
int8_t MOTOR_RM_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_RM_t *motor = manager->motors[i];
if (motor != NULL) {
if (MOTOR_RM_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
if (value > 1.0f) value = 1.0f;
if (value < -1.0f) value = -1.0f;
if (param->reverse){
value = -value;
}
MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param);
if (motor == NULL) return DEVICE_ERR_NO_DEV;
int8_t logical_index = MOTOR_RM_GetLogicalIndex(param->id, param->module);
if (logical_index < 0) return DEVICE_ERR;
MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg;
int16_t output_value = (int16_t)(value * (float)MOTOR_RM_GetLSB(param->module));
output_msg->output[logical_index] = output_value;
return DEVICE_OK;
}
int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg;
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t id = param->id;
switch (id) {
case M3508_M2006_FB_ID_BASE:
case M3508_M2006_FB_ID_BASE+1:
case M3508_M2006_FB_ID_BASE+2:
case M3508_M2006_FB_ID_BASE+3:
tx_frame.id = M3508_M2006_CTRL_ID_BASE;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
for (int i = 0; i < 4; i++) {
tx_frame.data[i*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF);
tx_frame.data[i*2+1] = (uint8_t)(output_msg->output[i] & 0xFF);
}
break;
case M3508_M2006_FB_ID_BASE+4:
case M3508_M2006_FB_ID_BASE+5:
case M3508_M2006_FB_ID_BASE+6:
case M3508_M2006_FB_ID_BASE+7:
tx_frame.id = M3508_M2006_CTRL_ID_EXTAND;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
for (int i = 4; i < 8; i++) {
tx_frame.data[(i-4)*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF);
tx_frame.data[(i-4)*2+1] = (uint8_t)(output_msg->output[i] & 0xFF);
}
break;
case GM6020_FB_ID_BASE+4:
case GM6020_FB_ID_BASE+5:
case GM6020_FB_ID_BASE+6:
tx_frame.id = GM6020_CTRL_ID_EXTAND;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
for (int i = 8; i < 11; i++) {
tx_frame.data[(i-8)*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF);
tx_frame.data[(i-8)*2+1] = (uint8_t)(output_msg->output[i] & 0xFF);
}
tx_frame.data[6] = 0;
tx_frame.data[7] = 0;
break;
default:
return DEVICE_ERR;
}
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
MOTOR_RM_t* MOTOR_RM_GetMotor(MOTOR_RM_Param_t *param) {
if (param == NULL) return NULL;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return NULL;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_RM_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
return motor;
}
}
return NULL;
}
int8_t MOTOR_RM_Relax(MOTOR_RM_Param_t *param) {
return MOTOR_RM_SetOutput(param, 0.0f);
}
int8_t MOTOR_RM_Offine(MOTOR_RM_Param_t *param) {
MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param);
if (motor) {
motor->motor.header.online = false;
return DEVICE_OK;
}
return DEVICE_ERR_NO_DEV;
}

132
assets/User_code/device/motor_rm.h Normal file
View File

@@ -0,0 +1,132 @@
#pragma once
#include "motor.h"
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define MOTOR_RM_MAX_MOTORS 11
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
MOTOR_M2006,
MOTOR_M3508,
MOTOR_GM6020,
} MOTOR_RM_Module_t;
/*一个can最多控制11个电机*/
typedef union {
int16_t output[MOTOR_RM_MAX_MOTORS];
struct {
int16_t m3508_m2006_id201;
int16_t m3508_m2006_id202;
int16_t m3508_m2006_id203;
int16_t m3508_m2006_id204;
int16_t m3508_m2006_gm6020_id205;
int16_t m3508_m2006_gm6020_id206;
int16_t m3508_m2006_gm6020_id207;
int16_t m3508_m2006_gm6020_id208;
int16_t gm6020_id209;
int16_t gm6020_id20A;
int16_t gm6020_id20B;
} named;
} MOTOR_RM_MsgOutput_t;
/*每个电机需要的参数*/
typedef struct {
BSP_CAN_t can;
uint16_t id;
MOTOR_RM_Module_t module;
bool reverse;
bool gear;
} MOTOR_RM_Param_t;
typedef MOTOR_Feedback_t MOTOR_RM_Feedback_t;
typedef struct {
MOTOR_RM_Param_t param;
MOTOR_RM_Feedback_t feedback;
MOTOR_t motor;
// 多圈相关变量，仅gear模式下有效
uint16_t last_raw_angle;
int32_t gearbox_round_count;
float gearbox_total_angle;
} MOTOR_RM_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct {
BSP_CAN_t can;
MOTOR_RM_MsgOutput_t output_msg;
MOTOR_RM_t *motors[MOTOR_RM_MAX_MOTORS];
uint8_t motor_count;
} MOTOR_RM_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 注册一个RM电机
* @param param 电机参数
* @return
*/
int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return
*/
int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param);
/**
* @brief 设置一个电机的输出
* @param param 电机参数
* @param value 输出值，范围[-1.0, 1.0]
* @return
*/
int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value);
/**
* @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可
* @param param 电机参数
* @return
*/
int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return
*/
MOTOR_RM_t* MOTOR_RM_GetMotor(MOTOR_RM_Param_t *param);
/**
* @brief 使电机松弛（设置输出为0）
* @param param
* @return
*/
int8_t MOTOR_RM_Relax(MOTOR_RM_Param_t *param);
/**
* @brief 使电机离线（设置在线状态为false）
* @param param
* @return
*/
int8_t MOTOR_RM_Offine(MOTOR_RM_Param_t *param);
/**
* @brief
* @param
* @return
*/
int8_t MOTOR_RM_UpdateAll(void);
#ifdef __cplusplus
}
#endif

249
assets/User_code/device/motor_vesc.c Normal file
View File

@@ -0,0 +1,249 @@
/*
VESC电机驱动
*/
/* Includes ----------------------------------------------------------------- */
#include "motor_vesc.h"
#include <stdbool.h>
#include <string.h>
#include "bsp/can.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/**************************************
* 限幅函数
**************************************/
void assert_param_duty(float *duty){
// 如果 duty 是 -1.0 ~ 1.0，则最大值用 wtrcfg_VESC_COMMAND_DUTY_MAX / 100
float max_duty = wtrcfg_VESC_COMMAND_DUTY_MAX / 100.0f;
if (fabsf(*duty) > max_duty) {
*duty = (*duty > 0) ? max_duty : -max_duty;
}
}
void assert_param_current(float *current){
if( fabsf(*current) > wtrcfg_VESC_COMMAND_CURRENT_MAX )
*current = *current > 0 ? wtrcfg_VESC_COMMAND_CURRENT_MAX : - wtrcfg_VESC_COMMAND_CURRENT_MAX ;
}
void assert_param_rpm(float *rpm){
if( fabsf(*rpm) > wtrcfg_VESC_COMMAND_ERPM_MAX )
*rpm = *rpm > 0 ? wtrcfg_VESC_COMMAND_ERPM_MAX : - wtrcfg_VESC_COMMAND_ERPM_MAX ;
}
void assert_param_pos(float *pos){
if( fabsf(*pos) > wtrcfg_VESC_COMMAND_POS_MAX )
*pos = *pos > 0 ? wtrcfg_VESC_COMMAND_POS_MAX : - wtrcfg_VESC_COMMAND_POS_MAX ;
}
static VESC_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
// 获取指定CAN总线的电机管理器指针
static VESC_CANManager_t* MOTOR_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return NULL;
return can_managers[can];
}
// 为指定CAN总线创建电机管理器
static int8_t MOTOR_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (VESC_CANManager_t*)BSP_Malloc(sizeof(VESC_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(VESC_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
// 解析CAN报文，更新电机反馈信息
static void Motor_VESC_Decode(VESC_t *motor, BSP_CAN_Message_t *msg)
{
if (motor == NULL || msg == NULL) return;
motor->motor.feedback.rotor_speed =
((int32_t)msg->data[0] << 24) |
((int32_t)msg->data[1] << 16) |
((int32_t)msg->data[2] << 8) |
((int32_t)msg->data[3]);
// torque_current: 低 2 字节 (data[4], data[5])
int16_t raw_current = (int16_t)((msg->data[5] << 8) | msg->data[4]);
motor->motor.feedback.torque_current = raw_current / 1000.0f; // 从 0.1A -> A
// duty_cycle: 低 2 字节 (data[6], data[7])
int16_t raw_duty = (int16_t)((msg->data[7] << 8) | msg->data[6]);
//motor->motor.feedback.duty_cycle = raw_duty / 1000.0f; // 从千分之一 -> (-1.0 ~ 1.0)
}
/* Exported functions ------------------------------------------------------- */
// 注册一个新的电机实例到管理器
int8_t VESC_Register(VESC_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
if (MOTOR_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
VESC_CANManager_t *manager = MOTOR_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR;
// 检查是否已注册
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
return DEVICE_ERR_INITED;
}
}
// 检查数量
if (manager->motor_count >= VESC_MAX_MOTORS) return DEVICE_ERR;
// 创建新电机实例
VESC_t *new_motor = (VESC_t*)BSP_Malloc(sizeof(VESC_t));
if (new_motor == NULL) return DEVICE_ERR;
memcpy(&new_motor->param, param, sizeof(VESC_Param_t));
memset(&new_motor->motor, 0, sizeof(MOTOR_t));
new_motor->motor.reverse = param->reverse;
// 注册CAN接收ID
if (BSP_CAN_RegisterId(param->can, param->id, 3) != BSP_OK) {
BSP_Free(new_motor);
return DEVICE_ERR;
}
manager->motors[manager->motor_count] = new_motor;
manager->motor_count++;
return DEVICE_OK;
}
// 更新指定电机的反馈数据（扩展帧方式）
int8_t VESC_Update(VESC_Param_t *param)
{
if (param == NULL) return DEVICE_ERR_NULL;
VESC_CANManager_t *manager = MOTOR_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
VESC_t *motor = NULL;
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
motor = manager->motors[i];
break;
}
}
if (motor == NULL) return DEVICE_ERR_NO_DEV;
// 根据电机 ID 获取对应扩展帧 ID
uint32_t ext_id = 0;
switch (param->id) {
case VESC_1: ext_id = CAN_VESC5065_M1_MSG1; break;
case VESC_2: ext_id = CAN_VESC5065_M2_MSG1; break;
case VESC_4: ext_id = CAN_VESC5065_M3_MSG1; break;
default: return DEVICE_ERR_NO_DEV;
}
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, ext_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
uint64_t now_time = BSP_TIME_Get();
if (now_time - motor->motor.header.last_online_time > 1000) {
motor->motor.header.online = false;
return DEVICE_ERR_NO_DEV;
}
return DEVICE_ERR;
}
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
Motor_VESC_Decode(motor, &rx_msg);
return DEVICE_OK;
}
// 更新所有CAN总线下所有电机的反馈数据
int8_t VESC_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
VESC_CANManager_t *manager = MOTOR_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
VESC_t *motor = manager->motors[i];
if (motor != NULL) {
if (VESC_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
// 获取指定参数对应的电机实例指针
VESC_t* VESC_GetMotor(VESC_Param_t *param) {
if (param == NULL) return NULL;
VESC_CANManager_t *manager = MOTOR_GetCANManager(param->can);
if (manager == NULL) return NULL;
for (int i = 0; i < manager->motor_count; i++) {
VESC_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
return motor;
}
}
return NULL;
}
// 设置指定电机的输出值
int8_t VESC_SetOutput(VESC_Param_t *param, float value)
{
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t command_id;
if (param->reverse) {
value = -value;
}
switch (param->mode)
{
case DUTY_CONTROL: {
assert_param_duty(&value); // 调用你现有的限幅函数
command_id = CAN_PACKET_SET_DUTY;
int32_t duty_val = (int32_t)(value * 1e5f); // duty 放大 1e5
memcpy(&tx_frame.data[0], &duty_val, 4);
tx_frame.dlc = 4;
break;
}
case RPM_CONTROL: {
assert_param_rpm(&value);
command_id = CAN_PACKET_SET_RPM;
int32_t rpm_val = (int32_t)value;
memcpy(&tx_frame.data[0], &rpm_val, 4);
tx_frame.dlc = 4;
break;
}
case CURRENT_CONTROL: {
assert_param_current(&value);
command_id = CAN_PACKET_SET_CURRENT;
int32_t cur_val = (int32_t)(value * 1e3f); // A -> mA （0-50A）
memcpy(&tx_frame.data[0], &cur_val, 4);
tx_frame.dlc = 4;
break;
}
case POSITION_CONTROL: {
assert_param_pos(&value);
command_id = CAN_PACKET_SET_POS;
memcpy(&tx_frame.data[0], &value, 4);
tx_frame.dlc = 4;
break;
}
default:
return DEVICE_ERR;
}
tx_frame.id = (param->id << 5) | command_id;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
int8_t VESC_Relax(VESC_Param_t *param) {
return VESC_SetOutput(param, 0.0f);
}
int8_t VESC_Offine(VESC_Param_t *param) {
VESC_t *motor = VESC_GetMotor(param);
if (motor) {
motor->motor.header.online = false;
return DEVICE_OK;
}
return DEVICE_ERR_NO_DEV;
}

146
assets/User_code/device/motor_vesc.h Normal file
View File

@@ -0,0 +1,146 @@
#pragma once
#ifdef __cplusplus
extern "C"
{
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Private define ----------------------------------------------------------- */
#define wtrcfg_VESC_COMMAND_DUTY_MAX 100
#define wtrcfg_VESC_COMMAND_CURRENT_MAX 10
#define wtrcfg_VESC_COMMAND_POS_MAX 360
#define wtrcfg_VESC_COMMAND_ERPM_MAX 35000
#define wtrcfg_VESC_UART_TIMEOUT 0xff
// VESC数量根据实际情况调整
#define VESC_MAX_MOTORS 4
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum
{
VESC_1 = 1,
VESC_2 = 2,
VESC_3 = 3,
VESC_4 = 4,
CAN_VESC5065_M1_MSG1 = 0x901, // vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
CAN_VESC5065_M2_MSG1 = 0x902,
CAN_VESC5065_M3_MSG1 = 0x903,
CAN_VESC5065_M4_MSG1 = 0x904,
}VESC_ID;
typedef enum
{
CAN_PACKET_SET_DUTY = 0,
CAN_PACKET_SET_CURRENT = 1,
CAN_PACKET_SET_CURRENT_BRAKE = 2,
CAN_PACKET_SET_RPM = 3,
CAN_PACKET_SET_POS = 4,
CAN_PACKET_FILL_RX_BUFFER = 5,
CAN_PACKET_FILL_RX_BUFFER_LONG = 6,
CAN_PACKET_PROCESS_RX_BUFFER = 7,
CAN_PACKET_PROCESS_SHORT_BUFFER = 8,
CAN_PACKET_STATUS = 9,
CAN_PACKET_SET_CURRENT_REL = 10,
CAN_PACKET_SET_CURRENT_BRAKE_REL = 11,
CAN_PACKET_SET_CURRENT_HANDBRAKE = 12,
CAN_PACKET_SET_CURRENT_HANDBRAKE_REL = 13
} CAN_PACKET_ID;
// Control Modes
typedef enum
{
DUTY_CONTROL = 0x0,
RPM_CONTROL = 0x1,
CURRENT_CONTROL = 0x2,
POSITION_CONTROL = 0x3
} Control_Mode;
/*每个电机需要的参数*/
typedef struct
{
BSP_CAN_t can;
uint16_t id;
uint16_t mode;
bool reverse;
} VESC_Param_t;
/*电机实例*/
typedef struct ODrive_t
{
VESC_Param_t param;
MOTOR_t motor;
} VESC_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct
{
BSP_CAN_t can;
VESC_t *motors[VESC_MAX_MOTORS];
uint8_t motor_count;
} VESC_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 注册一个vesc电机
* @param param 电机参数
* @return
*/
int8_t VESC_Register(VESC_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return
*/
int8_t VESC_Update(VESC_Param_t *param);
/**
* @brief 更新所有电机数据
* @return
*/
int8_t VESC_UpdateAll(void);
/**
* @brief 设置一个电机的输出
* @param param 电机参数
* @param value 输出值
* @return
*/
int8_t VESC_SetOutput(VESC_Param_t *param, float value);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return
*/
VESC_t* VESC_GetMotor(VESC_Param_t *param);
/**
* @brief 使电机松弛（设置输出为0）
* @param param
* @return
*/
int8_t VESC_Relax(VESC_Param_t *param);
/**
* @brief 使电机离线（设置在线状态为false）
* @param param
* @return
*/
int8_t VESC_Offine(VESC_Param_t *param);
#ifdef __cplusplus
}
#endif

58
assets/User_code/device/ops9.c Normal file
View File

@@ -0,0 +1,58 @@
/*
ACTION全场定位码盘ops9
*/
/* Includes ----------------------------------------------------------------- */
#include "device/ops9.h"
#include <string.h>
#include "bsp/uart.h"
#include "bsp/time.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static osThreadId_t thread_alert;
static bool inited = false;
/* Private function -------------------------------------------------------- */
static void OPS9_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_OPS9_RAW_REDY);
}
/* Exported functions ------------------------------------------------------- */
int8_t OPS9_init(OPS9_t *ops9) {
if (ops9 == NULL) return DEVICE_ERR_NULL;
if (inited) return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_OPS9, BSP_UART_RX_CPLT_CB,
OPS9_RxCpltCallback);
inited = true;
return DEVICE_OK;
}
int8_t OPS9_Restart(void) {
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_OPS9));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_OPS9));
return DEVICE_OK;
}
int8_t OPS9_StartDmaRecv(OPS9_t *ops9) {
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_OPS9),
(uint8_t *)&(ops9->data),
sizeof(ops9->data)) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
bool OPS9_WaitDmaCplt(uint32_t timeout) {
return (osThreadFlagsWait(SIGNAL_OPS9_RAW_REDY, osFlagsWaitAll, timeout) ==
SIGNAL_OPS9_RAW_REDY);
}

49
assets/User_code/device/ops9.h Normal file
View File

@@ -0,0 +1,49 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include <stdint.h>
#include "component/user_math.h"
#include "device/device.h"
/* Exported constants ------------------------------------------------------- */
#define OPS9_HEADER 0x0D0A
#define OPS9_TAIL 0x0A0D
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
// 数据包结构体
typedef struct __packed {
uint16_t header; // 2字节
float yaw; // 4字节
float pitch; // 4字节
float roll; // 4字节
float x; // 4字节
float y; // 4字节
float angular_velocity; // 4字节
uint16_t tail; // 2字节
} OPS9_Data_t; // 共28字节
typedef struct {
DEVICE_Header_t header; // 设备头
OPS9_Data_t data; // 存储接收到的数据
} OPS9_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t OPS9_init(OPS9_t *ops9);
int8_t OPS9_Restart(void);
int8_t OPS9_StartDmaRecv(OPS9_t *ops9);
bool OPS9_WaitDmaCplt(uint32_t timeout);
#ifdef __cplusplus
}
#endif

328
assets/User_code/device/rc_can.c Normal file
View File

@@ -0,0 +1,328 @@
/* Includes ----------------------------------------------------------------- */
#include "device/rc_can.h"
#include "bsp/time.h"
#include "device/device.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private constants -------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private types ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER VARIABLE BEGIN */
/* USER VARIABLE END */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
/* Private function prototypes ---------------------------------------------- */
static int8_t RC_CAN_ValidateParams(const RC_CAN_Param_t *param);
static int8_t RC_CAN_RegisterIds(RC_CAN_t *rc_can);
/* Exported functions ------------------------------------------------------- */
/**
* @brief 初始化RC CAN发送模块
* @param rc_can RC_CAN结构体指针
* @param param 初始化参数
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_Init(RC_CAN_t *rc_can, RC_CAN_Param_t *param) {
if (rc_can == NULL || param == NULL) {
return DEVICE_ERR_NULL;
}
// 参数验证
if (RC_CAN_ValidateParams(param) != DEVICE_OK) {
return DEVICE_ERR;
}
rc_can->param = *param;
// 初始化header
rc_can->header.online = false;
rc_can->header.last_online_time = 0;
// 手动初始化数据结构
rc_can->data.joy.ch_l_x = 0.0f;
rc_can->data.joy.ch_l_y = 0.0f;
rc_can->data.joy.ch_r_x = 0.0f;
rc_can->data.joy.ch_r_y = 0.0f;
rc_can->data.sw.sw_l = RC_CAN_SW_ERR;
rc_can->data.sw.sw_r = RC_CAN_SW_ERR;
rc_can->data.sw.ch_res = 0.0f;
rc_can->data.mouse.x = 0.0f;
rc_can->data.mouse.y = 0.0f;
rc_can->data.mouse.z = 0.0f;
rc_can->data.mouse.mouse_l = false;
rc_can->data.mouse.mouse_r = false;
rc_can->data.keyboard.key_value = 0;
for (int i = 0; i < 6; i++) {
rc_can->data.keyboard.keys[i] = RC_CAN_KEY_NONE;
}
// 注册CAN ID队列（从机模式需要接收数据）
if (rc_can->param.mode == RC_CAN_MODE_SLAVE) {
return RC_CAN_RegisterIds(rc_can);
}
return DEVICE_OK;
}
/**
* @brief 更新并发送数据到CAN总线
* @param rc_can RC_CAN结构体指针
* @param data_type 数据类型
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_SendData(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type) {
if (rc_can == NULL) {
return DEVICE_ERR_NULL;
}
if (rc_can->param.mode != RC_CAN_MODE_MASTER) {
return DEVICE_ERR;
}
BSP_CAN_StdDataFrame_t frame;
frame.dlc = 8;
// 边界裁剪宏
#define RC_CAN_CLAMP(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
switch (data_type) {
case RC_CAN_DATA_JOYSTICK: {
frame.id = rc_can->param.joy_id;
float l_x = RC_CAN_CLAMP(rc_can->data.joy.ch_l_x, -0.999969f, 0.999969f);
float l_y = RC_CAN_CLAMP(rc_can->data.joy.ch_l_y, -0.999969f, 0.999969f);
float r_x = RC_CAN_CLAMP(rc_can->data.joy.ch_r_x, -0.999969f, 0.999969f);
float r_y = RC_CAN_CLAMP(rc_can->data.joy.ch_r_y, -0.999969f, 0.999969f);
int16_t l_x_i = (int16_t)(l_x * 32768.0f);
int16_t l_y_i = (int16_t)(l_y * 32768.0f);
int16_t r_x_i = (int16_t)(r_x * 32768.0f);
int16_t r_y_i = (int16_t)(r_y * 32768.0f);
frame.data[0] = (uint8_t)(l_x_i & 0xFF);
frame.data[1] = (uint8_t)((l_x_i >> 8) & 0xFF);
frame.data[2] = (uint8_t)(l_y_i & 0xFF);
frame.data[3] = (uint8_t)((l_y_i >> 8) & 0xFF);
frame.data[4] = (uint8_t)(r_x_i & 0xFF);
frame.data[5] = (uint8_t)((r_x_i >> 8) & 0xFF);
frame.data[6] = (uint8_t)(r_y_i & 0xFF);
frame.data[7] = (uint8_t)((r_y_i >> 8) & 0xFF);
break;
}
case RC_CAN_DATA_SWITCH: {
frame.id = rc_can->param.sw_id;
frame.data[0] = (uint8_t)(rc_can->data.sw.sw_l);
frame.data[1] = (uint8_t)(rc_can->data.sw.sw_r);
float ch_res = RC_CAN_CLAMP(rc_can->data.sw.ch_res, -0.999969f, 0.999969f);
int16_t ch_res_i = (int16_t)(ch_res * 32768.0f);
frame.data[2] = (uint8_t)(ch_res_i & 0xFF);
frame.data[3] = (uint8_t)((ch_res_i >> 8) & 0xFF);
frame.data[4] = 0; // 保留字节
frame.data[5] = 0; // 保留字节
frame.data[6] = 0; // 保留字节
frame.data[7] = 0; // 保留字节
break;
}
case RC_CAN_DATA_MOUSE: {
frame.id = rc_can->param.mouse_id;
// 鼠标x/y/z一般为增量，若有极限也可加clamp
int16_t x = (int16_t)(rc_can->data.mouse.x);
int16_t y = (int16_t)(rc_can->data.mouse.y);
int16_t z = (int16_t)(rc_can->data.mouse.z);
frame.data[0] = (uint8_t)(x & 0xFF);
frame.data[1] = (uint8_t)((x >> 8) & 0xFF);
frame.data[2] = (uint8_t)(y & 0xFF);
frame.data[3] = (uint8_t)((y >> 8) & 0xFF);
frame.data[4] = (uint8_t)(z & 0xFF);
frame.data[5] = (uint8_t)((z >> 8) & 0xFF);
frame.data[6] = (uint8_t)(rc_can->data.mouse.mouse_l ? 1 : 0);
frame.data[7] = (uint8_t)(rc_can->data.mouse.mouse_r ? 1 : 0);
break;
}
case RC_CAN_DATA_KEYBOARD: {
frame.id = rc_can->param.keyboard_id;
frame.data[0] = (uint8_t)(rc_can->data.keyboard.key_value & 0xFF);
frame.data[1] = (uint8_t)((rc_can->data.keyboard.key_value >> 8) & 0xFF);
for (int i = 0; i < 6; i++) {
frame.data[2 + i] = (i < 6) ? (uint8_t)(rc_can->data.keyboard.keys[i]) : 0;
}
break;
}
default:
return DEVICE_ERR;
}
#undef RC_CAN_CLAMP
if (BSP_CAN_Transmit(rc_can->param.can, BSP_CAN_FORMAT_STD_DATA, frame.id,
frame.data, frame.dlc) != BSP_OK) {
return DEVICE_ERR;
}
return DEVICE_OK;
}
/**
* @brief 接收并更新CAN数据
* @param rc_can RC_CAN结构体指针
* @param data_type 数据类型
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_Update(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type) {
if (rc_can == NULL) {
return DEVICE_ERR_NULL;
}
// 只有从机模式才能接收数据
if (rc_can->param.mode != RC_CAN_MODE_SLAVE) {
return DEVICE_ERR;
}
BSP_CAN_Message_t msg;
switch (data_type) {
case RC_CAN_DATA_JOYSTICK:
if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.joy_id, &msg,
BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
return DEVICE_ERR;
}
// 解包数据
int16_t ch_l_x = (int16_t)((msg.data[1] << 8) | msg.data[0]);
int16_t ch_l_y = (int16_t)((msg.data[3] << 8) | msg.data[2]);
int16_t ch_r_x = (int16_t)((msg.data[5] << 8) | msg.data[4]);
int16_t ch_r_y = (int16_t)((msg.data[7] << 8) | msg.data[6]);
// 转换为浮点数（范围：-1.0到1.0）
rc_can->data.joy.ch_l_x = (float)ch_l_x / 32768.0f;
rc_can->data.joy.ch_l_y = (float)ch_l_y / 32768.0f;
rc_can->data.joy.ch_r_x = (float)ch_r_x / 32768.0f;
rc_can->data.joy.ch_r_y = (float)ch_r_y / 32768.0f;
break;
case RC_CAN_DATA_SWITCH:
if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.sw_id, &msg,
BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
return DEVICE_ERR;
}
// 解包数据
rc_can->data.sw.sw_l = (RC_CAN_SW_t)msg.data[0];
rc_can->data.sw.sw_r = (RC_CAN_SW_t)msg.data[1];
int16_t ch_res = (int16_t)((msg.data[3] << 8) | msg.data[2]);
rc_can->data.sw.ch_res = (float)ch_res / 32768.0f;
break;
case RC_CAN_DATA_MOUSE:
if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.mouse_id, &msg,
BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
return DEVICE_ERR;
}
// 解包数据
int16_t x = (int16_t)((msg.data[1] << 8) | msg.data[0]);
int16_t y = (int16_t)((msg.data[3] << 8) | msg.data[2]);
int16_t z = (int16_t)((msg.data[5] << 8) | msg.data[4]);
rc_can->data.mouse.x = (float)x;
rc_can->data.mouse.y = (float)y;
rc_can->data.mouse.z = (float)z;
rc_can->data.mouse.mouse_l = (msg.data[6] & 0x01) ? true : false;
rc_can->data.mouse.mouse_r = (msg.data[7] & 0x01) ? true : false;
break;
case RC_CAN_DATA_KEYBOARD:
if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.keyboard_id, &msg,
BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
return DEVICE_ERR;
}
if (msg.dlc < 2) {
return DEVICE_ERR;
}
// 解包数据
rc_can->data.keyboard.key_value =
(uint16_t)((msg.data[1] << 8) | msg.data[0]);
for (int i = 0; i < 6 && (i + 2) < msg.dlc; i++) {
rc_can->data.keyboard.keys[i] = (RC_CAN_Key_t)(msg.data[2 + i]);
}
// 清空未使用的按键位置
for (int i = (msg.dlc > 2 ? msg.dlc - 2 : 0); i < 6; i++) {
rc_can->data.keyboard.keys[i] = RC_CAN_KEY_NONE;
}
break;
default:
return DEVICE_ERR;
}
// 更新header状态
rc_can->header.online = true;
rc_can->header.last_online_time = BSP_TIME_Get_us();
return DEVICE_OK;
}
/* Private functions -------------------------------------------------------- */
/**
* @brief 验证RC_CAN参数
* @param param 参数指针
* @return DEVICE_OK 成功，其他值失败
*/
static int8_t RC_CAN_ValidateParams(const RC_CAN_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
// 检查CAN总线有效性
if (param->can >= BSP_CAN_NUM) {
return DEVICE_ERR;
}
// 检查工作模式有效性
if (param->mode != RC_CAN_MODE_MASTER && param->mode != RC_CAN_MODE_SLAVE) {
return DEVICE_ERR;
}
// 检查CAN ID是否重复
if (param->joy_id == param->sw_id || param->joy_id == param->mouse_id ||
param->joy_id == param->keyboard_id || param->sw_id == param->mouse_id ||
param->sw_id == param->keyboard_id ||
param->mouse_id == param->keyboard_id) {
return DEVICE_ERR;
}
return DEVICE_OK;
}
/**
* @brief 注册CAN ID
* @param rc_can RC_CAN结构体指针
* @return DEVICE_OK 成功，其他值失败
*/
static int8_t RC_CAN_RegisterIds(RC_CAN_t *rc_can) {
if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.joy_id, 0) !=
BSP_OK) {
return DEVICE_ERR;
}
if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.sw_id, 0) != BSP_OK) {
return DEVICE_ERR;
}
if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.mouse_id, 0) !=
BSP_OK) {
return DEVICE_ERR;
}
if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.keyboard_id, 0) !=
BSP_OK) {
return DEVICE_ERR;
}
return DEVICE_OK;
}
int8_t RC_CAN_OFFLINE(RC_CAN_t *rc_can){
if (rc_can == NULL) {
return DEVICE_ERR_NULL;
}
rc_can->header.online = false;
return DEVICE_OK;
}
/* USER CODE BEGIN */
/* USER CODE END */

157
assets/User_code/device/rc_can.h Normal file
View File

@@ -0,0 +1,157 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "bsp/can.h"
#include "device/device.h"
#include <stdint.h>
#include <stdbool.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
RC_CAN_SW_ERR = 0,
RC_CAN_SW_UP = 1,
RC_CAN_SW_MID = 3,
RC_CAN_SW_DOWN = 2,
} RC_CAN_SW_t;
typedef enum {
RC_CAN_MODE_MASTER = 0, // 主机模式
RC_CAN_MODE_SLAVE = 1, // 从机模式
} RC_CAN_Mode_t;
typedef enum {
RC_CAN_DATA_JOYSTICK = 0,
RC_CAN_DATA_SWITCH,
RC_CAN_DATA_MOUSE,
RC_CAN_DATA_KEYBOARD
} RC_CAN_DataType_t;
typedef enum {
RC_CAN_KEY_NONE = 0xFF, // 无按键
RC_CAN_KEY_W = 0,
RC_CAN_KEY_S,
RC_CAN_KEY_A,
RC_CAN_KEY_D,
RC_CAN_KEY_SHIFT,
RC_CAN_KEY_CTRL,
RC_CAN_KEY_Q,
RC_CAN_KEY_E,
RC_CAN_KEY_R,
RC_CAN_KEY_F,
RC_CAN_KEY_G,
RC_CAN_KEY_Z,
RC_CAN_KEY_X,
RC_CAN_KEY_C,
RC_CAN_KEY_V,
RC_CAN_KEY_B,
RC_CAN_KEY_NUM,
} RC_CAN_Key_t;
// 遥杆数据包
typedef struct {
float ch_l_x;
float ch_l_y;
float ch_r_x;
float ch_r_y;
} RC_CAN_JoyData_t;
// 拨杆数据包
typedef struct {
RC_CAN_SW_t sw_l; // 左拨杆状态
RC_CAN_SW_t sw_r; // 右拨杆状态
float ch_res; // 第五通道
} RC_CAN_SwitchData_t;
// 鼠标数据包
typedef struct {
float x; // 鼠标X轴移动
float y; // 鼠标Y轴移动
float z; // 鼠标Z轴(滚轮)
bool mouse_l; // 鼠标左键
bool mouse_r; // 鼠标右键
} RC_CAN_MouseData_t;
// 键盘数据包
typedef struct {
uint16_t key_value; // 键盘按键位映射
RC_CAN_Key_t keys[16];
} RC_CAN_KeyboardData_t;
typedef struct {
RC_CAN_JoyData_t joy;
RC_CAN_SwitchData_t sw;
RC_CAN_MouseData_t mouse;
RC_CAN_KeyboardData_t keyboard;
} RC_CAN_Data_t;
// RC_CAN 参数结构
typedef struct {
BSP_CAN_t can; // 使用的CAN总线
RC_CAN_Mode_t mode; // 工作模式
uint16_t joy_id; // 遥杆CAN ID
uint16_t sw_id; // 拨杆CAN ID
uint16_t mouse_id; // 鼠标CAN ID
uint16_t keyboard_id; // 键盘CAN ID
} RC_CAN_Param_t;
// RC_CAN 主结构
typedef struct {
DEVICE_Header_t header;
RC_CAN_Param_t param;
RC_CAN_Data_t data;
} RC_CAN_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化RC CAN发送模块
* @param rc_can RC_CAN结构体指针
* @param param 初始化参数
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_Init(RC_CAN_t *rc_can, RC_CAN_Param_t *param);
/**
* @brief 更新并发送数据到CAN总线
* @param rc_can RC_CAN结构体指针
* @param data_type 数据类型
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_SendData(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type);
/**
* @brief 接收并更新CAN数据
* @param rc_can RC_CAN结构体指针
* @param data_type 数据类型
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_Update(RC_CAN_t *rc_can , RC_CAN_DataType_t data_type);
int8_t RC_CAN_OFFLINE(RC_CAN_t *rc_can);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

47
assets/User_code/device/servo.c Normal file
View File

@@ -0,0 +1,47 @@
/*
pwm<77><6D><EFBFBD>ƶ<EFBFBD><C6B6>
*/
/*Includes -----------------------------------------*/
#include "bsp/pwm.h"
#include "servo.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
#define SERVO_MIN_DUTY 0.025f
#define SERVO_MAX_DUTY 0.125f
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/**
* @brief
* @param
* @retval BSP_OK / BSP_ERR
*/
int8_t SERVO_Init(SERVO_t *servo) {
if (servo == NULL) return BSP_ERR;
return BSP_PWM_Start(servo->pwm_ch);
}
int8_t SERVO_SetAngle(SERVO_t *servo, float angle) {
if (servo == NULL) return BSP_ERR;
/*<2A><><EFBFBD>ƽǶȷ<C7B6>Χ*/
if (angle < 0.0f) angle = 0.0f;
if (angle > 180.0f) angle = 180.0f;
/*<2A>Ƕ<EFBFBD>ӳ<EFBFBD>䵽ռ<E4B5BD>ձ<EFBFBD>*/
float duty = servo->min_duty + (angle / 180.0f) * (servo->max_duty - servo->min_duty);
return BSP_PWM_Set(servo->pwm_ch, duty);
}
int8_t SERVO_Stop(SERVO_t *servo) {
if (servo == NULL) return BSP_ERR;
return BSP_PWM_Stop(servo->pwm_ch);
}

68
assets/User_code/device/servo.h Normal file
View File

@@ -0,0 +1,68 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include "bsp/pwm.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/**
* @brief
*/
typedef struct {
BSP_PWM_Channel_t pwm_ch;
float min_duty;
float max_duty;
} SERVO_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/**
* @brief
* @param servo
* @retval BSP_OK / BSP_ERR
*/
int8_t SERVO_Init(SERVO_t *servo);
/**
* @brief
* @param servo
* @param angle
* @retval BSP_OK / BSP_ERR
*/
int8_t SERVO_SetAngle(SERVO_t *servo, float angle);
/**
* @brief
* @param servo
* @retval BSP_OK / BSP_ERR
*/
int8_t SERVO_Stop(SERVO_t *servo);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

106
assets/User_code/device/vofa.c Normal file
View File

@@ -0,0 +1,106 @@
/* Includes ----------------------------------------------------------------- */
#include <stdio.h>
#include <string.h>
#include "device/vofa.h"
#include "bsp/uart.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */
#define MAX_CHANNEL 64u // 根据实际最大通道数调整
#define JUSTFLOAT_TAIL 0x7F800000
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */
static uint8_t vofa_tx_buf[sizeof(float) * MAX_CHANNEL + sizeof(uint32_t)];
static VOFA_Protocol_t current_protocol = VOFA_PROTOCOL_FIREWATER; // 默认协议
/* Private function -------------------------------------------------------- */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
/************************ RawData *************************/
void VOFA_RawData_Send(const char* data, bool dma) {
BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t*)data, strlen(data), dma);
}
/************************ FireWater *************************/
void VOFA_FireWater_Send(float *channels, uint8_t channel_count, bool dma)
{
if (channel_count == 0 || channel_count > MAX_CHANNEL)
return;
char *buf = (char *)vofa_tx_buf;
size_t len = 0;
for (uint8_t i = 0; i < channel_count; ++i) {
len += snprintf(buf + len,
sizeof(vofa_tx_buf) - len,
"%s%.2f",
(i ? "," : ""),
channels[i]);
}
snprintf(buf + len, sizeof(vofa_tx_buf) - len, "\n");
BSP_UART_Transmit(BSP_UART_VOFA, vofa_tx_buf, strlen(buf), dma);
}
/************************ JustFloat *************************/
void VOFA_JustFloat_Send(float *channels, uint8_t channel_count, bool dma)
{
if (channel_count == 0 || channel_count > MAX_CHANNEL)
return;
memcpy(vofa_tx_buf, channels, channel_count * sizeof(float));
uint32_t tail = JUSTFLOAT_TAIL; // 0x7F800000
memcpy(vofa_tx_buf + channel_count * sizeof(float), &tail, sizeof(tail));
BSP_UART_Transmit(BSP_UART_VOFA, vofa_tx_buf, channel_count * sizeof(float) + sizeof(tail), dma);
}
/* Exported functions ------------------------------------------------------- */
int8_t VOFA_init(VOFA_Protocol_t protocol) {
current_protocol = protocol;
return DEVICE_OK;
}
int8_t VOFA_Send(float* channels, uint8_t channel_count, bool dma) {
switch (current_protocol) {
case VOFA_PROTOCOL_RAWDATA:
{
char data[256];
if (channel_count >= 1) {
sprintf(data, "Channel1: %.2f", channels[0]);
if (channel_count >= 2) {
sprintf(data + strlen(data), ", Channel2: %.2f", channels[1]);
}
strcat(data, "\n");
VOFA_RawData_Send(data, dma);
}
}
break;
case VOFA_PROTOCOL_FIREWATER:
VOFA_FireWater_Send(channels, channel_count, dma);
break;
case VOFA_PROTOCOL_JUSTFLOAT:
VOFA_JustFloat_Send(channels, channel_count, dma);
break;
default:
return DEVICE_ERR;
}
return DEVICE_OK;
}

39
assets/User_code/device/vofa.h Normal file
View File

@@ -0,0 +1,39 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "bsp/uart.h"
#include "device/device.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
typedef enum {
VOFA_PROTOCOL_RAWDATA,
VOFA_PROTOCOL_FIREWATER,
VOFA_PROTOCOL_JUSTFLOAT,
} VOFA_Protocol_t;
/**
* @brief 初始化VOFA设备
* @param protocol 设置通信协议
* @return
*/
int8_t VOFA_init(VOFA_Protocol_t protocol);
/**
* @brief 发送数据到VOFA
* @param channels 要发送的通道数据
* @param channel_count 通道数量
* @param dma 是否使用DMA发送
* @return
*/
int8_t VOFA_Send(float* channels, uint8_t channel_count, bool dma);
#ifdef __cplusplus
}
#endif

114
assets/User_code/device/ws2812.c Normal file
View File

@@ -0,0 +1,114 @@
/* Includes ----------------------------------------------------------------- */
#include "ws2812.h"
#include "device.h"
#include "bsp/pwm.h"
#include <stdlib.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */
#define DEVICE_WS2812_T1H (uint16_t)(BSP_PWM_GetAutoReloadPreload(BSP_PWM_WS2812) * 0.56) // High-level width of logic-1 pulse
#define DEVICE_WS2812_T0H (BSP_PWM_GetAutoReloadPreload(BSP_PWM_WS2812) * 0.29) // High-level width of logic-0 pulse
#define DEVICE_WS2812_WS_REST 40 // Number of reset pulses (low level) after data stream
#define DEVICE_WS2812_DATA_LEN 24 // WS2812 data length: 24 bits (GRB) per LED
#define DEVICE_WS2812_RST_NUM 50 // Extra reset pulses reserved at the end of the buffer
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */
static uint16_t DEVICE_WS2812_LED_NUM; // Total number of LEDs
static uint16_t *DEVICE_WS2812_RGB_Buff = NULL;// PWM duty buffer for DMA
/* Private function -------------------------------------------------------- */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
/* Exported functions ------------------------------------------------------- */
/**
* Set color of a single WS2812 LED
* @param num LED index (1-based)
* @param R Red value (0-255)
* @param G Green value (0-255)
* @param B Blue value (0-255)
* @return DEVICE_OK on success, DEVICE_ERR if num is invalid
*/
uint8_t DEVICE_WS2812_Set(uint16_t num, uint8_t R, uint8_t G, uint8_t B)
{
if(num<1 || num>DEVICE_WS2812_LED_NUM) return DEVICE_ERR;
uint32_t indexx = (num-1) * DEVICE_WS2812_DATA_LEN;
/* WS2812 uses GRB order, MSB first */
for (uint8_t i = 0; i < 8; i++) {
// G
DEVICE_WS2812_RGB_Buff[indexx + i] = (G & (0x80 >> i)) ? DEVICE_WS2812_T1H : DEVICE_WS2812_T0H;
// R
DEVICE_WS2812_RGB_Buff[indexx + i + 8] = (R & (0x80 >> i)) ? DEVICE_WS2812_T1H : DEVICE_WS2812_T0H;
// B
DEVICE_WS2812_RGB_Buff[indexx + i + 16] = (B & (0x80 >> i)) ? DEVICE_WS2812_T1H : DEVICE_WS2812_T0H;
}
return DEVICE_OK;
}
/**
* Initialize WS2812 driver
* @param ledNum Number of LEDs in the strip
* @return DEVICE_OK on success, DEVICE_ERR if memory allocation or PWM setup fails
*/
uint8_t DEVICE_WS2812_Init(uint16_t ledNum)
{
DEVICE_WS2812_LED_NUM = ledNum;
if (DEVICE_WS2812_RGB_Buff != NULL)
{
free(DEVICE_WS2812_RGB_Buff);
DEVICE_WS2812_RGB_Buff = NULL;
}
/* Allocate new buffer: 24 PWM samples per LED + reset pulses */
size_t bufLen = ledNum * DEVICE_WS2812_DATA_LEN + DEVICE_WS2812_RST_NUM;
DEVICE_WS2812_RGB_Buff = (uint16_t *)malloc(bufLen * sizeof(uint16_t));
if (DEVICE_WS2812_RGB_Buff == NULL)
return DEVICE_ERR;
/* Initialize all LEDs to dim green */
for (int i = 1; i <= ledNum; i++)
DEVICE_WS2812_Set(i, 0, 20, 0);
/* Configure PWM frequency to 800 kHz and start DMA */
if (BSP_PWM_SetFreq(BSP_PWM_WS2812, 800000) == DEVICE_OK)
BSP_PWM_Start_DMA(
BSP_PWM_WS2812,
(uint32_t *)DEVICE_WS2812_RGB_Buff,
bufLen);
else
return DEVICE_ERR;
return DEVICE_OK;
}
/**
* De-initialize WS2812 driver
* Frees the DMA buffer and stops PWM
*/
void DEVICE_WS2812_DeInit()
{
for (int i = 1; i <= DEVICE_WS2812_LED_NUM; i++)
DEVICE_WS2812_Set(i, 0, 0, 0);
if (DEVICE_WS2812_RGB_Buff != NULL)
{
free(DEVICE_WS2812_RGB_Buff);
DEVICE_WS2812_RGB_Buff = NULL;
}
BSP_PWM_Stop_DMA(BSP_PWM_WS2812);
}

19
assets/User_code/device/ws2812.h Normal file
View File

@@ -0,0 +1,19 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
uint8_t DEVICE_WS2812_Init(uint16_t led_num);
uint8_t DEVICE_WS2812_Set(uint16_t num, uint8_t R, uint8_t G, uint8_t B);
void DEVICE_WS2812_DeInit();
#ifdef __cplusplus
}
#endif