Compare commits
2 Commits
8ccffa1b1a
...
3252ba19e6
| Author | SHA1 | Date | |
|---|---|---|---|
| 3252ba19e6 | |||
| 360fda0c82 |
@ -78,7 +78,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
User/device/motor_dm.c
|
||||
User/device/motor_rm.c
|
||||
User/device/supercap.c
|
||||
|
||||
User/device/gimbal_imu.c
|
||||
# User/module sources
|
||||
User/module/chassis.c
|
||||
|
||||
|
||||
@ -106,7 +106,7 @@ void MX_FDCAN2_Init(void)
|
||||
hfdcan2.Init.ExtFiltersNbr = 1;
|
||||
hfdcan2.Init.RxFifo0ElmtsNbr = 0;
|
||||
hfdcan2.Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_8;
|
||||
hfdcan2.Init.RxFifo1ElmtsNbr = 16;
|
||||
hfdcan2.Init.RxFifo1ElmtsNbr = 20;
|
||||
hfdcan2.Init.RxFifo1ElmtSize = FDCAN_DATA_BYTES_8;
|
||||
hfdcan2.Init.RxBuffersNbr = 0;
|
||||
hfdcan2.Init.RxBufferSize = FDCAN_DATA_BYTES_8;
|
||||
|
||||
@ -2,6 +2,7 @@
|
||||
#include "fdcan.h"
|
||||
#include "bsp/fdcan.h"
|
||||
#include "bsp/bsp.h"
|
||||
#include "stm32h7xx_hal_fdcan.h"
|
||||
#include <fdcan.h>
|
||||
#include <cmsis_os2.h>
|
||||
#include <string.h>
|
||||
@ -27,13 +28,13 @@
|
||||
#define FDCAN2_FILTER_CONFIG_TABLE(X) \
|
||||
X(0, FDCAN_STANDARD_ID, FDCAN_FILTER_MASK, 0x000 , 0x000 , 0) \
|
||||
X(1, FDCAN_EXTENDED_ID, FDCAN_FILTER_MASK, 0x00000000, 0x00000000, 0)
|
||||
#define FDCAN2_GLOBAL_FILTER FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
|
||||
#define FDCAN2_GLOBAL_FILTER FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
|
||||
#endif
|
||||
#ifdef FDCAN3_EN
|
||||
#define FDCAN3_FILTER_CONFIG_TABLE(X) \
|
||||
X(0, FDCAN_STANDARD_ID, FDCAN_FILTER_MASK, 0x000 , 0x000 , 0) \
|
||||
X(1, FDCAN_EXTENDED_ID, FDCAN_FILTER_MASK, 0x00000000, 0x00000000, 0)
|
||||
#define FDCAN3_GLOBAL_FILTER FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
|
||||
#define FDCAN3_GLOBAL_FILTER FDCAN_REJECT, FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
|
||||
#endif
|
||||
|
||||
/* ====宏展开实现==== */
|
||||
@ -65,13 +66,21 @@ typedef struct BSP_FDCAN_QueueNode {
|
||||
} BSP_FDCAN_QueueNode_t;
|
||||
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
static BSP_FDCAN_QueueNode_t *queue_list = NULL;
|
||||
static osMutexId_t queue_mutex = NULL;
|
||||
static void (*FDCAN_Callback[BSP_FDCAN_NUM][HAL_FDCAN_CB_NUM])(void);
|
||||
static bool inited = false;
|
||||
static BSP_FDCAN_IdParser_t id_parser = NULL;
|
||||
static BSP_FDCAN_TxQueue_t tx_queues[BSP_FDCAN_NUM];
|
||||
static const uint8_t fdcan_dlc2len[16] = {0,1,2,3,4,5,6,7,8,12,16,20,24,32,48,64};
|
||||
// static BSP_FDCAN_QueueNode_t *queue_list = NULL;
|
||||
// static osMutexId_t queue_mutex = NULL;
|
||||
// static void (*FDCAN_Callback[BSP_FDCAN_NUM][HAL_FDCAN_CB_NUM])(void);
|
||||
// static bool inited = false;
|
||||
// static BSP_FDCAN_IdParser_t id_parser = NULL;
|
||||
// static BSP_FDCAN_TxQueue_t tx_queues[BSP_FDCAN_NUM];
|
||||
// static const uint8_t fdcan_dlc2len[16] = {0,1,2,3,4,5,6,7,8,12,16,20,24,32,48,64};
|
||||
|
||||
BSP_FDCAN_QueueNode_t *queue_list = NULL;
|
||||
osMutexId_t queue_mutex = NULL;
|
||||
void (*FDCAN_Callback[BSP_FDCAN_NUM][HAL_FDCAN_CB_NUM])(void);
|
||||
bool inited = false;
|
||||
BSP_FDCAN_IdParser_t id_parser = NULL;
|
||||
BSP_FDCAN_TxQueue_t tx_queues[BSP_FDCAN_NUM];
|
||||
const uint8_t fdcan_dlc2len[16] = {0,1,2,3,4,5,6,7,8,12,16,20,24,32,48,64};
|
||||
|
||||
/* Private function prototypes ---------------------------------------------- */
|
||||
static BSP_FDCAN_t FDCAN_Get(FDCAN_HandleTypeDef *hfdcan);
|
||||
@ -578,4 +587,42 @@ uint32_t BSP_FDCAN_ParseId(uint32_t original_id, BSP_FDCAN_FrameType_t frame_typ
|
||||
return BSP_FDCAN_DefaultIdParser(original_id, frame_type);
|
||||
}
|
||||
/* */
|
||||
|
||||
int8_t BSP_FDCAN_WaitTxMailboxEmpty(BSP_FDCAN_t can, uint32_t timeout) {
|
||||
if (!inited) {
|
||||
return BSP_ERR_INITED;
|
||||
}
|
||||
if (can >= BSP_FDCAN_NUM) {
|
||||
return BSP_ERR;
|
||||
}
|
||||
|
||||
FDCAN_HandleTypeDef *hfdcan = BSP_FDCAN_GetHandle(can);
|
||||
if (hfdcan == NULL) {
|
||||
return BSP_ERR_NULL;
|
||||
}
|
||||
|
||||
uint32_t start_time = HAL_GetTick();
|
||||
|
||||
// 如果超时时间为0，立即检查并返回
|
||||
if (timeout == 0) {
|
||||
uint32_t free_level = HAL_FDCAN_GetTxFifoFreeLevel(hfdcan);
|
||||
return (free_level > 0) ? BSP_OK : BSP_ERR_TIMEOUT;
|
||||
}
|
||||
|
||||
// 等待至少有一个 FIFO 空闲
|
||||
while (true) {
|
||||
uint32_t free_level = HAL_FDCAN_GetTxFifoFreeLevel(hfdcan);
|
||||
if (free_level > 0) {
|
||||
return BSP_OK;
|
||||
}
|
||||
|
||||
// 检查超时
|
||||
if (timeout != BSP_FDCAN_TIMEOUT_FOREVER) {
|
||||
uint32_t elapsed = HAL_GetTick() - start_time;
|
||||
if (elapsed >= timeout) {
|
||||
return BSP_ERR_TIMEOUT;
|
||||
}
|
||||
}
|
||||
|
||||
osDelay(1);
|
||||
}
|
||||
}
|
||||
|
||||
@ -132,6 +132,7 @@ int32_t BSP_FDCAN_GetQueueCount(BSP_FDCAN_t can, uint32_t can_id);
|
||||
int8_t BSP_FDCAN_FlushQueue(BSP_FDCAN_t can, uint32_t can_id);
|
||||
int8_t BSP_FDCAN_RegisterIdParser(BSP_FDCAN_IdParser_t parser);
|
||||
uint32_t BSP_FDCAN_ParseId(uint32_t original_id, BSP_FDCAN_FrameType_t frame_type);
|
||||
int8_t BSP_FDCAN_WaitTxMailboxEmpty(BSP_FDCAN_t can, uint32_t timeout);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
271
User/device/dm_imu.c
Normal file
271
User/device/dm_imu.c
Normal file
@ -0,0 +1,271 @@
|
||||
/*
|
||||
DM_IMU数据获取（CAN）
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "dm_imu.h"
|
||||
|
||||
#include "bsp/fdcan.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_FDCAN_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_FDCAN_StdDataFrame_t frame = {
|
||||
.id = imu->param.can_id,
|
||||
.dlc = 4,
|
||||
};
|
||||
memcpy(frame.data, tx_data, 4);
|
||||
BSP_FDCAN_WaitTxMailboxEmpty(imu->param.can, 1); // 等待发送邮箱空闲
|
||||
int8_t result = BSP_FDCAN_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_FDCAN_Message_t msg;
|
||||
int8_t result;
|
||||
bool data_received = false;
|
||||
|
||||
// 持续接收所有可用消息
|
||||
while ((result = BSP_FDCAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE)) == BSP_OK) {
|
||||
// 验证回复数据格式（至少检查数据长度）
|
||||
if (msg.dlc < 3) {
|
||||
continue; // 跳过无效消息
|
||||
}
|
||||
|
||||
// 根据数据位的第0位确定反馈报文类型
|
||||
uint8_t rid = msg.data[0] & 0x0F; // 取第0位的低4位作为RID
|
||||
|
||||
// 根据RID类型解析数据
|
||||
int8_t parse_result = DEVICE_ERR;
|
||||
switch (rid) {
|
||||
case 0x01: // RID_ACCL
|
||||
parse_result = DM_IMU_ParseAccelData(imu, msg.data, msg.dlc);
|
||||
break;
|
||||
case 0x02: // RID_GYRO
|
||||
parse_result = DM_IMU_ParseGyroData(imu, msg.data, msg.dlc);
|
||||
break;
|
||||
case 0x03: // RID_EULER
|
||||
parse_result = DM_IMU_ParseEulerData(imu, msg.data, msg.dlc);
|
||||
break;
|
||||
case 0x04: // RID_QUATERNION
|
||||
parse_result = DM_IMU_ParseQuaternionData(imu, msg.data, msg.dlc);
|
||||
break;
|
||||
default:
|
||||
continue; // 跳过未知类型的消息
|
||||
}
|
||||
|
||||
// 如果解析成功，标记为收到数据
|
||||
if (parse_result == DEVICE_OK) {
|
||||
data_received = true;
|
||||
}
|
||||
}
|
||||
|
||||
// 如果收到任何有效数据，更新设备状态
|
||||
if (data_received) {
|
||||
imu->header.online = true;
|
||||
imu->header.last_online_time = BSP_TIME_Get_ms();
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz）
|
||||
*/
|
||||
int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){
|
||||
if (imu == NULL) {
|
||||
return DEVICE_ERR_NULL;
|
||||
}
|
||||
switch (count) {
|
||||
case 0:
|
||||
DM_IMU_Request(imu, RID_ACCL);
|
||||
break;
|
||||
case 1:
|
||||
DM_IMU_Request(imu, RID_GYRO);
|
||||
break;
|
||||
case 2:
|
||||
DM_IMU_Request(imu, RID_EULER);
|
||||
break;
|
||||
case 3:
|
||||
DM_IMU_Request(imu, RID_QUATERNION);
|
||||
DM_IMU_Update(imu); // 更新所有数据
|
||||
break;
|
||||
}
|
||||
count++;
|
||||
if (count >= 4) {
|
||||
count = 0; // 重置计数器
|
||||
}
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 检查设备是否在线
|
||||
*/
|
||||
bool DM_IMU_IsOnline(DM_IMU_t *imu) {
|
||||
if (imu == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
uint32_t current_time = BSP_TIME_Get_ms();
|
||||
return imu->header.online &&
|
||||
(current_time - imu->header.last_online_time < DM_IMU_OFFLINE_TIMEOUT);
|
||||
}
|
||||
90
User/device/dm_imu.h
Normal file
90
User/device/dm_imu.h
Normal file
@ -0,0 +1,90 @@
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "device/device.h"
|
||||
#include "component/ahrs/ahrs.h"
|
||||
#include "bsp/fdcan.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_FDCAN_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
|
||||
165
User/device/gimbal_imu.c
Normal file
165
User/device/gimbal_imu.c
Normal file
@ -0,0 +1,165 @@
|
||||
/*
|
||||
* Gimbal IMU Device - 接收来自云台的CAN IMU数据
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "gimbal_imu.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#include "bsp/fdcan.h"
|
||||
#include "bsp/time.h"
|
||||
|
||||
/* Private function prototypes ---------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @brief 无符号整数转换为浮点数
|
||||
*
|
||||
* @param x_int 输入的整数值
|
||||
* @param x_min 最小值
|
||||
* @param x_max 最大值
|
||||
* @param bits 位数
|
||||
* @return float 转换后的浮点数
|
||||
*/
|
||||
static float uint_to_float(uint32_t 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 解析CAN消息中的IMU数据
|
||||
*/
|
||||
static void GIMBAL_IMU_ParseMessage(GIMBAL_IMU_FROMCAN_t *gimbal_imu, uint32_t id, const uint8_t *data) {
|
||||
if (gimbal_imu == NULL || data == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* 判断是哪个ID的数据 */
|
||||
if (id == gimbal_imu->param.accl_id) {
|
||||
/* 解析加速度计数据 - 21位精度解包 */
|
||||
uint32_t acc_x = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
|
||||
uint32_t acc_y = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
|
||||
uint32_t acc_z = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
|
||||
|
||||
gimbal_imu->data.accl.x = uint_to_float(acc_x, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
|
||||
gimbal_imu->data.accl.y = uint_to_float(acc_y, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
|
||||
gimbal_imu->data.accl.z = uint_to_float(acc_z, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
|
||||
|
||||
} else if (id == gimbal_imu->param.gyro_id) {
|
||||
/* 解析陀螺仪数据 - 21位精度解包 */
|
||||
uint32_t gyro_x = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
|
||||
uint32_t gyro_y = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
|
||||
uint32_t gyro_z = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
|
||||
|
||||
gimbal_imu->data.gyro.x = uint_to_float(gyro_x, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
|
||||
gimbal_imu->data.gyro.y = uint_to_float(gyro_y, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
|
||||
gimbal_imu->data.gyro.z = uint_to_float(gyro_z, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
|
||||
|
||||
} else if (id == gimbal_imu->param.eulr_id) {
|
||||
/* 解析欧拉角数据 - 21位精度解包 */
|
||||
uint32_t pit = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
|
||||
uint32_t yaw = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
|
||||
uint32_t rol = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
|
||||
|
||||
gimbal_imu->data.eulr.pit = uint_to_float(pit, GIMBAL_IMU_PITCH_MIN, GIMBAL_IMU_PITCH_MAX, 21);
|
||||
gimbal_imu->data.eulr.yaw = uint_to_float(yaw, GIMBAL_IMU_YAW_MIN, GIMBAL_IMU_YAW_MAX, 21);
|
||||
gimbal_imu->data.eulr.rol = uint_to_float(rol, GIMBAL_IMU_ROLL_MIN, GIMBAL_IMU_ROLL_MAX, 21);
|
||||
|
||||
} else if (id == gimbal_imu->param.quat_id) {
|
||||
/* 解析四元数数据 - 使用14位精度解包 */
|
||||
uint16_t q0 = ((uint16_t)data[1] << 6) | ((uint16_t)data[2] >> 2);
|
||||
uint16_t q1 = (((uint16_t)data[2] & 0x03) << 12) | ((uint16_t)data[3] << 4) | ((uint16_t)data[4] >> 4);
|
||||
uint16_t q2 = (((uint16_t)data[4] & 0x0F) << 10) | ((uint16_t)data[5] << 2) | ((uint16_t)data[6] >> 6);
|
||||
uint16_t q3 = (((uint16_t)data[6] & 0x3F) << 8) | (uint16_t)data[7];
|
||||
|
||||
gimbal_imu->data.quat.q0 = uint_to_float(q0, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
|
||||
gimbal_imu->data.quat.q1 = uint_to_float(q1, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
|
||||
gimbal_imu->data.quat.q2 = uint_to_float(q2, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
|
||||
gimbal_imu->data.quat.q3 = uint_to_float(q3, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
|
||||
}
|
||||
}
|
||||
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @brief 初始化Gimbal IMU设备
|
||||
* @param gimbal_imu Gimbal IMU结构体指针
|
||||
* @param param 参数配置
|
||||
* @return 初始化结果
|
||||
* - DEVICE_OK: 成功
|
||||
* - DEVICE_ERR_NULL: 参数为空
|
||||
*/
|
||||
int8_t GIMBAL_IMU_Init(GIMBAL_IMU_FROMCAN_t *gimbal_imu, const GIMBAL_IMU_Param_t *param) {
|
||||
if (gimbal_imu == NULL || param == NULL) {
|
||||
return DEVICE_ERR_NULL;
|
||||
}
|
||||
|
||||
/* 清空数据 */
|
||||
memset(gimbal_imu, 0, sizeof(GIMBAL_IMU_FROMCAN_t));
|
||||
|
||||
/* 复制参数 */
|
||||
gimbal_imu->param = *param;
|
||||
|
||||
/* 设置设备类型 */
|
||||
gimbal_imu->header.online = false;
|
||||
|
||||
/* 注册CAN ID到消息队列 */
|
||||
BSP_FDCAN_RegisterId(param->can, param->accl_id, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
|
||||
BSP_FDCAN_RegisterId(param->can, param->gyro_id, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
|
||||
BSP_FDCAN_RegisterId(param->can, param->eulr_id, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
|
||||
BSP_FDCAN_RegisterId(param->can, param->quat_id, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
|
||||
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 更新Gimbal IMU设备状态 - 从CAN队列读取并解析数据
|
||||
* @param gimbal_imu Gimbal IMU结构体指针
|
||||
* @return 更新结果
|
||||
* - DEVICE_OK: 成功
|
||||
* - DEVICE_ERR_NULL: 参数为空
|
||||
*/
|
||||
int8_t GIMBAL_IMU_Update(GIMBAL_IMU_FROMCAN_t *gimbal_imu) {
|
||||
if (gimbal_imu == NULL) {
|
||||
return DEVICE_ERR_NULL;
|
||||
}
|
||||
|
||||
BSP_FDCAN_Message_t msg;
|
||||
bool received = false;
|
||||
|
||||
/* 读取所有可用的CAN消息 */
|
||||
while (BSP_FDCAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.accl_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||||
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.accl_id, msg.data);
|
||||
received = true;
|
||||
}
|
||||
|
||||
while (BSP_FDCAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.gyro_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||||
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.gyro_id, msg.data);
|
||||
received = true;
|
||||
}
|
||||
|
||||
while (BSP_FDCAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.eulr_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||||
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.eulr_id, msg.data);
|
||||
received = true;
|
||||
}
|
||||
|
||||
while (BSP_FDCAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.quat_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||||
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.quat_id, msg.data);
|
||||
received = true;
|
||||
}
|
||||
|
||||
/* 更新在线状态和时间戳 */
|
||||
if (received) {
|
||||
gimbal_imu->header.online = true;
|
||||
gimbal_imu->header.last_online_time = BSP_TIME_Get();
|
||||
} else {
|
||||
/* 超时检测 - 100ms */
|
||||
uint64_t now_time = BSP_TIME_Get();
|
||||
if (now_time - gimbal_imu->header.last_online_time > 100000) { /* 100ms超时，单位微秒 */
|
||||
gimbal_imu->header.online = false;
|
||||
}
|
||||
}
|
||||
|
||||
return DEVICE_OK;
|
||||
}
|
||||
88
User/device/gimbal_imu.h
Normal file
88
User/device/gimbal_imu.h
Normal file
@ -0,0 +1,88 @@
|
||||
/*
|
||||
* Gimbal IMU Device - 接收来自云台的CAN IMU数据
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <math.h>
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "bsp/fdcan.h"
|
||||
#include "component/ahrs/ahrs.h"
|
||||
#include "device/device.h"
|
||||
|
||||
/* Exported constants ------------------------------------------------------- */
|
||||
|
||||
/* 数据范围定义（与发送端保持一致） */
|
||||
#define GIMBAL_IMU_ACCEL_MAX (58.8f)
|
||||
#define GIMBAL_IMU_ACCEL_MIN (-58.8f)
|
||||
#define GIMBAL_IMU_GYRO_MAX (34.88f)
|
||||
#define GIMBAL_IMU_GYRO_MIN (-34.88f)
|
||||
#define GIMBAL_IMU_PITCH_MAX (90.0f)
|
||||
#define GIMBAL_IMU_PITCH_MIN (-90.0f)
|
||||
#define GIMBAL_IMU_ROLL_MAX (180.0f)
|
||||
#define GIMBAL_IMU_ROLL_MIN (-180.0f)
|
||||
#define GIMBAL_IMU_YAW_MAX (180.0f)
|
||||
#define GIMBAL_IMU_YAW_MIN (-180.0f)
|
||||
#define GIMBAL_IMU_QUAT_MIN (-1.0f)
|
||||
#define GIMBAL_IMU_QUAT_MAX (1.0f)
|
||||
|
||||
/* Exported macro ----------------------------------------------------------- */
|
||||
/* Exported types ----------------------------------------------------------- */
|
||||
|
||||
/* Gimbal IMU参数配置 */
|
||||
typedef struct {
|
||||
BSP_FDCAN_t can; /* 使用的CAN总线 */
|
||||
uint16_t accl_id;
|
||||
uint16_t gyro_id;
|
||||
uint16_t eulr_id;
|
||||
uint16_t quat_id;
|
||||
} GIMBAL_IMU_Param_t;
|
||||
|
||||
typedef struct {
|
||||
AHRS_Accl_t accl; /* 加速度计数据 */
|
||||
AHRS_Gyro_t gyro; /* 陀螺仪数据 */
|
||||
AHRS_Eulr_t eulr; /* 欧拉角数据 */
|
||||
AHRS_Quaternion_t quat; /* 四元数数据 */
|
||||
float temp; /* 温度数据 (摄氏度) */
|
||||
} GIMBAL_IMU_Data_t;
|
||||
|
||||
|
||||
/* Gimbal IMU完整数据结构 */
|
||||
typedef struct {
|
||||
DEVICE_Header_t header;
|
||||
GIMBAL_IMU_Param_t param; /* 参数配置 */
|
||||
GIMBAL_IMU_Data_t data; /* IMU数据 */
|
||||
} GIMBAL_IMU_FROMCAN_t;
|
||||
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @brief 初始化Gimbal IMU设备
|
||||
* @param gimbal_imu Gimbal IMU结构体指针
|
||||
* @param param 参数配置
|
||||
* @return 初始化结果
|
||||
* - DEVICE_OK: 成功
|
||||
* - DEVICE_ERR_NULL: 参数为空
|
||||
*/
|
||||
int8_t GIMBAL_IMU_Init(GIMBAL_IMU_FROMCAN_t *gimbal_imu, const GIMBAL_IMU_Param_t *param);
|
||||
|
||||
/**
|
||||
* @brief 更新Gimbal IMU设备状态
|
||||
* @param gimbal_imu Gimbal IMU结构体指针
|
||||
* @return 更新结果
|
||||
* - DEVICE_OK: 成功
|
||||
* - DEVICE_ERR_NULL: 参数为空
|
||||
*/
|
||||
int8_t GIMBAL_IMU_Update(GIMBAL_IMU_FROMCAN_t *gimbal_imu);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
@ -124,7 +124,7 @@ int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
|
||||
c->feedback.motor[i].temp = 0;
|
||||
}
|
||||
//³õʼ»¯PIDºÍ»ìºÏÆ÷
|
||||
PID_Init(&c->pid.follow, KPID_MODE_NO_D, target_freq, ¶m->pid.follow_pid_param);
|
||||
PID_Init(&c->pid.follow, KPID_MODE_CALC_D, target_freq, ¶m->pid.follow_pid_param);
|
||||
Mixer_Init(&c->mixer, mixer_mode);
|
||||
//ÇåÁãÔ˶¯ÏòÁ¿ºÍÊä³ö
|
||||
c->move_vec.vx = c->move_vec.vy = c->move_vec.wz = 0.0f;
|
||||
|
||||
@ -38,7 +38,7 @@ static void CMD_RC_BuildChassisCmd(CMD_t *ctx) {
|
||||
/* 从RC输入生成云台命令 */
|
||||
static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) {
|
||||
CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
|
||||
|
||||
ctx->output.gimbal.cmd.is_ai = false;
|
||||
/* 根据拨杆选择云台模式 */
|
||||
switch (ctx->input.rc.sw[0]) {
|
||||
case CMD_SW_UP:
|
||||
@ -56,8 +56,8 @@ static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) {
|
||||
}
|
||||
|
||||
/* 左摇杆控制云台 */
|
||||
ctx->output.gimbal.cmd.delta_yaw = -ctx->input.rc.joy_left.x * 2.0f;
|
||||
ctx->output.gimbal.cmd.delta_pit = -ctx->input.rc.joy_left.y * 1.5f;
|
||||
ctx->output.gimbal.cmd.delta_yaw = -ctx->input.rc.joy_left.x * 4.0f;
|
||||
ctx->output.gimbal.cmd.delta_pit = -ctx->input.rc.joy_left.y * 2.5f;
|
||||
}
|
||||
|
||||
/* 从RC输入生成射击命令 */
|
||||
@ -143,7 +143,7 @@ static void CMD_PC_BuildGimbalCmd(CMD_t *ctx) {
|
||||
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
|
||||
return;
|
||||
}
|
||||
|
||||
ctx->output.gimbal.cmd.is_ai = false;
|
||||
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
|
||||
/* 鼠标控制云台 */
|
||||
@ -191,9 +191,14 @@ static void CMD_NUC_BuildGimbalCmd(CMD_t *ctx) {
|
||||
}
|
||||
|
||||
/* 使用AI提供的云台控制数据 */
|
||||
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
ctx->output.gimbal.cmd.delta_yaw = ctx->input.nuc.gimbal.setpoint.yaw;
|
||||
ctx->output.gimbal.cmd.delta_pit = ctx->input.nuc.gimbal.setpoint.pit;
|
||||
|
||||
if (ctx->input.nuc.mode!=0) {
|
||||
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELATIVE;
|
||||
ctx->output.gimbal.cmd.is_ai = true;
|
||||
ctx->output.gimbal.cmd.setpoint_yaw = ctx->input.nuc.gimbal.setpoint.yaw;
|
||||
ctx->output.gimbal.cmd.setpoint_pit = ctx->input.nuc.gimbal.setpoint.pit;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* 从NUC/AI输入生成射击命令 */
|
||||
@ -205,17 +210,23 @@ static void CMD_NUC_BuildShootCmd(CMD_t *ctx) {
|
||||
|
||||
/* 根据AI模式决定射击行为 */
|
||||
switch (ctx->input.nuc.mode) {
|
||||
case 1:
|
||||
case 0:
|
||||
ctx->output.shoot.cmd.ready = false;
|
||||
ctx->output.shoot.cmd.firecmd = false;
|
||||
break;
|
||||
case 2:
|
||||
case 1:
|
||||
ctx->output.shoot.cmd.ready = true;
|
||||
ctx->output.shoot.cmd.firecmd = false;
|
||||
break;
|
||||
case 3:
|
||||
ctx->output.shoot.cmd.ready = true;
|
||||
ctx->output.shoot.cmd.firecmd = true;
|
||||
case 2:
|
||||
if (ctx->input.rc.sw[1]==CMD_SW_DOWN) {
|
||||
ctx->output.shoot.cmd.ready = true;
|
||||
ctx->output.shoot.cmd.firecmd = true;
|
||||
}else {
|
||||
ctx->output.shoot.cmd.ready = true;
|
||||
ctx->output.shoot.cmd.firecmd = false;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@ -8,6 +8,7 @@
|
||||
#include "device/motor_dm.h"
|
||||
#include "module/cmd/cmd.h"
|
||||
#include <stdbool.h>
|
||||
#include "device/gimbal_imu.h"
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
@ -15,9 +16,21 @@
|
||||
|
||||
/* Exported variables ------------------------------------------------------- */
|
||||
|
||||
|
||||
|
||||
// 机器人参数配置
|
||||
Config_RobotParam_t robot_config = {
|
||||
|
||||
.ai_param = {
|
||||
.can = BSP_FDCAN_2,
|
||||
.vision_id = 0x104,
|
||||
},
|
||||
.imu_param = {
|
||||
.can=BSP_FDCAN_2,
|
||||
.accl_id=0x100,
|
||||
.gyro_id=0x101,
|
||||
.eulr_id=0x102,
|
||||
.quat_id=0x103,
|
||||
},
|
||||
.chassis_param = {
|
||||
/* DJI3508µç»ú*/
|
||||
.motor_param = {
|
||||
@ -55,7 +68,7 @@ Config_RobotParam_t robot_config = {
|
||||
.pid = {
|
||||
/* µ×Å̵ç»ú PID */
|
||||
.motor_pid_param = {
|
||||
.k = 0.001f,
|
||||
.k = 0.0015f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
@ -67,10 +80,10 @@ Config_RobotParam_t robot_config = {
|
||||
|
||||
/* ¸úËæ */
|
||||
.follow_pid_param = {
|
||||
.k = 0.5f,
|
||||
.k = 1.2f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i = 0.5f,
|
||||
.d = 0.01f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
@ -96,54 +109,98 @@ Config_RobotParam_t robot_config = {
|
||||
|
||||
.gimbal_param = {
|
||||
.pid = {
|
||||
.yaw_omega = {
|
||||
.k = 1.0f,
|
||||
.p = 1.0f,
|
||||
.i = 0.3f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
.absolute = {
|
||||
.yaw_omega = {
|
||||
.k = 1.0f,//1
|
||||
.p = 1.0f,
|
||||
.i = 0.3f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
},
|
||||
.yaw_angle = {
|
||||
.k = 4.0f,
|
||||
.p = 5.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 10.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
},
|
||||
.pit_omega = {
|
||||
.k = 1.0f,//0.4
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
},
|
||||
.pit_angle = {
|
||||
.k = 6.0f,
|
||||
.p = 3.0f,
|
||||
.i = 2.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 2.0f,
|
||||
.out_limit = 10.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
},
|
||||
},
|
||||
.yaw_angle = {
|
||||
.k = 6.0f,
|
||||
.p = 4.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.05f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 10.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
},
|
||||
.pit_omega = {
|
||||
.k = 0.4f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
},
|
||||
.pit_angle = {
|
||||
.k = 8.0f,
|
||||
.p = 5.0f,
|
||||
.i = 2.5f,
|
||||
.d = 0.03f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 10.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
.relative = {
|
||||
.yaw_omega = {
|
||||
.k = 0.8f,
|
||||
.p = 0.8f,
|
||||
.i = 0.3f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
},
|
||||
.yaw_angle = {
|
||||
.k = 5.0f,
|
||||
.p = 2.5f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 10.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
},
|
||||
.pit_omega = {
|
||||
.k = 0.8f,//0.4
|
||||
.p = 0.8f,
|
||||
.i = 0.3f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
},
|
||||
.pit_angle = {
|
||||
.k = 5.0f,
|
||||
.p = 2.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.25f,
|
||||
.i_limit = 2.0f,
|
||||
.out_limit = 10.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
},
|
||||
},
|
||||
},
|
||||
.mech_zero = {
|
||||
.yaw = 0.0f,
|
||||
.pit = 2.98220015, //0.195206895
|
||||
.pit = 3.15f,
|
||||
},
|
||||
.travel = {
|
||||
.yaw = -1.0f,
|
||||
.pit = 0.6,
|
||||
.pit = 0.6f,
|
||||
},
|
||||
.low_pass_cutoff_freq = {
|
||||
.out = -1.0f,
|
||||
@ -338,9 +395,9 @@ Config_RobotParam_t robot_config = {
|
||||
.move_slow_mult = 0.5f,
|
||||
},
|
||||
.rc_mode_map = {
|
||||
.sw_left_up = CHASSIS_MODE_BREAK,
|
||||
.sw_left_up = CHASSIS_MODE_RELAX,
|
||||
.sw_left_mid = CHASSIS_MODE_FOLLOW_GIMBAL,
|
||||
.sw_left_down = CHASSIS_MODE_BREAK,
|
||||
.sw_left_down = CHASSIS_MODE_RELAX,
|
||||
.gimbal_sw_up = GIMBAL_MODE_ABSOLUTE,
|
||||
.gimbal_sw_mid = GIMBAL_MODE_RELATIVE,
|
||||
.gimbal_sw_down = GIMBAL_MODE_ABSOLUTE,
|
||||
|
||||
@ -18,12 +18,16 @@ extern "C" {
|
||||
#include "module/track.h"
|
||||
#include "module/cmd/cmd.h"
|
||||
#include "component/PowerControl.h"
|
||||
#include "device/gimbal_imu.h"
|
||||
#include "module/vision_bridge.h"
|
||||
typedef struct {
|
||||
Shoot_Params_t shoot_param;
|
||||
Gimbal_Params_t gimbal_param;
|
||||
Chassis_Params_t chassis_param;
|
||||
Track_Params_t track_param;
|
||||
CMD_Config_t cmd_param;
|
||||
GIMBAL_IMU_Param_t imu_param;
|
||||
AI_Param_t ai_param;
|
||||
} Config_RobotParam_t;
|
||||
|
||||
extern power_model_t cha;
|
||||
|
||||
@ -31,10 +31,15 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
|
||||
if (mode == g->mode)
|
||||
return GIMBAL_OK;
|
||||
|
||||
PID_Reset(&g->pid.yaw_angle);
|
||||
PID_Reset(&g->pid.yaw_omega);
|
||||
PID_Reset(&g->pid.pit_angle);
|
||||
PID_Reset(&g->pid.pit_omega);
|
||||
PID_Reset(&g->pid.absolute.yaw_angle);
|
||||
PID_Reset(&g->pid.absolute.yaw_omega);
|
||||
PID_Reset(&g->pid.absolute.pit_angle);
|
||||
PID_Reset(&g->pid.absolute.pit_omega);
|
||||
PID_Reset(&g->pid.relative.yaw_angle);
|
||||
PID_Reset(&g->pid.relative.yaw_omega);
|
||||
PID_Reset(&g->pid.relative.pit_angle);
|
||||
PID_Reset(&g->pid.relative.pit_omega);
|
||||
|
||||
LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f);
|
||||
LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
|
||||
|
||||
@ -77,14 +82,23 @@ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Params_t *param,
|
||||
g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */
|
||||
|
||||
/* 初始化云台电机控制PID和LPF */
|
||||
PID_Init(&(g->pid.yaw_angle), KPID_MODE_CALC_D, target_freq,
|
||||
&(g->param->pid.yaw_angle));
|
||||
PID_Init(&(g->pid.yaw_omega), KPID_MODE_CALC_D, target_freq,
|
||||
&(g->param->pid.yaw_omega));
|
||||
PID_Init(&(g->pid.pit_angle), KPID_MODE_CALC_D, target_freq,
|
||||
&(g->param->pid.pit_angle));
|
||||
PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq,
|
||||
&(g->param->pid.pit_omega));
|
||||
PID_Init(&(g->pid.absolute.yaw_angle), KPID_MODE_CALC_D, target_freq,
|
||||
&(g->param->pid.absolute.yaw_angle));
|
||||
PID_Init(&(g->pid.absolute.yaw_omega), KPID_MODE_CALC_D, target_freq,
|
||||
&(g->param->pid.absolute.yaw_omega));
|
||||
PID_Init(&(g->pid.absolute.pit_angle), KPID_MODE_CALC_D, target_freq,
|
||||
&(g->param->pid.absolute.pit_angle));
|
||||
PID_Init(&(g->pid.absolute.pit_omega), KPID_MODE_CALC_D, target_freq,
|
||||
&(g->param->pid.absolute.pit_omega));
|
||||
|
||||
PID_Init(&(g->pid.relative.yaw_angle), KPID_MODE_CALC_D, target_freq,
|
||||
&(g->param->pid.relative.yaw_angle));
|
||||
PID_Init(&(g->pid.relative.yaw_omega), KPID_MODE_CALC_D, target_freq,
|
||||
&(g->param->pid.relative.yaw_omega));
|
||||
PID_Init(&(g->pid.relative.pit_angle), KPID_MODE_CALC_D, target_freq,
|
||||
&(g->param->pid.relative.pit_angle));
|
||||
PID_Init(&(g->pid.relative.pit_omega), KPID_MODE_CALC_D, target_freq,
|
||||
&(g->param->pid.relative.pit_omega));
|
||||
|
||||
LowPassFilter2p_Init(&g->filter_out.yaw, target_freq,
|
||||
g->param->low_pass_cutoff_freq.out);
|
||||
@ -162,6 +176,8 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
|
||||
|
||||
Gimbal_SetMode(g, g_cmd->mode);
|
||||
|
||||
g->out.pit_feedforward = g_cmd->feedforward_pit;
|
||||
|
||||
/* 处理yaw控制命令，软件限位 - 使用电机绝对角度 */
|
||||
float delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f;
|
||||
if (g->param->travel.yaw > 0) {
|
||||
@ -221,6 +237,10 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
|
||||
CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
|
||||
// g->setpoint.eulr.pit = (g_cmd->delta_pit)/3.0f + 2.98f + 1/3.0f; //2.98f为机械零点位置
|
||||
|
||||
if (g_cmd->is_ai) {
|
||||
g->setpoint.eulr.yaw = g_cmd->setpoint_yaw;
|
||||
g->setpoint.eulr.pit = g_cmd->setpoint_pit;
|
||||
}
|
||||
/* 控制相关逻辑 */
|
||||
float yaw_omega_set_point, pit_omega_set_point;
|
||||
switch (g->mode) {
|
||||
@ -230,14 +250,14 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
|
||||
break;
|
||||
|
||||
case GIMBAL_MODE_ABSOLUTE:
|
||||
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
|
||||
yaw_omega_set_point = PID_Calc(&(g->pid.absolute.yaw_angle), g->setpoint.eulr.yaw,
|
||||
g->feedback.motor.yaw.rotor_abs_angle, 0.0f, g->dt);
|
||||
g->out.yaw = PID_Calc(&(g->pid.pit_omega), yaw_omega_set_point,
|
||||
g->out.yaw = PID_Calc(&(g->pid.absolute.yaw_omega), yaw_omega_set_point,
|
||||
g->feedback.imu.gyro.z, 0.f, g->dt);
|
||||
|
||||
pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
|
||||
pit_omega_set_point = PID_Calc(&(g->pid.absolute.pit_angle), g->setpoint.eulr.pit,
|
||||
g->feedback.motor.pit.rotor_abs_angle, 0.0f, g->dt);
|
||||
g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
|
||||
g->out.pit = PID_Calc(&(g->pid.absolute.pit_omega), pit_omega_set_point,
|
||||
g->feedback.imu.gyro.x, 0.f, g->dt);
|
||||
|
||||
/* 输出滤波 */
|
||||
@ -246,14 +266,14 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
|
||||
break;
|
||||
|
||||
case GIMBAL_MODE_RELATIVE:
|
||||
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
|
||||
yaw_omega_set_point = PID_Calc(&(g->pid.relative.yaw_angle), g->setpoint.eulr.yaw,
|
||||
g->feedback.imu.eulr.yaw, 0.0f, g->dt);
|
||||
g->out.yaw = PID_Calc(&(g->pid.pit_omega), yaw_omega_set_point,
|
||||
g->out.yaw = PID_Calc(&(g->pid.relative.yaw_omega), yaw_omega_set_point,
|
||||
g->feedback.imu.gyro.z, 0.f, g->dt);
|
||||
|
||||
pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
|
||||
pit_omega_set_point = PID_Calc(&(g->pid.relative.pit_angle), g->setpoint.eulr.pit,
|
||||
g->feedback.imu.eulr.pit, 0.0f, g->dt);
|
||||
g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
|
||||
g->out.pit = PID_Calc(&(g->pid.relative.pit_omega), pit_omega_set_point,
|
||||
g->feedback.imu.gyro.x, 0.f, g->dt);
|
||||
|
||||
/* 输出滤波 */
|
||||
@ -273,7 +293,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
|
||||
*/
|
||||
void Gimbal_Output(Gimbal_t *g){
|
||||
MOTOR_MIT_Output_t output_pit = {0};
|
||||
output_pit.torque = g->out.pit * 5.0f; // 乘以减速比
|
||||
output_pit.torque = g->out.pit * 5.0f; //+ g->out.pit_feedforward; // 乘以减速比 并加上前馈
|
||||
output_pit.kd = 1.0f;
|
||||
MOTOR_DM_MITCtrl(&g->param->pit_motor, &output_pit);
|
||||
|
||||
|
||||
@ -36,6 +36,10 @@ typedef struct {
|
||||
Gimbal_Mode_t mode;
|
||||
float delta_yaw;
|
||||
float delta_pit;
|
||||
float feedforward_pit;
|
||||
float setpoint_yaw;
|
||||
float setpoint_pit;
|
||||
bool is_ai;
|
||||
} Gimbal_CMD_t;
|
||||
|
||||
/* 软件限位 */
|
||||
@ -49,10 +53,19 @@ typedef struct {
|
||||
MOTOR_DM_Param_t pit_motor; /* pitch轴电机参数 */
|
||||
MOTOR_DM_Param_t yaw_motor; /* yaw轴电机参数 */
|
||||
struct {
|
||||
KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */
|
||||
KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */
|
||||
KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */
|
||||
KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */
|
||||
struct {
|
||||
KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */
|
||||
KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */
|
||||
KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */
|
||||
KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */
|
||||
} absolute;
|
||||
|
||||
struct {
|
||||
KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */
|
||||
KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */
|
||||
KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */
|
||||
KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */
|
||||
} relative;
|
||||
} pid;
|
||||
|
||||
/* 低通滤波器截止频率 */
|
||||
@ -90,6 +103,7 @@ typedef struct {
|
||||
typedef struct {
|
||||
float yaw; /* yaw轴电机输出 */
|
||||
float pit; /* pitch轴电机输出 */
|
||||
float pit_feedforward;
|
||||
} Gimbal_Output_t;
|
||||
/*
|
||||
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体。
|
||||
@ -110,10 +124,18 @@ typedef struct {
|
||||
} setpoint;
|
||||
|
||||
struct {
|
||||
KPID_t yaw_angle; /* yaw轴角位置环PID */
|
||||
KPID_t yaw_omega; /* yaw轴角速度环PID */
|
||||
KPID_t pit_angle; /* pitch轴角位置环PID */
|
||||
KPID_t pit_omega; /* pitch轴角速度环PID */
|
||||
struct {
|
||||
KPID_t yaw_angle; /* yaw轴角位置环PID */
|
||||
KPID_t yaw_omega; /* yaw轴角速度环PID */
|
||||
KPID_t pit_angle; /* pitch轴角位置环PID */
|
||||
KPID_t pit_omega; /* pitch轴角速度环PID */
|
||||
} absolute;
|
||||
struct {
|
||||
KPID_t yaw_angle; /* yaw轴角位置环PID */
|
||||
KPID_t yaw_omega; /* yaw轴角速度环PID */
|
||||
KPID_t pit_angle; /* pitch轴角位置环PID */
|
||||
KPID_t pit_omega; /* pitch轴角速度环PID */
|
||||
} relative;
|
||||
} pid;
|
||||
|
||||
struct {
|
||||
|
||||
@ -46,7 +46,7 @@ void Task(void *argument) {
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
static bool last_firecmd;
|
||||
|
||||
float maxTrigrpm=1500.0f;
|
||||
float maxTrigrpm=4000.0f;
|
||||
/* Private function -------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
@ -177,7 +177,7 @@ int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
|
||||
s->target_variable.fric_rpm=5000.0f;
|
||||
break;
|
||||
case SHOOT_PROJECTILE_42MM:
|
||||
s->target_variable.fric_rpm=6500.0f;
|
||||
s->target_variable.fric_rpm=3600.0f;//6500
|
||||
break;
|
||||
}
|
||||
return SHOOT_OK;
|
||||
|
||||
@ -3,10 +3,21 @@
|
||||
#include "bsp/uart.h"
|
||||
#include "component/crc16.h"
|
||||
#include <string.h>
|
||||
#include "bsp/fdcan.h"
|
||||
|
||||
int8_t AI_Init(AI_t *ai) {
|
||||
#define AI_CMD_CAN_DLC (8u) /* 1字节mode + 3.5字节yaw + 3.5字节pit */
|
||||
#define AI_CMD_ANGLE_SCALE (10000000.0f) /* 0.0000001 rad per LSB */
|
||||
|
||||
int8_t AI_Init(AI_t *ai, AI_Param_t *param) {
|
||||
if (ai == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
BSP_FDCAN_Init();
|
||||
|
||||
memset(ai, 0, sizeof(AI_t));
|
||||
|
||||
ai->param = param;
|
||||
BSP_FDCAN_RegisterId(ai->param->can, ai->param->vision_id, 3);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -29,7 +40,7 @@ int8_t AI_ParseForHost(AI_t* ai, AI_RawData_t* raw_data){
|
||||
ai->tohost.q[2]=raw_data->q[2];
|
||||
ai->tohost.q[3]=raw_data->q[3];
|
||||
ai->tohost.bullet_count=10;
|
||||
ai->tohost.bullet_speed=17.5;
|
||||
ai->tohost.bullet_speed=10.5;
|
||||
|
||||
ai->tohost.crc16=CRC16_Calc(((const uint8_t*)&(ai->tohost)),sizeof(ai->tohost)-sizeof(uint16_t), CRC16_INIT );
|
||||
if(CRC16_Verify(((const uint8_t*)&(ai->tohost)), sizeof(ai->tohost))!=true){
|
||||
@ -60,3 +71,60 @@ int8_t AI_Get(AI_t *ai, AI_cmd_t* outcmd) {
|
||||
outcmd->gimbal.vel.yaw = ai->fromhost.yaw_vel;
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
|
||||
/* 打包并通过 CAN2 发送AI命令给下层板（mode + yaw + pit） */
|
||||
void AI_SendCmdOnFDCAN(AI_t *ai, const AI_cmd_t *cmd) {
|
||||
if (cmd == NULL) return;
|
||||
|
||||
/* 将float角度转换为int32_t定点数（0.0000001 rad/LSB） */
|
||||
const int32_t yaw = (int32_t)(cmd->gimbal.setpoint.yaw * AI_CMD_ANGLE_SCALE);
|
||||
const int32_t pit = (int32_t)(cmd->gimbal.setpoint.pit * AI_CMD_ANGLE_SCALE);
|
||||
|
||||
BSP_FDCAN_StdDataFrame_t frame = {0};
|
||||
frame.id = ai->param->vision_id;
|
||||
frame.dlc = AI_CMD_CAN_DLC;
|
||||
|
||||
/* mode(1字节) + yaw(3.5字节) + pit(3.5字节) */
|
||||
frame.data[0] = cmd->mode;
|
||||
frame.data[1] = (uint8_t)((yaw >> 20) & 0xFF);
|
||||
frame.data[2] = (uint8_t)((yaw >> 12) & 0xFF);
|
||||
frame.data[3] = (uint8_t)((yaw >> 4) & 0xFF);
|
||||
frame.data[4] = (uint8_t)(((yaw & 0xF) << 4) | ((pit >> 24) & 0xF));
|
||||
frame.data[5] = (uint8_t)((pit >> 16) & 0xFF);
|
||||
frame.data[6] = (uint8_t)((pit >> 8) & 0xFF);
|
||||
frame.data[7] = (uint8_t)(pit & 0xFF);
|
||||
|
||||
(void)BSP_FDCAN_TransmitStdDataFrame(ai->param->can, &frame);
|
||||
}
|
||||
|
||||
/* 从CAN消息解析AI命令 (mode + yaw + pit) */
|
||||
void AI_ParseCmdFromCan(AI_t *ai, AI_cmd_t *cmd) {
|
||||
|
||||
if (cmd == NULL) {
|
||||
return;
|
||||
}
|
||||
BSP_FDCAN_Message_t msg;
|
||||
if (BSP_FDCAN_GetMessage(ai->param->can, ai->param->vision_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) != 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* 解析mode (1字节) */
|
||||
cmd->mode = msg.data[0];
|
||||
|
||||
/* 解析yaw (3.5字节) */
|
||||
int32_t yaw_raw = (int32_t)((msg.data[1] << 20) | (msg.data[2] << 12) | (msg.data[3] << 4) | ((msg.data[4] >> 4) & 0xF));
|
||||
if (yaw_raw & 0x08000000) yaw_raw |= 0xF0000000;
|
||||
cmd->gimbal.setpoint.yaw = (float)yaw_raw / AI_CMD_ANGLE_SCALE;
|
||||
|
||||
/* 解析pit (3.5字节) */
|
||||
int32_t pit_raw = (int32_t)(((msg.data[4] & 0xF) << 24) | (msg.data[5] << 16) | (msg.data[6] << 8) | msg.data[7]);
|
||||
if (pit_raw & 0x08000000) pit_raw |= 0xF0000000;
|
||||
cmd->gimbal.setpoint.pit = (float)pit_raw / AI_CMD_ANGLE_SCALE;
|
||||
|
||||
/* 其他字段根据需要可以初始化为0 */
|
||||
cmd->gimbal.vel.yaw = 0.0f;
|
||||
cmd->gimbal.vel.pit = 0.0f;
|
||||
cmd->gimbal.accl.yaw = 0.0f;
|
||||
cmd->gimbal.accl.pit = 0.0f;
|
||||
}
|
||||
@ -5,7 +5,7 @@
|
||||
#pragma once
|
||||
|
||||
#include "component\user_math.h"
|
||||
|
||||
#include "bsp\fdcan.h"
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
@ -69,12 +69,20 @@ typedef struct {
|
||||
uint16_t bullet_count; // 子弹累计发送次数
|
||||
}AI_RawData_t;
|
||||
|
||||
typedef struct {
|
||||
BSP_FDCAN_t can;
|
||||
uint16_t vision_id;
|
||||
}AI_Param_t;
|
||||
|
||||
typedef struct __attribute__((packed)) {
|
||||
struct AI_ToHost tohost;
|
||||
struct AI_FromHost fromhost;
|
||||
AI_Param_t *param;
|
||||
}AI_t;
|
||||
|
||||
|
||||
int8_t AI_Init(AI_t *ai, AI_Param_t *param);
|
||||
|
||||
int8_t AI_StartReceiving(AI_t *ai);
|
||||
|
||||
int8_t AI_Get(AI_t *ai, AI_cmd_t* ai_cmd);
|
||||
@ -83,6 +91,9 @@ int8_t AI_ParseForHost(AI_t* ai, AI_RawData_t* raw_data);
|
||||
|
||||
int8_t AI_StartSend(AI_t *ai);
|
||||
|
||||
void AI_SendCmdOnCan(AI_t *ai, const AI_cmd_t *cmd);
|
||||
|
||||
void AI_ParseCmdFromCan(AI_t *ai, AI_cmd_t *cmd);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -17,6 +17,8 @@
|
||||
#include "module/gimbal.h"
|
||||
#include "module/chassis.h"
|
||||
#include "device/bmi088.h"
|
||||
#include "device/dm_imu.h"
|
||||
#include "module/config.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
@ -32,13 +34,14 @@ BMI088_t bmi088;
|
||||
AHRS_t chassis_ahrs;
|
||||
AHRS_Magn_t magn;
|
||||
AHRS_Eulr_t eulr_chassis;
|
||||
AHRS_Eulr_t eulr_gimbal;
|
||||
KPID_t imu_temp_ctrl_pid;
|
||||
|
||||
Gimbal_IMU_t gimbal_to_send;
|
||||
//Chassis_IMU_t chassis_to_send;
|
||||
AHRS_Quaternion_t imu_quat_for_ai;
|
||||
|
||||
GIMBAL_IMU_FROMCAN_t imu_to_can;
|
||||
|
||||
BMI088_Cali_t cali_bmi088= {
|
||||
.gyro_offset = {-0.00147764047f,-0.00273479894f,0.00154074503f},
|
||||
};
|
||||
@ -105,7 +108,8 @@ void Task_atti_esti(void *argument) {
|
||||
PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D,
|
||||
1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
|
||||
BSP_PWM_Start(BSP_PWM_IMU_HEAT);
|
||||
|
||||
|
||||
GIMBAL_IMU_Init(&imu_to_can, &Config_GetRobotParam()->imu_param);
|
||||
/* 注册按钮回调函数并启用中断 */
|
||||
BSP_GPIO_RegisterCallback(BSP_GPIO_USER_KEY, start_gyro_calibration);
|
||||
BSP_GPIO_EnableIRQ(BSP_GPIO_USER_KEY);
|
||||
@ -163,20 +167,20 @@ void Task_atti_esti(void *argument) {
|
||||
|
||||
osKernelUnlock();
|
||||
|
||||
if (fdcan_ready) {
|
||||
Atti_UpdateEulrFromCan();
|
||||
}
|
||||
//从can读取头部Imu数据
|
||||
GIMBAL_IMU_Update(&imu_to_can);
|
||||
|
||||
/* 创建修改后的数据副本用于发送到消息队列 */
|
||||
gimbal_to_send.eulr = eulr_gimbal;
|
||||
gimbal_to_send.gyro = bmi088.gyro;
|
||||
gimbal_to_send.eulr=imu_to_can.data.eulr;
|
||||
gimbal_to_send.gyro=imu_to_can.data.gyro;
|
||||
|
||||
osMessageQueueReset(task_runtime.msgq.gimbal.imu);
|
||||
osMessageQueuePut(task_runtime.msgq.gimbal.imu, &gimbal_to_send, 0, 0);
|
||||
|
||||
imu_quat_for_ai.q0 = chassis_ahrs.quat.q0;
|
||||
imu_quat_for_ai.q1 = chassis_ahrs.quat.q1;
|
||||
imu_quat_for_ai.q2 = chassis_ahrs.quat.q2;
|
||||
imu_quat_for_ai.q3 = chassis_ahrs.quat.q3;
|
||||
imu_quat_for_ai.q0 = imu_to_can.data.quat.q0;
|
||||
imu_quat_for_ai.q1 = imu_to_can.data.quat.q1;
|
||||
imu_quat_for_ai.q2 = imu_to_can.data.quat.q2;
|
||||
imu_quat_for_ai.q3 = imu_to_can.data.quat.q3;
|
||||
osMessageQueuePut(task_runtime.msgq.ai.rawdata_FromIMU, &imu_quat_for_ai, 0, 0);
|
||||
|
||||
// chassis_to_send.eulr = eulr_to_send;
|
||||
@ -193,22 +197,3 @@ void Task_atti_esti(void *argument) {
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static void Atti_UpdateEulrFromCan(void) {
|
||||
BSP_FDCAN_Message_t rx_msg;
|
||||
|
||||
/* Drain queue to keep the latest gimbal orientation */
|
||||
while (BSP_FDCAN_GetMessage(BSP_FDCAN_2, ATTI_EULR_CAN_ID, &rx_msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||||
if (rx_msg.dlc < ATTI_EULR_CAN_DLC) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const int16_t yaw_raw = (int16_t)((rx_msg.data[0] << 8) | rx_msg.data[1]);
|
||||
const int16_t pit_raw = (int16_t)((rx_msg.data[2] << 8) | rx_msg.data[3]);
|
||||
const int16_t rol_raw = (int16_t)((rx_msg.data[4] << 8) | rx_msg.data[5]);
|
||||
|
||||
eulr_gimbal.yaw = ((float)yaw_raw) / ATTI_EULR_SCALE;
|
||||
eulr_gimbal.pit = ((float)pit_raw) / ATTI_EULR_SCALE;
|
||||
eulr_gimbal.rol = ((float)rol_raw) / ATTI_EULR_SCALE;
|
||||
}
|
||||
}
|
||||
@ -15,14 +15,12 @@
|
||||
#include "module/track.h"
|
||||
#include "module/cmd/cmd.h"
|
||||
#include "bsp/fdcan.h"
|
||||
#include "module/vision_bridge.h"
|
||||
//#define DEAD_AREA 0.05f
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
#define AI_CMD_CAN_ID (0x101u)
|
||||
#define AI_CMD_CAN_DLC (5u) /* 1字节mode + 2字节yaw + 2字节pit */
|
||||
#define AI_CMD_ANGLE_SCALE (10000.0f) /* 0.0001 rad per LSB */
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
@ -38,10 +36,11 @@ Chassis_CMD_t *cmd_for_chassis;
|
||||
Gimbal_CMD_t *cmd_for_gimbal;
|
||||
Track_CMD_t *cmd_for_track;
|
||||
static CMD_t cmd;
|
||||
|
||||
AI_t ai;
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
static void AI_ParseCmdFromCan(const BSP_FDCAN_Message_t *msg, AI_cmd_t *cmd);
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
void Task_cmd(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
@ -55,9 +54,8 @@ void Task_cmd(void *argument) {
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
CMD_Init(&cmd, &Config_GetRobotParam()->cmd_param);
|
||||
|
||||
AI_Init(&ai, &Config_GetRobotParam()->ai_param);
|
||||
/* 注册CAN接收ID */
|
||||
BSP_FDCAN_RegisterId(BSP_FDCAN_2, AI_CMD_CAN_ID, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
@ -70,10 +68,7 @@ void Task_cmd(void *argument) {
|
||||
#endif
|
||||
|
||||
/* 从CAN2接收AI命令 */
|
||||
BSP_FDCAN_Message_t can_msg;
|
||||
if (BSP_FDCAN_GetMessage(BSP_FDCAN_2, AI_CMD_CAN_ID, &can_msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
|
||||
AI_ParseCmdFromCan(&can_msg, &cmd_ai);
|
||||
}
|
||||
AI_ParseCmdFromCan( &ai,&cmd_ai);
|
||||
|
||||
CMD_Update(&cmd);
|
||||
|
||||
@ -82,6 +77,14 @@ void Task_cmd(void *argument) {
|
||||
cmd_for_gimbal = CMD_GetGimbalCmd(&cmd);
|
||||
cmd_for_shoot = CMD_GetShootCmd(&cmd);
|
||||
cmd_for_track = CMD_GetTrackCmd(&cmd);
|
||||
|
||||
if (cmd_for_shoot->firecmd) {
|
||||
/* 发射时添加pitch前馈，抵消后坐力。此时为负值表示向下压。需要根据实际情况调整 */
|
||||
cmd_for_gimbal->feedforward_pit = -1.0f;
|
||||
} else {
|
||||
cmd_for_gimbal->feedforward_pit = 0.0f;
|
||||
}
|
||||
|
||||
osMessageQueueReset(task_runtime.msgq.gimbal.cmd);
|
||||
osMessageQueuePut(task_runtime.msgq.gimbal.cmd, cmd_for_gimbal, 0, 0);
|
||||
osMessageQueueReset(task_runtime.msgq.shoot.cmd);
|
||||
@ -94,28 +97,4 @@ void Task_cmd(void *argument) {
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* 从CAN消息解析AI命令 (mode + yaw + pit) */
|
||||
static void AI_ParseCmdFromCan(const BSP_FDCAN_Message_t *msg, AI_cmd_t *cmd) {
|
||||
if (msg == NULL || cmd == NULL || msg->dlc < AI_CMD_CAN_DLC) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* 解析mode (1字节) */
|
||||
cmd->mode = msg->data[0];
|
||||
|
||||
/* 解析yaw (2字节，高字节在前) */
|
||||
int16_t yaw_raw = (int16_t)((msg->data[1] << 8) | msg->data[2]);
|
||||
cmd->gimbal.setpoint.yaw = (float)yaw_raw / AI_CMD_ANGLE_SCALE;
|
||||
|
||||
/* 解析pit (2字节，高字节在前) */
|
||||
int16_t pit_raw = (int16_t)((msg->data[3] << 8) | msg->data[4]);
|
||||
cmd->gimbal.setpoint.pit = (float)pit_raw / AI_CMD_ANGLE_SCALE;
|
||||
|
||||
/* 其他字段根据需要可以初始化为0 */
|
||||
cmd->gimbal.vel.yaw = 0.0f;
|
||||
cmd->gimbal.vel.pit = 0.0f;
|
||||
cmd->gimbal.accl.yaw = 0.0f;
|
||||
cmd->gimbal.accl.pit = 0.0f;
|
||||
}
|
||||
@ -44,7 +44,8 @@ void Task_ctrl_gimbal(void *argument) {
|
||||
Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
|
||||
}
|
||||
osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
|
||||
|
||||
// gimbal_cmd.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
// gimbal_cmd.feedforward_pit = 0.0f;
|
||||
|
||||
Gimbal_UpdateFeedback(&gimbal);
|
||||
|
||||
|
||||
@ -96,7 +96,7 @@ FDCAN2.NominalSyncJumpWidth=5
|
||||
FDCAN2.NominalTimeSeg1=14
|
||||
FDCAN2.NominalTimeSeg2=5
|
||||
FDCAN2.ProtocolException=ENABLE
|
||||
FDCAN2.RxFifo1ElmtsNbr=16
|
||||
FDCAN2.RxFifo1ElmtsNbr=20
|
||||
FDCAN2.StdFiltersNbr=1
|
||||
FDCAN2.TxFifoQueueElmtsNbr=16
|
||||
FDCAN3.AutoRetransmission=ENABLE
|
||||
|
||||
@ -1,38 +1,59 @@
|
||||
|
||||
|
||||
GraphedExpression="(eulr_gimbal).yaw", Color=#e56a6f
|
||||
GraphedExpression="(eulr_gimbal).pit", Color=#35792b
|
||||
OpenDocument="tasks.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3637
|
||||
|
||||
Breakpoint=D:/CUBEMX/hero/god-yuan-hero/User/module/gimbal.c:158:10, State=BP_STATE_DISABLED
|
||||
GraphedExpression="((((gimbal).feedback).imu).eulr).yaw", Color=#e56a6f, Show=0
|
||||
GraphedExpression="(cmd_ai).mode", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#35792b
|
||||
OpenDocument="stm32h7xx_hal_uart_ex.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart_ex.c", Line=994
|
||||
OpenDocument="cmsis_gcc.h", FilePath="D:/CUBEMX/hero/god-yuan-hero/Drivers/CMSIS/Include/cmsis_gcc.h", Line=1193
|
||||
OpenDocument="startup_stm32h723xx.s", FilePath="D:/CUBEMX/hero/god-yuan-hero/startup_stm32h723xx.s", Line=48
|
||||
OpenDocument="cmd.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/cmd.c", Line=51
|
||||
OpenDocument="cmd.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/module/cmd/cmd.c", Line=0
|
||||
OpenDocument="vision_bridge.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/module/vision_bridge.c", Line=78
|
||||
OpenDocument="tasks.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3419
|
||||
OpenDocument="motor.h", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/device/motor.h", Line=20
|
||||
OpenDocument="filter.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/component/filter/filter.c", Line=42
|
||||
OpenDocument="user_math.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/component/user_math.c", Line=45
|
||||
OpenDocument="pid.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/component/pid.c", Line=61
|
||||
OpenDocument="gimbal.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/module/gimbal.c", Line=220
|
||||
OpenDocument="ctrl_gimbal.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/ctrl_gimbal.c", Line=16
|
||||
OpenDocument="fdcan.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/bsp/fdcan.c", Line=57
|
||||
OpenDocument="main.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Core/Src/main.c", Line=60
|
||||
OpenDocument="queue.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Middlewares/Third_Party/FreeRTOS/Source/queue.c", Line=1275
|
||||
OpenDocument="chassis.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/module/chassis.c", Line=213
|
||||
OpenDocument="stm32h7xx_hal_uart.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart.c", Line=3332
|
||||
OpenDocument="atti_esti.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/atti_esti.c", Line=4
|
||||
OpenDocument="fdcan.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Core/Src/fdcan.c", Line=0
|
||||
OpenDocument="time.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/bsp/time.c", Line=17
|
||||
OpenToolbar="Debug", Floating=0, x=0, y=0
|
||||
OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=726, h=641, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
|
||||
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=326, h=930, TabPos=0, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=726, h=288, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=727, h=620, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
|
||||
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=326, h=930, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=727, h=309, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Memory 1", DockArea=BOTTOM, x=0, y=0, w=239, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0xFFFF5088
|
||||
OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=1, w=726, h=641, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=326, h=930, TabPos=1, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=1, w=727, h=620, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=326, h=930, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=866, h=525, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
|
||||
OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1453, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=1, CodePaneShown=1, PinCursor="Cursor Movable", TimePerDiv="1 ns / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="1187;-113", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=1, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1226;-69", CodeGraphLegendShown=1, CodeGraphLegendPosition="1177;0"
|
||||
OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1453, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="200 ms / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="21;187", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1232;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="1248;0"
|
||||
OpenWindow="Console", DockArea=BOTTOM, x=2, y=0, w=866, h=525, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
|
||||
TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[120;144;294]
|
||||
TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Source"], ColWidths=[1594;104;100;100;798]
|
||||
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;1014]
|
||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (eulr_gimbal).yaw";" (eulr_gimbal).pit"], ColWidths=[100;100;100;100]
|
||||
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;144;144;124;144;110;164;154]
|
||||
TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[120;144;463]
|
||||
TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Source"], ColWidths=[1594;104;100;100;100]
|
||||
TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
|
||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[250;282;91;100]
|
||||
TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
|
||||
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;1014]
|
||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[250;229;145;100]
|
||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ((((gimbal).feedback).imu).eulr).yaw";" (cmd_ai).mode"], ColWidths=[100;100;100;100]
|
||||
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;144;114;114;114;114;110;126;126]
|
||||
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;366]
|
||||
WatchedExpression="cmd", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="eulr_gimbal", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="aiToCmd", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="eulr_gimbal", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="bmi088", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="chassis", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="gimbal", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="shoot", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="track_cmd", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="track", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="robot_config", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="robot_config", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="imu_to_can", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="cmd_ai", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="ai", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="queue_list", RefreshRate=5, Window=Watched Data 1
|
||||
Loading…
Reference in New Issue
Block a user