ELCT/Simba-ELCT-FrameWork
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

加入大疆C板例程bsp

Browse Source
This commit is contained in:
Qi Bin
2026-04-12 20:34:47 +08:00
parent 7475077b9e
commit b68ac54a34
51 changed files with 5950 additions and 1290 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
Application/Task/Inc/CAN_Task.h Normal file
View File

@@ -0,0 +1,10 @@
#ifndef CAN_TASK_H
#define CAN_TASK_H
#endif

141
Application/Task/Inc/Config.h Normal file
View File

@@ -0,0 +1,141 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : Config.c
* @brief : Configuare the Robot Functions
* @author : Yan Yuanbin
* @date : 2023/05/21
* @version : v1.0
******************************************************************************
* @attention : To be perfected
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef ROBOT_CONFIG_H
#define ROBOT_CONFIG_H
/* Includes ------------------------------------------------------------------*/
#include "stdint.h"
#include "stdbool.h"
#include "stdlib.h"
#include "string.h"
#include "math.h"
/* General physics and mathematics constants ---------------------------------*/
/**
* @brief the value of local gravity acceleration
*/
#define VAL_LIMIT(x,min,max) do{ \
if ((x) > (max)) {(x) = (max);} \
else if ((x) < (min)) {(x) = (min);} \
}while(0U)
#define GravityAccel 9.718f
#define Angle_to_rad 0.01745329f
#define Rad_to_angle 57.2957732f
/**
* @brief Euler's Number
*/
#define Euler_Number 2.718281828459045f
/**
* @brief radian system rotation degrees system , 180.f/PI
*/
#define RadiansToDegrees 57.295779513f
/**
* @brief degrees system rotation radian system , PI/180.f
*/
#define DegreesToRadians 0.01745329251f
/* Vision reslove constants -------------------------------------------------*/
/**
* @brief Decision Marking mode
* 0: select the minimum yaw armor
* 1: select the minimum distance armor
*/
#define Yaw_Distance_Decision 0
/**
* @brief ballistic coefficient
* @note 17mm: 0.038
* 42mm: 0.019
*/
#define Bullet_Coefficient 0.038f
/**
* @brief the half width of little armor
*/
#define LittleArmor_HalfWidth 0.07f
/**
* @brief the half width of Large armor
*/
#define LargeArmor_HalfWidth 0.1175f
/* IMU reslove constants ---------------------------------------------------*/
/**
* @brief the flag of bmi088 Calibration
* 0: DISABLE
* 1: ENABLE
*/
#define IMU_Calibration_ENABLE 0U
/**
* @brief the index of pitch angle update
*/
#define IMU_ANGLE_INDEX_PITCH 2U
/**
* @brief the index of yaw angle update
*/
#define IMU_ANGLE_INDEX_YAW 0U
/**
* @brief the index of roll angle update
*/
#define IMU_ANGLE_INDEX_ROLL 1U
/**
* @brief the index of pitch gyro update
*/
#define IMU_GYRO_INDEX_PITCH 0U
/**
* @brief the index of yaw gyro update
*/
#define IMU_GYRO_INDEX_YAW 2U
/**
* @brief the index of roll gyro update
*/
#define IMU_GYRO_INDEX_ROLL 1U
/**
* @brief the index of pitch accel update
*/
#define IMU_ACCEL_INDEX_PITCH 0U
/**
* @brief the index of yaw accel update
*/
#define IMU_ACCEL_INDEX_YAW 2U
/**
* @brief the index of roll accel update
*/
#define IMU_ACCEL_INDEX_ROLL 1U
/* Remote reslove constants -----------------------------------------------*/
/**
* @brief the flag of remote control receive frame data
* @note 0: CAN
* 1: USART
*/
#define REMOTE_FRAME_USART_CAN 0U
#endif //ROBOT_CONFIG_H

50
Application/Task/Inc/Control_Task.h Normal file
View File

@@ -0,0 +1,50 @@
/**
******************************************************************************
* @file : Control_Task.c
* @brief : Control task
* @author : Yan Yuanbin
* @date : 2023/04/27
* @version : v1.0
******************************************************************************
* @attention : None
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef CONTROL_TASK_H
#define CONTROL_TASK_H
/* Includes ------------------------------------------------------------------*/
#include "stdint.h"
#include "stdbool.h"
/**
* @brief typedef structure that contains the information of chassis control
*/
typedef struct
{
struct{
float Chassis_Velocity;
}Target;
struct{
float Chassis_Velocity;
}Measure;
int16_t SendValue[4];
}Control_Info_Typedef;
extern Control_Info_Typedef Control_Info;
#endif //CONTROL_TASK_H

49
Application/Task/Inc/INS_Task.h Normal file
View File

@@ -0,0 +1,49 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : INS_Task.h
* @brief : INS task
* @author : Yan Yuanbin
* @date : 2023/04/27
* @version : v1.0
******************************************************************************
* @attention : None
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef INS_TASK_H
#define INS_TASK_H
/* Includes ------------------------------------------------------------------*/
#include "stdint.h"
/* Exported types ------------------------------------------------------------*/
/**
* @brief typedef structure that contains the information for the INS.
*/
typedef struct
{
float Pitch_Angle;
float Yaw_Angle;
float Yaw_TolAngle;
float Roll_Angle;
float Pitch_Gyro;
float Yaw_Gyro;
float Roll_Gyro;
float Angle[3];
float Gyro[3];
float Accel[3];
float Last_Yaw_Angle;
int16_t YawRoundCount;
}INS_Info_Typedef;
/* Externs---------------------------------------------------------*/
extern INS_Info_Typedef INS_Info;
#endif //INS_TASK_H

52
Application/Task/Src/CAN_Task.c Normal file
View File

@@ -0,0 +1,52 @@
/**
******************************************************************************
* @file : CAN_Task.c
* @brief : CAN task
* @author : GrassFam Wang
* @date : 2025/1/22
* @version : v1.1
******************************************************************************
* @attention : None
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "cmsis_os.h"
#include "CAN_Task.h"
#include "Control_Task.h"
#include "INS_Task.h"
#include "Motor.h"
#include "bsp_can.h"
#include "Remote_Control.h"
#include "Control_Task.h"
/* USER CODE BEGIN Header_CAN_Task */
/**
* @brief Function implementing the StartCANTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_CAN_Task */
void CAN_Task(void const * argument)
{
TickType_t CAN_Task_SysTick = 0;
for(;;)
{
CAN_Task_SysTick = osKernelSysTick();
if(CAN_Task_SysTick % 2 == 0){
//500Hz<48><7A><EFBFBD><EFBFBD> <20>뱣֤<EBB1A3><D6A4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>osDelay(1)
}
osDelay(1);
}
}

84
Application/Task/Src/Control_Task.c Normal file
View File

@@ -0,0 +1,84 @@
/**
******************************************************************************
* @file : Control_Task.c
* @brief : Control task
* @author : GrassFan Wang
* @date : 2025/01/22
* @version : v1.1
******************************************************************************
* @attention : None
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "Control_Task.h"
#include "cmsis_os.h"
#include "Control_Task.h"
#include "bsp_uart.h"
#include "Remote_Control.h"
#include "PID.h"
#include "Motor.h"
static void Control_Init(Control_Info_Typedef *Control_Info);
static void Control_Measure_Update(Control_Info_Typedef *Control_Info);
static void Control_Target_Update(Control_Info_Typedef *Control_Info);
static void Control_Info_Update(Control_Info_Typedef *Control_Info);
Control_Info_Typedef Control_Info;
// KP KI KD Alpha Deadband I_MAX Output_MAX
static float Chassis_PID_Param[7] = {13.f,0.1f,0.f,0.f, 0.f, 5000.f, 12000.f};
PID_Info_TypeDef Chassis_PID;
void Control_Task(void const * argument)
{
/* USER CODE BEGIN Control_Task */
TickType_t Control_Task_SysTick = 0;
Control_Init(&Control_Info);
/* Infinite loop */
for(;;)
{
Control_Task_SysTick = osKernelSysTick();
Control_Measure_Update(&Control_Info);
Control_Target_Update(&Control_Info);
Control_Info_Update(&Control_Info);
USART_Vofa_Justfloat_Transmit(Control_Info.Measure.Chassis_Velocity,0.f,0.f);
osDelay(1);
}
}
/* USER CODE END Control_Task */
static void Control_Init(Control_Info_Typedef *Control_Info){
PID_Init(&Chassis_PID,PID_POSITION,Chassis_PID_Param);
}
static void Control_Measure_Update(Control_Info_Typedef *Control_Info){
Control_Info->Measure.Chassis_Velocity = Chassis_Motor[0].Data.Velocity;
}
static void Control_Target_Update(Control_Info_Typedef *Control_Info){
Control_Info->Target.Chassis_Velocity = remote_ctrl.rc.ch[3] * 5.f;
}
static void Control_Info_Update(Control_Info_Typedef *Control_Info){
PID_Calculate(&Chassis_PID, Control_Info->Target.Chassis_Velocity, Control_Info->Measure.Chassis_Velocity);
Control_Info->SendValue[0] = (int16_t)(Chassis_PID.Output);
}

186
Application/Task/Src/INS_Task.c Normal file
View File

@@ -0,0 +1,186 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : INS_Task.c
* @brief : INS task
* @author : GrassFan Wang
* @date : 2025/01/22
* @version : v1.0
******************************************************************************
* @attention : None
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "cmsis_os.h"
#include "INS_Task.h"
#include "bmi088.h"
#include "lpf.h"
#include "pid.h"
#include "config.h"
#include "tim.h"
#include "Quaternion.h"
#include "bsp_pwm.h"
/**
* @brief the structure that contains the information for the INS.
*/
INS_Info_Typedef INS_Info;
/**
* @brief the array that contains the data of LPF2p coefficients.
*/
static float INS_LPF2p_Alpha[3]={1.929454039488895f, -0.93178349823448126f, 0.002329458745586203f};
/**
* @brief the structure that contains the Information of accel LPF2p.
*/
LowPassFilter2p_Info_TypeDef INS_AccelPF2p[3];
/**
* @brief the Initialize data of state transition matrix.
*/
static float QuaternionEKF_A_Data[36]={1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0,
0, 0, 1, 0, 0, 0,
0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 1};
/**
* @brief the Initialize data of posteriori covariance matrix.
*/
static float QuaternionEKF_P_Data[36]= {100000, 0.1, 0.1, 0.1, 0.1, 0.1,
0.1, 100000, 0.1, 0.1, 0.1, 0.1,
0.1, 0.1, 100000, 0.1, 0.1, 0.1,
0.1, 0.1, 0.1, 100000, 0.1, 0.1,
0.1, 0.1, 0.1, 0.1, 100, 0.1,
0.1, 0.1, 0.1, 0.1, 0.1, 100};
/**
* @brief the Initialize data of Temperature Control PID.
*/
static float TemCtrl_PID_Param[PID_PARAMETER_NUM]={1200,20,0,0,0,0,2000};
/**
* @brief the structure that contains the Information of Temperature Control PID.
*/
PID_Info_TypeDef TempCtrl_PID;
/**
* @brief Initializes the INS_Task.
*/
static void INS_Task_Init(void);
/**
* @brief Control the BMI088 temperature
*/
static void BMI088_Temp_Control(float temp);
/* USER CODE BEGIN Header_INS_Task */
/**
* @brief Function implementing the StartINSTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_INS_Task */
void INS_Task(void const * argument)
{
/* USER CODE BEGIN INS_Task */
TickType_t INS_Task_SysTick = 0;
/* Initializes the INS_Task. */
INS_Task_Init();
/* Infinite loop */
for(;;)
{
INS_Task_SysTick = osKernelSysTick();
/* Update the BMI088 measurement */
BMI088_Info_Update(&BMI088_Info);
/* Accel measurement LPF2p */
INS_Info.Accel[0] = LowPassFilter2p_Update(&INS_AccelPF2p[0],BMI088_Info.Accel[0]);
INS_Info.Accel[1] = LowPassFilter2p_Update(&INS_AccelPF2p[1],BMI088_Info.Accel[1]);
INS_Info.Accel[2] = LowPassFilter2p_Update(&INS_AccelPF2p[2],BMI088_Info.Accel[2]);
/* Update the INS gyro in radians */
INS_Info.Gyro[0] = BMI088_Info.Gyro[0];
INS_Info.Gyro[1] = BMI088_Info.Gyro[1];
INS_Info.Gyro[2] = BMI088_Info.Gyro[2];
/* Update the QuaternionEKF */
QuaternionEKF_Update(&Quaternion_Info,INS_Info.Gyro,INS_Info.Accel,0.001f);
memcpy(INS_Info.Angle,Quaternion_Info.EulerAngle,sizeof(INS_Info.Angle));
/* Update the Euler angle in degrees. */
INS_Info.Pitch_Angle = Quaternion_Info.EulerAngle[IMU_ANGLE_INDEX_PITCH]*57.295779513f;
INS_Info.Yaw_Angle = Quaternion_Info.EulerAngle[IMU_ANGLE_INDEX_YAW] *57.295779513f;
INS_Info.Roll_Angle = Quaternion_Info.EulerAngle[IMU_ANGLE_INDEX_ROLL]*57.295779513f;
/* Update the yaw total angle */
if(INS_Info.Yaw_Angle - INS_Info.Last_Yaw_Angle < -180.f)
{
INS_Info.YawRoundCount++;
}
else if(INS_Info.Yaw_Angle - INS_Info.Last_Yaw_Angle > 180.f)
{
INS_Info.YawRoundCount--;
}
INS_Info.Last_Yaw_Angle = INS_Info.Yaw_Angle;
INS_Info.Yaw_TolAngle = INS_Info.Yaw_Angle + INS_Info.YawRoundCount*360.f;
/* Update the INS gyro in degrees */
INS_Info.Pitch_Gyro = INS_Info.Gyro[IMU_GYRO_INDEX_PITCH]*RadiansToDegrees;
INS_Info.Yaw_Gyro = INS_Info.Gyro[IMU_GYRO_INDEX_YAW]*RadiansToDegrees;
INS_Info.Roll_Gyro = INS_Info.Gyro[IMU_GYRO_INDEX_ROLL]*RadiansToDegrees;
if(INS_Task_SysTick%5 == 0)
{
BMI088_Temp_Control(BMI088_Info.Temperature);
}
osDelayUntil(&INS_Task_SysTick,1);
}
/* USER CODE END INS_Task */
}
//------------------------------------------------------------------------------
/**
* @brief Initializes the INS_Task.
*/
static void INS_Task_Init(void)
{
/* Initializes the Second order lowpass filter */
LowPassFilter2p_Init(&INS_AccelPF2p[0],INS_LPF2p_Alpha);
LowPassFilter2p_Init(&INS_AccelPF2p[1],INS_LPF2p_Alpha);
LowPassFilter2p_Init(&INS_AccelPF2p[2],INS_LPF2p_Alpha);
/* Initializes the Temperature Control PID */
PID_Init(&TempCtrl_PID,PID_POSITION,TemCtrl_PID_Param);
/* Initializes the Quaternion EKF */
QuaternionEKF_Init(&Quaternion_Info,10.f, 0.001f, 1000000.f,QuaternionEKF_A_Data,QuaternionEKF_P_Data);
}
//------------------------------------------------------------------------------
/**
* @brief Control the BMI088 temperature
* @param temp measure of the BMI088 temperature
* @retval none
*/
static void BMI088_Temp_Control(float Temp)
{
PID_Calculate(&TempCtrl_PID,40.f,Temp);
VAL_LIMIT(TempCtrl_PID.Output,-TempCtrl_PID.Param.LimitOutput,TempCtrl_PID.Param.LimitOutput);
Heat_Power_Control((uint16_t)(TempCtrl_PID.Output));
}
//------------------------------------------------------------------------------