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

有分类功能了

Browse Source
This commit is contained in:
Robofish
2025-11-25 20:45:10 +08:00
parent dd801f5d6a
commit 77b9eb978d
164 changed files with 1283 additions and 13008 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
assets/User_code/component/.gitkeep
View File

352
assets/User_code/component/FreeRTOS_CLI.c
View File

@@ -1,352 +0,0 @@
/*
* 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;
}

108
assets/User_code/component/FreeRTOS_CLI.h
View File

@@ -1,108 +0,0 @@
/*
* 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 */

417
assets/User_code/component/ahrs.c
View File

@@ -1,417 +0,0 @@
/*
开源的AHRS算法。
MadgwickAHRS
*/
#include "ahrs.h"
#include <string.h>
#include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
#define BETA_IMU (0.033f)
#define BETA_AHRS (0.041f)
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 2 * proportional gain (Kp) */
static float beta = BETA_IMU;
/**
* @brief 不使用磁力计计算姿态
*
* @param ahrs 姿态解算主结构体
* @param accl 加速度计数据
* @param gyro 陀螺仪数据
* @return int8_t 0对应没有错误
*/
static int8_t AHRS_UpdateIMU(AHRS_t *ahrs, const AHRS_Accl_t *accl,
const AHRS_Gyro_t *gyro) {
if (ahrs == NULL) return -1;
if (accl == NULL) return -1;
if (gyro == NULL) return -1;
beta = BETA_IMU;
float ax = accl->x;
float ay = accl->y;
float az = accl->z;
float gx = gyro->x;
float gy = gyro->y;
float gz = gyro->z;
float recip_norm;
float s0, s1, s2, s3;
float q_dot1, q_dot2, q_dot3, q_dot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2,
q3q3;
/* Rate of change of quaternion from gyroscope */
q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy -
ahrs->quat.q3 * gz);
q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz -
ahrs->quat.q3 * gy);
q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz +
ahrs->quat.q3 * gx);
q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy -
ahrs->quat.q2 * gx);
/* Compute feedback only if accelerometer measurement valid (avoids NaN in
* accelerometer normalisation) */
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
/* Normalise accelerometer measurement */
recip_norm = InvSqrt(ax * ax + ay * ay + az * az);
ax *= recip_norm;
ay *= recip_norm;
az *= recip_norm;
/* Auxiliary variables to avoid repeated arithmetic */
_2q0 = 2.0f * ahrs->quat.q0;
_2q1 = 2.0f * ahrs->quat.q1;
_2q2 = 2.0f * ahrs->quat.q2;
_2q3 = 2.0f * ahrs->quat.q3;
_4q0 = 4.0f * ahrs->quat.q0;
_4q1 = 4.0f * ahrs->quat.q1;
_4q2 = 4.0f * ahrs->quat.q2;
_8q1 = 8.0f * ahrs->quat.q1;
_8q2 = 8.0f * ahrs->quat.q2;
q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
/* Gradient decent algorithm corrective step */
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * ahrs->quat.q1 -
_2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * ahrs->quat.q2 + _2q0 * ax + _4q2 * q3q3 -
_2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * ahrs->quat.q3 - _2q1 * ax +
4.0f * q2q2 * ahrs->quat.q3 - _2q2 * ay;
/* normalise step magnitude */
recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
s0 *= recip_norm;
s1 *= recip_norm;
s2 *= recip_norm;
s3 *= recip_norm;
/* Apply feedback step */
q_dot1 -= beta * s0;
q_dot2 -= beta * s1;
q_dot3 -= beta * s2;
q_dot4 -= beta * s3;
}
/* Integrate rate of change of quaternion to yield quaternion */
ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq;
ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq;
ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq;
ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq;
/* Normalise quaternion */
recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 +
ahrs->quat.q1 * ahrs->quat.q1 +
ahrs->quat.q2 * ahrs->quat.q2 +
ahrs->quat.q3 * ahrs->quat.q3);
ahrs->quat.q0 *= recip_norm;
ahrs->quat.q1 *= recip_norm;
ahrs->quat.q2 *= recip_norm;
ahrs->quat.q3 *= recip_norm;
return 0;
}
/**
* @brief 初始化姿态解算
*
* @param ahrs 姿态解算主结构体
* @param magn 磁力计数据
* @param sample_freq 采样频率
* @return int8_t 0对应没有错误
*/
int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq) {
if (ahrs == NULL) return -1;
ahrs->inv_sample_freq = 1.0f / sample_freq;
ahrs->quat.q0 = 1.0f;
ahrs->quat.q1 = 0.0f;
ahrs->quat.q2 = 0.0f;
ahrs->quat.q3 = 0.0f;
if (magn) {
float yaw = -atan2(magn->y, magn->x);
if ((magn->x == 0.0f) && (magn->y == 0.0f) && (magn->z == 0.0f)) {
ahrs->quat.q0 = 0.800884545f;
ahrs->quat.q1 = 0.00862364192f;
ahrs->quat.q2 = -0.00283267116f;
ahrs->quat.q3 = 0.598749936f;
} else if ((yaw < (M_PI / 2.0f)) || (yaw > 0.0f)) {
ahrs->quat.q0 = 0.997458339f;
ahrs->quat.q1 = 0.000336312107f;
ahrs->quat.q2 = -0.0057230792f;
ahrs->quat.q3 = 0.0740156546;
} else if ((yaw < M_PI) || (yaw > (M_PI / 2.0f))) {
ahrs->quat.q0 = 0.800884545f;
ahrs->quat.q1 = 0.00862364192f;
ahrs->quat.q2 = -0.00283267116f;
ahrs->quat.q3 = 0.598749936f;
} else if ((yaw < 90.0f) || (yaw > M_PI)) {
ahrs->quat.q0 = 0.800884545f;
ahrs->quat.q1 = 0.00862364192f;
ahrs->quat.q2 = -0.00283267116f;
ahrs->quat.q3 = 0.598749936f;
} else if ((yaw < 90.0f) || (yaw > 0.0f)) {
ahrs->quat.q0 = 0.800884545f;
ahrs->quat.q1 = 0.00862364192f;
ahrs->quat.q2 = -0.00283267116f;
ahrs->quat.q3 = 0.598749936f;
}
}
return 0;
}
/**
* @brief 姿态运算更新一次
* @note 输入数据必须是NED(North East Down) 参考坐标系
*
* @param ahrs 姿态解算主结构体
* @param accl 加速度计数据
* @param gyro 陀螺仪数据
* @param magn 磁力计数据
* @return int8_t 0对应没有错误
*/
int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl,
const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn) {
if (ahrs == NULL) return -1;
if (accl == NULL) return -1;
if (gyro == NULL) return -1;
beta = BETA_AHRS;
float recip_norm;
float s0, s1, s2, s3;
float q_dot1, q_dot2, q_dot3, q_dot4;
float hx, hy;
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1,
_2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3,
q2q2, q2q3, q3q3;
if (magn == NULL) return AHRS_UpdateIMU(ahrs, accl, gyro);
float mx = magn->x;
float my = magn->y;
float mz = magn->z;
/* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in */
/* magnetometer normalisation) */
if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
return AHRS_UpdateIMU(ahrs, accl, gyro);
}
float ax = accl->x;
float ay = accl->y;
float az = accl->z;
float gx = gyro->x;
float gy = gyro->y;
float gz = gyro->z;
/* Rate of change of quaternion from gyroscope */
q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy -
ahrs->quat.q3 * gz);
q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz -
ahrs->quat.q3 * gy);
q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz +
ahrs->quat.q3 * gx);
q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy -
ahrs->quat.q2 * gx);
/* Compute feedback only if accelerometer measurement valid (avoids NaN in
* accelerometer normalisation) */
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
/* Normalise accelerometer measurement */
recip_norm = InvSqrt(ax * ax + ay * ay + az * az);
ax *= recip_norm;
ay *= recip_norm;
az *= recip_norm;
/* Normalise magnetometer measurement */
recip_norm = InvSqrt(mx * mx + my * my + mz * mz);
mx *= recip_norm;
my *= recip_norm;
mz *= recip_norm;
/* Auxiliary variables to avoid repeated arithmetic */
_2q0mx = 2.0f * ahrs->quat.q0 * mx;
_2q0my = 2.0f * ahrs->quat.q0 * my;
_2q0mz = 2.0f * ahrs->quat.q0 * mz;
_2q1mx = 2.0f * ahrs->quat.q1 * mx;
_2q0 = 2.0f * ahrs->quat.q0;
_2q1 = 2.0f * ahrs->quat.q1;
_2q2 = 2.0f * ahrs->quat.q2;
_2q3 = 2.0f * ahrs->quat.q3;
_2q0q2 = 2.0f * ahrs->quat.q0 * ahrs->quat.q2;
_2q2q3 = 2.0f * ahrs->quat.q2 * ahrs->quat.q3;
q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
q0q1 = ahrs->quat.q0 * ahrs->quat.q1;
q0q2 = ahrs->quat.q0 * ahrs->quat.q2;
q0q3 = ahrs->quat.q0 * ahrs->quat.q3;
q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
q1q2 = ahrs->quat.q1 * ahrs->quat.q2;
q1q3 = ahrs->quat.q1 * ahrs->quat.q3;
q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
q2q3 = ahrs->quat.q2 * ahrs->quat.q3;
q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
/* Reference direction of Earth's magnetic field */
hx = mx * q0q0 - _2q0my * ahrs->quat.q3 +
_2q0mz * ahrs->quat.q2 + mx * q1q1 +
_2q1 * my * ahrs->quat.q2 + _2q1 * mz * ahrs->quat.q3 -
mx * q2q2 - mx * q3q3;
hy = _2q0mx * ahrs->quat.q3 + my * q0q0 -
_2q0mz * ahrs->quat.q1 + _2q1mx * ahrs->quat.q2 -
my * q1q1 + my * q2q2 + _2q2 * mz * ahrs->quat.q3 - my * q3q3;
// _2bx = sqrtf(hx * hx + hy * hy);
// 改为invsqrt
_2bx = 1.f / InvSqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * ahrs->quat.q2 + _2q0my * ahrs->quat.q1 +
mz * q0q0 + _2q1mx * ahrs->quat.q3 - mz * q1q1 +
_2q2 * my * ahrs->quat.q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
/* Gradient decent algorithm corrective step */
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q1 * (2.0f * q0q1 + _2q2q3 - ay) -
_2bz * ahrs->quat.q2 *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(-_2bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
_2bx * ahrs->quat.q2 *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q0 * (2.0f * q0q1 + _2q2q3 - ay) -
4.0f * ahrs->quat.q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
_2bz * ahrs->quat.q3 *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(_2bx * ahrs->quat.q2 + _2bz * ahrs->quat.q0) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
(_2bx * ahrs->quat.q3 - _4bz * ahrs->quat.q1) *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q3 * (2.0f * q0q1 + _2q2q3 - ay) -
4.0f * ahrs->quat.q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
(-_4bx * ahrs->quat.q2 - _2bz * ahrs->quat.q0) *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(_2bx * ahrs->quat.q1 + _2bz * ahrs->quat.q3) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
(_2bx * ahrs->quat.q0 - _4bz * ahrs->quat.q2) *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q2 * (2.0f * q0q1 + _2q2q3 - ay) +
(-_4bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(-_2bx * ahrs->quat.q0 + _2bz * ahrs->quat.q2) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
_2bx * ahrs->quat.q1 *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
/* normalise step magnitude */
recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
s0 *= recip_norm;
s1 *= recip_norm;
s2 *= recip_norm;
s3 *= recip_norm;
/* Apply feedback step */
q_dot1 -= beta * s0;
q_dot2 -= beta * s1;
q_dot3 -= beta * s2;
q_dot4 -= beta * s3;
}
/* Integrate rate of change of quaternion to yield quaternion */
ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq;
ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq;
ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq;
ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq;
/* Normalise quaternion */
recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 +
ahrs->quat.q1 * ahrs->quat.q1 +
ahrs->quat.q2 * ahrs->quat.q2 +
ahrs->quat.q3 * ahrs->quat.q3);
ahrs->quat.q0 *= recip_norm;
ahrs->quat.q1 *= recip_norm;
ahrs->quat.q2 *= recip_norm;
ahrs->quat.q3 *= recip_norm;
return 0;
}
/**
* @brief 通过姿态解算主结构体中的四元数计算欧拉角
*
* @param eulr 欧拉角
* @param ahrs 姿态解算主结构体
* @return int8_t 0对应没有错误
*/
int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs) {
if (eulr == NULL) return -1;
if (ahrs == NULL) return -1;
const float sinr_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q1 +
ahrs->quat.q2 * ahrs->quat.q3);
const float cosr_cosp =
1.0f - 2.0f * (ahrs->quat.q1 * ahrs->quat.q1 +
ahrs->quat.q2 * ahrs->quat.q2);
eulr->pit = atan2f(sinr_cosp, cosr_cosp);
const float sinp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q2 -
ahrs->quat.q3 * ahrs->quat.q1);
if (fabsf(sinp) >= 1.0f)
eulr->rol = copysignf(M_PI / 2.0f, sinp);
else
eulr->rol = asinf(sinp);
const float siny_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q3 +
ahrs->quat.q1 * ahrs->quat.q2);
const float cosy_cosp =
1.0f - 2.0f * (ahrs->quat.q2 * ahrs->quat.q2 +
ahrs->quat.q3 * ahrs->quat.q3);
eulr->yaw = atan2f(siny_cosp, cosy_cosp);
#if 0
eulr->yaw *= M_RAD2DEG_MULT;
eulr->rol *= M_RAD2DEG_MULT;
eulr->pit *= M_RAD2DEG_MULT;
#endif
return 0;
}
/**
* \brief 将对应数据置零
*
* \param eulr 被操作的数据
*/
void AHRS_ResetEulr(AHRS_Eulr_t *eulr) { memset(eulr, 0, sizeof(*eulr)); }
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

114
assets/User_code/component/ahrs.h
View File

@@ -1,114 +0,0 @@
/*
开源的AHRS算法。
MadgwickAHRS
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 欧拉角（Euler angle） */
typedef struct {
float yaw; /* 偏航角（Yaw angle） */
float pit; /* 俯仰角（Pitch angle） */
float rol; /* 翻滚角（Roll angle） */
} AHRS_Eulr_t;
/* 加速度计 Accelerometer */
typedef struct {
float x;
float y;
float z;
} AHRS_Accl_t;
/* 陀螺仪 Gyroscope */
typedef struct {
float x;
float y;
float z;
} AHRS_Gyro_t;
/* 磁力计 Magnetometer */
typedef struct {
float x;
float y;
float z;
} AHRS_Magn_t;
/* 四元数 */
typedef struct {
float q0;
float q1;
float q2;
float q3;
} AHRS_Quaternion_t;
/* 姿态解算算法主结构体 */
typedef struct {
/* 四元数 */
AHRS_Quaternion_t quat;
float inv_sample_freq; /* 采样频率的的倒数 */
} AHRS_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/**
* @brief 初始化姿态解算
*
* @param ahrs 姿态解算主结构体
* @param magn 磁力计数据
* @param sample_freq 采样频率
* @return int8_t 0对应没有错误
*/
int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq);
/**
* @brief 姿态运算更新一次
*
* @param ahrs 姿态解算主结构体
* @param accl 加速度计数据
* @param gyro 陀螺仪数据
* @param magn 磁力计数据
* @return int8_t 0对应没有错误
*/
int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl,
const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn);
/**
* @brief 通过姿态解算主结构体中的四元数计算欧拉角
*
* @param eulr 欧拉角
* @param ahrs 姿态解算主结构体
* @return int8_t 0对应没有错误
*/
int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs);
/**
* \brief 将对应数据置零
*
* \param eulr 被操作的数据
*/
void AHRS_ResetEulr(AHRS_Eulr_t *eulr);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

70
assets/User_code/component/capacity.c
View File

@@ -1,70 +0,0 @@
/*
剩余电量算法。
通过电压值计算剩余电量。
*/
#include "capacity.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/**
* @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;
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

47
assets/User_code/component/capacity.h
View File

@@ -1,47 +0,0 @@
/*
剩余电量算法。
通过电压值计算剩余电量。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/**
* @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);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

62
assets/User_code/component/crc16.c
View File

@@ -1,62 +0,0 @@
#include "crc16.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
static const uint16_t crc16_tab[256] = {
0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48,
0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108,
0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 0x9cc9, 0x8d40, 0xbfdb,
0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399,
0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e,
0xfae7, 0xc87c, 0xd9f5, 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e,
0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd,
0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285,
0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44,
0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 0x6306, 0x728f, 0x4014,
0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5,
0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3,
0x242a, 0x16b1, 0x0738, 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862,
0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e,
0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1,
0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483,
0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, 0x2942, 0x38cb, 0x0a50,
0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710,
0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7,
0x6e6e, 0x5cf5, 0x4d7c, 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1,
0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72,
0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e,
0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf,
0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, 0xf78f, 0xe606, 0xd49d,
0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c,
0x3de3, 0x2c6a, 0x1ef1, 0x0f78};
static inline uint16_t CRC16_Byte(uint16_t crc, const uint8_t data) {
return (crc >> 8) ^ crc16_tab[(crc ^ data) & 0xff];
}
uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc) {
while (len--) crc = CRC16_Byte(crc, *buf++);
return crc;
}
bool CRC16_Verify(const uint8_t *buf, size_t len) {
if (len < 2) return false;
uint16_t expected = CRC16_Calc(buf, len - sizeof(uint16_t), CRC16_INIT);
return expected ==
((const uint16_t *)((const uint8_t *)buf +
(len % 2)))[len / sizeof(uint16_t) - 1];
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

30
assets/User_code/component/crc16.h
View File

@@ -1,30 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define CRC16_INIT 0XFFFF
uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc);
bool CRC16_Verify(const uint8_t *buf, size_t len);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

52
assets/User_code/component/crc8.c
View File

@@ -1,52 +0,0 @@
#include "crc8.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
static const uint8_t crc8_tab[256] = {
0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20,
0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e,
0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 0x23, 0x7d, 0x9f, 0xc1,
0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62,
0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e,
0x1d, 0x43, 0xa1, 0xff, 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5,
0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, 0xdb, 0x85, 0x67, 0x39,
0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a,
0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45,
0xc6, 0x98, 0x7a, 0x24, 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b,
0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 0x8c, 0xd2, 0x30, 0x6e,
0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd,
0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31,
0xb2, 0xec, 0x0e, 0x50, 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c,
0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 0x32, 0x6c, 0x8e, 0xd0,
0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73,
0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea,
0x69, 0x37, 0xd5, 0x8b, 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4,
0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 0xe9, 0xb7, 0x55, 0x0b,
0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8,
0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54,
0xd7, 0x89, 0x6b, 0x35,
};
uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc) {
/* loop over the buffer data */
while (len-- > 0) crc = crc8_tab[(crc ^ *buf++) & 0xff];
return crc;
}
bool CRC8_Verify(const uint8_t *buf, size_t len) {
if (len < 2) return false;
uint8_t expected = CRC8_Calc(buf, len - sizeof(uint8_t), CRC8_INIT);
return expected == buf[len - sizeof(uint8_t)];
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

30
assets/User_code/component/crc8.h
View File

@@ -1,30 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define CRC8_INIT 0xFF
uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc);
bool CRC8_Verify(const uint8_t *buf, size_t len);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

8
assets/User_code/component/dependencies.csv
View File

@@ -1,8 +0,0 @@
ahrs,component/user_math.h
capacity,component/user_math.h
cmd,component/ahrs
error_detect,bsp/mm
pid,component/filter
filter,component/ahrs
mixer,component/user_math.h
ui,component/user_math.h
1 ahrs component/user_math.h
2 capacity component/user_math.h
3 cmd component/ahrs
4 error_detect bsp/mm
5 pid component/filter
6 filter component/ahrs
7 mixer component/user_math.h
8 ui component/user_math.h

14
assets/User_code/component/describe.csv
View File

@@ -1,14 +0,0 @@
pid,好用的
ahrs,开源的AHRS算法，MadgwickAHRS
capacity,电池容量计算
cmd,通用控制命令
crc8,CRC8校验rm
crc16,CRC16校验rm
error_detect,错误检测
filter,各类滤波器
FreeRTOS_CLI,FreeRTOS命令行接口
limiter,限幅器
mixer,混控器
ui,用户交互
user_math,用户自定义数学函数
pid,PID控制器
1 pid 好用的
2 ahrs 开源的AHRS算法，MadgwickAHRS
3 capacity 电池容量计算
4 cmd 通用控制命令
5 crc8 CRC8校验rm
6 crc16 CRC16校验rm
7 error_detect 错误检测
8 filter 各类滤波器
9 FreeRTOS_CLI FreeRTOS命令行接口
10 limiter 限幅器
11 mixer 混控器
12 ui 用户交互
13 user_math 用户自定义数学函数
14 pid PID控制器

67
assets/User_code/component/error_detect.c
View File

@@ -1,67 +0,0 @@
/*
错误检测。
*/
#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;
}

82
assets/User_code/component/error_detect.h
View File

@@ -1,82 +0,0 @@
/*
错误检测。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
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;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
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);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

185
assets/User_code/component/filter.c
View File

@@ -1,185 +0,0 @@
/*
各类滤波器。
*/
#include "filter.h"
#include "user_math.h"
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param cutoff_freq 截止频率
*/
void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
float cutoff_freq) {
if (f == NULL) return;
f->cutoff_freq = cutoff_freq;
f->delay_element_1 = 0.0f;
f->delay_element_2 = 0.0f;
if (f->cutoff_freq <= 0.0f) {
/* no filtering */
f->b0 = 1.0f;
f->b1 = 0.0f;
f->b2 = 0.0f;
f->a1 = 0.0f;
f->a2 = 0.0f;
return;
}
const float fr = sample_freq / f->cutoff_freq;
const float ohm = tanf(M_PI / fr);
const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm;
f->b0 = ohm * ohm / c;
f->b1 = 2.0f * f->b0;
f->b2 = f->b0;
f->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c;
}
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) {
if (f == NULL) return 0.0f;
/* do the filtering */
float delay_element_0 =
sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
if (isinf(delay_element_0)) {
/* don't allow bad values to propagate via the filter */
delay_element_0 = sample;
}
const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
f->delay_element_2 * f->b2;
f->delay_element_2 = f->delay_element_1;
f->delay_element_1 = delay_element_0;
/* return the value. Should be no need to check limits */
return output;
}
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) {
if (f == NULL) return 0.0f;
const float dval = sample / (f->b0 + f->b1 + f->b2);
if (isfinite(dval)) {
f->delay_element_1 = dval;
f->delay_element_2 = dval;
} else {
f->delay_element_1 = sample;
f->delay_element_2 = sample;
}
return LowPassFilter2p_Apply(f, sample);
}
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param notch_freq 中心频率
* @param bandwidth 带宽
*/
void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
float bandwidth) {
if (f == NULL) return;
f->notch_freq = notch_freq;
f->bandwidth = bandwidth;
f->delay_element_1 = 0.0f;
f->delay_element_2 = 0.0f;
if (notch_freq <= 0.0f) {
/* no filtering */
f->b0 = 1.0f;
f->b1 = 0.0f;
f->b2 = 0.0f;
f->a1 = 0.0f;
f->a2 = 0.0f;
return;
}
const float alpha = tanf(M_PI * bandwidth / sample_freq);
const float beta = -cosf(M_2PI * notch_freq / sample_freq);
const float a0_inv = 1.0f / (alpha + 1.0f);
f->b0 = a0_inv;
f->b1 = 2.0f * beta * a0_inv;
f->b2 = a0_inv;
f->a1 = f->b1;
f->a2 = (1.0f - alpha) * a0_inv;
}
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
inline float NotchFilter_Apply(NotchFilter_t *f, float sample) {
if (f == NULL) return 0.0f;
/* Direct Form II implementation */
const float delay_element_0 =
sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
f->delay_element_2 * f->b2;
f->delay_element_2 = f->delay_element_1;
f->delay_element_1 = delay_element_0;
return output;
}
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Reset(NotchFilter_t *f, float sample) {
if (f == NULL) return 0.0f;
float dval = sample;
if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) {
dval = dval / (f->b0 + f->b1 + f->b2);
}
f->delay_element_1 = dval;
f->delay_element_2 = dval;
return NotchFilter_Apply(f, sample);
}

120
assets/User_code/component/filter.h
View File

@@ -1,120 +0,0 @@
/*
各类滤波器。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 二阶低通滤波器 */
typedef struct {
float cutoff_freq; /* 截止频率 */
float a1;
float a2;
float b0;
float b1;
float b2;
float delay_element_1;
float delay_element_2;
} LowPassFilter2p_t;
/* 带阻滤波器 */
typedef struct {
float notch_freq; /* 阻止频率 */
float bandwidth; /* 带宽 */
float a1;
float a2;
float b0;
float b1;
float b2;
float delay_element_1;
float delay_element_2;
} NotchFilter_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param cutoff_freq 截止频率
*/
void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
float cutoff_freq);
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample);
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample);
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param notch_freq 中心频率
* @param bandwidth 带宽
*/
void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
float bandwidth);
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Apply(NotchFilter_t *f, float sample);
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Reset(NotchFilter_t *f, float sample);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

107
assets/User_code/component/limiter.c
View File

@@ -1,107 +0,0 @@
/*
限制器
*/
#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;
}

63
assets/User_code/component/limiter.h
View File

@@ -1,63 +0,0 @@
/*
限制器
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/**
* @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);

94
assets/User_code/component/mixer.c
View File

@@ -1,94 +0,0 @@
/*
混合器
*/
#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;
}

76
assets/User_code/component/mixer.h
View File

@@ -1,76 +0,0 @@
/*
混合器
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/** 四轮布局 */
/* 前 */
/* 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; /* 混合器主结构体 */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/**
* @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);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

158
assets/User_code/component/pid.c
View File

@@ -1,158 +0,0 @@
/*
Modified from
https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.cpp
参考资料：
https://github.com/PX4/Firmware/issues/12362
https://dev.px4.io/master/en/flight_stack/controller_diagrams.html
https://docs.px4.io/master/en/config_mc/pid_tuning_guide_multicopter.html#standard_form
https://www.controleng.com/articles/not-all-pid-controllers-are-the-same/
https://en.wikipedia.org/wiki/PID_controller
http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
*/
#include "pid.h"
#define SIGMA 0.000001f
/**
* @brief 初始化PID
*
* @param pid PID结构体
* @param mode PID模式
* @param sample_freq 采样频率
* @param param PID参数
* @return int8_t 0对应没有错误
*/
int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
const KPID_Params_t *param) {
if (pid == NULL) return -1;
if (!isfinite(param->p)) return -1;
if (!isfinite(param->i)) return -1;
if (!isfinite(param->d)) return -1;
if (!isfinite(param->i_limit)) return -1;
if (!isfinite(param->out_limit)) return -1;
pid->param = param;
float dt_min = 1.0f / sample_freq;
if (isfinite(dt_min))
pid->dt_min = dt_min;
else
return -1;
LowPassFilter2p_Init(&(pid->dfilter), sample_freq, pid->param->d_cutoff_freq);
pid->mode = mode;
PID_Reset(pid);
return 0;
}
/**
* @brief PID计算
*
* @param pid PID结构体
* @param sp 设定值
* @param fb 反馈值
* @param fb_dot 反馈值微分
* @param dt 间隔时间
* @return float 计算的输出
*/
float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt) {
if (!isfinite(sp) || !isfinite(fb) || !isfinite(fb_dot) || !isfinite(dt)) {
return pid->last.out;
}
/* 计算误差值 */
const float err = CircleError(sp, fb, pid->param->range);
/* 计算P项 */
const float k_err = err * pid->param->k;
/* 计算D项 */
const float k_fb = pid->param->k * fb;
const float filtered_k_fb = LowPassFilter2p_Apply(&(pid->dfilter), k_fb);
float d;
switch (pid->mode) {
case KPID_MODE_CALC_D:
/* 通过fb计算D，避免了由于sp变化导致err突变的问题 */
/* 当sp不变时，err的微分等于负的fb的微分 */
d = (filtered_k_fb - pid->last.k_fb) / fmaxf(dt, pid->dt_min);
break;
case KPID_MODE_SET_D:
d = fb_dot;
break;
case KPID_MODE_NO_D:
d = 0.0f;
break;
}
pid->last.err = err;
pid->last.k_fb = filtered_k_fb;
if (!isfinite(d)) d = 0.0f;
/* 计算PD输出 */
float output = (k_err * pid->param->p) - (d * pid->param->d);
/* 计算I项 */
const float i = pid->i + (k_err * dt);
const float i_out = i * pid->param->i;
if (pid->param->i > SIGMA) {
/* 检查是否饱和 */
if (isfinite(i)) {
if ((fabsf(output + i_out) <= pid->param->out_limit) &&
(fabsf(i) <= pid->param->i_limit)) {
/* 未饱和，使用新积分 */
pid->i = i;
}
}
}
/* 计算PID输出 */
output += i_out;
/* 限制输出 */
if (isfinite(output)) {
if (pid->param->out_limit > SIGMA) {
output = AbsClip(output, pid->param->out_limit);
}
pid->last.out = output;
}
return pid->last.out;
}
/**
* @brief 重置微分项
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_ResetIntegral(KPID_t *pid) {
if (pid == NULL) return -1;
pid->i = 0.0f;
return 0;
}
/**
* @brief 重置PID
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_Reset(KPID_t *pid) {
if (pid == NULL) return -1;
pid->i = 0.0f;
pid->last.err = 0.0f;
pid->last.k_fb = 0.0f;
pid->last.out = 0.0f;
LowPassFilter2p_Reset(&(pid->dfilter), 0.0f);
return 0;
}

107
assets/User_code/component/pid.h
View File

@@ -1,107 +0,0 @@
/*
Modified from
https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.h
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include "filter.h"
#include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* PID模式 */
typedef enum {
KPID_MODE_NO_D = 0, /* 不使用微分项，PI控制器 */
KPID_MODE_CALC_D, /* 根据反馈的值计算离散微分，忽略PID_Calc中的fb_dot */
KPID_MODE_SET_D /* 直接提供微分值，PID_Calc中的fb_dot将被使用，(Gyros) */
} KPID_Mode_t;
/* PID参数 */
typedef struct {
float k; /* 控制器增益，设置为1用于并行模式 */
float p; /* 比例项增益，设置为1用于标准形式 */
float i; /* 积分项增益 */
float d; /* 微分项增益 */
float i_limit; /* 积分项上限 */
float out_limit; /* 输出绝对值限制 */
float d_cutoff_freq; /* D项低通截止频率 */
float range; /* 计算循环误差时使用，大于0时启用 */
} KPID_Params_t;
/* PID主结构体 */
typedef struct {
KPID_Mode_t mode;
const KPID_Params_t *param;
float dt_min; /* 最小PID_Calc调用间隔 */
float i; /* 积分 */
struct {
float err; /* 上次误差 */
float k_fb; /* 上次反馈值 */
float out; /* 上次输出 */
} last;
LowPassFilter2p_t dfilter; /* D项低通滤波器 */
} KPID_t;
/**
* @brief 初始化PID
*
* @param pid PID结构体
* @param mode PID模式
* @param sample_freq 采样频率
* @param param PID参数
* @return int8_t 0对应没有错误
*/
int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
const KPID_Params_t *param);
/**
* @brief PID计算
*
* @param pid PID结构体
* @param sp 设定值
* @param fb 反馈值
* @param fb_dot 反馈值微分
* @param dt 间隔时间
* @return float 计算的输出
*/
float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt);
/**
* @brief 重置微分项
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_ResetIntegral(KPID_t *pid);
/**
* @brief 重置PID
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_Reset(KPID_t *pid);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

301
assets/User_code/component/ui.c
View File

@@ -1,301 +0,0 @@
/*
UI相关命令
*/
#include "component/ui.h"
#include <stdio.h>
/**
* @brief UI_绘制直线段
*
* @param grapic_line 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 终点x坐标
* @param y_end 终点y坐标
* @return int8_t
*/
int8_t UI_DrawLine(UI_Ele_t *grapic_line, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t x_end,
uint16_t y_end) {
if (grapic_line == NULL) return -1;
snprintf((char *)grapic_line->name, 2, "%s", name);
grapic_line->layer = layer;
grapic_line->type_op = type_op;
grapic_line->type_ele = 0;
grapic_line->color = color;
grapic_line->width = width;
grapic_line->x_start = x_start;
grapic_line->y_start = y_start;
grapic_line->x_end = x_end;
grapic_line->y_end = y_end;
return 0;
}
/**
* @brief UI_绘制矩形
*
* @param grapic_rectangle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 对角顶点x坐标
* @param y_end 对角顶点y坐标
* @return int8_t
*/
int8_t UI_DrawRectangle(UI_Ele_t *grapic_rectangle, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t width, uint16_t x_start, uint16_t y_start,
uint16_t x_end, uint16_t y_end) {
if (grapic_rectangle == NULL) return -1;
snprintf((char *)grapic_rectangle->name, 2, "%s", name);
grapic_rectangle->type_op = type_op;
grapic_rectangle->type_ele = 1;
grapic_rectangle->layer = layer;
grapic_rectangle->color = color;
grapic_rectangle->width = width;
grapic_rectangle->x_start = x_start;
grapic_rectangle->y_start = y_start;
grapic_rectangle->x_end = x_end;
grapic_rectangle->y_end = y_end;
return 0;
}
/**
* @brief UI_绘制正圆
*
* @param grapic_cycle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param radius 半径
* @return int8_t
*/
int8_t UI_DrawCycle(UI_Ele_t *grapic_cycle, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t radius) {
if (grapic_cycle == NULL) return -1;
snprintf((char *)grapic_cycle->name, 2, "%s", name);
grapic_cycle->type_op = type_op;
grapic_cycle->layer = layer;
grapic_cycle->type_ele = 2;
grapic_cycle->color = color;
grapic_cycle->width = width;
grapic_cycle->x_start = x_center;
grapic_cycle->y_start = y_center;
grapic_cycle->radius = radius;
return 0;
}
/**
* @brief UI_绘制椭圆
*
* @param grapic_oval 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawOval(UI_Ele_t *grapic_oval, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t x_semiaxis,
uint16_t y_semiaxis) {
if (grapic_oval == NULL) return -1;
snprintf((char *)grapic_oval->name, 2, "%s", name);
grapic_oval->type_op = type_op;
grapic_oval->type_ele = 3;
grapic_oval->layer = layer;
grapic_oval->color = color;
grapic_oval->width = width;
grapic_oval->x_start = x_center;
grapic_oval->y_start = y_center;
grapic_oval->x_end = x_semiaxis;
grapic_oval->y_end = y_semiaxis;
return 0;
}
/**
* @brief UI_绘制圆弧
*
* @param grapic_arc 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param angle_start 起始角度
* @param angle_end 终止角度
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawArc(UI_Ele_t *grapic_arc, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t angle_start,
uint16_t angle_end, uint16_t width, uint16_t x_center,
uint16_t y_center, uint16_t x_semiaxis, uint16_t y_semiaxis) {
if (grapic_arc == NULL) return -1;
snprintf((char *)grapic_arc->name, 2, "%s", name);
grapic_arc->type_op = type_op;
grapic_arc->type_ele = 4;
grapic_arc->layer = layer;
grapic_arc->color = color;
grapic_arc->angle_start = angle_start;
grapic_arc->angle_end = angle_end;
grapic_arc->width = width;
grapic_arc->x_start = x_center;
grapic_arc->y_start = y_center;
grapic_arc->x_end = x_semiaxis;
grapic_arc->y_end = y_semiaxis;
return 0;
}
/**
* @brief UI_绘制浮点数
*
* @param grapic_float 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param digits 小数点后有效位数
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param float_high 32位浮点数
* @param float_middle 32位浮点数
* @param float_low 32位浮点数
* @return int8_t
*/
int8_t UI_DrawFloating(UI_Ele_t *grapic_floating, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t digits, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t float_high,
uint16_t float_middle, uint16_t float_low) {
if (grapic_floating == NULL) return -1;
snprintf((char *)grapic_floating->name, 2, "%s", name);
grapic_floating->type_op = type_op;
grapic_floating->type_ele = 5;
grapic_floating->layer = layer;
grapic_floating->color = color;
grapic_floating->angle_start = font_size;
grapic_floating->angle_end = digits;
grapic_floating->width = width;
grapic_floating->x_start = x_start;
grapic_floating->y_start = y_start;
grapic_floating->radius = float_high;
grapic_floating->x_end = float_middle;
grapic_floating->y_end = float_low;
return 0;
}
/**
* @brief UI_绘制整型数
*
* @param grapic_integer 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param int32_t_high 32位整型数
* @param int32_t_middle 32位整型数
* @param int32_t_low 32位整型数
* @return int8_t
*/
int8_t UI_DrawInteger(UI_Ele_t *grapic_integer, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t width, uint16_t x_start,
uint16_t y_start, uint16_t int32_t_high,
uint16_t int32_t_middle, uint16_t int32_t_low) {
if (grapic_integer == NULL) return -1;
snprintf((char *)grapic_integer->name, 2, "%s", name);
grapic_integer->type_op = type_op;
grapic_integer->type_ele = 6;
grapic_integer->layer = layer;
grapic_integer->color = color;
grapic_integer->angle_start = font_size;
grapic_integer->width = width;
grapic_integer->x_start = x_start;
grapic_integer->y_start = y_start;
grapic_integer->radius = int32_t_high;
grapic_integer->x_end = int32_t_middle;
grapic_integer->y_end = int32_t_low;
return 0;
}
/**
* @brief UI_绘制字符
*
* @param grapic_character 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param length 字符长度
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param character 字符串首地址
* @return int8_t
*/
int8_t UI_DrawCharacter(UI_Drawcharacter_t *grapic_character, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t length, uint16_t width,
uint16_t x_start, uint16_t y_start,
const char *character) {
if (grapic_character == NULL) return -1;
snprintf((char *)grapic_character->grapic.name, 2, "%s", name);
grapic_character->grapic.type_op = type_op;
grapic_character->grapic.type_ele = 7;
grapic_character->grapic.layer = layer;
grapic_character->grapic.color = color;
grapic_character->grapic.angle_start = font_size;
grapic_character->grapic.angle_end = length;
grapic_character->grapic.width = width;
grapic_character->grapic.x_start = x_start;
grapic_character->grapic.y_start = y_start;
snprintf((char *)grapic_character->character, 29, "%s", character);
return 0;
}
/**
* @brief UI_删除图层
*
* @param del 结构体
* @param opt 操作
* @param layer 图层
* @return int8_t
*/
int8_t UI_DelLayer(UI_Del_t *del, uint8_t opt, uint8_t layer) {
if (del == NULL) return -1;
del->del_operation = opt;
del->layer = layer;
return 0;
}

284
assets/User_code/component/ui.h
View File

@@ -1,284 +0,0 @@
/*
UI相关命令
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <string.h>
#include "component/user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
#define UI_DEL_OPERATION_NOTHING (0)
#define UI_DEL_OPERATION_DEL (1)
#define UI_DEL_OPERATION_DEL_ALL (2)
#define UI_GRAPIC_OPERATION_NOTHING (0)
#define UI_GRAPIC_OPERATION_ADD (1)
#define UI_GRAPIC_OPERATION_REWRITE (2)
#define UI_GRAPIC_OPERATION_DEL (3)
#define UI_GRAPIC_LAYER_CONST (0)
#define UI_GRAPIC_LAYER_AUTOAIM (1)
#define UI_GRAPIC_LAYER_CHASSIS (2)
#define UI_GRAPIC_LAYER_CAP (3)
#define UI_GRAPIC_LAYER_GIMBAL (4)
#define UI_GRAPIC_LAYER_SHOOT (5)
#define UI_GRAPIC_LAYER_CMD (6)
#define UI_DEFAULT_WIDTH (0x01)
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define UI_CHAR_DEFAULT_WIDTH (0x02)
typedef enum {
RED_BLUE,
YELLOW,
GREEN,
ORANGE,
PURPLISH_RED,
PINK,
CYAN,
BLACK,
WHITE
} UI_Color_t;
typedef struct __packed {
uint8_t op;
uint8_t num_layer;
} UI_InterStudent_UIDel_t;
typedef struct __packed {
uint8_t name[3];
uint8_t type_op : 3;
uint8_t type_ele : 3;
uint8_t layer : 4;
uint8_t color : 4;
uint16_t angle_start : 9;
uint16_t angle_end : 9;
uint16_t width : 10;
uint16_t x_start : 11;
uint16_t y_start : 11;
uint16_t radius : 10;
uint16_t x_end : 11;
uint16_t y_end : 11;
} UI_Ele_t;
typedef struct __packed {
UI_Ele_t grapic;
} UI_Drawgrapic_1_t;
typedef struct __packed {
UI_Ele_t grapic[2];
} UI_Drawgrapic_2_t;
typedef struct __packed {
UI_Ele_t grapic[5];
} UI_Drawgrapic_5_t;
typedef struct __packed {
UI_Ele_t grapic[7];
} UI_Drawgrapic_7_t;
typedef struct __packed {
UI_Ele_t grapic;
uint8_t character[30];
} UI_Drawcharacter_t;
typedef struct __packed {
uint8_t del_operation;
uint8_t layer;
} UI_Del_t;
/**
* @brief UI_绘制直线段
*
* @param grapic_line 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 终点x坐标
* @param y_end 终点y坐标
* @return int8_t
*/
int8_t UI_DrawLine(UI_Ele_t *grapic_line, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t x_end,
uint16_t y_end);
/**
* @brief UI_绘制矩形
*
* @param grapic_rectangle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 对角顶点x坐标
* @param y_end 对角顶点y坐标
* @return int8_t
*/
int8_t UI_DrawRectangle(UI_Ele_t *grapic_rectangle, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t width, uint16_t x_start, uint16_t y_start,
uint16_t x_end, uint16_t y_end);
/**
* @brief UI_绘制正圆
*
* @param grapic_cycle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param radius 半径
* @return int8_t
*/
int8_t UI_DrawCycle(UI_Ele_t *grapic_cycle, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t radius);
/**
* @brief UI_绘制椭圆
*
* @param grapic_oval 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawOval(UI_Ele_t *grapic_oval, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t x_semiaxis,
uint16_t y_semiaxis);
/**
* @brief UI_绘制圆弧
*
* @param grapic_arc 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param angle_start 起始角度
* @param angle_end 终止角度
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawArc(UI_Ele_t *grapic_arc, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t angle_start,
uint16_t angle_end, uint16_t width, uint16_t x_center,
uint16_t y_center, uint16_t x_semiaxis, uint16_t y_semiaxis);
/**
* @brief UI_绘制浮点数
*
* @param grapic_float 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param digits 小数点后有效位数
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param float_high 32位浮点数
* @param float_middle 32位浮点数
* @param float_low 32位浮点数
* @return int8_t
*/
int8_t UI_DrawFloating(UI_Ele_t *grapic_floating, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t digits, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t float_high,
uint16_t float_middle, uint16_t float_low);
/**
* @brief UI_绘制整型数
*
* @param grapic_integer 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param int32_t_high 32位整型数
* @param int32_t_middle 32位整型数
* @param int32_t_low 32位整型数
* @return int8_t
*/
int8_t UI_DrawInteger(UI_Ele_t *grapic_integer, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t width, uint16_t x_start,
uint16_t y_start, uint16_t int32_t_high,
uint16_t int32_t_middle, uint16_t int32_t_low);
/**
* @brief UI_绘制字符
*
* @param grapic_character 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param length 字符长度
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param character 字符串首地址
* @return int8_t
*/
int8_t UI_DrawCharacter(UI_Drawcharacter_t *grapic_character, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t length, uint16_t width,
uint16_t x_start, uint16_t y_start,
const char *character);
/**
* @brief UI_删除图层
*
* @param del 结构体
* @param opt 操作
* @param layer 图层
* @return int8_t
*/
int8_t UI_DelLayer(UI_Del_t *del, uint8_t opt, uint8_t layer);
#ifdef __cplusplus
}
#endif

134
assets/User_code/component/user_math.c
View File

@@ -1,134 +0,0 @@
/*
自定义的数学运算。
*/
#include "user_math.h"
#include <string.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
inline float InvSqrt(float x) {
//#if 0
/* Fast inverse square-root */
/* See: http://en.wikipedia.org/wiki/Fast_inverse_square_root */
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
y = y * (1.5f - (halfx * y * y));
return y;
//#else
// return 1.0f / sqrtf(x);
//#endif
}
inline float AbsClip(float in, float limit) {
return (in < -limit) ? -limit : ((in > limit) ? limit : in);
}
float fAbs(float in){
return (in > 0) ? in : -in;
}
inline void Clip(float *origin, float min, float max) {
if (*origin > max) *origin = max;
if (*origin < min) *origin = min;
}
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 计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x
* 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
* \param sp 设定值
* \param fb 反馈值
* \param range 被操作的值变化范围，正数时起效
* \return 函数运行结果
*/
inline float CircleError(float sp, float fb, float range) {
float error = sp - fb;
if (range > 0.0f) {
float half_range = range / 2.0f;
if (error > half_range)
error -= range;
else if (error < -half_range)
error += range;
}
return error;
}
/**
* \brief 循环加法，适用于被操作的值在（0,range）范围内循环的情况
* \param origin 被操作的值
* \param delta 变化量
* \param range 被操作的值变化范围，正数时起效
*/
inline void CircleAdd(float *origin, float delta, float range) {
float out = *origin + delta;
if (range > 0.0f) {
if (out >= range)
out -= range;
else if (out < 0.0f)
out += range;
}
*origin = out;
}
/**
* @brief 循环值取反
*
* @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();
// }
// }
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

179
assets/User_code/component/user_math.h
View File

@@ -1,179 +0,0 @@
/*
自定义的数学运算。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <float.h>
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stddef.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
#define M_DEG2RAD_MULT (0.01745329251f)
#define M_RAD2DEG_MULT (57.2957795131f)
#ifndef M_PI_2
#define M_PI_2 1.57079632679f
#endif
#ifndef M_PI
#define M_PI 3.14159265358979323846f
#endif
#ifndef M_2PI
#define M_2PI 6.28318530717958647692f
#endif
#ifndef __packed
#define __packed __attribute__((__packed__))
#endif /* __packed */
#define max(a, b) \
({ \
__typeof__(a) _a = (a); \
__typeof__(b) _b = (b); \
_a > _b ? _a : _b; \
})
#define min(a, b) \
({ \
__typeof__(a) _a = (a); \
__typeof__(b) _b = (b); \
_a < _b ? _a : _b; \
})
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 移动向量 */
typedef struct {
float vx; /* 前后平移 */
float vy; /* 左右平移 */
float wz; /* 转动 */
} MoveVector_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
float InvSqrt(float x);
float AbsClip(float in, float limit);
float fAbs(float in);
void Clip(float *origin, float min, float max);
float Sign(float in);
/**
* \brief 将运动向量置零
*
* \param mv 被操作的值
*/
void ResetMoveVector(MoveVector_t *mv);
/**
* \brief 计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x
* 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
* \param sp 设定值
* \param fb 反馈值
* \param range 被操作的值变化范围，正数时起效
* \return 函数运行结果
*/
float CircleError(float sp, float fb, float range);
/**
* \brief 循环加法，适用于被操作的值在（0,range）范围内循环的情况
* \param origin 被操作的值
* \param delta 变化量
* \param range 被操作的值变化范围，正数时起效
*/
void CircleAdd(float *origin, float delta, float range);
/**
* @brief 循环值取反
*
* @param origin 被操作的值
*/
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
#ifdef DEBUG
/**
* @brief 如果表达式的值为假则运行处理函数
*
*/
#define ASSERT(expr) \
do { \
if (!(expr)) { \
VerifyFailed(__FILE__, __LINE__); \
} \
} while (0)
#else
/**
* @brief 未定DEBUG，表达式不会运行，断言被忽略
*
*/
#define ASSERT(expr) ((void)(0))
#endif
#ifdef DEBUG
/**
* @brief 如果表达式的值为假则运行处理函数
*
*/
#define VERIFY(expr) \
do { \
if (!(expr)) { \
VerifyFailed(__FILE__, __LINE__); \
} \
} while (0)
#else
/**
* @brief 表达式会运行，忽略表达式结果
*
*/
#define VERIFY(expr) ((void)(expr))
#endif
// /**
// * @brief 断言失败处理
// *
// * @param file 文件名
// * @param line 行号
// */
// void VerifyFailed(const char *file, uint32_t line);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */