mirror of
https://github.com/goldenfishs/MRobot.git
synced 2025-09-14 12:54:33 +08:00
准备device
This commit is contained in:
parent
2c9309ae1e
commit
d99e9e1ec8
5
.vscode/settings.json
vendored
Normal file
5
.vscode/settings.json
vendored
Normal file
@ -0,0 +1,5 @@
|
||||
{
|
||||
"files.associations": {
|
||||
"user_math.h": "c"
|
||||
}
|
||||
}
|
||||
352
assets/User_code/component/FreeRTOS_CLI.c
Normal file
352
assets/User_code/component/FreeRTOS_CLI.c
Normal file
@ -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;
|
||||
}
|
||||
|
||||
108
assets/User_code/component/FreeRTOS_CLI.h
Normal file
108
assets/User_code/component/FreeRTOS_CLI.h
Normal file
@ -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 */
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
@ -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
|
||||
12
assets/User_code/component/ballistics.c
Normal file
12
assets/User_code/component/ballistics.c
Normal file
@ -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; }
|
||||
23
assets/User_code/component/ballistics.h
Normal file
23
assets/User_code/component/ballistics.h
Normal file
@ -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
|
||||
59
assets/User_code/component/capacity.c
Normal file
59
assets/User_code/component/capacity.c
Normal file
@ -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;
|
||||
}
|
||||
35
assets/User_code/component/capacity.h
Normal file
35
assets/User_code/component/capacity.h
Normal file
@ -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
|
||||
375
assets/User_code/component/cmd.c
Normal file
375
assets/User_code/component/cmd.c
Normal file
@ -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;
|
||||
}
|
||||
306
assets/User_code/component/cmd.h
Normal file
306
assets/User_code/component/cmd.h
Normal file
@ -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
|
||||
@ -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,
|
||||
@ -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);
|
||||
@ -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,
|
||||
@ -1,7 +1,3 @@
|
||||
/*
|
||||
参考了Linux
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
67
assets/User_code/component/error_detect.c
Normal file
67
assets/User_code/component/error_detect.c
Normal file
@ -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;
|
||||
}
|
||||
66
assets/User_code/component/error_detect.h
Normal file
66
assets/User_code/component/error_detect.h
Normal file
@ -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
|
||||
@ -3,7 +3,6 @@
|
||||
*/
|
||||
|
||||
#include "filter.h"
|
||||
#include <stddef.h>
|
||||
|
||||
#include "user_math.h"
|
||||
|
||||
|
||||
@ -8,6 +8,8 @@
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "user_math.h"
|
||||
|
||||
/* 二阶低通滤波器 */
|
||||
typedef struct {
|
||||
float cutoff_freq; /* 截止频率 */
|
||||
|
||||
@ -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;
|
||||
}
|
||||
@ -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
|
||||
107
assets/User_code/component/limiter.c
Normal file
107
assets/User_code/component/limiter.c
Normal file
@ -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;
|
||||
}
|
||||
55
assets/User_code/component/limiter.h
Normal file
55
assets/User_code/component/limiter.h
Normal file
@ -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);
|
||||
94
assets/User_code/component/mixer.c
Normal file
94
assets/User_code/component/mixer.c
Normal file
@ -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;
|
||||
}
|
||||
60
assets/User_code/component/mixer.h
Normal file
60
assets/User_code/component/mixer.h
Normal file
@ -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
|
||||
@ -13,9 +13,6 @@
|
||||
|
||||
#include "pid.h"
|
||||
|
||||
#include <stddef.h>
|
||||
#include "user_math.h"
|
||||
|
||||
#define SIGMA 0.000001f
|
||||
|
||||
/**
|
||||
|
||||
@ -12,6 +12,7 @@ extern "C" {
|
||||
#include <stdint.h>
|
||||
|
||||
#include "filter.h"
|
||||
#include "user_math.h"
|
||||
|
||||
/* PID模式 */
|
||||
typedef enum {
|
||||
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
@ -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);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user