改imu
This commit is contained in:
parent
31147537b5
commit
e0b2485aaf
@ -81,6 +81,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
|
||||
# User/task sources
|
||||
User/task/ai.c
|
||||
User/task/atti_esti.c
|
||||
User/task/init.c
|
||||
User/task/rc.c
|
||||
User/task/user_task.c
|
||||
|
||||
@ -68,7 +68,7 @@
|
||||
#define configTICK_RATE_HZ ((TickType_t)1000)
|
||||
#define configMAX_PRIORITIES ( 56 )
|
||||
#define configMINIMAL_STACK_SIZE ((uint16_t)128)
|
||||
#define configTOTAL_HEAP_SIZE ((size_t)15360)
|
||||
#define configTOTAL_HEAP_SIZE ((size_t)0x10000)
|
||||
#define configMAX_TASK_NAME_LEN ( 16 )
|
||||
#define configUSE_TRACE_FACILITY 1
|
||||
#define configUSE_16_BIT_TICKS 0
|
||||
|
||||
@ -66,8 +66,8 @@ FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 1024K
|
||||
/* Highest address of the user mode stack */
|
||||
_estack = ORIGIN(DTCMRAM) + LENGTH(DTCMRAM); /* end of RAM */
|
||||
/* Generate a link error if heap and stack don't fit into RAM */
|
||||
_Min_Heap_Size = 0x200; /* required amount of heap */
|
||||
_Min_Stack_Size = 0x800; /* required amount of stack */
|
||||
_Min_Heap_Size = 0x1000; /* required amount of heap */
|
||||
_Min_Stack_Size = 0x2000; /* required amount of stack */
|
||||
|
||||
/* Define output sections */
|
||||
SECTIONS
|
||||
|
||||
96
User/task/atti_esti.c
Normal file
96
User/task/atti_esti.c
Normal file
@ -0,0 +1,96 @@
|
||||
/*
|
||||
atti_esti Task
|
||||
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "bsp/mm.h"
|
||||
#include "bsp/pwm.h"
|
||||
#include "bsp/gpio.h"
|
||||
#include "component/ahrs.h"
|
||||
#include "component/pid.h"
|
||||
#include "device/bmi088.h"
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
static BMI088_t bmi088 __attribute__((section(".dma_buffer"))) __attribute__((aligned(32)));
|
||||
|
||||
AHRS_t gimbal_ahrs;
|
||||
AHRS_Magn_t magn;
|
||||
AHRS_Eulr_t eulr_to_send;
|
||||
|
||||
KPID_t imu_temp_ctrl_pid;
|
||||
|
||||
|
||||
BMI088_Cali_t cali_bmi088= {
|
||||
.gyro_offset = {-0.00149519544f,-0.00268901442f,0.00169837975f},
|
||||
};
|
||||
|
||||
static const KPID_Params_t imu_temp_ctrl_pid_param = {
|
||||
.k = 0.3f,
|
||||
.p = 1.0f,
|
||||
.i = 0.01f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
};
|
||||
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
void Task_atti_esti(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
|
||||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / ATTI_ESTI_FREQ;
|
||||
|
||||
osDelay(ATTI_ESTI_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
BMI088_Init(&bmi088,&cali_bmi088);
|
||||
AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
|
||||
PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D,
|
||||
1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
|
||||
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
BMI088_WaitNew();
|
||||
BMI088_AcclStartDmaRecv();
|
||||
BMI088_AcclWaitDmaCplt();
|
||||
|
||||
BMI088_GyroStartDmaRecv();
|
||||
BMI088_GyroWaitDmaCplt();
|
||||
|
||||
/* 锁住RTOS内核防止数据解析过程中断，造成错误 */
|
||||
osKernelLock();
|
||||
/* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */
|
||||
BMI088_ParseAccl(&bmi088);
|
||||
BMI088_ParseGyro(&bmi088);
|
||||
// IST8310_Parse(&ist8310);
|
||||
|
||||
/* 根据设备接收到的数据进行姿态解析 */
|
||||
AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
|
||||
|
||||
/* 根据解析出来的四元数计算欧拉角 */
|
||||
AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
|
||||
|
||||
osKernelUnlock();
|
||||
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
}
|
||||
@ -12,3 +12,10 @@
|
||||
function: Task_rc
|
||||
name: rc
|
||||
stack: 256
|
||||
- delay: 0
|
||||
description: ''
|
||||
freq_control: true
|
||||
frequency: 500.0
|
||||
function: Task_atti_esti
|
||||
name: atti_esti
|
||||
stack: 256
|
||||
|
||||
@ -32,6 +32,7 @@ void Task_Init(void *argument) {
|
||||
/* 创建任务线程 */
|
||||
task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
|
||||
task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
|
||||
task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti);
|
||||
|
||||
// 创建消息队列
|
||||
/* USER MESSAGE BEGIN */
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/*
|
||||
rc Task
|
||||
遥控器接收任务 - 处理DR16遥控器数据接收
|
||||
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
|
||||
@ -19,3 +19,8 @@ const osThreadAttr_t attr_rc = {
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 256 * 4,
|
||||
};
|
||||
const osThreadAttr_t attr_atti_esti = {
|
||||
.name = "atti_esti",
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 256 * 4,
|
||||
};
|
||||
@ -15,11 +15,13 @@ extern "C" {
|
||||
/* 任务运行频率 */
|
||||
#define AI_FREQ (500.0)
|
||||
#define RC_FREQ (500.0)
|
||||
#define ATTI_ESTI_FREQ (500.0)
|
||||
|
||||
/* 任务初始化延时ms */
|
||||
#define TASK_INIT_DELAY (100u)
|
||||
#define AI_INIT_DELAY (0)
|
||||
#define RC_INIT_DELAY (0)
|
||||
#define ATTI_ESTI_INIT_DELAY (0)
|
||||
|
||||
/* Exported defines --------------------------------------------------------- */
|
||||
/* Exported macro ----------------------------------------------------------- */
|
||||
@ -31,6 +33,7 @@ typedef struct {
|
||||
struct {
|
||||
osThreadId_t ai;
|
||||
osThreadId_t rc;
|
||||
osThreadId_t atti_esti;
|
||||
} thread;
|
||||
|
||||
/* USER MESSAGE BEGIN */
|
||||
@ -54,18 +57,21 @@ typedef struct {
|
||||
struct {
|
||||
UBaseType_t ai;
|
||||
UBaseType_t rc;
|
||||
UBaseType_t atti_esti;
|
||||
} stack_water_mark;
|
||||
|
||||
/* 各任务运行频率 */
|
||||
struct {
|
||||
float ai;
|
||||
float rc;
|
||||
float atti_esti;
|
||||
} freq;
|
||||
|
||||
/* 任务最近运行时间 */
|
||||
struct {
|
||||
float ai;
|
||||
float rc;
|
||||
float atti_esti;
|
||||
} last_up_time;
|
||||
|
||||
} Task_Runtime_t;
|
||||
@ -77,11 +83,13 @@ extern Task_Runtime_t task_runtime;
|
||||
extern const osThreadAttr_t attr_init;
|
||||
extern const osThreadAttr_t attr_ai;
|
||||
extern const osThreadAttr_t attr_rc;
|
||||
extern const osThreadAttr_t attr_atti_esti;
|
||||
|
||||
/* 任务函数声明 */
|
||||
void Task_Init(void *argument);
|
||||
void Task_ai(void *argument);
|
||||
void Task_rc(void *argument);
|
||||
void Task_atti_esti(void *argument);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
@ -391,8 +391,9 @@ FDCAN3.StdFiltersNbr=1
|
||||
FDCAN3.TxBuffersNbr=0
|
||||
FDCAN3.TxElmtSize=FDCAN_DATA_BYTES_64
|
||||
FDCAN3.TxFifoQueueElmtsNbr=23
|
||||
FREERTOS.IPParameters=Tasks01
|
||||
FREERTOS.IPParameters=Tasks01,configTOTAL_HEAP_SIZE
|
||||
FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
|
||||
FREERTOS.configTOTAL_HEAP_SIZE=0x10000
|
||||
File.Version=6
|
||||
GPIO.groupedBy=Group By Peripherals
|
||||
KeepUserPlacement=false
|
||||
@ -698,7 +699,7 @@ ProjectManager.DeviceId=STM32H723VGTx
|
||||
ProjectManager.FirmwarePackage=STM32Cube FW_H7 V1.12.1
|
||||
ProjectManager.FreePins=false
|
||||
ProjectManager.HalAssertFull=false
|
||||
ProjectManager.HeapSize=0x200
|
||||
ProjectManager.HeapSize=0x1000
|
||||
ProjectManager.KeepUserCode=true
|
||||
ProjectManager.LastFirmware=true
|
||||
ProjectManager.LibraryCopy=0
|
||||
@ -710,7 +711,7 @@ ProjectManager.ProjectFileName=balance_infantry.ioc
|
||||
ProjectManager.ProjectName=balance_infantry
|
||||
ProjectManager.ProjectStructure=
|
||||
ProjectManager.RegisterCallBack=
|
||||
ProjectManager.StackSize=0x800
|
||||
ProjectManager.StackSize=0x2000
|
||||
ProjectManager.TargetToolchain=CMake
|
||||
ProjectManager.ToolChainLocation=
|
||||
ProjectManager.UAScriptAfterPath=
|
||||
|
||||
Loading…
Reference in New Issue
Block a user