diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..3cf2627
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,5 @@
+{
+    "files.associations": {
+        "user_math.h": "c"
+    }
+}
\ No newline at end of file
diff --git a/assets/User_code/component/FreeRTOS_CLI.c b/assets/User_code/component/FreeRTOS_CLI.c
new file mode 100644
index 0000000..32421de
--- /dev/null
+++ b/assets/User_code/component/FreeRTOS_CLI.c
@@ -0,0 +1,352 @@
+/*
+ * FreeRTOS+CLI V1.0.4
+ * Copyright (C) 2017 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of
+ * this software and associated documentation files (the "Software"), to deal in
+ * the Software without restriction, including without limitation the rights to
+ * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+ * the Software, and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
+ * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+ * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * http://www.FreeRTOS.org
+ * http://aws.amazon.com/freertos
+ *
+ * 1 tab == 4 spaces!
+ */
+
+/* Standard includes. */
+#include <string.h>
+#include <stdint.h>
+
+/* FreeRTOS includes. */
+#include "FreeRTOS.h"
+#include "task.h"
+
+/* Utils includes. */
+#include "FreeRTOS_CLI.h"
+
+/* If the application writer needs to place the buffer used by the CLI at a
+fixed address then set configAPPLICATION_PROVIDES_cOutputBuffer to 1 in
+FreeRTOSConfig.h, then declare an array with the following name and size in 
+one of the application files:
+	char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ];
+*/
+#ifndef configAPPLICATION_PROVIDES_cOutputBuffer
+	#define configAPPLICATION_PROVIDES_cOutputBuffer 0
+#endif
+
+typedef struct xCOMMAND_INPUT_LIST
+{
+	const CLI_Command_Definition_t *pxCommandLineDefinition;
+	struct xCOMMAND_INPUT_LIST *pxNext;
+} CLI_Definition_List_Item_t;
+
+/*
+ * The callback function that is executed when "help" is entered.  This is the
+ * only default command that is always present.
+ */
+static BaseType_t prvHelpCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString );
+
+/*
+ * Return the number of parameters that follow the command name.
+ */
+static int8_t prvGetNumberOfParameters( const char *pcCommandString );
+
+/* The definition of the "help" command.  This command is always at the front
+of the list of registered commands. */
+static const CLI_Command_Definition_t xHelpCommand =
+{
+	"help",
+	"\r\nhelp:\r\n Lists all the registered commands\r\n\r\n",
+	prvHelpCommand,
+	0
+};
+
+/* The definition of the list of commands.  Commands that are registered are
+added to this list. */
+static CLI_Definition_List_Item_t xRegisteredCommands =
+{
+	&xHelpCommand,	/* The first command in the list is always the help command, defined in this file. */
+	NULL			/* The next pointer is initialised to NULL, as there are no other registered commands yet. */
+};
+
+/* A buffer into which command outputs can be written is declared here, rather
+than in the command console implementation, to allow multiple command consoles
+to share the same buffer.  For example, an application may allow access to the
+command interpreter by UART and by Ethernet.  Sharing a buffer is done purely
+to save RAM.  Note, however, that the command console itself is not re-entrant,
+so only one command interpreter interface can be used at any one time.  For that
+reason, no attempt at providing mutual exclusion to the cOutputBuffer array is
+attempted.
+
+configAPPLICATION_PROVIDES_cOutputBuffer is provided to allow the application
+writer to provide their own cOutputBuffer declaration in cases where the
+buffer needs to be placed at a fixed address (rather than by the linker). */
+
+#if( configAPPLICATION_PROVIDES_cOutputBuffer == 0 )
+	static char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ];
+#else
+	extern char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ];
+#endif
+
+
+/*---------------------------------------------------------- */
+
+BaseType_t FreeRTOS_CLIRegisterCommand( const CLI_Command_Definition_t * const pxCommandToRegister )
+{
+static CLI_Definition_List_Item_t *pxLastCommandInList = &xRegisteredCommands;
+CLI_Definition_List_Item_t *pxNewListItem;
+BaseType_t xReturn = pdFAIL;
+
+	/* Check the parameter is not NULL. */
+	configASSERT( pxCommandToRegister );
+
+	/* Create a new list item that will reference the command being registered. */
+	pxNewListItem = ( CLI_Definition_List_Item_t * ) pvPortMalloc( sizeof( CLI_Definition_List_Item_t ) );
+	configASSERT( pxNewListItem );
+
+	if( pxNewListItem != NULL )
+	{
+		taskENTER_CRITICAL();
+		{
+			/* Reference the command being registered from the newly created
+			list item. */
+			pxNewListItem->pxCommandLineDefinition = pxCommandToRegister;
+
+			/* The new list item will get added to the end of the list, so
+			pxNext has nowhere to point. */
+			pxNewListItem->pxNext = NULL;
+
+			/* Add the newly created list item to the end of the already existing
+			list. */
+			pxLastCommandInList->pxNext = pxNewListItem;
+
+			/* Set the end of list marker to the new list item. */
+			pxLastCommandInList = pxNewListItem;
+		}
+		taskEXIT_CRITICAL();
+
+		xReturn = pdPASS;
+	}
+
+	return xReturn;
+}
+/*---------------------------------------------------------- */
+
+BaseType_t FreeRTOS_CLIProcessCommand( const char * const pcCommandInput, char * pcWriteBuffer, size_t xWriteBufferLen  )
+{
+static const CLI_Definition_List_Item_t *pxCommand = NULL;
+BaseType_t xReturn = pdTRUE;
+const char *pcRegisteredCommandString;
+size_t xCommandStringLength;
+
+	/* Note:  This function is not re-entrant.  It must not be called from more
+	thank one task. */
+
+	if( pxCommand == NULL )
+	{
+		/* Search for the command string in the list of registered commands. */
+		for( pxCommand = &xRegisteredCommands; pxCommand != NULL; pxCommand = pxCommand->pxNext )
+		{
+			pcRegisteredCommandString = pxCommand->pxCommandLineDefinition->pcCommand;
+			xCommandStringLength = strlen( pcRegisteredCommandString );
+
+			/* To ensure the string lengths match exactly, so as not to pick up
+			a sub-string of a longer command, check the byte after the expected
+			end of the string is either the end of the string or a space before
+			a parameter. */
+			if( ( pcCommandInput[ xCommandStringLength ] == ' ' ) || ( pcCommandInput[ xCommandStringLength ] == 0x00 ) )
+			{
+				if( strncmp( pcCommandInput, pcRegisteredCommandString, xCommandStringLength ) == 0 )
+				{
+					/* The command has been found.  Check it has the expected
+					number of parameters.  If cExpectedNumberOfParameters is -1,
+					then there could be a variable number of parameters and no
+					check is made. */
+					if( pxCommand->pxCommandLineDefinition->cExpectedNumberOfParameters >= 0 )
+					{
+						if( prvGetNumberOfParameters( pcCommandInput ) != pxCommand->pxCommandLineDefinition->cExpectedNumberOfParameters )
+						{
+							xReturn = pdFALSE;
+						}
+					}
+
+					break;
+				}
+			}
+		}
+	}
+
+	if( ( pxCommand != NULL ) && ( xReturn == pdFALSE ) )
+	{
+		/* The command was found, but the number of parameters with the command
+		was incorrect. */
+		strncpy( pcWriteBuffer, "Incorrect command parameter(s).  Enter \"help\" to view a list of available commands.\r\n\r\n", xWriteBufferLen );
+		pxCommand = NULL;
+	}
+	else if( pxCommand != NULL )
+	{
+		/* Call the callback function that is registered to this command. */
+		xReturn = pxCommand->pxCommandLineDefinition->pxCommandInterpreter( pcWriteBuffer, xWriteBufferLen, pcCommandInput );
+
+		/* If xReturn is pdFALSE, then no further strings will be returned
+		after this one, and	pxCommand can be reset to NULL ready to search
+		for the next entered command. */
+		if( xReturn == pdFALSE )
+		{
+			pxCommand = NULL;
+		}
+	}
+	else
+	{
+		/* pxCommand was NULL, the command was not found. */
+		strncpy( pcWriteBuffer, "Command not recognised.  Enter 'help' to view a list of available commands.\r\n\r\n", xWriteBufferLen );
+		xReturn = pdFALSE;
+	}
+
+	return xReturn;
+}
+/*---------------------------------------------------------- */
+
+char *FreeRTOS_CLIGetOutputBuffer( void )
+{
+	return cOutputBuffer;
+}
+/*---------------------------------------------------------- */
+
+const char *FreeRTOS_CLIGetParameter( const char *pcCommandString, UBaseType_t uxWantedParameter, BaseType_t *pxParameterStringLength )
+{
+UBaseType_t uxParametersFound = 0;
+const char *pcReturn = NULL;
+
+	*pxParameterStringLength = 0;
+
+	while( uxParametersFound < uxWantedParameter )
+	{
+		/* Index the character pointer past the current word.  If this is the start
+		of the command string then the first word is the command itself. */
+		while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) != ' ' ) )
+		{
+			pcCommandString++;
+		}
+
+		/* Find the start of the next string. */
+		while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) == ' ' ) )
+		{
+			pcCommandString++;
+		}
+
+		/* Was a string found? */
+		if( *pcCommandString != 0x00 )
+		{
+			/* Is this the start of the required parameter? */
+			uxParametersFound++;
+
+			if( uxParametersFound == uxWantedParameter )
+			{
+				/* How long is the parameter? */
+				pcReturn = pcCommandString;
+				while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) != ' ' ) )
+				{
+					( *pxParameterStringLength )++;
+					pcCommandString++;
+				}
+
+				if( *pxParameterStringLength == 0 )
+				{
+					pcReturn = NULL;
+				}
+
+				break;
+			}
+		}
+		else
+		{
+			break;
+		}
+	}
+
+	return pcReturn;
+}
+/*---------------------------------------------------------- */
+
+static BaseType_t prvHelpCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString )
+{
+static const CLI_Definition_List_Item_t * pxCommand = NULL;
+BaseType_t xReturn;
+
+	( void ) pcCommandString;
+
+	if( pxCommand == NULL )
+	{
+		/* Reset the pxCommand pointer back to the start of the list. */
+		pxCommand = &xRegisteredCommands;
+	}
+
+	/* Return the next command help string, before moving the pointer on to
+	the next command in the list. */
+	strncpy( pcWriteBuffer, pxCommand->pxCommandLineDefinition->pcHelpString, xWriteBufferLen );
+	pxCommand = pxCommand->pxNext;
+
+	if( pxCommand == NULL )
+	{
+		/* There are no more commands in the list, so there will be no more
+		strings to return after this one and pdFALSE should be returned. */
+		xReturn = pdFALSE;
+	}
+	else
+	{
+		xReturn = pdTRUE;
+	}
+
+	return xReturn;
+}
+/*---------------------------------------------------------- */
+
+static int8_t prvGetNumberOfParameters( const char *pcCommandString )
+{
+int8_t cParameters = 0;
+BaseType_t xLastCharacterWasSpace = pdFALSE;
+
+	/* Count the number of space delimited words in pcCommandString. */
+	while( *pcCommandString != 0x00 )
+	{
+		if( ( *pcCommandString ) == ' ' )
+		{
+			if( xLastCharacterWasSpace != pdTRUE )
+			{
+				cParameters++;
+				xLastCharacterWasSpace = pdTRUE;
+			}
+		}
+		else
+		{
+			xLastCharacterWasSpace = pdFALSE;
+		}
+
+		pcCommandString++;
+	}
+
+	/* If the command string ended with spaces, then there will have been too
+	many parameters counted. */
+	if( xLastCharacterWasSpace == pdTRUE )
+	{
+		cParameters--;
+	}
+
+	/* The value returned is one less than the number of space delimited words,
+	as the first word should be the command itself. */
+	return cParameters;
+}
+
diff --git a/assets/User_code/component/FreeRTOS_CLI.h b/assets/User_code/component/FreeRTOS_CLI.h
new file mode 100644
index 0000000..e6d8266
--- /dev/null
+++ b/assets/User_code/component/FreeRTOS_CLI.h
@@ -0,0 +1,108 @@
+/*
+ * FreeRTOS+CLI V1.0.4
+ * Copyright (C) 2017 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of
+ * this software and associated documentation files (the "Software"), to deal in
+ * the Software without restriction, including without limitation the rights to
+ * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+ * the Software, and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
+ * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+ * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * http://www.FreeRTOS.org
+ * http://aws.amazon.com/freertos
+ *
+ * 1 tab == 4 spaces!
+ */
+
+#ifndef COMMAND_INTERPRETER_H
+#define COMMAND_INTERPRETER_H
+
+/* This config should be defined in FreeRTOSConfig.h. But due to the limition of CubeMX I put it here. */
+#define configCOMMAND_INT_MAX_OUTPUT_SIZE  512
+
+/* The prototype to which callback functions used to process command line
+commands must comply.  pcWriteBuffer is a buffer into which the output from
+executing the command can be written, xWriteBufferLen is the length, in bytes of
+the pcWriteBuffer buffer, and pcCommandString is the entire string as input by
+the user (from which parameters can be extracted).*/
+typedef BaseType_t (*pdCOMMAND_LINE_CALLBACK)( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString );
+
+/* The structure that defines command line commands.  A command line command
+should be defined by declaring a const structure of this type. */
+typedef struct xCOMMAND_LINE_INPUT
+{
+	const char * const pcCommand;				/* The command that causes pxCommandInterpreter to be executed.  For example "help".  Must be all lower case. */
+	const char * const pcHelpString;			/* String that describes how to use the command.  Should start with the command itself, and end with "\r\n".  For example "help: Returns a list of all the commands\r\n". */
+	const pdCOMMAND_LINE_CALLBACK pxCommandInterpreter;	/* A pointer to the callback function that will return the output generated by the command. */
+	int8_t cExpectedNumberOfParameters;			/* Commands expect a fixed number of parameters, which may be zero. */
+} CLI_Command_Definition_t;
+
+/* For backward compatibility. */
+#define xCommandLineInput CLI_Command_Definition_t
+
+/*
+ * Register the command passed in using the pxCommandToRegister parameter.
+ * Registering a command adds the command to the list of commands that are
+ * handled by the command interpreter.  Once a command has been registered it
+ * can be executed from the command line.
+ */
+BaseType_t FreeRTOS_CLIRegisterCommand( const CLI_Command_Definition_t * const pxCommandToRegister );
+
+/*
+ * Runs the command interpreter for the command string "pcCommandInput".  Any
+ * output generated by running the command will be placed into pcWriteBuffer.
+ * xWriteBufferLen must indicate the size, in bytes, of the buffer pointed to
+ * by pcWriteBuffer.
+ *
+ * FreeRTOS_CLIProcessCommand should be called repeatedly until it returns pdFALSE.
+ *
+ * pcCmdIntProcessCommand is not reentrant.  It must not be called from more
+ * than one task - or at least - by more than one task at a time.
+ */
+BaseType_t FreeRTOS_CLIProcessCommand( const char * const pcCommandInput, char * pcWriteBuffer, size_t xWriteBufferLen  );
+
+/*---------------------------------------------------------- */
+
+/*
+ * A buffer into which command outputs can be written is declared in the
+ * main command interpreter, rather than in the command console implementation,
+ * to allow application that provide access to the command console via multiple
+ * interfaces to share a buffer, and therefore save RAM.  Note, however, that
+ * the command interpreter itself is not re-entrant, so only one command
+ * console interface can be used at any one time.  For that reason, no attempt
+ * is made to provide any mutual exclusion mechanism on the output buffer.
+ *
+ * FreeRTOS_CLIGetOutputBuffer() returns the address of the output buffer.
+ */
+char *FreeRTOS_CLIGetOutputBuffer( void );
+
+/*
+ * Return a pointer to the xParameterNumber'th word in pcCommandString.
+ */
+const char *FreeRTOS_CLIGetParameter( const char *pcCommandString, UBaseType_t uxWantedParameter, BaseType_t *pxParameterStringLength );
+
+#endif /* COMMAND_INTERPRETER_H */
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/assets/User_code/component/QuaternionEKF.c b/assets/User_code/component/QuaternionEKF.c
deleted file mode 100644
index 6c69918..0000000
--- a/assets/User_code/component/QuaternionEKF.c
+++ /dev/null
@@ -1,492 +0,0 @@
-/**
- ******************************************************************************
- * @file    QuaternionEKF.c
- * @author  Wang Hongxi
- * @version V1.2.0
- * @date    2022/3/8
- * @brief   attitude update with gyro bias estimate and chi-square test
- ******************************************************************************
- * @attention
- * 1st order LPF transfer function:
- *     1
- *  ———————
- *  as + 1
- ******************************************************************************
- */
-#include "QuaternionEKF.h"
-
-QEKF_INS_t QEKF_INS;
-
-const float IMU_QuaternionEKF_F[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};
-float IMU_QuaternionEKF_P[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};
-float IMU_QuaternionEKF_K[18];
-float IMU_QuaternionEKF_H[18];
-
-static float invSqrt(float x);
-static void IMU_QuaternionEKF_Observe(KalmanFilter_t *kf);
-static void IMU_QuaternionEKF_F_Linearization_P_Fading(KalmanFilter_t *kf);
-static void IMU_QuaternionEKF_SetH(KalmanFilter_t *kf);
-static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf);
-
-/**
- * @brief Quaternion EKF initialization and some reference value
- * @param[in] process_noise1 quaternion process noise    10
- * @param[in] process_noise2 gyro bias process noise     0.001
- * @param[in] measure_noise  accel measure noise         1000000
- * @param[in] lambda         fading coefficient          0.9996
- * @param[in] lpf            lowpass filter coefficient  0
- */
-void IMU_QuaternionEKF_Init(float process_noise1, float process_noise2, float measure_noise, float lambda, float lpf)
-{
-    QEKF_INS.Initialized = 1;
-    QEKF_INS.Q1 = process_noise1;
-    QEKF_INS.Q2 = process_noise2;
-    QEKF_INS.R = measure_noise;
-    QEKF_INS.ChiSquareTestThreshold = 1e-8;
-    QEKF_INS.ConvergeFlag = 0;
-    QEKF_INS.ErrorCount = 0;
-    QEKF_INS.UpdateCount = 0;
-    if (lambda > 1)
-    {
-        lambda = 1;
-    }
-    QEKF_INS.lambda = lambda;
-    QEKF_INS.accLPFcoef = lpf;
-
-    // 初始化矩阵维度信息
-    Kalman_Filter_Init(&QEKF_INS.IMU_QuaternionEKF, 6, 0, 3);
-    Matrix_Init(&QEKF_INS.ChiSquare, 1, 1, (float *)QEKF_INS.ChiSquare_Data);
-
-    // 姿态初始化
-    QEKF_INS.IMU_QuaternionEKF.xhat_data[0] = 1;
-    QEKF_INS.IMU_QuaternionEKF.xhat_data[1] = 0;
-    QEKF_INS.IMU_QuaternionEKF.xhat_data[2] = 0;
-    QEKF_INS.IMU_QuaternionEKF.xhat_data[3] = 0;
-
-    // 自定义函数初始化,用于扩展或增加kf的基础功能
-    QEKF_INS.IMU_QuaternionEKF.User_Func0_f = IMU_QuaternionEKF_Observe;
-    QEKF_INS.IMU_QuaternionEKF.User_Func1_f = IMU_QuaternionEKF_F_Linearization_P_Fading;
-    QEKF_INS.IMU_QuaternionEKF.User_Func2_f = IMU_QuaternionEKF_SetH;
-    QEKF_INS.IMU_QuaternionEKF.User_Func3_f = IMU_QuaternionEKF_xhatUpdate;
-
-    // 设定标志位,用自定函数替换kf标准步骤中的SetK(计算增益)以及xhatupdate(后验估计/融合)
-    QEKF_INS.IMU_QuaternionEKF.SkipEq3 = TRUE;
-    QEKF_INS.IMU_QuaternionEKF.SkipEq4 = TRUE;
-
-    memcpy(QEKF_INS.IMU_QuaternionEKF.F_data, IMU_QuaternionEKF_F, sizeof(IMU_QuaternionEKF_F));
-    memcpy(QEKF_INS.IMU_QuaternionEKF.P_data, IMU_QuaternionEKF_P, sizeof(IMU_QuaternionEKF_P));
-}
-
-/**
- * @brief Quaternion EKF update
- * @param[in]       gyro x y z in rad/s
- * @param[in]       accel x y z in m/s²
- * @param[in]       update period in s
- */
-void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt)
-{
-    // 0.5(Ohm-Ohm^bias)*deltaT,用于更新工作点处的状态转移F矩阵
-    static float halfgxdt, halfgydt, halfgzdt;
-    static float accelInvNorm;
-    if (!QEKF_INS.Initialized)
-    {
-        IMU_QuaternionEKF_Init(10, 0.001, 1000000 * 10, 0.9996 * 0 + 1, 0);
-    }
-
-    /*   F, number with * represent vals to be set
-     0      1*     2*     3*     4     5
-     6*     7      8*     9*    10    11
-    12*    13*    14     15*    16    17
-    18*    19*    20*    21     22    23
-    24     25     26     27     28    29
-    30     31     32     33     34    35
-    */
-    QEKF_INS.dt = dt;
-
-    QEKF_INS.Gyro[0] = gx - QEKF_INS.GyroBias[0];
-    QEKF_INS.Gyro[1] = gy - QEKF_INS.GyroBias[1];
-    QEKF_INS.Gyro[2] = gz - QEKF_INS.GyroBias[2];
-
-    // set F
-    halfgxdt = 0.5f * QEKF_INS.Gyro[0] * dt;
-    halfgydt = 0.5f * QEKF_INS.Gyro[1] * dt;
-    halfgzdt = 0.5f * QEKF_INS.Gyro[2] * dt;
-
-    // 此部分设定状态转移矩阵F的左上角部分 4x4子矩阵,即0.5(Ohm-Ohm^bias)*deltaT,右下角有一个2x2单位阵已经初始化好了
-    // 注意在predict步F的右上角是4x2的零矩阵,因此每次predict的时候都会调用memcpy用单位阵覆盖前一轮线性化后的矩阵
-    memcpy(QEKF_INS.IMU_QuaternionEKF.F_data, IMU_QuaternionEKF_F, sizeof(IMU_QuaternionEKF_F));
-
-    QEKF_INS.IMU_QuaternionEKF.F_data[1] = -halfgxdt;
-    QEKF_INS.IMU_QuaternionEKF.F_data[2] = -halfgydt;
-    QEKF_INS.IMU_QuaternionEKF.F_data[3] = -halfgzdt;
-
-    QEKF_INS.IMU_QuaternionEKF.F_data[6] = halfgxdt;
-    QEKF_INS.IMU_QuaternionEKF.F_data[8] = halfgzdt;
-    QEKF_INS.IMU_QuaternionEKF.F_data[9] = -halfgydt;
-
-    QEKF_INS.IMU_QuaternionEKF.F_data[12] = halfgydt;
-    QEKF_INS.IMU_QuaternionEKF.F_data[13] = -halfgzdt;
-    QEKF_INS.IMU_QuaternionEKF.F_data[15] = halfgxdt;
-
-    QEKF_INS.IMU_QuaternionEKF.F_data[18] = halfgzdt;
-    QEKF_INS.IMU_QuaternionEKF.F_data[19] = halfgydt;
-    QEKF_INS.IMU_QuaternionEKF.F_data[20] = -halfgxdt;
-
-    // accel low pass filter,加速度过一下低通滤波平滑数据,降低撞击和异常的影响
-    if (QEKF_INS.UpdateCount == 0) // 如果是第一次进入,需要初始化低通滤波
-    {
-        QEKF_INS.Accel[0] = ax;
-        QEKF_INS.Accel[1] = ay;
-        QEKF_INS.Accel[2] = az;
-    }
-    QEKF_INS.Accel[0] = QEKF_INS.Accel[0] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + ax * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
-    QEKF_INS.Accel[1] = QEKF_INS.Accel[1] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + ay * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
-    QEKF_INS.Accel[2] = QEKF_INS.Accel[2] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + az * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
-
-    // set z,单位化重力加速度向量
-    accelInvNorm = invSqrt(QEKF_INS.Accel[0] * QEKF_INS.Accel[0] + QEKF_INS.Accel[1] * QEKF_INS.Accel[1] + QEKF_INS.Accel[2] * QEKF_INS.Accel[2]);
-    for (uint8_t i = 0; i < 3; i++)
-    {
-        QEKF_INS.IMU_QuaternionEKF.MeasuredVector[i] = QEKF_INS.Accel[i] * accelInvNorm; // 用加速度向量更新量测值
-    }
-
-    // get body state
-    QEKF_INS.gyro_norm = 1.0f / invSqrt(QEKF_INS.Gyro[0] * QEKF_INS.Gyro[0] +
-                                        QEKF_INS.Gyro[1] * QEKF_INS.Gyro[1] +
-                                        QEKF_INS.Gyro[2] * QEKF_INS.Gyro[2]);
-    QEKF_INS.accl_norm = 1.0f / accelInvNorm;
-
-    // 如果角速度小于阈值且加速度处于设定范围内,认为运动稳定,加速度可以用于修正角速度
-    // 稍后在最后的姿态更新部分会利用StableFlag来确定
-    if (QEKF_INS.gyro_norm < 0.3f && QEKF_INS.accl_norm > 9.8f - 0.5f && QEKF_INS.accl_norm < 9.8f + 0.5f)
-    {
-        QEKF_INS.StableFlag = 1;
-    }
-    else
-    {
-        QEKF_INS.StableFlag = 0;
-    }
-
-    // set Q R,过程噪声和观测噪声矩阵
-    QEKF_INS.IMU_QuaternionEKF.Q_data[0] = QEKF_INS.Q1 * QEKF_INS.dt;
-    QEKF_INS.IMU_QuaternionEKF.Q_data[7] = QEKF_INS.Q1 * QEKF_INS.dt;
-    QEKF_INS.IMU_QuaternionEKF.Q_data[14] = QEKF_INS.Q1 * QEKF_INS.dt;
-    QEKF_INS.IMU_QuaternionEKF.Q_data[21] = QEKF_INS.Q1 * QEKF_INS.dt;
-    QEKF_INS.IMU_QuaternionEKF.Q_data[28] = QEKF_INS.Q2 * QEKF_INS.dt;
-    QEKF_INS.IMU_QuaternionEKF.Q_data[35] = QEKF_INS.Q2 * QEKF_INS.dt;
-    QEKF_INS.IMU_QuaternionEKF.R_data[0] = QEKF_INS.R;
-    QEKF_INS.IMU_QuaternionEKF.R_data[4] = QEKF_INS.R;
-    QEKF_INS.IMU_QuaternionEKF.R_data[8] = QEKF_INS.R;
-
-    // 调用kalman_filter.c封装好的函数,注意几个User_Funcx_f的调用
-    Kalman_Filter_Update(&QEKF_INS.IMU_QuaternionEKF);
-
-    // 获取融合后的数据,包括四元数和xy零飘值
-    QEKF_INS.q[0] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[0];
-    QEKF_INS.q[1] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[1];
-    QEKF_INS.q[2] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[2];
-    QEKF_INS.q[3] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[3];
-    QEKF_INS.GyroBias[0] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[4];
-    QEKF_INS.GyroBias[1] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[5];
-    QEKF_INS.GyroBias[2] = 0; // 大部分时候z轴通天,无法观测yaw的漂移
-
-    // 利用四元数反解欧拉角
-    QEKF_INS.Yaw = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f;
-    QEKF_INS.Pitch = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f;
-    QEKF_INS.Roll = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
-
-    // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
-    if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast > 180.0f)
-    {
-        QEKF_INS.YawRoundCount--;
-    }
-    else if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast < -180.0f)
-    {
-        QEKF_INS.YawRoundCount++;
-    }
-    QEKF_INS.YawTotalAngle = 360.0f * QEKF_INS.YawRoundCount + QEKF_INS.Yaw;
-    QEKF_INS.YawAngleLast = QEKF_INS.Yaw;
-    QEKF_INS.UpdateCount++; // 初始化低通滤波用,计数测试用
-}
-
-/**
- * @brief 用于更新线性化后的状态转移矩阵F右上角的一个4x2分块矩阵,稍后用于协方差矩阵P的更新;
- *        并对零漂的方差进行限制,防止过度收敛并限幅防止发散
- *
- * @param kf
- */
-static void IMU_QuaternionEKF_F_Linearization_P_Fading(KalmanFilter_t *kf)
-{
-    static float q0, q1, q2, q3;
-    static float qInvNorm;
-
-    q0 = kf->xhatminus_data[0];
-    q1 = kf->xhatminus_data[1];
-    q2 = kf->xhatminus_data[2];
-    q3 = kf->xhatminus_data[3];
-
-    // quaternion normalize
-    qInvNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
-    for (uint8_t i = 0; i < 4; i++)
-    {
-        kf->xhatminus_data[i] *= qInvNorm;
-    }
-    /*  F, number with * represent vals to be set
-     0     1     2     3     4*     5*
-     6     7     8     9    10*    11*
-    12    13    14    15    16*    17*
-    18    19    20    21    22*    23*
-    24    25    26    27    28     29
-    30    31    32    33    34     35
-    */
-    // set F
-    kf->F_data[4] = q1 * QEKF_INS.dt / 2;
-    kf->F_data[5] = q2 * QEKF_INS.dt / 2;
-
-    kf->F_data[10] = -q0 * QEKF_INS.dt / 2;
-    kf->F_data[11] = q3 * QEKF_INS.dt / 2;
-
-    kf->F_data[16] = -q3 * QEKF_INS.dt / 2;
-    kf->F_data[17] = -q0 * QEKF_INS.dt / 2;
-
-    kf->F_data[22] = q2 * QEKF_INS.dt / 2;
-    kf->F_data[23] = -q1 * QEKF_INS.dt / 2;
-
-    // fading filter,防止零飘参数过度收敛
-    kf->P_data[28] /= QEKF_INS.lambda;
-    kf->P_data[35] /= QEKF_INS.lambda;
-
-    // 限幅,防止发散
-    if (kf->P_data[28] > 10000)
-    {
-        kf->P_data[28] = 10000;
-    }
-    if (kf->P_data[35] > 10000)
-    {
-        kf->P_data[35] = 10000;
-    }
-}
-
-/**
- * @brief 在工作点处计算观测函数h(x)的Jacobi矩阵H
- *
- * @param kf
- */
-static void IMU_QuaternionEKF_SetH(KalmanFilter_t *kf)
-{
-    static float doubleq0, doubleq1, doubleq2, doubleq3;
-    /* H
-     0     1     2     3     4     5
-     6     7     8     9    10    11
-    12    13    14    15    16    17
-    last two cols are zero
-    */
-    // set H
-    doubleq0 = 2 * kf->xhatminus_data[0];
-    doubleq1 = 2 * kf->xhatminus_data[1];
-    doubleq2 = 2 * kf->xhatminus_data[2];
-    doubleq3 = 2 * kf->xhatminus_data[3];
-
-    memset(kf->H_data, 0, sizeof_float * kf->zSize * kf->xhatSize);
-
-    kf->H_data[0] = -doubleq2;
-    kf->H_data[1] = doubleq3;
-    kf->H_data[2] = -doubleq0;
-    kf->H_data[3] = doubleq1;
-
-    kf->H_data[6] = doubleq1;
-    kf->H_data[7] = doubleq0;
-    kf->H_data[8] = doubleq3;
-    kf->H_data[9] = doubleq2;
-
-    kf->H_data[12] = doubleq0;
-    kf->H_data[13] = -doubleq1;
-    kf->H_data[14] = -doubleq2;
-    kf->H_data[15] = doubleq3;
-}
-
-/**
- * @brief 利用观测值和先验估计得到最优的后验估计
- *        加入了卡方检验以判断融合加速度的条件是否满足
- *        同时引入发散保护保证恶劣工况下的必要量测更新
- *
- * @param kf
- */
-static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf)
-{
-    static float q0, q1, q2, q3;
-
-    kf->MatStatus = Matrix_Transpose(&kf->H, &kf->HT); // z|x => x|z
-    kf->temp_matrix.numRows = kf->H.numRows;
-    kf->temp_matrix.numCols = kf->Pminus.numCols;
-    kf->MatStatus = Matrix_Multiply(&kf->H, &kf->Pminus, &kf->temp_matrix); // temp_matrix = H·P'(k)
-    kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
-    kf->temp_matrix1.numCols = kf->HT.numCols;
-    kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1); // temp_matrix1 = H·P'(k)·HT
-    kf->S.numRows = kf->R.numRows;
-    kf->S.numCols = kf->R.numCols;
-    kf->MatStatus = Matrix_Add(&kf->temp_matrix1, &kf->R, &kf->S); // S = H P'(k) HT + R
-    kf->MatStatus = Matrix_Inverse(&kf->S, &kf->temp_matrix1);     // temp_matrix1 = inv(H·P'(k)·HT + R)
-
-    q0 = kf->xhatminus_data[0];
-    q1 = kf->xhatminus_data[1];
-    q2 = kf->xhatminus_data[2];
-    q3 = kf->xhatminus_data[3];
-
-    kf->temp_vector.numRows = kf->H.numRows;
-    kf->temp_vector.numCols = 1;
-    // 计算预测得到的重力加速度方向(通过姿态获取的)
-    kf->temp_vector_data[0] = 2 * (q1 * q3 - q0 * q2);
-    kf->temp_vector_data[1] = 2 * (q0 * q1 + q2 * q3);
-    kf->temp_vector_data[2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; // temp_vector = h(xhat'(k))
-
-    // 计算预测值和各个轴的方向余弦
-    for (uint8_t i = 0; i < 3; i++)
-    {
-        QEKF_INS.OrientationCosine[i] = acosf(fabsf(kf->temp_vector_data[i]));
-    }
-
-    // 利用加速度计数据修正
-    kf->temp_vector1.numRows = kf->z.numRows;
-    kf->temp_vector1.numCols = 1;
-    kf->MatStatus = Matrix_Subtract(&kf->z, &kf->temp_vector, &kf->temp_vector1); // temp_vector1 = z(k) - h(xhat'(k))
-
-    // chi-square test,卡方检验
-    kf->temp_matrix.numRows = kf->temp_vector1.numRows;
-    kf->temp_matrix.numCols = 1;
-    kf->MatStatus = Matrix_Multiply(&kf->temp_matrix1, &kf->temp_vector1, &kf->temp_matrix); // temp_matrix = inv(H·P'(k)·HT + R)·(z(k) - h(xhat'(k)))
-    kf->temp_vector.numRows = 1;
-    kf->temp_vector.numCols = kf->temp_vector1.numRows;
-    kf->MatStatus = Matrix_Transpose(&kf->temp_vector1, &kf->temp_vector); // temp_vector = z(k) - h(xhat'(k))'
-    kf->MatStatus = Matrix_Multiply(&kf->temp_vector, &kf->temp_matrix, &QEKF_INS.ChiSquare);
-    // rk is small,filter converged/converging
-    if (QEKF_INS.ChiSquare_Data[0] < 0.5f * QEKF_INS.ChiSquareTestThreshold)
-    {
-        QEKF_INS.ConvergeFlag = 1;
-    }
-    // rk is bigger than thre but once converged
-    if (QEKF_INS.ChiSquare_Data[0] > QEKF_INS.ChiSquareTestThreshold && QEKF_INS.ConvergeFlag)
-    {
-        if (QEKF_INS.StableFlag)
-        {
-            QEKF_INS.ErrorCount++; // 载体静止时仍无法通过卡方检验
-        }
-        else
-        {
-            QEKF_INS.ErrorCount = 0;
-        }
-
-        if (QEKF_INS.ErrorCount > 50)
-        {
-            // 滤波器发散
-            QEKF_INS.ConvergeFlag = 0;
-            kf->SkipEq5 = FALSE; // step-5 is cov mat P updating
-        }
-        else
-        {
-            //  残差未通过卡方检验 仅预测
-            //  xhat(k) = xhat'(k)
-            //  P(k) = P'(k)
-            memcpy(kf->xhat_data, kf->xhatminus_data, sizeof_float * kf->xhatSize);
-            memcpy(kf->P_data, kf->Pminus_data, sizeof_float * kf->xhatSize * kf->xhatSize);
-            kf->SkipEq5 = TRUE; // part5 is P updating
-            return;
-        }
-    }
-    else // if divergent or rk is not that big/acceptable,use adaptive gain
-    {
-        // scale adaptive,rk越小则增益越大,否则更相信预测值
-        if (QEKF_INS.ChiSquare_Data[0] > 0.1f * QEKF_INS.ChiSquareTestThreshold && QEKF_INS.ConvergeFlag)
-        {
-            QEKF_INS.AdaptiveGainScale = (QEKF_INS.ChiSquareTestThreshold - QEKF_INS.ChiSquare_Data[0]) / (0.9f * QEKF_INS.ChiSquareTestThreshold);
-        }
-        else
-        {
-            QEKF_INS.AdaptiveGainScale = 1;
-        }
-        QEKF_INS.ErrorCount = 0;
-        kf->SkipEq5 = FALSE;
-    }
-
-    // cal kf-gain K
-    kf->temp_matrix.numRows = kf->Pminus.numRows;
-    kf->temp_matrix.numCols = kf->HT.numCols;
-    kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->HT, &kf->temp_matrix); // temp_matrix = P'(k)·HT
-    kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
-
-    // implement adaptive
-    for (uint8_t i = 0; i < kf->K.numRows * kf->K.numCols; i++)
-    {
-        kf->K_data[i] *= QEKF_INS.AdaptiveGainScale;
-    }
-    for (uint8_t i = 4; i < 6; i++)
-    {
-        for (uint8_t j = 0; j < 3; j++)
-        {
-            kf->K_data[i * 3 + j] *= QEKF_INS.OrientationCosine[i - 4] / 1.5707963f; // 1 rad
-        }
-    }
-
-    kf->temp_vector.numRows = kf->K.numRows;
-    kf->temp_vector.numCols = 1;
-    kf->MatStatus = Matrix_Multiply(&kf->K, &kf->temp_vector1, &kf->temp_vector); // temp_vector = K(k)·(z(k) - H·xhat'(k))
-
-    // 零漂修正限幅,一般不会有过大的漂移
-    if (QEKF_INS.ConvergeFlag)
-    {
-        for (uint8_t i = 4; i < 6; i++)
-        {
-            if (kf->temp_vector.pData[i] > 1e-2f * QEKF_INS.dt)
-            {
-                kf->temp_vector.pData[i] = 1e-2f * QEKF_INS.dt;
-            }
-            if (kf->temp_vector.pData[i] < -1e-2f * QEKF_INS.dt)
-            {
-                kf->temp_vector.pData[i] = -1e-2f * QEKF_INS.dt;
-            }
-        }
-    }
-
-    // 不修正yaw轴数据
-    kf->temp_vector.pData[3] = 0;
-    kf->MatStatus = Matrix_Add(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
-}
-
-/**
- * @brief EKF观测环节,其实就是把数据复制一下
- *
- * @param kf kf类型定义
- */
-static void IMU_QuaternionEKF_Observe(KalmanFilter_t *kf)
-{
-    memcpy(IMU_QuaternionEKF_P, kf->P_data, sizeof(IMU_QuaternionEKF_P));
-    memcpy(IMU_QuaternionEKF_K, kf->K_data, sizeof(IMU_QuaternionEKF_K));
-    memcpy(IMU_QuaternionEKF_H, kf->H_data, sizeof(IMU_QuaternionEKF_H));
-}
-
-/**
- * @brief 自定义1/sqrt(x),速度更快
- *
- * @param x x
- * @return float
- */
-static float invSqrt(float x)
-{
-    float halfx = 0.5f * x;
-    float y = x;
-    long i = *(long *)&y;
-    i = 0x5f375a86 - (i >> 1);
-    y = *(float *)&i;
-    y = y * (1.5f - (halfx * y * y));
-    return y;
-}
diff --git a/assets/User_code/component/QuaternionEKF.h b/assets/User_code/component/QuaternionEKF.h
deleted file mode 100644
index f7b4d7e..0000000
--- a/assets/User_code/component/QuaternionEKF.h
+++ /dev/null
@@ -1,75 +0,0 @@
-/**
- ******************************************************************************
- * @file    QuaternionEKF.h
- * @author  Wang Hongxi
- * @version V1.2.0
- * @date    2022/3/8
- * @brief   attitude update with gyro bias estimate and chi-square test
- ******************************************************************************
- * @attention
- *
- ******************************************************************************
- */
-#ifndef _QUAT_EKF_H
-#define _QUAT_EKF_H
-#include "kalman_filter.h"
-
-/* boolean type definitions */
-#ifndef TRUE
-#define TRUE 1 /**< boolean true  */
-#endif
-
-#ifndef FALSE
-#define FALSE 0 /**< boolean fails */
-#endif
-
-typedef struct
-{
-    uint8_t Initialized;
-    KalmanFilter_t IMU_QuaternionEKF;
-    uint8_t ConvergeFlag;
-    uint8_t StableFlag;
-    uint64_t ErrorCount;
-    uint64_t UpdateCount;
-
-    float q[4];        // 四元数估计值
-    float GyroBias[3]; // 陀螺仪零偏估计值
-
-    float Gyro[3];
-    float Accel[3];
-
-    float OrientationCosine[3];
-
-    float accLPFcoef;
-    float gyro_norm;
-    float accl_norm;
-    float AdaptiveGainScale;
-
-    float Roll;
-    float Pitch;
-    float Yaw;
-
-    float YawTotalAngle;
-
-    float Q1; // 四元数更新过程噪声
-    float Q2; // 陀螺仪零偏过程噪声
-    float R;  // 加速度计量测噪声
-
-    float dt; // 姿态更新周期
-    mat ChiSquare;
-    float ChiSquare_Data[1];      // 卡方检验检测函数
-    float ChiSquareTestThreshold; // 卡方检验阈值
-    float lambda;                 // 渐消因子
-
-    int16_t YawRoundCount;
-
-    float YawAngleLast;
-} QEKF_INS_t;
-
-extern QEKF_INS_t QEKF_INS;
-extern float chiSquare;
-extern float ChiSquareTestThreshold;
-void IMU_QuaternionEKF_Init(float process_noise1, float process_noise2, float measure_noise, float lambda, float lpf);
-void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt);
-
-#endif
diff --git a/assets/User_code/component/ballistics.c b/assets/User_code/component/ballistics.c
new file mode 100644
index 0000000..4b5572f
--- /dev/null
+++ b/assets/User_code/component/ballistics.c
@@ -0,0 +1,12 @@
+/*
+  弹道补偿算法。
+*/
+
+#include "ballistics.h"
+
+void Ballistics_Init(Ballistics_t *b) { (void)b; }
+void Ballistics_Apply(Ballistics_t *b, float bullet_speed) {
+  (void)b;
+  (void)bullet_speed;
+}
+void Ballistics_Reset(Ballistics_t *b) { (void)b; }
diff --git a/assets/User_code/component/ballistics.h b/assets/User_code/component/ballistics.h
new file mode 100644
index 0000000..3b9faa6
--- /dev/null
+++ b/assets/User_code/component/ballistics.h
@@ -0,0 +1,23 @@
+/*
+  弹道补偿算法。
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "ahrs.h"
+
+typedef struct {
+  AHRS_Eulr_t *eulr;
+} Ballistics_t;
+
+void Ballistics_Init(Ballistics_t *b);
+void Ballistics_Apply(Ballistics_t *b, float bullet_speed);
+void Ballistics_Reset(Ballistics_t *b);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/assets/User_code/component/capacity.c b/assets/User_code/component/capacity.c
new file mode 100644
index 0000000..6865e54
--- /dev/null
+++ b/assets/User_code/component/capacity.c
@@ -0,0 +1,59 @@
+/*
+  剩余电量算法。
+
+  通过电压值计算剩余电量。
+*/
+
+#include "capacity.h"
+
+/**
+ * @brief 通过电压计算电池剩余电量
+ *
+ * @param volt 电压值
+ * @return float 剩余电量比例
+ */
+float Capacity_GetBatteryRemain(float volt) {
+  float percentage;
+  float volt_2 = volt * volt;
+  float volt_3 = volt_2 * volt;
+
+  if (volt < 19.5f)
+    percentage = 0.0f;
+
+  else if (volt < 21.9f)
+    percentage = 0.005664f * volt_3 - 0.3386f * volt_2 + 6.765f * volt - 45.17f;
+
+  else if (volt < 25.5f)
+    percentage = 0.02269f * volt_3 - 1.654f * volt_2 + 40.34f * volt - 328.4f;
+
+  else
+    percentage = 1.0f;
+
+  if (percentage < 0.0f)
+    percentage = 0.0f;
+
+  else if (percentage > 1.0f)
+    percentage = 1.0f;
+
+  return percentage;
+}
+
+/**
+ * @brief
+ *
+ * @param vcap 电容电压
+ * @param vbat 电池电压
+ * @param v_cutoff 截止电压
+ * @return float 电容剩余电量比例
+ */
+float Capacity_GetCapacitorRemain(float vcap, float vbat, float v_cutoff) {
+  float percentage = (vcap - v_cutoff) / (vbat - v_cutoff);
+
+  if (percentage < 0.0f)
+    percentage = 0.0f;
+
+  else if (percentage > 1.0f)
+    percentage = 1.0f;
+
+  return percentage;
+}
diff --git a/assets/User_code/component/capacity.h b/assets/User_code/component/capacity.h
new file mode 100644
index 0000000..a3839d4
--- /dev/null
+++ b/assets/User_code/component/capacity.h
@@ -0,0 +1,35 @@
+/*
+  剩余电量算法。
+
+  通过电压值计算剩余电量。
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "user_math.h"
+
+/**
+ * @brief 通过电压计算电池剩余电量
+ *
+ * @param volt 电压值
+ * @return float 剩余电量比例
+ */
+float Capacity_GetBatteryRemain(float volt);
+
+/**
+ * @brief
+ *
+ * @param vcap 电容电压
+ * @param vbat 电池电压
+ * @param v_cutoff 截止电压
+ * @return float 电容剩余电量比例
+ */
+float Capacity_GetCapacitorRemain(float vcap, float vbat, float v_cutoff);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/assets/User_code/component/cmd.c b/assets/User_code/component/cmd.c
new file mode 100644
index 0000000..205869c
--- /dev/null
+++ b/assets/User_code/component/cmd.c
@@ -0,0 +1,375 @@
+/*
+  控制命令
+*/
+
+#include "cmd.h"
+
+#include <string.h>
+
+/**
+ * @brief 行为转换为对应按键
+ *
+ * @param cmd 主结构体
+ * @param behavior 行为
+ * @return uint16_t 行为对应的按键
+ */
+static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *cmd,
+                                               CMD_Behavior_t behavior) {
+  return cmd->param->map.key_map[behavior].key;
+}
+
+static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd,
+                                                    CMD_Behavior_t behavior) {
+  return cmd->param->map.key_map[behavior].active;
+}
+
+/**
+ * @brief 检查按键是否按下
+ *
+ * @param rc 遥控器数据
+ * @param key 按键名称
+ * @param stateful 是否为状态切换按键
+ * @return true 按下
+ * @return false 未按下
+ */
+static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key) {
+  /* 按下按键为鼠标左、右键 */
+  if (key == CMD_L_CLICK) {
+    return rc->mouse.l_click;
+  }
+  if (key == CMD_R_CLICK) {
+    return rc->mouse.r_click;
+  }
+  return rc->key & (1u << key);
+}
+
+static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd,
+                                   CMD_Behavior_t behavior) {
+  CMD_KeyValue_t key = CMD_BehaviorToKey(cmd, behavior);
+  CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior);
+
+  bool now_key_pressed, last_key_pressed;
+
+  /* 按下按键为鼠标左、右键 */
+  if (key == CMD_L_CLICK) {
+    now_key_pressed = rc->mouse.l_click;
+    last_key_pressed = cmd->mouse_last.l_click;
+  } else if (key == CMD_R_CLICK) {
+    now_key_pressed = rc->mouse.r_click;
+    last_key_pressed = cmd->mouse_last.r_click;
+  } else {
+    now_key_pressed = rc->key & (1u << key);
+    last_key_pressed = cmd->key_last & (1u << key);
+  }
+
+  switch (active) {
+    case CMD_ACTIVE_PRESSING:
+      return now_key_pressed && !last_key_pressed;
+    case CMD_ACTIVE_RASING:
+      return !now_key_pressed && last_key_pressed;
+    case CMD_ACTIVE_PRESSED:
+      return now_key_pressed;
+  }
+}
+
+/**
+ * @brief 解析pc行为逻辑
+ *
+ * @param rc 遥控器数据
+ * @param cmd 主结构体
+ * @param dt_sec 两次解析的间隔
+ */
+static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
+  cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
+
+  /* 云台设置为鼠标控制欧拉角的变化，底盘的控制向量设置为零 */
+  cmd->gimbal.delta_eulr.yaw =
+      (float)rc->mouse.x * dt_sec * cmd->param->sens_mouse;
+  cmd->gimbal.delta_eulr.pit =
+      (float)(-rc->mouse.y) * dt_sec * cmd->param->sens_mouse;
+  cmd->chassis.ctrl_vec.vx = cmd->chassis.ctrl_vec.vy = 0.0f;
+  cmd->shoot.reverse_trig = false;
+
+  /* 按键行为映射相关逻辑 */
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE)) {
+    cmd->chassis.ctrl_vec.vy += cmd->param->move.move_sense;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK)) {
+    cmd->chassis.ctrl_vec.vy -= cmd->param->move.move_sense;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT)) {
+    cmd->chassis.ctrl_vec.vx -= cmd->param->move.move_sense;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT)) {
+    cmd->chassis.ctrl_vec.vx += cmd->param->move.move_sense;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE)) {
+    cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_fast_sense;
+    cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_fast_sense;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE)) {
+    cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_slow_sense;
+    cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_slow_sense;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE)) {
+    /* 切换至开火模式，设置相应的射击频率和弹丸初速度 */
+    cmd->shoot.mode = SHOOT_MODE_LOADED;
+    cmd->shoot.fire = true;
+  } else {
+    /* 切换至准备模式，停止射击 */
+    cmd->shoot.mode = SHOOT_MODE_LOADED;
+    cmd->shoot.fire = false;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE)) {
+    /* 每按一次依次切换开火下一个模式 */
+    cmd->shoot.fire_mode++;
+    cmd->shoot.fire_mode %= FIRE_MODE_NUM;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR)) {
+    /* 切换到小陀螺模式 */
+    cmd->chassis.mode = CHASSIS_MODE_ROTOR;
+    cmd->chassis.mode_rotor = ROTOR_MODE_RAND;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER)) {
+    /* 每按一次开、关弹舱盖 */
+    cmd->shoot.cover_open = !cmd->shoot.cover_open;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF)) {
+    if (cmd->ai_status == AI_STATUS_HITSWITCH) {
+      /* 停止ai的打符模式，停用host控制 */
+      CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP);
+      cmd->host_overwrite = false;
+      cmd->ai_status = AI_STATUS_STOP;
+    } else if (cmd->ai_status == AI_STATUS_AUTOAIM) {
+      /* 自瞄模式中切换失败提醒 */
+    } else {
+      /* ai切换至打符模式，启用host控制 */
+      CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START);
+      cmd->ai_status = AI_STATUS_HITSWITCH;
+      cmd->host_overwrite = true;
+    }
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM)) {
+    if (cmd->ai_status == AI_STATUS_AUTOAIM) {
+      /* 停止ai的自瞄模式，停用host控制 */
+      cmd->host_overwrite = false;
+      cmd->ai_status = AI_STATUS_STOP;
+      CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
+    } else {
+      /* ai切换至自瞄模式，启用host控制 */
+      cmd->ai_status = AI_STATUS_AUTOAIM;
+      cmd->host_overwrite = true;
+      CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
+    }
+  } else {
+    cmd->host_overwrite = false;
+    // TODO: 修复逻辑
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG)) {
+    /* 按下拨弹反转 */
+    cmd->shoot.reverse_trig = true;
+  }
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35)) {
+    cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL_35;
+  }
+  /* 保存当前按下的键位状态 */
+  cmd->key_last = rc->key;
+  memcpy(&(cmd->mouse_last), &(rc->mouse), sizeof(cmd->mouse_last));
+}
+
+/**
+ * @brief 解析rc行为逻辑
+ *
+ * @param rc 遥控器数据
+ * @param cmd 主结构体
+ * @param dt_sec 两次解析的间隔
+ */
+static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
+  switch (rc->sw_l) {
+      /* 左拨杆相应行为选择和解析 */
+    case CMD_SW_UP:
+      cmd->chassis.mode = CHASSIS_MODE_BREAK;
+      break;
+
+    case CMD_SW_MID:
+      cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
+      break;
+
+    case CMD_SW_DOWN:
+      cmd->chassis.mode = CHASSIS_MODE_ROTOR;
+      cmd->chassis.mode_rotor = ROTOR_MODE_CW;
+      break;
+
+    case CMD_SW_ERR:
+      cmd->chassis.mode = CHASSIS_MODE_RELAX;
+      break;
+  }
+  switch (rc->sw_r) {
+      /* 右拨杆相应行为选择和解析*/ 
+    case CMD_SW_UP:
+      cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
+      cmd->shoot.mode = SHOOT_MODE_SAFE;
+      break;
+
+    case CMD_SW_MID:
+      cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
+      cmd->shoot.fire = false;
+      cmd->shoot.mode = SHOOT_MODE_LOADED;
+      break;
+
+    case CMD_SW_DOWN:
+      cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
+      cmd->shoot.mode = SHOOT_MODE_LOADED;
+      cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
+      cmd->shoot.fire = true;
+      break;
+		/*
+		    case CMD_SW_UP:
+      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
+      cmd->shoot.mode = SHOOT_MODE_SAFE;
+      break;
+
+    case CMD_SW_MID:
+      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
+      cmd->shoot.fire = false;
+      cmd->shoot.mode = SHOOT_MODE_LOADED;
+      break;
+
+    case CMD_SW_DOWN:
+      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
+      cmd->shoot.mode = SHOOT_MODE_LOADED;
+      cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
+      cmd->shoot.fire = true;
+      break;
+		*/
+    case CMD_SW_ERR:
+      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
+      cmd->shoot.mode = SHOOT_MODE_RELAX;
+  }
+  /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
+  cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
+  cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
+  cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc;
+  cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
+}
+
+/**
+ * @brief rc失控时机器人恢复放松模式
+ *
+ * @param cmd 主结构体
+ */
+static void CMD_RcLostLogic(CMD_t *cmd) {
+  /* 机器人底盘、云台、射击运行模式恢复至放松模式 */
+  cmd->chassis.mode = CHASSIS_MODE_RELAX;
+  cmd->gimbal.mode = GIMBAL_MODE_RELAX;
+  cmd->shoot.mode = SHOOT_MODE_RELAX;
+}
+
+/**
+ * @brief 初始化命令解析
+ *
+ * @param cmd 主结构体
+ * @param param 参数
+ * @return int8_t 0对应没有错误
+ */
+int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param) {
+  /* 指针检测 */
+  if (cmd == NULL) return -1;
+  if (param == NULL) return -1;
+
+  /* 设置机器人的命令参数，初始化控制方式为rc控制 */
+  cmd->pc_ctrl = false;
+  cmd->param = param;
+
+  return 0;
+}
+
+/**
+ * @brief 检查是否启用上位机控制指令覆盖
+ *
+ * @param cmd 主结构体
+ * @return true 启用
+ * @return false 不启用
+ */
+inline bool CMD_CheckHostOverwrite(CMD_t *cmd) { return cmd->host_overwrite; }
+
+/**
+ * @brief 解析命令
+ *
+ * @param rc 遥控器数据
+ * @param cmd 命令
+ * @param dt_sec 两次解析的间隔
+ * @return int8_t 0对应没有错误
+ */
+int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
+  /* 指针检测 */
+  if (rc == NULL) return -1;
+  if (cmd == NULL) return -1;
+
+  /* 在pc控制和rc控制间切换 */
+  if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
+      CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_Q))
+    cmd->pc_ctrl = true;
+
+  if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
+      CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_E))
+    cmd->pc_ctrl = false;
+  /*c当rc丢控时，恢复机器人至默认状态 */
+  if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
+    CMD_RcLostLogic(cmd);
+  } else {
+    if (cmd->pc_ctrl) {
+      CMD_PcLogic(rc, cmd, dt_sec);
+    } else {
+      CMD_RcLogic(rc, cmd, dt_sec);
+    }
+  }
+  return 0;
+}
+
+/**
+ * @brief 解析上位机命令
+ *
+ * @param host host数据
+ * @param cmd 命令
+ * @param dt_sec 两次解析的间隔
+ * @return int8_t 0对应没有错误
+ */
+int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
+  (void)dt_sec; /* 未使用dt_sec，消除警告 */
+  /* 指针检测 */
+  if (host == NULL) return -1;
+  if (cmd == NULL) return -1;
+
+  /* 云台欧拉角设置为host相应的变化的欧拉角 */
+  cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw;
+  cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;
+
+  /* host射击命令，设置不同的射击频率和弹丸初速度 */
+  if (host->fire) {
+    cmd->shoot.mode = SHOOT_MODE_LOADED;
+    cmd->shoot.fire = true;
+  } else {
+    cmd->shoot.mode = SHOOT_MODE_SAFE;
+  }
+  return 0;
+}
+
+/**
+ * @brief 添加向Referee发送的命令
+ *
+ * @param ref 命令队列
+ * @param cmd 要添加的命令
+ * @return int8_t 0对应没有错误
+ */
+int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd) {
+  /* 指针检测 */
+  if (ref == NULL) return -1;
+  /* 越界检测 */
+  if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0) return -1;
+
+  /* 添加机器人当前行为状态到画图的命令队列中 */
+  ref->cmd[ref->counter] = cmd;
+  ref->counter++;
+  return 0;
+}
diff --git a/assets/User_code/component/cmd.h b/assets/User_code/component/cmd.h
new file mode 100644
index 0000000..0b352dd
--- /dev/null
+++ b/assets/User_code/component/cmd.h
@@ -0,0 +1,306 @@
+/*
+  控制命令
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "ahrs.h"
+
+#define CMD_REFEREE_MAX_NUM (3) /* 发送命令限定的最大数量 */
+
+/* 机器人型号 */
+typedef enum {
+  ROBOT_MODEL_INFANTRY = 0, /* 步兵机器人 */
+  ROBOT_MODEL_HERO,         /* 英雄机器人 */
+  ROBOT_MODEL_ENGINEER,     /* 工程机器人 */
+  ROBOT_MODEL_DRONE,        /* 空中机器人 */
+  ROBOT_MODEL_SENTRY,       /* 哨兵机器人 */
+  ROBOT_MODEL_NUM,          /* 型号数量 */
+} CMD_RobotModel_t;
+
+/* 底盘运行模式 */
+typedef enum {
+  CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
+  CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */
+  CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */
+  CHASSIS_MODE_FOLLOW_GIMBAL_35, /* 通过闭环控制使车头方向35度跟随云台 */
+  CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */
+  CHASSIS_MODE_INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */
+  CHASSIS_MODE_OPEN, /* 开环模式。底盘运行不受PID控制，直接输出到电机 */
+} CMD_ChassisMode_t;
+
+/* 云台运行模式 */
+typedef enum {
+  GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
+  GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
+  GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
+} CMD_GimbalMode_t;
+
+/* 射击运行模式 */
+typedef enum {
+  SHOOT_MODE_RELAX,  /* 放松模式，电机不输出 */
+  SHOOT_MODE_SAFE,   /* 保险模式，电机闭环控制保持静止 */
+  SHOOT_MODE_LOADED, /* 上膛模式，摩擦轮开启。随时准备开火 */
+} CMD_ShootMode_t;
+
+typedef enum {
+  FIRE_MODE_SINGLE, /* 单发开火模式  */
+  FIRE_MODE_BURST,  /* N连发开火模式 */
+  FIRE_MODE_CONT,   /* 持续开火模式 */
+  FIRE_MODE_NUM,
+} CMD_FireMode_t;
+
+/* 小陀螺转动模式 */
+typedef enum {
+  ROTOR_MODE_CW,   /* 顺时针转动 */
+  ROTOR_MODE_CCW,  /* 逆时针转动 */
+  ROTOR_MODE_RAND, /* 随机转动 */
+} CMD_RotorMode_t;
+
+/* 底盘控制命令 */
+typedef struct {
+  CMD_ChassisMode_t mode;     /* 底盘运行模式 */
+  CMD_RotorMode_t mode_rotor; /* 小陀螺转动模式 */
+  MoveVector_t ctrl_vec;      /* 底盘控制向量 */
+} CMD_ChassisCmd_t;
+
+/* 云台控制命令 */
+typedef struct {
+  CMD_GimbalMode_t mode;  /* 云台运行模式 */
+  AHRS_Eulr_t delta_eulr; /* 欧拉角变化角度 */
+} CMD_GimbalCmd_t;
+
+/* 射击控制命令 */
+typedef struct {
+  CMD_ShootMode_t mode;     /* 射击运行模式 */
+  CMD_FireMode_t fire_mode; /* 开火模式 */
+  bool fire;                /*开火*/
+  bool cover_open;          /* 弹舱盖开关 */
+  bool reverse_trig;        /* 拨弹电机状态 */
+} CMD_ShootCmd_t;
+
+/* 拨杆位置 */
+typedef enum {
+  CMD_SW_ERR = 0,
+  CMD_SW_UP = 1,
+  CMD_SW_MID = 3,
+  CMD_SW_DOWN = 2,
+} CMD_SwitchPos_t;
+
+/* 键盘按键值 */
+typedef enum {
+  CMD_KEY_W = 0,
+  CMD_KEY_S,
+  CMD_KEY_A,
+  CMD_KEY_D,
+  CMD_KEY_SHIFT,
+  CMD_KEY_CTRL,
+  CMD_KEY_Q,
+  CMD_KEY_E,
+  CMD_KEY_R,
+  CMD_KEY_F,
+  CMD_KEY_G,
+  CMD_KEY_Z,
+  CMD_KEY_X,
+  CMD_KEY_C,
+  CMD_KEY_V,
+  CMD_KEY_B,
+  CMD_L_CLICK,
+  CMD_R_CLICK,
+  CMD_KEY_NUM,
+} CMD_KeyValue_t;
+
+/* 行为值序列 */
+typedef enum {
+  CMD_BEHAVIOR_FORE = 0,       /* 向前 */
+  CMD_BEHAVIOR_BACK,           /* 向后 */
+  CMD_BEHAVIOR_LEFT,           /* 向左 */
+  CMD_BEHAVIOR_RIGHT,          /* 向右 */
+  CMD_BEHAVIOR_ACCELERATE,     /* 加速 */
+  CMD_BEHAVIOR_DECELEBRATE,    /* 减速 */
+  CMD_BEHAVIOR_FIRE,           /* 开火 */
+  CMD_BEHAVIOR_FIRE_MODE,      /* 切换开火模式 */
+  CMD_BEHAVIOR_BUFF,           /* 打符模式 */
+  CMD_BEHAVIOR_AUTOAIM,        /* 自瞄模式 */
+  CMD_BEHAVIOR_OPENCOVER,      /* 弹舱盖开关 */
+  CMD_BEHAVIOR_ROTOR,          /* 小陀螺模式 */
+  CMD_BEHAVIOR_REVTRIG,        /* 反转拨弹 */
+  CMD_BEHAVIOR_FOLLOWGIMBAL35, /* 跟随云台呈35度 */
+  CMD_BEHAVIOR_NUM,
+} CMD_Behavior_t;
+
+typedef enum {
+  CMD_ACTIVE_PRESSING, /* 按下时触发 */
+  CMD_ACTIVE_RASING,   /* 抬起时触发 */
+  CMD_ACTIVE_PRESSED,  /* 按住时触发 */
+} CMD_ActiveType_t;
+
+typedef struct {
+  CMD_ActiveType_t active;
+  CMD_KeyValue_t key;
+} CMD_KeyMapItem_t;
+
+/* 行为映射的对应按键数组 */
+typedef struct {
+  CMD_KeyMapItem_t key_map[CMD_BEHAVIOR_NUM];
+} CMD_KeyMap_Params_t;
+
+/* 位移灵敏度参数 */
+typedef struct {
+  float move_sense;      /* 移动灵敏度 */
+  float move_fast_sense; /* 加速灵敏度 */
+  float move_slow_sense; /* 减速灵敏度 */
+} CMD_Move_Params_t;
+
+typedef struct {
+  uint16_t width;
+  uint16_t height;
+} CMD_Screen_t;
+
+/* 命令参数 */
+typedef struct {
+  float sens_mouse;        /* 鼠标灵敏度 */
+  float sens_rc;           /* 遥控器摇杆灵敏度 */
+  CMD_KeyMap_Params_t map; /* 按键映射行为命令 */
+  CMD_Move_Params_t move;  /* 位移灵敏度参数 */
+  CMD_Screen_t screen;     /* 屏幕分辨率参数 */
+} CMD_Params_t;
+
+/* AI行为状态 */
+typedef enum {
+  AI_STATUS_STOP,      /* 停止状态 */
+  AI_STATUS_AUTOAIM,   /* 自瞄状态 */
+  AI_STATUS_HITSWITCH, /* 打符状态 */
+  AI_STATUS_AUTOMATIC  /* 自动状态 */
+} CMD_AI_Status_t;
+
+/* UI所用行为状态 */
+typedef enum {
+  CMD_UI_NOTHING,          /* 当前无状态 */
+  CMD_UI_AUTO_AIM_START,   /* 自瞄状态开启 */
+  CMD_UI_AUTO_AIM_STOP,    /* 自瞄状态关闭 */
+  CMD_UI_HIT_SWITCH_START, /* 打符状态开启 */
+  CMD_UI_HIT_SWITCH_STOP   /* 打符状态关闭 */
+} CMD_UI_t;
+
+/*裁判系统发送的命令*/
+typedef struct {
+  CMD_UI_t cmd[CMD_REFEREE_MAX_NUM]; /* 命令数组 */
+  uint8_t counter;                   /* 命令计数 */
+} CMD_RefereeCmd_t;
+
+typedef struct {
+  bool pc_ctrl;        /* 是否使用键鼠控制 */
+  bool host_overwrite; /* 是否Host控制 */
+  uint16_t key_last;   /* 上次按键键值 */
+
+  struct {
+    int16_t x;
+    int16_t y;
+    int16_t z;
+    bool l_click; /* 左键 */
+    bool r_click; /* 右键 */
+  } mouse_last;   /* 鼠标值 */
+
+  CMD_AI_Status_t ai_status; /* AI状态 */
+
+  const CMD_Params_t *param; /* 命令参数 */
+
+  CMD_ChassisCmd_t chassis; /* 底盘控制命令 */
+  CMD_GimbalCmd_t gimbal;   /* 云台控制命令 */
+  CMD_ShootCmd_t shoot;     /* 射击控制命令 */
+  CMD_RefereeCmd_t referee; /* 裁判系统发送命令 */
+} CMD_t;
+
+typedef struct {
+  float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
+  float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
+  float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
+  float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
+
+  float ch_res; /* 第五通道值 */
+
+  CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */
+  CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */
+
+  struct {
+    int16_t x;
+    int16_t y;
+    int16_t z;
+    bool l_click; /* 左键 */
+    bool r_click; /* 右键 */
+  } mouse;        /* 鼠标值 */
+
+  uint16_t key; /* 按键值 */
+
+  uint16_t res; /* 保留，未启用 */
+} CMD_RC_t;
+
+typedef struct {
+  AHRS_Eulr_t gimbal_delta; /* 欧拉角的变化量 */
+
+  struct {
+    float vx;         /* x轴移动速度 */
+    float vy;         /* y轴移动速度 */
+    float wz;         /* z轴转动速度 */
+  } chassis_move_vec; /* 底盘移动向量 */
+
+  bool fire; /* 开火状态 */
+} CMD_Host_t;
+
+/**
+ * @brief 解析行为命令
+ *
+ * @param rc 遥控器数据
+ * @param cmd 主结构体
+ */
+int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param);
+
+/**
+ * @brief 检查是否启用上位机控制指令覆盖
+ *
+ * @param cmd 主结构体
+ * @return true 启用
+ * @return false 不启用
+ */
+bool CMD_CheckHostOverwrite(CMD_t *cmd);
+
+/**
+ * @brief 解析命令
+ *
+ * @param rc 遥控器数据
+ * @param cmd 命令
+ * @param dt_sec 两次解析的间隔
+ * @return int8_t 0对应没有错误
+ */
+int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec);
+
+/**
+ * @brief 解析上位机命令
+ *
+ * @param host host数据
+ * @param cmd 命令
+ * @param dt_sec 两次解析的间隔
+ * @return int8_t 0对应没有错误
+ */
+int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec);
+
+/**
+ * @brief 添加向Referee发送的命令
+ *
+ * @param ref 命令队列
+ * @param cmd 要添加的命令
+ * @return int8_t 0对应没有错误
+ */
+int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/assets/User_code/component/crc16_rm.c b/assets/User_code/component/crc16.c
similarity index 99%
rename from assets/User_code/component/crc16_rm.c
rename to assets/User_code/component/crc16.c
index cacdad1..2d9de2a 100644
--- a/assets/User_code/component/crc16_rm.c
+++ b/assets/User_code/component/crc16.c
@@ -1,4 +1,4 @@
-#include "crc16_rm.h"
+#include "crc16.h"
 
 static const uint16_t crc16_tab[256] = {
     0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48,
diff --git a/assets/User_code/component/crc16_rm.h b/assets/User_code/component/crc16.h
similarity index 91%
rename from assets/User_code/component/crc16_rm.h
rename to assets/User_code/component/crc16.h
index 9f349ec..dc54b30 100644
--- a/assets/User_code/component/crc16_rm.h
+++ b/assets/User_code/component/crc16.h
@@ -6,6 +6,8 @@ extern "C" {
 
 #include <stdbool.h>
 
+#include "user_math.h"
+
 #define CRC16_INIT 0XFFFF
 
 uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc);
diff --git a/assets/User_code/component/crc8_rm.c b/assets/User_code/component/crc8.c
similarity index 97%
rename from assets/User_code/component/crc8_rm.c
rename to assets/User_code/component/crc8.c
index 3b27aec..bf0bcf9 100644
--- a/assets/User_code/component/crc8_rm.c
+++ b/assets/User_code/component/crc8.c
@@ -1,8 +1,4 @@
-/*
-  参考了Linux
-*/
-
-#include "crc8_rm.h"
+#include "crc8.h"
 
 static const uint8_t crc8_tab[256] = {
     0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20,
diff --git a/assets/User_code/component/crc8_rm.h b/assets/User_code/component/crc8.h
similarity index 92%
rename from assets/User_code/component/crc8_rm.h
rename to assets/User_code/component/crc8.h
index e273e68..61b221a 100644
--- a/assets/User_code/component/crc8_rm.h
+++ b/assets/User_code/component/crc8.h
@@ -1,7 +1,3 @@
-/*
-  参考了Linux
-*/
-
 #pragma once
 
 #ifdef __cplusplus
diff --git a/assets/User_code/component/error_detect.c b/assets/User_code/component/error_detect.c
new file mode 100644
index 0000000..b29e591
--- /dev/null
+++ b/assets/User_code/component/error_detect.c
@@ -0,0 +1,67 @@
+/*
+  错误检测。
+*/
+
+#include "error_detect.h"
+
+#include <stddef.h>
+#include <string.h>
+
+#include "bsp/mm.h"
+
+static ErrorDetect_t ged;
+static bool inited = false;
+
+int8_t ErrorDetect_Init(void) {
+  if (inited) return -1;
+
+  memset(&ged, 0x00, sizeof(ged));
+
+  for (uint8_t i = 0; i < ERROR_DETECT_UNIT_NUM; i++) {
+    ged.error[i].enable = true;
+    ged.error[i].priority = i;
+    ged.error[i].patient_lost = 500;
+    ged.error[i].patient_work = 500;
+  }
+  return 0;
+}
+
+void ErrorDetect_Processing(uint32_t sys_time) {
+  for (uint8_t i = 0; i < ERROR_DETECT_UNIT_NUM; i++) {
+    if (!ged.error[i].enable) continue;
+
+    if (sys_time - ged.error[i].showup > ged.error[i].patient_lost) {
+      ged.error[i].is_lost = true;
+      ged.error[i].found_lost = sys_time;
+    } else if (sys_time - ged.error[i].showup > ged.error[i].patient_lost) {
+    } else {
+      ged.error[i].cycle_time = ged.error[i].showup - ged.error[i].showup_last;
+    }
+  }
+}
+
+bool ErrorDetect_ErrorExist(ErrorDetect_Unit_t unit) {
+  if (unit == ERROR_DETECT_UNIT_NO_DEV) {
+    for (uint8_t i = ERROR_DETECT_UNIT_NUM; i > 0; i--) {
+      if (ged.error[i].error_exist) return true;
+    }
+    return false;
+  } else {
+    return ged.error[unit].error_exist;
+  }
+}
+
+ErrorDetect_Unit_t ErrorDetect_GetErrorUnit(void) {
+  for (uint8_t i = ERROR_DETECT_UNIT_NUM; i > 0; i--) {
+    if (ged.error[i].error_exist) return i;
+  }
+  return ERROR_DETECT_UNIT_NO_DEV;
+}
+
+const ErrorDetect_Error_t *ErrorDetect_GetDetail(ErrorDetect_Unit_t unit) {
+  return &ged.error[unit];
+}
+
+void ErrorDetect_Update(ErrorDetect_Unit_t unit, uint32_t time_current) {
+  ged.error[unit].showup = time_current;
+}
diff --git a/assets/User_code/component/error_detect.h b/assets/User_code/component/error_detect.h
new file mode 100644
index 0000000..eb79614
--- /dev/null
+++ b/assets/User_code/component/error_detect.h
@@ -0,0 +1,66 @@
+/*
+  错误检测。
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef enum {
+  /* Low priority */
+  ERROR_DETECT_UNIT_NO_DEV = 0,
+  ERROR_DETECT_UNIT_REFEREE,
+  ERROR_DETECT_UNIT_CHASSIS_M1,
+  ERROR_DETECT_UNIT_CHASSIS_M2,
+  ERROR_DETECT_UNIT_CHASSIS_M3,
+  ERROR_DETECT_UNIT_CHASSIS_M4,
+  ERROR_DETECT_UNIT_TRIGGER,
+  ERROR_DETECT_UNIT_FEED,
+  ERROR_DETECT_UNIT_GIMBAL_YAW,
+  ERROR_DETECT_UNIT_GIMBAL_PIT,
+  ERROR_DETECT_UNIT_GYRO,
+  ERROR_DETECT_UNIT_ACCL,
+  ERROR_DETECT_UNIT_MAGN,
+  ERROR_DETECT_UNIT_DBUS,
+  ERROR_DETECT_UNIT_NUM,
+  /* High priority */
+} ErrorDetect_Unit_t;
+
+typedef struct {
+  bool enable;
+  uint8_t priority;
+  uint32_t patient_lost;
+  uint32_t patient_work;
+
+  uint32_t showup;
+  uint32_t showup_last;
+  uint32_t cycle_time;
+  uint32_t duration_lost;
+  uint32_t duration_work;
+  uint32_t found_lost;
+  bool error_exist;
+  bool is_lost;
+  uint8_t data_is_error;
+
+} ErrorDetect_Error_t;
+
+typedef struct {
+  ErrorDetect_Error_t error[ERROR_DETECT_UNIT_NUM];
+} ErrorDetect_t;
+
+int8_t ErrorDetect_Init(void);
+void ErrorDetect_Processing(uint32_t sys_time);
+bool ErrorDetect_ErrorExist(ErrorDetect_Unit_t unit);
+ErrorDetect_Unit_t ErrorDetect_GetErrorUnit(void);
+const ErrorDetect_Error_t *ErrorDetect_GetDetail(ErrorDetect_Unit_t unit);
+
+void ErrorDetect_Update(ErrorDetect_Unit_t unit, uint32_t time_current);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/assets/User_code/component/filter.c b/assets/User_code/component/filter.c
index 5905b36..5375b8e 100644
--- a/assets/User_code/component/filter.c
+++ b/assets/User_code/component/filter.c
@@ -3,7 +3,6 @@
 */
 
 #include "filter.h"
-#include <stddef.h>
 
 #include "user_math.h"
 
diff --git a/assets/User_code/component/filter.h b/assets/User_code/component/filter.h
index b8fe50b..c075946 100644
--- a/assets/User_code/component/filter.h
+++ b/assets/User_code/component/filter.h
@@ -8,6 +8,8 @@
 extern "C" {
 #endif
 
+#include "user_math.h"
+
 /* 二阶低通滤波器 */
 typedef struct {
   float cutoff_freq; /* 截止频率 */
diff --git a/assets/User_code/component/ist8310.c b/assets/User_code/component/ist8310.c
deleted file mode 100644
index 1e2d5aa..0000000
--- a/assets/User_code/component/ist8310.c
+++ /dev/null
@@ -1,167 +0,0 @@
-/*
-        IST8310 地磁传感器。
-
-*/
-
-/* Includes ----------------------------------------------------------------- */
-#include "ist8310.h"
-
-#include <gpio.h>
-#include <stdbool.h>
-#include <string.h>
-
-#include "bsp\delay.h"
-#include "bsp\gpio.h"
-#include "bsp\i2c.h"
-
-/* 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)
-/* Private macro ------------------------------------------------------------ */
-#define IST8310_SET() \
-  HAL_GPIO_WritePin(CMPS_RST_GPIO_Port, CMPS_RST_Pin, GPIO_PIN_SET)
-#define IST8310_RESET() \
-  HAL_GPIO_WritePin(CMPS_RST_GPIO_Port, CMPS_RST_Pin, GPIO_PIN_RESET)
-
-/* Private typedef ---------------------------------------------------------- */
-/* Private variables -------------------------------------------------------- */
-uint8_t ist8310_rxbuf[IST8310_LEN_RX_BUFF];
-
-static osThreadId_t thread_alert;
-static bool inited = false;
-
-/* Private function  -------------------------------------------------------- */
-static void IST8310_WriteSingle(uint8_t reg, uint8_t data) {
-  HAL_I2C_Mem_Write(BSP_I2C_GetHandle(BSP_I2C_COMP), IST8310_IIC_ADDRESS, reg,
-                    I2C_MEMADD_SIZE_8BIT, &data, 1, 100);
-}
-
-static uint8_t IST8310_ReadSingle(uint8_t reg) {
-  uint8_t buf = 0;
-  HAL_I2C_Mem_Read(BSP_I2C_GetHandle(BSP_I2C_COMP), IST8310_IIC_ADDRESS, reg,
-                   I2C_MEMADD_SIZE_8BIT, &buf, 1, 100);
-  return buf;
-}
-
-static void IST8310_Read(uint8_t reg, uint8_t *data, uint8_t len) {
-  if (data == NULL) return;
-
-  HAL_I2C_Mem_Read_DMA(BSP_I2C_GetHandle(BSP_I2C_COMP), IST8310_IIC_ADDRESS,
-                       reg, I2C_MEMADD_SIZE_8BIT, data, len);
-}
-
-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_Delay(50);
-  IST8310_SET();
-  BSP_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_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;
-}
diff --git a/assets/User_code/component/ist8310.h b/assets/User_code/component/ist8310.h
deleted file mode 100644
index b11128e..0000000
--- a/assets/User_code/component/ist8310.h
+++ /dev/null
@@ -1,48 +0,0 @@
-#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 {
-  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
diff --git a/assets/User_code/component/limiter.c b/assets/User_code/component/limiter.c
new file mode 100644
index 0000000..71e4bf1
--- /dev/null
+++ b/assets/User_code/component/limiter.c
@@ -0,0 +1,107 @@
+/*
+  限制器
+*/
+
+#include "limiter.h"
+
+#include <math.h>
+#include <stddef.h>
+
+#define POWER_BUFF_THRESHOLD 20
+#define CHASSIS_POWER_CHECK_FREQ 10
+#define CHASSIS_POWER_FACTOR_PASS 0.9f
+#define CHASSIS_POWER_FACTOR_NO_PASS 1.5f
+
+#define CHASSIS_MOTOR_CIRCUMFERENCE 0.12f
+
+/**
+ * @brief 限制底盘功率不超过power_limit
+ *
+ * @param power_limit 最大功率
+ * @param motor_out 电机输出值
+ * @param speed 电机转速
+ * @param len 电机数量
+ * @return int8_t 0对应没有错误
+ */
+int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
+                                float *speed, uint32_t len) {
+  /* power_limit小于0时不进行限制 */
+  if (motor_out == NULL || speed == NULL || power_limit < 0) return -1;
+
+  float sum_motor_out = 0.0f;
+  for (uint32_t i = 0; i < len; i++) {
+    /* 总功率计算 P=F(由转矩电流表示)*V(由转速表示) */
+    sum_motor_out +=
+        fabsf(motor_out[i]) * fabsf(speed[i]) * CHASSIS_MOTOR_CIRCUMFERENCE;
+  }
+
+  /* 保持每个电机输出值缩小时比例不变 */
+  if (sum_motor_out > power_limit) {
+    for (uint32_t i = 0; i < len; i++) {
+      motor_out[i] *= power_limit / sum_motor_out;
+    }
+  }
+
+  return 0;
+}
+
+/**
+ * @brief 电容输入功率计算
+ *
+ * @param power_in 底盘当前功率
+ * @param power_limit 裁判系统功率限制值
+ * @param power_buffer 缓冲能量
+ * @return float 裁判系统输出最大值
+ */
+float PowerLimit_CapInput(float power_in, float power_limit,
+                          float power_buffer) {
+  float target_power = 0.0f;
+
+  /* 计算下一个检测周期的剩余缓冲能量 */
+  float heat_buff = power_buffer - (float)(power_in - power_limit) /
+                                       (float)CHASSIS_POWER_CHECK_FREQ;
+  if (heat_buff < POWER_BUFF_THRESHOLD) { /* 功率限制 */
+    target_power = power_limit * CHASSIS_POWER_FACTOR_PASS;
+  } else {
+    target_power = power_limit * CHASSIS_POWER_FACTOR_NO_PASS;
+  }
+
+  return target_power;
+}
+
+/**
+ * @brief 使用缓冲能量计算底盘最大功率
+ *
+ * @param power_limit 裁判系统功率限制值
+ * @param power_buffer 缓冲能量
+ * @return float 底盘输出最大值
+ */
+float PowerLimit_TargetPower(float power_limit, float power_buffer) {
+  float target_power = 0.0f;
+
+  /* 根据剩余缓冲能量计算输出功率 */
+  target_power = power_limit * (power_buffer - 10.0f) / 20.0f;
+  if (target_power < 0.0f) target_power = 0.0f;
+
+  return target_power;
+}
+
+/**
+ * @brief 射击频率控制
+ *
+ * @param heat 当前热量
+ * @param heat_limit 热量上限
+ * @param cooling_rate 冷却速率
+ * @param heat_increase 冷却增加
+ * @param shoot_freq 经过热量限制后的射击频率
+ * @return float 射击频率
+ */
+float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
+                          float heat_increase, bool is_big) {
+  float heat_percent = heat / heat_limit;
+  float stable_freq = cooling_rate / heat_increase;
+  if (is_big)
+    return stable_freq;
+  else
+    return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq;
+}
diff --git a/assets/User_code/component/limiter.h b/assets/User_code/component/limiter.h
new file mode 100644
index 0000000..7c24cb5
--- /dev/null
+++ b/assets/User_code/component/limiter.h
@@ -0,0 +1,55 @@
+/*
+  限制器
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+
+/**
+ * @brief 限制底盘功率不超过power_limit
+ *
+ * @param power_limit 最大功率
+ * @param motor_out 电机输出值
+ * @param speed 电机转速
+ * @param len 电机数量
+ * @return int8_t 0对应没有错误
+ */
+int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
+                                float *speed, uint32_t len);
+/**
+ * @brief 电容输入功率计算
+ *
+ * @param power_in 底盘当前功率
+ * @param power_limit 裁判系统功率限制值
+ * @param power_buffer 缓冲能量
+ * @return float 裁判系统输出最大值
+ */
+float PowerLimit_CapInput(float power_in, float power_limit,
+                          float power_buffer);
+/**
+ * @brief 使用缓冲能量计算底盘最大功率
+ *
+ * @param power_limit 裁判系统功率限制值
+ * @param power_buffer 缓冲能量
+ * @return float 底盘输出最大值
+ */
+float PowerLimit_TargetPower(float power_limit, float power_buffer);
+
+/**
+ * @brief 射击频率控制
+ *
+ * @param heat 当前热量
+ * @param heat_limit 热量上限
+ * @param cooling_rate 冷却速率
+ * @param heat_increase 冷却增加
+ * @param shoot_freq 经过热量限制后的射击频率
+ * @return float 射击频率
+ */
+float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
+                          float heat_increase, bool is_big);
diff --git a/assets/User_code/component/mixer.c b/assets/User_code/component/mixer.c
new file mode 100644
index 0000000..554b92e
--- /dev/null
+++ b/assets/User_code/component/mixer.c
@@ -0,0 +1,94 @@
+/*
+  混合器
+*/
+
+#include "mixer.h"
+
+#include "math.h"
+
+/**
+ * @brief 初始化混合器
+ *
+ * @param mixer 混合器
+ * @param mode 混合器模式
+ * @return int8_t 0对应没有错误
+ */
+int8_t Mixer_Init(Mixer_t *mixer, Mixer_Mode_t mode) {
+  if (mixer == NULL) return -1;
+
+  mixer->mode = mode;
+  return 0;
+}
+
+/**
+ * @brief 计算输出
+ *
+ * @param mixer 混合器
+ * @param move_vec 运动向量
+ * @param out 输出数组
+ * @param len 输出数组长短
+ * @param scale 输出放大因子
+ * @return int8_t 0对应没有错误
+ */
+int8_t Mixer_Apply(Mixer_t *mixer, MoveVector_t *move_vec, float *out,
+                   int8_t len, float scale) {
+  if (mixer == NULL) return -1;
+
+  switch (mixer->mode) {
+    case MIXER_MECANUM:
+      if (len == 4) {
+        out[0] = move_vec->vx - move_vec->vy + move_vec->wz;
+        out[1] = move_vec->vx + move_vec->vy + move_vec->wz;
+        out[2] = -move_vec->vx + move_vec->vy + move_vec->wz;
+        out[3] = -move_vec->vx - move_vec->vy + move_vec->wz;
+      } else {
+        goto error;
+      }
+      break;
+
+    case MIXER_PARLFIX4:
+      if (len == 4) {
+        out[0] = -move_vec->vx;
+        out[1] = move_vec->vx;
+        out[2] = move_vec->vx;
+        out[3] = -move_vec->vx;
+      } else {
+        goto error;
+      }
+    case MIXER_PARLFIX2:
+      if (len == 2) {
+        out[0] = -move_vec->vx;
+        out[1] = move_vec->vx;
+      } else {
+        goto error;
+      }
+    case MIXER_SINGLE:
+      if (len == 1) {
+        out[0] = move_vec->vx;
+      } else {
+        goto error;
+      }
+    case MIXER_OMNICROSS:
+    case MIXER_OMNIPLUS:
+      goto error;
+  }
+
+  float abs_max = 0.f;
+  for (int8_t i = 0; i < len; i++) {
+    const float abs_val = fabsf(out[i]);
+    abs_max = (abs_val > abs_max) ? abs_val : abs_max;
+  }
+  if (abs_max > 1.f) {
+    for (int8_t i = 0; i < len; i++) {
+      out[i] /= abs_max;
+    }
+  }
+  for (int8_t i = 0; i < len; i++) {
+    out[i] *= scale;
+  }
+  return 0;
+
+error:
+  for (uint8_t i = 0; i < len; i++) out[i] = 0;
+  return -1;
+}
diff --git a/assets/User_code/component/mixer.h b/assets/User_code/component/mixer.h
new file mode 100644
index 0000000..98de3b0
--- /dev/null
+++ b/assets/User_code/component/mixer.h
@@ -0,0 +1,60 @@
+/*
+  混合器
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "user_math.h"
+
+/** 四轮布局 */
+/* 前 */
+/* 2 1 */
+/* 3 4 */
+
+/* 两轮布局 */
+/* 前 */
+/* 2 1 */
+
+/* 混合器模式 */
+typedef enum {
+  MIXER_MECANUM,   /* 麦克纳姆轮 */
+  MIXER_PARLFIX4,  /* 平行四驱动轮 */
+  MIXER_PARLFIX2,  /* 平行对侧两驱动轮 */
+  MIXER_OMNICROSS, /* 叉形全向轮 */
+  MIXER_OMNIPLUS,  /* 十字全向轮 */
+  MIXER_SINGLE,    /* 单个摩擦轮 */
+} Mixer_Mode_t;
+
+typedef struct {
+  Mixer_Mode_t mode;
+} Mixer_t; /* 混合器主结构体 */
+
+/**
+ * @brief 初始化混合器
+ *
+ * @param mixer 混合器
+ * @param mode 混合器模式
+ * @return int8_t 0对应没有错误
+ */
+int8_t Mixer_Init(Mixer_t *mixer, Mixer_Mode_t mode);
+
+/**
+ * @brief 计算输出
+ *
+ * @param mixer 混合器
+ * @param move_vec 运动向量
+ * @param out 输出数组
+ * @param len 输出数组长短
+ * @param scale 输出放大因子
+ * @return int8_t 0对应没有错误
+ */
+int8_t Mixer_Apply(Mixer_t *mixer, MoveVector_t *move_vec, float *out,
+                   int8_t len, float scale);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/assets/User_code/component/pid.c b/assets/User_code/component/pid.c
index 0eaa255..0a3c7d4 100644
--- a/assets/User_code/component/pid.c
+++ b/assets/User_code/component/pid.c
@@ -13,9 +13,6 @@
 
 #include "pid.h"
 
-#include <stddef.h>
-#include "user_math.h"
-
 #define SIGMA 0.000001f
 
 /**
diff --git a/assets/User_code/component/pid.h b/assets/User_code/component/pid.h
index 1af4410..a93acaa 100644
--- a/assets/User_code/component/pid.h
+++ b/assets/User_code/component/pid.h
@@ -12,6 +12,7 @@ extern "C" {
 #include <stdint.h>
 
 #include "filter.h"
+#include "user_math.h"
 
 /* PID模式 */
 typedef enum {
diff --git a/assets/User_code/component/user_math.c b/assets/User_code/component/user_math.c
index a196e80..1fceac2 100644
--- a/assets/User_code/component/user_math.c
+++ b/assets/User_code/component/user_math.c
@@ -5,7 +5,7 @@
 #include "user_math.h"
 
 #include <string.h>
-#include <stdint.h>
+
 inline float InvSqrt(float x) {
 //#if 0
   /* Fast inverse square-root */
@@ -38,6 +38,13 @@ inline void Clip(float *origin, float min, float max) {
 
 inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; }
 
+/**
+ * \brief 将运动向量置零
+ *
+ * \param mv 被操作的值
+ */
+inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); }
+
 /**
  * \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
  * 例如编码器：相差1.5PI其实等于相差-0.5PI
@@ -86,3 +93,40 @@ inline void CircleAdd(float *origin, float delta, float range) {
  * @param origin 被操作的值
  */
 inline void CircleReverse(float *origin) { *origin = -(*origin) + M_2PI; }
+
+/**
+ * @brief 根据目标弹丸速度计算摩擦轮转速
+ *
+ * @param bullet_speed 弹丸速度
+ * @param fric_radius 摩擦轮半径
+ * @param is17mm 是否为17mm
+ * @return 摩擦轮转速
+ */
+inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
+  if (bullet_speed == 0.0f) return 0.f;
+  if (is17mm) {
+    if (bullet_speed == 15.0f) return 4670.f;
+    if (bullet_speed == 18.0f) return 5200.f;
+    if (bullet_speed == 30.0f) return 7350.f;
+  } else {
+    if (bullet_speed == 10.0f) return 4450.f;
+    if (bullet_speed == 16.0f) return 5800.f;
+  }
+
+  /* 不为裁判系统设定值时,计算转速 */
+  return 60.0f * (float)bullet_speed / (M_2PI * fric_radius);
+}
+
+/**
+ * @brief 断言失败处理
+ *
+ * @param file 文件名
+ * @param line 行号
+ */
+void VerifyFailed(const char *file, uint32_t line) {
+  UNUSED(file);
+  UNUSED(line);
+  while (1) {
+    __NOP();
+  }
+}
diff --git a/assets/User_code/component/user_math.h b/assets/User_code/component/user_math.h
index 5b6f2d6..eda0545 100644
--- a/assets/User_code/component/user_math.h
+++ b/assets/User_code/component/user_math.h
@@ -8,10 +8,13 @@
 extern "C" {
 #endif
 
+#include "stm32f4xx.h"
+#define ARM_MATH_CM4
+
 #include <float.h>
 #include <math.h>
 #include <stdbool.h>
-#include <stdint.h>
+
 #define M_DEG2RAD_MULT (0.01745329251f)
 #define M_RAD2DEG_MULT (57.2957795131f)
 
@@ -54,6 +57,13 @@ void Clip(float *origin, float min, float max);
 
 float Sign(float in);
 
+/**
+ * \brief 将运动向量置零
+ *
+ * \param mv 被操作的值
+ */
+void ResetMoveVector(MoveVector_t *mv);
+
 /**
  * \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
  * 例如编码器：相差1.5PI其实等于相差-0.5PI
@@ -83,6 +93,16 @@ void CircleAdd(float *origin, float delta, float range);
  */
 void CircleReverse(float *origin);
 
+/**
+ * @brief 根据目标弹丸速度计算摩擦轮转速
+ *
+ * @param bullet_speed 弹丸速度
+ * @param fric_radius 摩擦轮半径
+ * @param is17mm 是否为17mm
+ * @return 摩擦轮转速
+ */
+float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
+
 #ifdef __cplusplus
 }
 #endif
@@ -128,3 +148,11 @@ void CircleReverse(float *origin);
  */
 #define VERIFY(expr) ((void)(expr))
 #endif
+
+/**
+ * @brief 断言失败处理
+ *
+ * @param file 文件名
+ * @param line 行号
+ */
+void VerifyFailed(const char *file, uint32_t line);