Compare commits
No commits in common. "单独底层纯转发" and "main" have entirely different histories.
File diff suppressed because one or more lines are too long
@ -47,31 +47,12 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
# Add user sources here
|
||||
# User/bsp sources
|
||||
User/bsp/can.c
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
User/bsp/dwt.c
|
||||
>>>>>>> main
|
||||
User/bsp/gpio.c
|
||||
User/bsp/mm.c
|
||||
User/bsp/pwm.c
|
||||
User/bsp/spi.c
|
||||
User/bsp/time.c
|
||||
<<<<<<< HEAD
|
||||
|
||||
# User/component sources
|
||||
User/component/ahrs.c
|
||||
User/component/filter.c
|
||||
User/component/pid.c
|
||||
User/component/user_math.c
|
||||
|
||||
# User/device sources
|
||||
User/device/bmi088.c
|
||||
User/device/motor.c
|
||||
User/device/motor_lk.c
|
||||
User/device/motor_lz.c
|
||||
|
||||
# User/module
|
||||
=======
|
||||
User/bsp/uart.c
|
||||
|
||||
# User/component sources
|
||||
@ -97,7 +78,6 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
User/device/rc_can.c
|
||||
|
||||
# User/module sources
|
||||
>>>>>>> main
|
||||
User/module/balance_chassis.c
|
||||
User/module/config.c
|
||||
|
||||
@ -105,14 +85,9 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
User/task/atti_esti.c
|
||||
User/task/blink.c
|
||||
User/task/ctrl_chassis.c
|
||||
<<<<<<< HEAD
|
||||
User/task/imu.c
|
||||
User/task/init.c
|
||||
=======
|
||||
User/task/ctrl_gimbal.c
|
||||
User/task/init.c
|
||||
User/task/rc.c
|
||||
>>>>>>> main
|
||||
User/task/user_task.c
|
||||
)
|
||||
|
||||
|
||||
@ -18,8 +18,6 @@
|
||||
}
|
||||
},
|
||||
{
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
"name": "RelWithDebInfo",
|
||||
"inherits": "default",
|
||||
"cacheVariables": {
|
||||
@ -27,14 +25,11 @@
|
||||
}
|
||||
},
|
||||
{
|
||||
>>>>>>> main
|
||||
"name": "Release",
|
||||
"inherits": "default",
|
||||
"cacheVariables": {
|
||||
"CMAKE_BUILD_TYPE": "Release"
|
||||
}
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
},
|
||||
{
|
||||
"name": "MinSizeRel",
|
||||
@ -42,7 +37,6 @@
|
||||
"cacheVariables": {
|
||||
"CMAKE_BUILD_TYPE": "MinSizeRel"
|
||||
}
|
||||
>>>>>>> main
|
||||
}
|
||||
],
|
||||
"buildPresets": [
|
||||
@ -51,10 +45,6 @@
|
||||
"configurePreset": "Debug"
|
||||
},
|
||||
{
|
||||
<<<<<<< HEAD
|
||||
"name": "Release",
|
||||
"configurePreset": "Release"
|
||||
=======
|
||||
"name": "RelWithDebInfo",
|
||||
"configurePreset": "RelWithDebInfo"
|
||||
},
|
||||
@ -65,7 +55,6 @@
|
||||
{
|
||||
"name": "MinSizeRel",
|
||||
"configurePreset": "MinSizeRel"
|
||||
>>>>>>> main
|
||||
}
|
||||
]
|
||||
}
|
||||
@ -187,7 +187,7 @@ standard names. */
|
||||
|
||||
/* USER CODE BEGIN Defines */
|
||||
/* Section where parameter definitions can be added (for instance, to override default ones in FreeRTOS.h) */
|
||||
// #define configAPPLICATION_ALLOCATED_HEAP 1
|
||||
#define configAPPLICATION_ALLOCATED_HEAP 1
|
||||
/* USER CODE END Defines */
|
||||
|
||||
#endif /* FREERTOS_CONFIG_H */
|
||||
|
||||
@ -59,7 +59,6 @@ void EXTI3_IRQHandler(void);
|
||||
void EXTI4_IRQHandler(void);
|
||||
void DMA1_Stream1_IRQHandler(void);
|
||||
void DMA1_Stream2_IRQHandler(void);
|
||||
void CAN1_TX_IRQHandler(void);
|
||||
void CAN1_RX0_IRQHandler(void);
|
||||
void CAN1_RX1_IRQHandler(void);
|
||||
void EXTI9_5_IRQHandler(void);
|
||||
@ -70,7 +69,6 @@ void TIM7_IRQHandler(void);
|
||||
void DMA2_Stream1_IRQHandler(void);
|
||||
void DMA2_Stream2_IRQHandler(void);
|
||||
void DMA2_Stream3_IRQHandler(void);
|
||||
void CAN2_TX_IRQHandler(void);
|
||||
void CAN2_RX0_IRQHandler(void);
|
||||
void CAN2_RX1_IRQHandler(void);
|
||||
void OTG_FS_IRQHandler(void);
|
||||
|
||||
@ -47,7 +47,7 @@ void MX_CAN1_Init(void)
|
||||
hcan1.Init.TimeTriggeredMode = DISABLE;
|
||||
hcan1.Init.AutoBusOff = DISABLE;
|
||||
hcan1.Init.AutoWakeUp = DISABLE;
|
||||
hcan1.Init.AutoRetransmission = ENABLE;
|
||||
hcan1.Init.AutoRetransmission = DISABLE;
|
||||
hcan1.Init.ReceiveFifoLocked = DISABLE;
|
||||
hcan1.Init.TransmitFifoPriority = ENABLE;
|
||||
if (HAL_CAN_Init(&hcan1) != HAL_OK)
|
||||
@ -79,7 +79,7 @@ void MX_CAN2_Init(void)
|
||||
hcan2.Init.TimeTriggeredMode = DISABLE;
|
||||
hcan2.Init.AutoBusOff = DISABLE;
|
||||
hcan2.Init.AutoWakeUp = DISABLE;
|
||||
hcan2.Init.AutoRetransmission = ENABLE;
|
||||
hcan2.Init.AutoRetransmission = DISABLE;
|
||||
hcan2.Init.ReceiveFifoLocked = DISABLE;
|
||||
hcan2.Init.TransmitFifoPriority = ENABLE;
|
||||
if (HAL_CAN_Init(&hcan2) != HAL_OK)
|
||||
@ -122,8 +122,6 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
|
||||
HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
|
||||
|
||||
/* CAN1 interrupt Init */
|
||||
HAL_NVIC_SetPriority(CAN1_TX_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(CAN1_TX_IRQn);
|
||||
HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
|
||||
HAL_NVIC_SetPriority(CAN1_RX1_IRQn, 5, 0);
|
||||
@ -157,8 +155,6 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
|
||||
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
|
||||
|
||||
/* CAN2 interrupt Init */
|
||||
HAL_NVIC_SetPriority(CAN2_TX_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(CAN2_TX_IRQn);
|
||||
HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn);
|
||||
HAL_NVIC_SetPriority(CAN2_RX1_IRQn, 5, 0);
|
||||
@ -190,7 +186,6 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
|
||||
HAL_GPIO_DeInit(GPIOD, GPIO_PIN_0|GPIO_PIN_1);
|
||||
|
||||
/* CAN1 interrupt Deinit */
|
||||
HAL_NVIC_DisableIRQ(CAN1_TX_IRQn);
|
||||
HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn);
|
||||
HAL_NVIC_DisableIRQ(CAN1_RX1_IRQn);
|
||||
/* USER CODE BEGIN CAN1_MspDeInit 1 */
|
||||
@ -216,7 +211,6 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
|
||||
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_5|GPIO_PIN_6);
|
||||
|
||||
/* CAN2 interrupt Deinit */
|
||||
HAL_NVIC_DisableIRQ(CAN2_TX_IRQn);
|
||||
HAL_NVIC_DisableIRQ(CAN2_RX0_IRQn);
|
||||
HAL_NVIC_DisableIRQ(CAN2_RX1_IRQn);
|
||||
/* USER CODE BEGIN CAN2_MspDeInit 1 */
|
||||
|
||||
@ -269,20 +269,6 @@ void DMA1_Stream2_IRQHandler(void)
|
||||
/* USER CODE END DMA1_Stream2_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles CAN1 TX interrupts.
|
||||
*/
|
||||
void CAN1_TX_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN CAN1_TX_IRQn 0 */
|
||||
|
||||
/* USER CODE END CAN1_TX_IRQn 0 */
|
||||
HAL_CAN_IRQHandler(&hcan1);
|
||||
/* USER CODE BEGIN CAN1_TX_IRQn 1 */
|
||||
|
||||
/* USER CODE END CAN1_TX_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles CAN1 RX0 interrupts.
|
||||
*/
|
||||
@ -423,20 +409,6 @@ void DMA2_Stream3_IRQHandler(void)
|
||||
/* USER CODE END DMA2_Stream3_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles CAN2 TX interrupts.
|
||||
*/
|
||||
void CAN2_TX_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN CAN2_TX_IRQn 0 */
|
||||
|
||||
/* USER CODE END CAN2_TX_IRQn 0 */
|
||||
HAL_CAN_IRQHandler(&hcan2);
|
||||
/* USER CODE BEGIN CAN2_TX_IRQn 1 */
|
||||
|
||||
/* USER CODE END CAN2_TX_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles CAN2 RX0 interrupts.
|
||||
*/
|
||||
|
||||
@ -6,19 +6,11 @@
|
||||
*
|
||||
* For more information about which c-functions
|
||||
* need which of these lowlevel functions
|
||||
<<<<<<< HEAD
|
||||
* please consult the Newlib or Picolibc libc-manual
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* Copyright (c) 2020-2025 STMicroelectronics.
|
||||
=======
|
||||
* please consult the Newlib libc-manual
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* Copyright (c) 2020-2024 STMicroelectronics.
|
||||
>>>>>>> main
|
||||
* All rights reserved.
|
||||
*
|
||||
* This software is licensed under terms that can be found in the LICENSE file
|
||||
@ -147,21 +139,13 @@ int _unlink(char *name)
|
||||
return -1;
|
||||
}
|
||||
|
||||
<<<<<<< HEAD
|
||||
clock_t _times(struct tms *buf)
|
||||
=======
|
||||
int _times(struct tms *buf)
|
||||
>>>>>>> main
|
||||
{
|
||||
(void)buf;
|
||||
return -1;
|
||||
}
|
||||
|
||||
<<<<<<< HEAD
|
||||
int _stat(const char *file, struct stat *st)
|
||||
=======
|
||||
int _stat(char *file, struct stat *st)
|
||||
>>>>>>> main
|
||||
{
|
||||
(void)file;
|
||||
st->st_mode = S_IFCHR;
|
||||
@ -190,74 +174,3 @@ int _execve(char *name, char **argv, char **env)
|
||||
errno = ENOMEM;
|
||||
return -1;
|
||||
}
|
||||
<<<<<<< HEAD
|
||||
|
||||
// --- Picolibc Specific Section ---
|
||||
#if defined(__PICOLIBC__)
|
||||
|
||||
/**
|
||||
* @brief Picolibc helper function to output a character to a FILE stream.
|
||||
* This redirects the output to the low-level __io_putchar function.
|
||||
* @param c Character to write.
|
||||
* @param file FILE stream pointer (ignored).
|
||||
* @retval int The character written.
|
||||
*/
|
||||
static int starm_putc(char c, FILE *file)
|
||||
{
|
||||
(void) file;
|
||||
__io_putchar(c);
|
||||
return c;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Picolibc helper function to input a character from a FILE stream.
|
||||
* This redirects the input from the low-level __io_getchar function.
|
||||
* @param file FILE stream pointer (ignored).
|
||||
* @retval int The character read, cast to an unsigned char then int.
|
||||
*/
|
||||
static int starm_getc(FILE *file)
|
||||
{
|
||||
unsigned char c;
|
||||
(void) file;
|
||||
c = __io_getchar();
|
||||
return c;
|
||||
}
|
||||
|
||||
// Define and initialize the standard I/O streams for Picolibc.
|
||||
// FDEV_SETUP_STREAM connects the starm_putc and starm_getc helper functions to a FILE structure.
|
||||
// _FDEV_SETUP_RW indicates the stream is for reading and writing.
|
||||
static FILE __stdio = FDEV_SETUP_STREAM(starm_putc,
|
||||
starm_getc,
|
||||
NULL,
|
||||
_FDEV_SETUP_RW);
|
||||
|
||||
// Assign the standard stream pointers (stdin, stdout, stderr) to the initialized stream.
|
||||
// Picolibc uses these pointers for standard I/O operations (printf, scanf, etc.).
|
||||
FILE *const stdin = &__stdio;
|
||||
__strong_reference(stdin, stdout);
|
||||
__strong_reference(stdin, stderr);
|
||||
|
||||
// Create strong aliases mapping standard C library function names (without underscore)
|
||||
// to the implemented system call stubs (with underscore). Picolibc uses these
|
||||
// standard names internally, so this linking is required.
|
||||
__strong_reference(_read, read);
|
||||
__strong_reference(_write, write);
|
||||
__strong_reference(_times, times);
|
||||
__strong_reference(_execve, execve);
|
||||
__strong_reference(_fork, fork);
|
||||
__strong_reference(_link, link);
|
||||
__strong_reference(_unlink, unlink);
|
||||
__strong_reference(_stat, stat);
|
||||
__strong_reference(_wait, wait);
|
||||
__strong_reference(_open, open);
|
||||
__strong_reference(_close, close);
|
||||
__strong_reference(_lseek, lseek);
|
||||
__strong_reference(_isatty, isatty);
|
||||
__strong_reference(_fstat, fstat);
|
||||
__strong_reference(_exit, exit);
|
||||
__strong_reference(_kill, kill);
|
||||
__strong_reference(_getpid, getpid);
|
||||
|
||||
#endif //__PICOLIBC__
|
||||
=======
|
||||
>>>>>>> main
|
||||
|
||||
@ -6,19 +6,11 @@
|
||||
*
|
||||
* For more information about which C functions
|
||||
* need which of these lowlevel functions
|
||||
<<<<<<< HEAD
|
||||
* please consult the Newlib or Picolibc libc manual
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* Copyright (c) 2025 STMicroelectronics.
|
||||
=======
|
||||
* please consult the newlib libc manual
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* Copyright (c) 2024 STMicroelectronics.
|
||||
>>>>>>> main
|
||||
* All rights reserved.
|
||||
*
|
||||
* This software is licensed under terms that can be found in the LICENSE file
|
||||
@ -31,10 +23,6 @@
|
||||
/* Includes */
|
||||
#include <errno.h>
|
||||
#include <stdint.h>
|
||||
<<<<<<< HEAD
|
||||
#include <stddef.h>
|
||||
=======
|
||||
>>>>>>> main
|
||||
|
||||
/**
|
||||
* Pointer to the current high watermark of the heap usage
|
||||
@ -89,13 +77,3 @@ void *_sbrk(ptrdiff_t incr)
|
||||
|
||||
return (void *)prev_heap_end;
|
||||
}
|
||||
<<<<<<< HEAD
|
||||
|
||||
#if defined(__PICOLIBC__)
|
||||
// Picolibc expects syscalls without the leading underscore.
|
||||
// This creates a strong alias so that
|
||||
// calls to `sbrk()` are resolved to our `_sbrk()` implementation.
|
||||
__strong_reference(_sbrk, sbrk);
|
||||
#endif
|
||||
=======
|
||||
>>>>>>> main
|
||||
|
||||
8
DevC.ioc
8
DevC.ioc
@ -21,8 +21,7 @@ CAN1.BS2=CAN_BS2_7TQ
|
||||
CAN1.CalculateBaudRate=1000000
|
||||
CAN1.CalculateTimeBit=1000
|
||||
CAN1.CalculateTimeQuantum=71.42857142857143
|
||||
CAN1.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,ABOM,CalculateTimeBit,CalculateBaudRate,NART
|
||||
CAN1.NART=ENABLE
|
||||
CAN1.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,ABOM,CalculateTimeBit,CalculateBaudRate
|
||||
CAN1.Prescaler=3
|
||||
CAN1.TXFP=ENABLE
|
||||
CAN2.BS1=CAN_BS1_6TQ
|
||||
@ -30,8 +29,7 @@ CAN2.BS2=CAN_BS2_7TQ
|
||||
CAN2.CalculateBaudRate=1000000
|
||||
CAN2.CalculateTimeBit=1000
|
||||
CAN2.CalculateTimeQuantum=71.42857142857143
|
||||
CAN2.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,CalculateTimeBit,CalculateBaudRate,NART
|
||||
CAN2.NART=ENABLE
|
||||
CAN2.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,CalculateTimeBit,CalculateBaudRate
|
||||
CAN2.Prescaler=3
|
||||
CAN2.TXFP=ENABLE
|
||||
Dma.I2C2_TX.2.Direction=DMA_MEMORY_TO_PERIPH
|
||||
@ -270,10 +268,8 @@ MxDb.Version=DB.6.0.150
|
||||
NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||
NVIC.CAN1_RX0_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true
|
||||
NVIC.CAN1_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.CAN1_TX_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.CAN2_RX0_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true
|
||||
NVIC.CAN2_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.CAN2_TX_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.DMA1_Stream1_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DMA1_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DMA1_Stream7_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:true\:true
|
||||
|
||||
@ -52,15 +52,12 @@
|
||||
/* Entry Point */
|
||||
ENTRY(Reset_Handler)
|
||||
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
/* Highest address of the user mode stack */
|
||||
_estack = ORIGIN(RAM) + LENGTH(RAM); /* end of RAM */
|
||||
/* Generate a link error if heap and stack don't fit into RAM */
|
||||
_Min_Heap_Size = 0x1000; /* required amount of heap */
|
||||
_Min_Stack_Size = 0x1000; /* required amount of stack */
|
||||
|
||||
>>>>>>> main
|
||||
/* Specify the memory areas */
|
||||
MEMORY
|
||||
{
|
||||
@ -69,15 +66,6 @@ CCMRAM (xrw) : ORIGIN = 0x10000000, LENGTH = 64K
|
||||
FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 1024K
|
||||
}
|
||||
|
||||
<<<<<<< HEAD
|
||||
/* Highest address of the user mode stack */
|
||||
_estack = ORIGIN(RAM) + LENGTH(RAM); /* end of RAM */
|
||||
/* Generate a link error if heap and stack don't fit into RAM */
|
||||
_Min_Heap_Size = 0x1000; /* required amount of heap */
|
||||
_Min_Stack_Size = 0x1000; /* required amount of stack */
|
||||
|
||||
=======
|
||||
>>>>>>> main
|
||||
/* Define output sections */
|
||||
SECTIONS
|
||||
{
|
||||
@ -160,8 +148,6 @@ SECTIONS
|
||||
. = ALIGN(4);
|
||||
} >FLASH
|
||||
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
/* used by the startup to initialize data */
|
||||
_sidata = LOADADDR(.data);
|
||||
|
||||
@ -179,7 +165,6 @@ SECTIONS
|
||||
_edata = .; /* define a global symbol at data end */
|
||||
} >RAM AT> FLASH
|
||||
|
||||
>>>>>>> main
|
||||
_siccmram = LOADADDR(.ccmram);
|
||||
|
||||
/* CCM-RAM section
|
||||
@ -199,72 +184,6 @@ SECTIONS
|
||||
_eccmram = .; /* create a global symbol at ccmram end */
|
||||
} >CCMRAM AT> FLASH
|
||||
|
||||
<<<<<<< HEAD
|
||||
/* used by the startup to initialize data */
|
||||
_sidata = LOADADDR(.data);
|
||||
|
||||
/* Initialized data sections goes into RAM, load LMA copy after code */
|
||||
.data :
|
||||
{
|
||||
. = ALIGN(4);
|
||||
_sdata = .; /* create a global symbol at data start */
|
||||
*(.data) /* .data sections */
|
||||
*(.data*) /* .data* sections */
|
||||
*(.RamFunc) /* .RamFunc sections */
|
||||
*(.RamFunc*) /* .RamFunc* sections */
|
||||
|
||||
. = ALIGN(4);
|
||||
} >RAM AT> FLASH
|
||||
|
||||
/* Initialized TLS data section */
|
||||
.tdata : ALIGN(4)
|
||||
{
|
||||
*(.tdata .tdata.* .gnu.linkonce.td.*)
|
||||
. = ALIGN(4);
|
||||
_edata = .; /* define a global symbol at data end */
|
||||
PROVIDE(__data_end = .);
|
||||
PROVIDE(__tdata_end = .);
|
||||
} >RAM AT> FLASH
|
||||
|
||||
PROVIDE( __tdata_start = ADDR(.tdata) );
|
||||
PROVIDE( __tdata_size = __tdata_end - __tdata_start );
|
||||
|
||||
PROVIDE( __data_start = ADDR(.data) );
|
||||
PROVIDE( __data_size = __data_end - __data_start );
|
||||
|
||||
PROVIDE( __tdata_source = LOADADDR(.tdata) );
|
||||
PROVIDE( __tdata_source_end = LOADADDR(.tdata) + SIZEOF(.tdata) );
|
||||
PROVIDE( __tdata_source_size = __tdata_source_end - __tdata_source );
|
||||
|
||||
PROVIDE( __data_source = LOADADDR(.data) );
|
||||
PROVIDE( __data_source_end = __tdata_source_end );
|
||||
PROVIDE( __data_source_size = __data_source_end - __data_source );
|
||||
/* Uninitialized data section */
|
||||
.tbss (NOLOAD) : ALIGN(4)
|
||||
{
|
||||
/* This is used by the startup in order to initialize the .bss secion */
|
||||
_sbss = .; /* define a global symbol at bss start */
|
||||
__bss_start__ = _sbss;
|
||||
*(.tbss .tbss.*)
|
||||
. = ALIGN(4);
|
||||
PROVIDE( __tbss_end = . );
|
||||
} >RAM
|
||||
|
||||
PROVIDE( __tbss_start = ADDR(.tbss) );
|
||||
PROVIDE( __tbss_size = __tbss_end - __tbss_start );
|
||||
PROVIDE( __tbss_offset = ADDR(.tbss) - ADDR(.tdata) );
|
||||
|
||||
PROVIDE( __tls_base = __tdata_start );
|
||||
PROVIDE( __tls_end = __tbss_end );
|
||||
PROVIDE( __tls_size = __tls_end - __tls_base );
|
||||
PROVIDE( __tls_align = MAX(ALIGNOF(.tdata), ALIGNOF(.tbss)) );
|
||||
PROVIDE( __tls_size_align = (__tls_size + __tls_align - 1) & ~(__tls_align - 1) );
|
||||
PROVIDE( __arm32_tls_tcb_offset = MAX(8, __tls_align) );
|
||||
PROVIDE( __arm64_tls_tcb_offset = MAX(16, __tls_align) );
|
||||
|
||||
.bss (NOLOAD) : ALIGN(4)
|
||||
{
|
||||
=======
|
||||
|
||||
/* Uninitialized data section */
|
||||
. = ALIGN(4);
|
||||
@ -273,25 +192,10 @@ SECTIONS
|
||||
/* This is used by the startup in order to initialize the .bss secion */
|
||||
_sbss = .; /* define a global symbol at bss start */
|
||||
__bss_start__ = _sbss;
|
||||
>>>>>>> main
|
||||
*(.bss)
|
||||
*(.bss*)
|
||||
*(COMMON)
|
||||
|
||||
<<<<<<< HEAD
|
||||
. = ALIGN(4);
|
||||
_ebss = .; /* define a global symbol at bss end */
|
||||
__bss_end__ = _ebss;
|
||||
PROVIDE( __bss_end = .);
|
||||
} >RAM
|
||||
PROVIDE( __non_tls_bss_start = ADDR(.bss) );
|
||||
|
||||
PROVIDE( __bss_start = __tbss_start );
|
||||
PROVIDE( __bss_size = __bss_end - __bss_start );
|
||||
|
||||
/* User_heap_stack section, used to check that there is enough RAM left */
|
||||
._user_heap_stack (NOLOAD) :
|
||||
=======
|
||||
. = ALIGN(4);
|
||||
_ebss = .; /* define a global symbol at bss end */
|
||||
__bss_end__ = _ebss;
|
||||
@ -299,7 +203,6 @@ SECTIONS
|
||||
|
||||
/* User_heap_stack section, used to check that there is enough RAM left */
|
||||
._user_heap_stack :
|
||||
>>>>>>> main
|
||||
{
|
||||
. = ALIGN(8);
|
||||
PROVIDE ( end = . );
|
||||
@ -314,14 +217,6 @@ SECTIONS
|
||||
/* Remove information from the standard libraries */
|
||||
/DISCARD/ :
|
||||
{
|
||||
<<<<<<< HEAD
|
||||
libc.a:* ( * )
|
||||
libm.a:* ( * )
|
||||
libgcc.a:* ( * )
|
||||
}
|
||||
|
||||
}
|
||||
=======
|
||||
libc.a ( * )
|
||||
libm.a ( * )
|
||||
libgcc.a ( * )
|
||||
@ -330,4 +225,3 @@ SECTIONS
|
||||
}
|
||||
|
||||
|
||||
>>>>>>> main
|
||||
|
||||
@ -5,11 +5,8 @@ can:
|
||||
- instance: CAN2
|
||||
name: '2'
|
||||
enabled: true
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
dwt:
|
||||
enabled: true
|
||||
>>>>>>> main
|
||||
gpio:
|
||||
configs:
|
||||
- custom_name: USER_KEY
|
||||
@ -128,11 +125,8 @@ spi:
|
||||
enabled: true
|
||||
time:
|
||||
enabled: true
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
uart:
|
||||
devices:
|
||||
- instance: USART3
|
||||
name: DR16
|
||||
enabled: true
|
||||
>>>>>>> main
|
||||
|
||||
214
User/bsp/can.c
214
User/bsp/can.c
@ -37,32 +37,16 @@ static osMutexId_t queue_mutex = NULL;
|
||||
static void (*CAN_Callback[BSP_CAN_NUM][BSP_CAN_CB_NUM])(void);
|
||||
static bool inited = false;
|
||||
static BSP_CAN_IdParser_t id_parser = NULL; /* ID解析器 */
|
||||
<<<<<<< HEAD
|
||||
static BSP_CAN_TxQueue_t tx_queues[BSP_CAN_NUM]; /* 每个CAN的发送队列 */
|
||||
=======
|
||||
>>>>>>> main
|
||||
|
||||
/* Private function prototypes ---------------------------------------------- */
|
||||
static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan);
|
||||
static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id);
|
||||
static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size);
|
||||
<<<<<<< HEAD
|
||||
static void BSP_CAN_RxFifo0Callback(void);
|
||||
static void BSP_CAN_RxFifo1Callback(void);
|
||||
static void BSP_CAN_TxCompleteCallback(void);
|
||||
static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header);
|
||||
static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
|
||||
static void BSP_CAN_TxQueueInit(BSP_CAN_t can);
|
||||
static bool BSP_CAN_TxQueuePush(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg);
|
||||
static bool BSP_CAN_TxQueuePop(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg);
|
||||
static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can);
|
||||
=======
|
||||
static int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id);
|
||||
static void BSP_CAN_RxFifo0Callback(void);
|
||||
static void BSP_CAN_RxFifo1Callback(void);
|
||||
static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header);
|
||||
static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
|
||||
>>>>>>> main
|
||||
|
||||
/* Private functions -------------------------------------------------------- */
|
||||
/* USER FUNCTION BEGIN */
|
||||
@ -137,9 +121,6 @@ static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queu
|
||||
return BSP_OK;
|
||||
}
|
||||
|
||||
<<<<<<< HEAD
|
||||
|
||||
=======
|
||||
/**
|
||||
* @brief 删除指定CAN ID的消息队列
|
||||
* @note 内部函数，已包含互斥锁保护
|
||||
@ -163,7 +144,6 @@ static int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id) {
|
||||
osMutexRelease(queue_mutex);
|
||||
return BSP_ERR; // 未找到
|
||||
}
|
||||
>>>>>>> main
|
||||
/**
|
||||
* @brief 获取帧类型
|
||||
*/
|
||||
@ -184,109 +164,6 @@ static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_
|
||||
}
|
||||
|
||||
/**
|
||||
<<<<<<< HEAD
|
||||
* @brief 初始化发送队列
|
||||
*/
|
||||
static void BSP_CAN_TxQueueInit(BSP_CAN_t can) {
|
||||
if (can >= BSP_CAN_NUM) return;
|
||||
|
||||
tx_queues[can].head = 0;
|
||||
tx_queues[can].tail = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 向发送队列添加消息（无锁）
|
||||
*/
|
||||
static bool BSP_CAN_TxQueuePush(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg) {
|
||||
if (can >= BSP_CAN_NUM || msg == NULL) return false;
|
||||
|
||||
BSP_CAN_TxQueue_t *queue = &tx_queues[can];
|
||||
uint32_t next_head = (queue->head + 1) % BSP_CAN_TX_QUEUE_SIZE;
|
||||
|
||||
// 队列满
|
||||
if (next_head == queue->tail) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 复制消息
|
||||
queue->buffer[queue->head] = *msg;
|
||||
|
||||
// 更新头指针（原子操作）
|
||||
queue->head = next_head;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 从发送队列取出消息（无锁）
|
||||
*/
|
||||
static bool BSP_CAN_TxQueuePop(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg) {
|
||||
if (can >= BSP_CAN_NUM || msg == NULL) return false;
|
||||
|
||||
BSP_CAN_TxQueue_t *queue = &tx_queues[can];
|
||||
|
||||
// 队列空
|
||||
if (queue->head == queue->tail) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 复制消息
|
||||
*msg = queue->buffer[queue->tail];
|
||||
|
||||
// 更新尾指针（原子操作）
|
||||
queue->tail = (queue->tail + 1) % BSP_CAN_TX_QUEUE_SIZE;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 检查发送队列是否为空
|
||||
*/
|
||||
static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can) {
|
||||
if (can >= BSP_CAN_NUM) return true;
|
||||
|
||||
return tx_queues[can].head == tx_queues[can].tail;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 处理所有CAN实例的发送队列
|
||||
*/
|
||||
static void BSP_CAN_TxCompleteCallback(void) {
|
||||
// 处理所有CAN实例的发送队列
|
||||
for (int i = 0; i < BSP_CAN_NUM; i++) {
|
||||
BSP_CAN_t can = (BSP_CAN_t)i;
|
||||
CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can);
|
||||
if (hcan == NULL) continue;
|
||||
|
||||
BSP_CAN_TxMessage_t msg;
|
||||
uint32_t mailbox;
|
||||
|
||||
// 尝试发送队列中的消息
|
||||
while (!BSP_CAN_TxQueueIsEmpty(can)) {
|
||||
// 检查是否有空闲邮箱
|
||||
if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) == 0) {
|
||||
break; // 没有空闲邮箱，等待下次中断
|
||||
}
|
||||
|
||||
// 从队列中取出消息
|
||||
if (!BSP_CAN_TxQueuePop(can, &msg)) {
|
||||
break;
|
||||
}
|
||||
|
||||
// 发送消息
|
||||
if (HAL_CAN_AddTxMessage(hcan, &msg.header, msg.data, &mailbox) != HAL_OK) {
|
||||
// 发送失败，消息已经从队列中移除，直接丢弃
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
=======
|
||||
>>>>>>> main
|
||||
* @brief FIFO0接收处理函数
|
||||
*/
|
||||
static void BSP_CAN_RxFifo0Callback(void) {
|
||||
@ -470,16 +347,7 @@ int8_t BSP_CAN_Init(void) {
|
||||
|
||||
// 清零回调函数数组
|
||||
memset(CAN_Callback, 0, sizeof(CAN_Callback));
|
||||
<<<<<<< HEAD
|
||||
|
||||
// 初始化发送队列
|
||||
for (int i = 0; i < BSP_CAN_NUM; i++) {
|
||||
BSP_CAN_TxQueueInit((BSP_CAN_t)i);
|
||||
}
|
||||
|
||||
=======
|
||||
|
||||
>>>>>>> main
|
||||
// 初始化ID解析器为默认解析器
|
||||
id_parser = BSP_CAN_DefaultIdParser;
|
||||
|
||||
@ -509,12 +377,6 @@ int8_t BSP_CAN_Init(void) {
|
||||
|
||||
// 自动注册CAN1接收回调函数
|
||||
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback);
|
||||
<<<<<<< HEAD
|
||||
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_TxCompleteCallback);
|
||||
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_TxCompleteCallback);
|
||||
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback);
|
||||
=======
|
||||
>>>>>>> main
|
||||
|
||||
// 激活CAN1中断
|
||||
HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING |
|
||||
@ -528,27 +390,15 @@ int8_t BSP_CAN_Init(void) {
|
||||
|
||||
// 自动注册CAN2接收回调函数
|
||||
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifo1Callback);
|
||||
<<<<<<< HEAD
|
||||
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_TxCompleteCallback);
|
||||
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_TxCompleteCallback);
|
||||
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback);
|
||||
|
||||
// 激活CAN2中断
|
||||
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING |
|
||||
CAN_IT_TX_MAILBOX_EMPTY); // 激活发送邮箱空中断
|
||||
=======
|
||||
|
||||
// 激活CAN2中断
|
||||
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);
|
||||
>>>>>>> main
|
||||
|
||||
|
||||
inited = true;
|
||||
return BSP_OK;
|
||||
}
|
||||
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
int8_t BSP_CAN_DeInit(void) {
|
||||
if (!inited) {
|
||||
return BSP_ERR;
|
||||
@ -582,7 +432,6 @@ int8_t BSP_CAN_DeInit(void) {
|
||||
inited = false;
|
||||
return BSP_OK;
|
||||
}
|
||||
>>>>>>> main
|
||||
|
||||
CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can) {
|
||||
if (can >= BSP_CAN_NUM) {
|
||||
@ -638,31 +487,6 @@ int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format,
|
||||
return BSP_ERR_NULL;
|
||||
}
|
||||
|
||||
<<<<<<< HEAD
|
||||
// 准备发送消息
|
||||
BSP_CAN_TxMessage_t tx_msg = {0};
|
||||
|
||||
switch (format) {
|
||||
case BSP_CAN_FORMAT_STD_DATA:
|
||||
tx_msg.header.StdId = id;
|
||||
tx_msg.header.IDE = CAN_ID_STD;
|
||||
tx_msg.header.RTR = CAN_RTR_DATA;
|
||||
break;
|
||||
case BSP_CAN_FORMAT_EXT_DATA:
|
||||
tx_msg.header.ExtId = id;
|
||||
tx_msg.header.IDE = CAN_ID_EXT;
|
||||
tx_msg.header.RTR = CAN_RTR_DATA;
|
||||
break;
|
||||
case BSP_CAN_FORMAT_STD_REMOTE:
|
||||
tx_msg.header.StdId = id;
|
||||
tx_msg.header.IDE = CAN_ID_STD;
|
||||
tx_msg.header.RTR = CAN_RTR_REMOTE;
|
||||
break;
|
||||
case BSP_CAN_FORMAT_EXT_REMOTE:
|
||||
tx_msg.header.ExtId = id;
|
||||
tx_msg.header.IDE = CAN_ID_EXT;
|
||||
tx_msg.header.RTR = CAN_RTR_REMOTE;
|
||||
=======
|
||||
CAN_TxHeaderTypeDef header = {0};
|
||||
uint32_t mailbox;
|
||||
|
||||
@ -686,38 +510,11 @@ int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format,
|
||||
header.ExtId = id;
|
||||
header.IDE = CAN_ID_EXT;
|
||||
header.RTR = CAN_RTR_REMOTE;
|
||||
>>>>>>> main
|
||||
break;
|
||||
default:
|
||||
return BSP_ERR;
|
||||
}
|
||||
|
||||
<<<<<<< HEAD
|
||||
tx_msg.header.DLC = dlc;
|
||||
tx_msg.header.TransmitGlobalTime = DISABLE;
|
||||
|
||||
// 复制数据
|
||||
if (data != NULL && dlc > 0) {
|
||||
memcpy(tx_msg.data, data, dlc);
|
||||
}
|
||||
|
||||
// 尝试直接发送到邮箱
|
||||
uint32_t mailbox;
|
||||
if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) > 0) {
|
||||
HAL_StatusTypeDef result = HAL_CAN_AddTxMessage(hcan, &tx_msg.header, tx_msg.data, &mailbox);
|
||||
if (result == HAL_OK) {
|
||||
return BSP_OK; // 发送成功
|
||||
}
|
||||
}
|
||||
|
||||
// 邮箱满，尝试放入队列
|
||||
if (BSP_CAN_TxQueuePush(can, &tx_msg)) {
|
||||
return BSP_OK; // 成功放入队列
|
||||
}
|
||||
|
||||
// 队列也满，丢弃数据
|
||||
return BSP_ERR; // 数据丢弃
|
||||
=======
|
||||
header.DLC = dlc;
|
||||
header.TransmitGlobalTime = DISABLE;
|
||||
|
||||
@ -728,7 +525,6 @@ int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format,
|
||||
}
|
||||
|
||||
return BSP_OK;
|
||||
>>>>>>> main
|
||||
}
|
||||
|
||||
int8_t BSP_CAN_TransmitStdDataFrame(BSP_CAN_t can, BSP_CAN_StdDataFrame_t *frame) {
|
||||
@ -760,15 +556,12 @@ int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size) {
|
||||
return BSP_CAN_CreateIdQueue(can, can_id, queue_size);
|
||||
}
|
||||
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
int8_t BSP_CAN_UnregisterIdQueue(BSP_CAN_t can, uint32_t can_id) {
|
||||
if (!inited) {
|
||||
return BSP_ERR_INITED;
|
||||
}
|
||||
return BSP_CAN_DeleteIdQueue(can, can_id);
|
||||
}
|
||||
>>>>>>> main
|
||||
|
||||
int8_t BSP_CAN_GetMessage(BSP_CAN_t can, uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout) {
|
||||
if (!inited) {
|
||||
@ -835,8 +628,6 @@ int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser) {
|
||||
return BSP_OK;
|
||||
}
|
||||
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
int8_t BSP_CAN_UnregisterIdParser(void) {
|
||||
if (!inited) {
|
||||
return BSP_ERR_INITED;
|
||||
@ -846,7 +637,6 @@ int8_t BSP_CAN_UnregisterIdParser(void) {
|
||||
return BSP_OK;
|
||||
}
|
||||
|
||||
>>>>>>> main
|
||||
uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
|
||||
if (id_parser != NULL) {
|
||||
return id_parser(original_id, frame_type);
|
||||
@ -854,9 +644,6 @@ uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
|
||||
return BSP_CAN_DefaultIdParser(original_id, frame_type);
|
||||
}
|
||||
|
||||
<<<<<<< HEAD
|
||||
|
||||
=======
|
||||
int8_t BSP_CAN_WaitTxMailboxEmpty(BSP_CAN_t can, uint32_t timeout) {
|
||||
if (!inited) {
|
||||
return BSP_ERR_INITED;
|
||||
@ -896,4 +683,3 @@ int8_t BSP_CAN_WaitTxMailboxEmpty(BSP_CAN_t can, uint32_t timeout) {
|
||||
osDelay(1);
|
||||
}
|
||||
}
|
||||
>>>>>>> main
|
||||
|
||||
@ -21,10 +21,6 @@ extern "C" {
|
||||
#define BSP_CAN_DEFAULT_QUEUE_SIZE 10
|
||||
#define BSP_CAN_TIMEOUT_IMMEDIATE 0
|
||||
#define BSP_CAN_TIMEOUT_FOREVER osWaitForever
|
||||
<<<<<<< HEAD
|
||||
#define BSP_CAN_TX_QUEUE_SIZE 32 /* 发送队列大小 */
|
||||
=======
|
||||
>>>>>>> main
|
||||
|
||||
/* USER DEFINE BEGIN */
|
||||
|
||||
@ -106,22 +102,6 @@ typedef struct {
|
||||
/* ID解析回调函数类型 */
|
||||
typedef uint32_t (*BSP_CAN_IdParser_t)(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
|
||||
|
||||
<<<<<<< HEAD
|
||||
/* CAN发送消息结构体 */
|
||||
typedef struct {
|
||||
CAN_TxHeaderTypeDef header; /* 发送头 */
|
||||
uint8_t data[BSP_CAN_MAX_DLC]; /* 数据 */
|
||||
} BSP_CAN_TxMessage_t;
|
||||
|
||||
/* 无锁环形队列结构体 */
|
||||
typedef struct {
|
||||
BSP_CAN_TxMessage_t buffer[BSP_CAN_TX_QUEUE_SIZE]; /* 缓冲区 */
|
||||
volatile uint32_t head; /* 队列头 */
|
||||
volatile uint32_t tail; /* 队列尾 */
|
||||
} BSP_CAN_TxQueue_t;
|
||||
|
||||
=======
|
||||
>>>>>>> main
|
||||
/* USER STRUCT BEGIN */
|
||||
|
||||
/* USER STRUCT END */
|
||||
@ -135,15 +115,12 @@ typedef struct {
|
||||
int8_t BSP_CAN_Init(void);
|
||||
|
||||
/**
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
* @brief 反初始化 CAN 模块
|
||||
* @return BSP_OK 成功，其他值失败
|
||||
*/
|
||||
int8_t BSP_CAN_DeInit(void);
|
||||
|
||||
/**
|
||||
>>>>>>> main
|
||||
* @brief 获取 CAN 句柄
|
||||
* @param can CAN 枚举
|
||||
* @return CAN_HandleTypeDef 指针，失败返回 NULL
|
||||
@ -196,22 +173,6 @@ int8_t BSP_CAN_TransmitExtDataFrame(BSP_CAN_t can, BSP_CAN_ExtDataFrame_t *frame
|
||||
*/
|
||||
int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame);
|
||||
|
||||
<<<<<<< HEAD
|
||||
|
||||
/**
|
||||
* @brief 获取发送队列中待发送消息数量
|
||||
* @param can CAN 枚举
|
||||
* @return 队列中消息数量，-1表示错误
|
||||
*/
|
||||
int32_t BSP_CAN_GetTxQueueCount(BSP_CAN_t can);
|
||||
|
||||
/**
|
||||
* @brief 清空发送队列
|
||||
* @param can CAN 枚举
|
||||
* @return BSP_OK 成功，其他值失败
|
||||
*/
|
||||
int8_t BSP_CAN_FlushTxQueue(BSP_CAN_t can);
|
||||
=======
|
||||
/**
|
||||
* @brief 等待CAN发送邮箱空闲
|
||||
* @param can CAN 枚举
|
||||
@ -219,7 +180,6 @@ int8_t BSP_CAN_FlushTxQueue(BSP_CAN_t can);
|
||||
* @return BSP_OK 成功，其他值失败
|
||||
*/
|
||||
int8_t BSP_CAN_WaitTxMailboxEmpty(BSP_CAN_t can, uint32_t timeout);
|
||||
>>>>>>> main
|
||||
|
||||
/**
|
||||
* @brief 注册 CAN ID 接收队列
|
||||
@ -230,9 +190,6 @@ int8_t BSP_CAN_WaitTxMailboxEmpty(BSP_CAN_t can, uint32_t timeout);
|
||||
*/
|
||||
int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size);
|
||||
|
||||
<<<<<<< HEAD
|
||||
|
||||
=======
|
||||
/**
|
||||
* @brief 注销 CAN ID 接收队列
|
||||
* @param can CAN 枚举
|
||||
@ -240,7 +197,6 @@ int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size);
|
||||
* @return BSP_OK 成功，其他值失败
|
||||
*/
|
||||
int8_t BSP_CAN_UnregisterIdQueue(BSP_CAN_t can, uint32_t can_id);
|
||||
>>>>>>> main
|
||||
|
||||
/**
|
||||
* @brief 获取 CAN 消息
|
||||
@ -275,14 +231,11 @@ int8_t BSP_CAN_FlushQueue(BSP_CAN_t can, uint32_t can_id);
|
||||
*/
|
||||
int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser);
|
||||
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
/**
|
||||
* @brief 注销ID解析器
|
||||
* @return BSP_OK 成功，其他值失败
|
||||
*/
|
||||
int8_t BSP_CAN_UnregisterIdParser(void);
|
||||
>>>>>>> main
|
||||
|
||||
/**
|
||||
* @brief 解析CAN ID
|
||||
|
||||
@ -41,13 +41,8 @@ typedef enum {
|
||||
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch);
|
||||
<<<<<<< HEAD
|
||||
int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle); //设置占空比，范围0.0~1.0
|
||||
int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq); //设置频率，单位Hz
|
||||
=======
|
||||
int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle);
|
||||
int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq);
|
||||
>>>>>>> main
|
||||
int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch);
|
||||
uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch);
|
||||
uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch);
|
||||
|
||||
@ -2,23 +2,17 @@ ahrs:
|
||||
dependencies:
|
||||
- component/user_math.h
|
||||
enabled: true
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
cmd:
|
||||
dependencies:
|
||||
- component/ahrs
|
||||
enabled: true
|
||||
>>>>>>> main
|
||||
filter:
|
||||
dependencies:
|
||||
- component/ahrs
|
||||
enabled: true
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
limiter:
|
||||
dependencies: []
|
||||
enabled: true
|
||||
>>>>>>> main
|
||||
pid:
|
||||
dependencies:
|
||||
- component/filter
|
||||
|
||||
@ -6,82 +6,89 @@
|
||||
#include <string.h>
|
||||
|
||||
/* Private macros ----------------------------------------------------------- */
|
||||
#define LQR_LIMIT(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
|
||||
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
|
||||
/* 从MATLAB仿真get_k.m得到的默认增益矩阵多项式拟合系数 */
|
||||
/* 这些系数是通过对不同腿长的LQR增益进行3阶多项式拟合得到的 */
|
||||
static LQR_GainMatrix_t default_gain_matrix = {
|
||||
/* K矩阵第一行 - 轮毂力矩T的增益系数 */
|
||||
.k11_coeff = {0.0f, -2845.3f, 1899.4f, -123.8f}, /* theta */
|
||||
.k12_coeff = {0.0f, -89.7f, 61.2f, -4.8f}, /* d_theta */
|
||||
.k13_coeff = {0.0f, 5479.2f, -3298.6f, 489.8f}, /* x */
|
||||
.k14_coeff = {0.0f, 312.4f, -178.9f, 34.2f}, /* d_x */
|
||||
.k15_coeff = {0.0f, -31250.0f, 18750.0f, -3125.0f}, /* phi */
|
||||
.k16_coeff = {0.0f, -89.7f, 61.2f, -4.8f}, /* d_phi */
|
||||
|
||||
/* K矩阵第二行 - 髋关节力矩Tp的增益系数 */
|
||||
.k21_coeff = {0.0f, 486.1f, -324.1f, 21.6f}, /* theta */
|
||||
.k22_coeff = {0.0f, 15.3f, -10.4f, 0.8f}, /* d_theta */
|
||||
.k23_coeff = {0.0f, -935.4f, 562.2f, -83.5f}, /* x */
|
||||
.k24_coeff = {0.0f, -53.3f, 30.5f, -5.8f}, /* d_x */
|
||||
.k25_coeff = {0.0f, 5333.3f, -3200.0f, 533.3f}, /* phi */
|
||||
.k26_coeff = {0.0f, 15.3f, -10.4f, 0.8f}, /* d_phi */
|
||||
};
|
||||
|
||||
/* Private function prototypes ---------------------------------------------- */
|
||||
|
||||
static int8_t LQR_CalculateErrorState(LQR_t *lqr) {
|
||||
if (lqr == NULL) {
|
||||
return -1;
|
||||
}
|
||||
/* 计算状态误差 */
|
||||
lqr->error_state.theta = lqr->current_state.theta - lqr->target_state.theta;
|
||||
lqr->error_state.d_theta = lqr->current_state.d_theta - lqr->target_state.d_theta;
|
||||
lqr->error_state.x = lqr->current_state.x - lqr->target_state.x;
|
||||
lqr->error_state.d_x = lqr->current_state.d_x - lqr->target_state.d_x;
|
||||
lqr->error_state.phi = lqr->current_state.phi - lqr->target_state.phi;
|
||||
lqr->error_state.d_phi = lqr->current_state.d_phi - lqr->target_state.d_phi;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static float LQR_PolynomialCalc(const float *coeff, float leg_length) {
|
||||
if (coeff == NULL) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
/* 计算3阶多项式: coeff[0]*L^3 + coeff[1]*L^2 + coeff[2]*L + coeff[3] */
|
||||
float L = leg_length;
|
||||
float L2 = L * L;
|
||||
float L3 = L2 * L;
|
||||
|
||||
return coeff[0] * L3 + coeff[1] * L2 + coeff[2] * L + coeff[3];
|
||||
}
|
||||
|
||||
|
||||
// static int8_t LQR_ApplyControlLimits(LQR_t *lqr) {
|
||||
// if (lqr == NULL) {
|
||||
// return -1;
|
||||
// }
|
||||
|
||||
// /* 限制轮毂力矩 */
|
||||
// if (fabsf(lqr->control_output.T) > lqr->param.max_wheel_torque) {
|
||||
// lqr->control_output.T = LQR_LIMIT(lqr->control_output.T,
|
||||
// -lqr->param.max_wheel_torque,
|
||||
// lqr->param.max_wheel_torque);
|
||||
// lqr->wheel_torque_limited = true;
|
||||
// }
|
||||
|
||||
// /* 限制髋关节力矩 */
|
||||
// if (fabsf(lqr->control_output.Tp) > lqr->param.max_joint_torque) {
|
||||
// lqr->control_output.Tp = LQR_LIMIT(lqr->control_output.Tp,
|
||||
// -lqr->param.max_joint_torque,
|
||||
// lqr->param.max_joint_torque);
|
||||
// lqr->joint_torque_limited = true;
|
||||
// }
|
||||
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
static int8_t LQR_CalculateErrorState(LQR_Controller_t *lqr);
|
||||
static int8_t LQR_ApplyControlLimits(LQR_Controller_t *lqr);
|
||||
|
||||
/* Public functions --------------------------------------------------------- */
|
||||
|
||||
int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix) {
|
||||
int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque) {
|
||||
if (lqr == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* 清零结构体 */
|
||||
memset(lqr, 0, sizeof(LQR_t));
|
||||
|
||||
lqr->gain_matrix = gain_matrix;
|
||||
memset(lqr, 0, sizeof(LQR_Controller_t));
|
||||
|
||||
/* 设置控制限制 */
|
||||
lqr->param.max_wheel_torque = max_wheel_torque;
|
||||
lqr->param.max_joint_torque = max_joint_torque;
|
||||
|
||||
/* 设置默认系统物理参数（从MATLAB仿真get_k_length.m获取） */
|
||||
lqr->param.R = 0.072f; /* 驱动轮半径 */
|
||||
lqr->param.l = 0.03f; /* 机体重心到转轴距离 */
|
||||
lqr->param.mw = 0.60f; /* 驱动轮质量 */
|
||||
lqr->param.mp = 1.8f; /* 摆杆质量 */
|
||||
lqr->param.M = 1.8f; /* 机体质量 */
|
||||
lqr->param.g = 9.8f; /* 重力加速度 */
|
||||
|
||||
/* 计算转动惯量 */
|
||||
lqr->param.Iw = lqr->param.mw * lqr->param.R * lqr->param.R;
|
||||
lqr->param.IM = lqr->param.M * (0.3f * 0.3f + 0.12f * 0.12f) / 12.0f;
|
||||
|
||||
/* 设置默认增益矩阵 */
|
||||
lqr->gain_matrix = &default_gain_matrix;
|
||||
|
||||
/* 设置默认目标状态（平衡状态） */
|
||||
memset(&lqr->param.target_state, 0, sizeof(LQR_State_t));
|
||||
|
||||
/* 初始化当前腿长为中等值 */
|
||||
lqr->current_leg_length = 0.25f;
|
||||
|
||||
/* 计算初始增益矩阵 */
|
||||
LQR_CalculateGainMatrix(lqr, lqr->current_leg_length);
|
||||
|
||||
lqr->initialized = true;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix) {
|
||||
if (lqr == NULL || gain_matrix == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
lqr->gain_matrix = gain_matrix;
|
||||
|
||||
/* 重新计算增益矩阵 */
|
||||
return LQR_CalculateGainMatrix(lqr, lqr->current_leg_length);
|
||||
}
|
||||
|
||||
int8_t LQR_UpdateState(LQR_t *lqr, const LQR_State_t *state) {
|
||||
int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state) {
|
||||
if (lqr == NULL || state == NULL) {
|
||||
return -1;
|
||||
}
|
||||
@ -93,25 +100,34 @@ int8_t LQR_UpdateState(LQR_t *lqr, const LQR_State_t *state) {
|
||||
return LQR_CalculateErrorState(lqr);
|
||||
}
|
||||
|
||||
int8_t LQR_SetTargetState(LQR_t *lqr, const LQR_State_t *target_state) {
|
||||
int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state) {
|
||||
if (lqr == NULL || target_state == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
lqr->target_state = *target_state;
|
||||
lqr->param.target_state = *target_state;
|
||||
|
||||
/* 重新计算状态误差 */
|
||||
return LQR_CalculateErrorState(lqr);
|
||||
}
|
||||
|
||||
int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length) {
|
||||
int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length) {
|
||||
if (lqr == NULL || lqr->gain_matrix == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* 限制腿长范围 */
|
||||
leg_length = LQR_LIMIT(leg_length, 0.1f, 0.4f);
|
||||
lqr->current_leg_length = leg_length;
|
||||
|
||||
/* 更新与腿长相关的物理参数 */
|
||||
lqr->param.L = leg_length / 2.0f; /* 摆杆重心到驱动轮轴距离 */
|
||||
lqr->param.LM = leg_length / 2.0f; /* 摆杆重心到其转轴距离 */
|
||||
|
||||
/* 计算摆杆转动惯量 */
|
||||
float leg_total_length = lqr->param.L + lqr->param.LM;
|
||||
lqr->param.Ip = lqr->param.mp * (leg_total_length * leg_total_length + 0.05f * 0.05f) / 12.0f;
|
||||
|
||||
/* 使用多项式拟合计算当前增益矩阵 */
|
||||
lqr->K_matrix[0][0] = LQR_PolynomialCalc(lqr->gain_matrix->k11_coeff, leg_length); /* K11: theta */
|
||||
lqr->K_matrix[0][1] = LQR_PolynomialCalc(lqr->gain_matrix->k12_coeff, leg_length); /* K12: d_theta */
|
||||
@ -130,7 +146,7 @@ int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int8_t LQR_Control(LQR_t *lqr) {
|
||||
int8_t LQR_Control(LQR_Controller_t *lqr) {
|
||||
if (lqr == NULL || !lqr->initialized) {
|
||||
return -1;
|
||||
}
|
||||
@ -159,22 +175,92 @@ int8_t LQR_Control(LQR_t *lqr) {
|
||||
lqr->K_matrix[1][5] * lqr->error_state.d_phi
|
||||
);
|
||||
|
||||
/* 应用控制限制 */
|
||||
return LQR_ApplyControlLimits(lqr);
|
||||
}
|
||||
|
||||
int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output) {
|
||||
if (lqr == NULL || output == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
*output = lqr->control_output;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int8_t LQR_Reset(LQR_t *lqr) {
|
||||
int8_t LQR_Reset(LQR_Controller_t *lqr) {
|
||||
if (lqr == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* 清零状态和输出 */
|
||||
memset(&lqr->current_state, 0, sizeof(LQR_State_t));
|
||||
memset(&lqr->target_state, 0, sizeof(LQR_State_t));
|
||||
memset(&lqr->error_state, 0, sizeof(LQR_State_t));
|
||||
memset(&lqr->control_output, 0, sizeof(LQR_Output_t));
|
||||
memset(&lqr->control_output, 0, sizeof(LQR_Input_t));
|
||||
memset(&lqr->param.target_state, 0, sizeof(LQR_State_t));
|
||||
|
||||
/* 重置限幅标志 */
|
||||
lqr->wheel_torque_limited = false;
|
||||
lqr->joint_torque_limited = false;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
float LQR_PolynomialCalc(const float *coeff, float leg_length) {
|
||||
if (coeff == NULL) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
/* 计算3阶多项式: coeff[0]*L^3 + coeff[1]*L^2 + coeff[2]*L + coeff[3] */
|
||||
float L = leg_length;
|
||||
float L2 = L * L;
|
||||
float L3 = L2 * L;
|
||||
|
||||
return coeff[0] * L3 + coeff[1] * L2 + coeff[2] * L + coeff[3];
|
||||
}
|
||||
|
||||
/* Private functions -------------------------------------------------------- */
|
||||
/* Private functions -------------------------------------------------------- */
|
||||
|
||||
static int8_t LQR_CalculateErrorState(LQR_Controller_t *lqr) {
|
||||
if (lqr == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* 计算状态误差 */
|
||||
lqr->error_state.theta = lqr->current_state.theta - lqr->param.target_state.theta;
|
||||
lqr->error_state.d_theta = lqr->current_state.d_theta - lqr->param.target_state.d_theta;
|
||||
lqr->error_state.x = lqr->current_state.x - lqr->param.target_state.x;
|
||||
lqr->error_state.d_x = lqr->current_state.d_x - lqr->param.target_state.d_x;
|
||||
lqr->error_state.phi = lqr->current_state.phi - lqr->param.target_state.phi;
|
||||
lqr->error_state.d_phi = lqr->current_state.d_phi - lqr->param.target_state.d_phi;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int8_t LQR_ApplyControlLimits(LQR_Controller_t *lqr) {
|
||||
if (lqr == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* 重置限幅标志 */
|
||||
lqr->wheel_torque_limited = false;
|
||||
lqr->joint_torque_limited = false;
|
||||
|
||||
/* 限制轮毂力矩 */
|
||||
if (fabsf(lqr->control_output.T) > lqr->param.max_wheel_torque) {
|
||||
lqr->control_output.T = LQR_LIMIT(lqr->control_output.T,
|
||||
-lqr->param.max_wheel_torque,
|
||||
lqr->param.max_wheel_torque);
|
||||
lqr->wheel_torque_limited = true;
|
||||
}
|
||||
|
||||
/* 限制髋关节力矩 */
|
||||
if (fabsf(lqr->control_output.Tp) > lqr->param.max_joint_torque) {
|
||||
lqr->control_output.Tp = LQR_LIMIT(lqr->control_output.Tp,
|
||||
-lqr->param.max_joint_torque,
|
||||
lqr->param.max_joint_torque);
|
||||
lqr->joint_torque_limited = true;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/*
|
||||
* LQR控制器
|
||||
* 用于轮腿平衡机器人的线性二次调节器控制：单腿建模
|
||||
* 用于轮腿平衡机器人的线性二次调节器控制
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
@ -21,6 +21,16 @@ extern "C" {
|
||||
|
||||
/* Exported types ----------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @brief LQR状态向量
|
||||
* 状态定义：
|
||||
* theta: 摆杆与竖直方向夹角 (rad)
|
||||
* d_theta: 摆杆角速度 (rad/s)
|
||||
* x: 驱动轮位移 (m)
|
||||
* d_x: 驱动轮速度 (m/s)
|
||||
* phi: 机体与水平夹角 (rad)
|
||||
* d_phi: 机体角速度 (rad/s)
|
||||
*/
|
||||
typedef struct {
|
||||
float theta; /* 摆杆角度 */
|
||||
float d_theta; /* 摆杆角速度 */
|
||||
@ -30,11 +40,19 @@ typedef struct {
|
||||
float d_phi; /* 机体俯仰角速度 */
|
||||
} LQR_State_t;
|
||||
|
||||
/**
|
||||
* @brief LQR控制输入向量
|
||||
*/
|
||||
typedef struct {
|
||||
float T; /* 轮毂力矩 (N·m) */
|
||||
float Tp; /* 髋关节力矩 (N·m) */
|
||||
} LQR_Output_t;
|
||||
} LQR_Input_t;
|
||||
|
||||
/**
|
||||
* @brief LQR增益矩阵参数
|
||||
* K矩阵的每个元素的多项式拟合系数
|
||||
* K(leg_length) = a[0]*L^3 + a[1]*L^2 + a[2]*L + a[3]
|
||||
*/
|
||||
typedef struct {
|
||||
/* K矩阵第一行(轮毂力矩T对应的增益) */
|
||||
float k11_coeff[LQR_POLY_ORDER]; /* K(1,1): theta */
|
||||
@ -53,18 +71,41 @@ typedef struct {
|
||||
float k26_coeff[LQR_POLY_ORDER]; /* K(2,6): d_phi */
|
||||
} LQR_GainMatrix_t;
|
||||
|
||||
/**
|
||||
* @brief LQR控制器参数
|
||||
*/
|
||||
typedef struct {
|
||||
/* 系统物理参数 */
|
||||
float R; /* 驱动轮半径 (m) */
|
||||
float L; /* 摆杆重心到驱动轮轴距离 (m) */
|
||||
float LM; /* 摆杆重心到其转轴距离 (m) */
|
||||
float l; /* 机体重心到其转轴距离 (m) */
|
||||
float mw; /* 驱动轮质量 (kg) */
|
||||
float mp; /* 摆杆质量 (kg) */
|
||||
float M; /* 机体质量 (kg) */
|
||||
float Iw; /* 驱动轮转动惯量 (kg·m²) */
|
||||
float Ip; /* 摆杆绕质心转动惯量 (kg·m²) */
|
||||
float IM; /* 机体绕质心转动惯量 (kg·m²) */
|
||||
float g; /* 重力加速度 (m/s²) */
|
||||
|
||||
/* 控制限制 */
|
||||
float max_wheel_torque; /* 轮毂电机最大力矩 (N·m) */
|
||||
float max_joint_torque; /* 关节电机最大力矩 (N·m) */
|
||||
|
||||
/* 目标状态 */
|
||||
LQR_State_t target_state; /* 目标状态 */
|
||||
} LQR_Param_t;
|
||||
|
||||
/**
|
||||
* @brief LQR控制器主结构体
|
||||
*/
|
||||
typedef struct {
|
||||
LQR_Param_t param; /* 控制器参数 */
|
||||
LQR_GainMatrix_t *gain_matrix; /* 增益矩阵参数指针 */
|
||||
|
||||
LQR_State_t current_state; /* 当前状态 */
|
||||
LQR_State_t target_state; /* 目标状态 */
|
||||
LQR_State_t error_state; /* 状态误差 */
|
||||
|
||||
LQR_Output_t control_output; /* 控制输出 */
|
||||
LQR_Input_t control_output; /* 控制输出 */
|
||||
|
||||
/* 运行时变量 */
|
||||
float current_leg_length; /* 当前腿长 */
|
||||
@ -72,7 +113,10 @@ typedef struct {
|
||||
|
||||
bool initialized; /* 初始化标志 */
|
||||
|
||||
} LQR_t;
|
||||
/* 限幅标志 */
|
||||
bool wheel_torque_limited; /* 轮毂力矩是否被限幅 */
|
||||
bool joint_torque_limited; /* 关节力矩是否被限幅 */
|
||||
} LQR_Controller_t;
|
||||
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
|
||||
@ -80,11 +124,20 @@ typedef struct {
|
||||
* @brief 初始化LQR控制器
|
||||
*
|
||||
* @param lqr LQR控制器结构体指针
|
||||
* @param max_wheel_torque 轮毂电机最大力矩
|
||||
* @param max_joint_torque 关节电机最大力矩
|
||||
* @return int8_t 0-成功, 其他-失败
|
||||
*/
|
||||
int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque);
|
||||
|
||||
/**
|
||||
* @brief 设置LQR增益矩阵参数
|
||||
*
|
||||
* @param lqr LQR控制器结构体指针
|
||||
* @param gain_matrix 增益矩阵参数指针
|
||||
* @return int8_t 0-成功, 其他-失败
|
||||
*/
|
||||
int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix);
|
||||
|
||||
int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix);
|
||||
|
||||
/**
|
||||
* @brief 更新当前状态
|
||||
@ -93,7 +146,7 @@ int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix);
|
||||
* @param state 当前状态
|
||||
* @return int8_t 0-成功, 其他-失败
|
||||
*/
|
||||
int8_t LQR_UpdateState(LQR_t *lqr, const LQR_State_t *current_state);
|
||||
int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state);
|
||||
|
||||
/**
|
||||
* @brief 设置目标状态
|
||||
@ -102,7 +155,7 @@ int8_t LQR_UpdateState(LQR_t *lqr, const LQR_State_t *current_state);
|
||||
* @param target_state 目标状态
|
||||
* @return int8_t 0-成功, 其他-失败
|
||||
*/
|
||||
int8_t LQR_SetTargetState(LQR_t *lqr, const LQR_State_t *target_state);
|
||||
int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state);
|
||||
|
||||
/**
|
||||
* @brief 根据当前腿长计算增益矩阵
|
||||
@ -111,7 +164,7 @@ int8_t LQR_SetTargetState(LQR_t *lqr, const LQR_State_t *target_state);
|
||||
* @param leg_length 当前腿长 (m)
|
||||
* @return int8_t 0-成功, 其他-失败
|
||||
*/
|
||||
int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length);
|
||||
int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length);
|
||||
|
||||
/**
|
||||
* @brief 执行LQR控制计算
|
||||
@ -119,7 +172,16 @@ int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length);
|
||||
* @param lqr LQR控制器结构体指针
|
||||
* @return int8_t 0-成功, 其他-失败
|
||||
*/
|
||||
int8_t LQR_Control(LQR_t *lqr);
|
||||
int8_t LQR_Control(LQR_Controller_t *lqr);
|
||||
|
||||
/**
|
||||
* @brief 获取控制输出
|
||||
*
|
||||
* @param lqr LQR控制器结构体指针
|
||||
* @param output 输出控制指令指针
|
||||
* @return int8_t 0-成功, 其他-失败
|
||||
*/
|
||||
int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output);
|
||||
|
||||
/**
|
||||
* @brief 重置LQR控制器
|
||||
@ -127,7 +189,16 @@ int8_t LQR_Control(LQR_t *lqr);
|
||||
* @param lqr LQR控制器结构体指针
|
||||
* @return int8_t 0-成功, 其他-失败
|
||||
*/
|
||||
int8_t LQR_Reset(LQR_t *lqr);
|
||||
int8_t LQR_Reset(LQR_Controller_t *lqr);
|
||||
|
||||
/**
|
||||
* @brief 根据多项式系数计算增益值
|
||||
*
|
||||
* @param coeff 多项式系数数组
|
||||
* @param leg_length 腿长
|
||||
* @return float 增益值
|
||||
*/
|
||||
float LQR_PolynomialCalc(const float *coeff, float leg_length);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
@ -25,13 +25,6 @@ extern "C" {
|
||||
#define M_PI 3.14159265358979323846f
|
||||
#endif
|
||||
|
||||
<<<<<<< HEAD
|
||||
#ifndef M_PI_2
|
||||
#define M_PI_2 1.57079632679489661923f
|
||||
#endif
|
||||
|
||||
=======
|
||||
>>>>>>> main
|
||||
#ifndef M_2PI
|
||||
#define M_2PI 6.28318530717958647692f
|
||||
#endif
|
||||
|
||||
@ -1,381 +0,0 @@
|
||||
/*
|
||||
BMI088 陀螺仪+加速度计传感器。
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "bmi088.h"
|
||||
|
||||
#include <cmsis_os2.h>
|
||||
#include <gpio.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "bsp/time.h"
|
||||
#include "bsp/gpio.h"
|
||||
#include "bsp/spi.h"
|
||||
#include "component/user_math.h"
|
||||
|
||||
/* USER INCLUDE BEGIN */
|
||||
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
#define BMI088_REG_ACCL_CHIP_ID (0x00)
|
||||
#define BMI088_REG_ACCL_ERR (0x02)
|
||||
#define BMI088_REG_ACCL_STATUS (0x03)
|
||||
#define BMI088_REG_ACCL_X_LSB (0x12)
|
||||
#define BMI088_REG_ACCL_X_MSB (0x13)
|
||||
#define BMI088_REG_ACCL_Y_LSB (0x14)
|
||||
#define BMI088_REG_ACCL_Y_MSB (0x15)
|
||||
#define BMI088_REG_ACCL_Z_LSB (0x16)
|
||||
#define BMI088_REG_ACCL_Z_MSB (0x17)
|
||||
#define BMI088_REG_ACCL_SENSORTIME_0 (0x18)
|
||||
#define BMI088_REG_ACCL_SENSORTIME_1 (0x19)
|
||||
#define BMI088_REG_ACCL_SENSORTIME_2 (0x1A)
|
||||
#define BMI088_REG_ACCL_INT_STAT_1 (0x1D)
|
||||
#define BMI088_REG_ACCL_TEMP_MSB (0x22)
|
||||
#define BMI088_REG_ACCL_TEMP_LSB (0x23)
|
||||
#define BMI088_REG_ACCL_CONF (0x40)
|
||||
#define BMI088_REG_ACCL_RANGE (0x41)
|
||||
#define BMI088_REG_ACCL_INT1_IO_CONF (0x53)
|
||||
#define BMI088_REG_ACCL_INT2_IO_CONF (0x54)
|
||||
#define BMI088_REG_ACCL_INT1_INT2_MAP_DATA (0x58)
|
||||
#define BMI088_REG_ACCL_SELF_TEST (0x6D)
|
||||
#define BMI088_REG_ACCL_PWR_CONF (0x7C)
|
||||
#define BMI088_REG_ACCL_PWR_CTRL (0x7D)
|
||||
#define BMI088_REG_ACCL_SOFTRESET (0x7E)
|
||||
|
||||
#define BMI088_REG_GYRO_CHIP_ID (0x00)
|
||||
#define BMI088_REG_GYRO_X_LSB (0x02)
|
||||
#define BMI088_REG_GYRO_X_MSB (0x03)
|
||||
#define BMI088_REG_GYRO_Y_LSB (0x04)
|
||||
#define BMI088_REG_GYRO_Y_MSB (0x05)
|
||||
#define BMI088_REG_GYRO_Z_LSB (0x06)
|
||||
#define BMI088_REG_GYRO_Z_MSB (0x07)
|
||||
#define BMI088_REG_GYRO_INT_STAT_1 (0x0A)
|
||||
#define BMI088_REG_GYRO_RANGE (0x0F)
|
||||
#define BMI088_REG_GYRO_BANDWIDTH (0x10)
|
||||
#define BMI088_REG_GYRO_LPM1 (0x11)
|
||||
#define BMI088_REG_GYRO_SOFTRESET (0x14)
|
||||
#define BMI088_REG_GYRO_INT_CTRL (0x15)
|
||||
#define BMI088_REG_GYRO_INT3_INT4_IO_CONF (0x16)
|
||||
#define BMI088_REG_GYRO_INT3_INT4_IO_MAP (0x18)
|
||||
#define BMI088_REG_GYRO_SELF_TEST (0x3C)
|
||||
|
||||
#define BMI088_CHIP_ID_ACCL (0x1E)
|
||||
#define BMI088_CHIP_ID_GYRO (0x0F)
|
||||
|
||||
#define BMI088_LEN_RX_BUFF (19)
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
#define BMI088_ACCL_NSS_SET() \
|
||||
BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_SET)
|
||||
#define BMI088_ACCL_NSS_RESET() \
|
||||
BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_RESET)
|
||||
|
||||
#define BMI088_GYRO_NSS_SET() \
|
||||
BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_SET)
|
||||
#define BMI088_GYRO_NSS_RESET() \
|
||||
BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_RESET)
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
typedef enum {
|
||||
BMI_ACCL,
|
||||
BMI_GYRO,
|
||||
} BMI_Device_t;
|
||||
|
||||
/* USER STRUCT BEGIN */
|
||||
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
static uint8_t buffer[2];
|
||||
static uint8_t bmi088_rxbuf[BMI088_LEN_RX_BUFF];
|
||||
|
||||
static osThreadId_t thread_alert;
|
||||
static bool inited = false;
|
||||
|
||||
/* Private function -------------------------------------------------------- */
|
||||
/* USER FUNCTION BEGIN */
|
||||
|
||||
/* USER FUNCTION END */
|
||||
|
||||
static void BMI_WriteSingle(BMI_Device_t dv, uint8_t reg, uint8_t data) {
|
||||
buffer[0] = (reg & 0x7f);
|
||||
buffer[1] = data;
|
||||
|
||||
BSP_TIME_Delay(1);
|
||||
switch (dv) {
|
||||
case BMI_ACCL:
|
||||
BMI088_ACCL_NSS_RESET();
|
||||
break;
|
||||
|
||||
case BMI_GYRO:
|
||||
BMI088_GYRO_NSS_RESET();
|
||||
break;
|
||||
}
|
||||
|
||||
BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 2u, false);
|
||||
|
||||
switch (dv) {
|
||||
case BMI_ACCL:
|
||||
BMI088_ACCL_NSS_SET();
|
||||
break;
|
||||
|
||||
case BMI_GYRO:
|
||||
BMI088_GYRO_NSS_SET();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static uint8_t BMI_ReadSingle(BMI_Device_t dv, uint8_t reg) {
|
||||
BSP_TIME_Delay(1);
|
||||
switch (dv) {
|
||||
case BMI_ACCL:
|
||||
BMI088_ACCL_NSS_RESET();
|
||||
break;
|
||||
|
||||
case BMI_GYRO:
|
||||
BMI088_GYRO_NSS_RESET();
|
||||
break;
|
||||
}
|
||||
buffer[0] = (uint8_t)(reg | 0x80);
|
||||
BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false);
|
||||
BSP_SPI_Receive(BSP_SPI_BMI088, buffer, 2u, false);
|
||||
|
||||
switch (dv) {
|
||||
case BMI_ACCL:
|
||||
BMI088_ACCL_NSS_SET();
|
||||
return buffer[1];
|
||||
|
||||
case BMI_GYRO:
|
||||
BMI088_GYRO_NSS_SET();
|
||||
return buffer[0];
|
||||
}
|
||||
}
|
||||
|
||||
static void BMI_Read(BMI_Device_t dv, uint8_t reg, uint8_t *data, uint8_t len) {
|
||||
if (data == NULL) return;
|
||||
|
||||
switch (dv) {
|
||||
case BMI_ACCL:
|
||||
BMI088_ACCL_NSS_RESET();
|
||||
break;
|
||||
|
||||
case BMI_GYRO:
|
||||
BMI088_GYRO_NSS_RESET();
|
||||
break;
|
||||
}
|
||||
buffer[0] = (uint8_t)(reg | 0x80);
|
||||
BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false);
|
||||
BSP_SPI_Receive(BSP_SPI_BMI088, data, len, true);
|
||||
}
|
||||
|
||||
static void BMI088_RxCpltCallback(void) {
|
||||
if (BSP_GPIO_ReadPin(BSP_GPIO_ACCL_CS) == GPIO_PIN_RESET) {
|
||||
BMI088_ACCL_NSS_SET();
|
||||
osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_RAW_REDY);
|
||||
}
|
||||
if (BSP_GPIO_ReadPin(BSP_GPIO_GYRO_CS) == GPIO_PIN_RESET) {
|
||||
BMI088_GYRO_NSS_SET();
|
||||
osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_RAW_REDY);
|
||||
}
|
||||
}
|
||||
|
||||
static void BMI088_AcclIntCallback(void) {
|
||||
osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_NEW_DATA);
|
||||
}
|
||||
|
||||
static void BMI088_GyroIntCallback(void) {
|
||||
osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_NEW_DATA);
|
||||
}
|
||||
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali) {
|
||||
if (bmi088 == 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;
|
||||
|
||||
bmi088->cali = cali;
|
||||
|
||||
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_SOFTRESET, 0xB6);
|
||||
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_SOFTRESET, 0xB6);
|
||||
BSP_TIME_Delay(30);
|
||||
|
||||
/* Switch accl to SPI mode. */
|
||||
BMI_ReadSingle(BMI_ACCL, BMI088_CHIP_ID_ACCL);
|
||||
|
||||
if (BMI_ReadSingle(BMI_ACCL, BMI088_REG_ACCL_CHIP_ID) != BMI088_CHIP_ID_ACCL)
|
||||
return DEVICE_ERR_NO_DEV;
|
||||
|
||||
if (BMI_ReadSingle(BMI_GYRO, BMI088_REG_GYRO_CHIP_ID) != BMI088_CHIP_ID_GYRO)
|
||||
return DEVICE_ERR_NO_DEV;
|
||||
|
||||
BSP_GPIO_DisableIRQ(BSP_GPIO_ACCL_INT);
|
||||
BSP_GPIO_DisableIRQ(BSP_GPIO_GYRO_INT);
|
||||
|
||||
BSP_SPI_RegisterCallback(BSP_SPI_BMI088, BSP_SPI_RX_CPLT_CB,
|
||||
BMI088_RxCpltCallback);
|
||||
BSP_GPIO_RegisterCallback(BSP_GPIO_ACCL_INT, BMI088_AcclIntCallback);
|
||||
BSP_GPIO_RegisterCallback(BSP_GPIO_GYRO_INT, BMI088_GyroIntCallback);
|
||||
|
||||
/* Accl init. */
|
||||
/* Filter setting: Normal. */
|
||||
/* ODR: 0xAB: 800Hz. 0xAA: 400Hz. 0xA9: 200Hz. 0xA8: 100Hz. 0xA6: 25Hz. */
|
||||
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_CONF, 0xAA);
|
||||
|
||||
/* 0x00: +-3G. 0x01: +-6G. 0x02: +-12G. 0x03: +-24G. */
|
||||
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_RANGE, 0x01);
|
||||
|
||||
/* INT1 as output. Push-pull. Active low. Output. */
|
||||
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_IO_CONF, 0x08);
|
||||
|
||||
/* Map data ready interrupt to INT1. */
|
||||
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_INT2_MAP_DATA, 0x04);
|
||||
|
||||
/* Turn on accl. Now we can read data. */
|
||||
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_PWR_CTRL, 0x04);
|
||||
BSP_TIME_Delay(50);
|
||||
|
||||
/* Gyro init. */
|
||||
/* 0x00: +-2000. 0x01: +-1000. 0x02: +-500. 0x03: +-250. 0x04: +-125. */
|
||||
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_RANGE, 0x01);
|
||||
|
||||
/* Filter bw: 47Hz. */
|
||||
/* ODR: 0x02: 1000Hz. 0x03: 400Hz. 0x06: 200Hz. 0x07: 100Hz. */
|
||||
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_BANDWIDTH, 0x03);
|
||||
|
||||
/* INT3 and INT4 as output. Push-pull. Active low. */
|
||||
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_CONF, 0x00);
|
||||
|
||||
/* Map data ready interrupt to INT3. */
|
||||
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_MAP, 0x01);
|
||||
|
||||
/* Enable new data interrupt. */
|
||||
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT_CTRL, 0x80);
|
||||
|
||||
BSP_TIME_Delay(10);
|
||||
|
||||
inited = true;
|
||||
|
||||
BSP_GPIO_EnableIRQ(BSP_GPIO_ACCL_INT);
|
||||
BSP_GPIO_EnableIRQ(BSP_GPIO_GYRO_INT);
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
bool BMI088_GyroStable(AHRS_Gyro_t *gyro) {
|
||||
return ((gyro->x < 0.03f) && (gyro->y < 0.03f) && (gyro->z < 0.03f));
|
||||
}
|
||||
|
||||
uint32_t BMI088_WaitNew() {
|
||||
return osThreadFlagsWait(
|
||||
SIGNAL_BMI088_ACCL_NEW_DATA | SIGNAL_BMI088_GYRO_NEW_DATA, osFlagsWaitAll,
|
||||
osWaitForever);
|
||||
}
|
||||
|
||||
int8_t BMI088_AcclStartDmaRecv() {
|
||||
BMI_Read(BMI_ACCL, BMI088_REG_ACCL_X_LSB, bmi088_rxbuf, BMI088_LEN_RX_BUFF);
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
uint32_t BMI088_AcclWaitDmaCplt() {
|
||||
return osThreadFlagsWait(SIGNAL_BMI088_ACCL_RAW_REDY, osFlagsWaitAll,
|
||||
osWaitForever);
|
||||
}
|
||||
|
||||
int8_t BMI088_GyroStartDmaRecv() {
|
||||
BMI_Read(BMI_GYRO, BMI088_REG_GYRO_X_LSB, bmi088_rxbuf + 7, 6u);
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
uint32_t BMI088_GyroWaitDmaCplt() {
|
||||
return osThreadFlagsWait(SIGNAL_BMI088_GYRO_RAW_REDY, osFlagsWaitAll,
|
||||
osWaitForever);
|
||||
}
|
||||
|
||||
int8_t BMI088_ParseAccl(BMI088_t *bmi088) {
|
||||
if (bmi088 == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
#if 1
|
||||
int16_t raw_x, raw_y, raw_z;
|
||||
memcpy(&raw_x, bmi088_rxbuf + 1, sizeof(raw_x));
|
||||
memcpy(&raw_y, bmi088_rxbuf + 3, sizeof(raw_y));
|
||||
memcpy(&raw_z, bmi088_rxbuf + 5, sizeof(raw_z));
|
||||
|
||||
bmi088->accl.x = (float)raw_x;
|
||||
bmi088->accl.y = (float)raw_y;
|
||||
bmi088->accl.z = (float)raw_z;
|
||||
|
||||
#else
|
||||
const int16_t *praw_x = (int16_t *)(bmi088_rxbuf + 1);
|
||||
const int16_t *praw_y = (int16_t *)(bmi088_rxbuf + 3);
|
||||
const int16_t *praw_z = (int16_t *)(bmi088_rxbuf + 5);
|
||||
|
||||
bmi088->accl.x = (float)*praw_x;
|
||||
bmi088->accl.y = (float)*praw_y;
|
||||
bmi088->accl.z = (float)*praw_z;
|
||||
|
||||
#endif
|
||||
|
||||
/* 3G: 10920. 6G: 5460. 12G: 2730. 24G: 1365. */
|
||||
bmi088->accl.x /= 5460.0f;
|
||||
bmi088->accl.y /= 5460.0f;
|
||||
bmi088->accl.z /= 5460.0f;
|
||||
|
||||
int16_t raw_temp =
|
||||
(uint16_t)((bmi088_rxbuf[17] << 3) | (bmi088_rxbuf[18] >> 5));
|
||||
|
||||
if (raw_temp > 1023) raw_temp -= 2048;
|
||||
|
||||
bmi088->temp = (float)raw_temp * 0.125f + 23.0f;
|
||||
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
int8_t BMI088_ParseGyro(BMI088_t *bmi088) {
|
||||
if (bmi088 == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
#if 1
|
||||
/* Gyroscope imu_raw -> degrees/sec -> radians/sec */
|
||||
int16_t raw_x, raw_y, raw_z;
|
||||
memcpy(&raw_x, bmi088_rxbuf + 7, sizeof(raw_x));
|
||||
memcpy(&raw_y, bmi088_rxbuf + 9, sizeof(raw_y));
|
||||
memcpy(&raw_z, bmi088_rxbuf + 11, sizeof(raw_z));
|
||||
|
||||
bmi088->gyro.x = (float)raw_x;
|
||||
bmi088->gyro.y = (float)raw_y;
|
||||
bmi088->gyro.z = (float)raw_z;
|
||||
|
||||
#else
|
||||
/* Gyroscope imu_raw -> degrees/sec -> radians/sec */
|
||||
const int16_t *raw_x = (int16_t *)(bmi088_rxbuf + 7);
|
||||
const int16_t *raw_y = (int16_t *)(bmi088_rxbuf + 9);
|
||||
const int16_t *raw_z = (int16_t *)(bmi088_rxbuf + 11);
|
||||
|
||||
bmi088->gyro.x = (float)*raw_x;
|
||||
bmi088->gyro.y = (float)*raw_y;
|
||||
bmi088->gyro.z = (float)*raw_z;
|
||||
#endif
|
||||
|
||||
/* FS125: 262.144. FS250: 131.072. FS500: 65.536. FS1000: 32.768.
|
||||
* FS2000: 16.384.*/
|
||||
bmi088->gyro.x /= 32.768f;
|
||||
bmi088->gyro.y /= 32.768f;
|
||||
bmi088->gyro.z /= 32.768f;
|
||||
|
||||
bmi088->gyro.x *= M_DEG2RAD_MULT;
|
||||
bmi088->gyro.y *= M_DEG2RAD_MULT;
|
||||
bmi088->gyro.z *= M_DEG2RAD_MULT;
|
||||
|
||||
bmi088->gyro.x -= bmi088->cali->gyro_offset.x;
|
||||
bmi088->gyro.y -= bmi088->cali->gyro_offset.y;
|
||||
bmi088->gyro.z -= bmi088->cali->gyro_offset.z;
|
||||
|
||||
return DEVICE_ERR_NULL;
|
||||
}
|
||||
|
||||
float BMI088_GetUpdateFreq(BMI088_t *bmi088) {
|
||||
(void)bmi088;
|
||||
return 400.0f;
|
||||
}
|
||||
@ -1,81 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "component/ahrs.h"
|
||||
#include "device/device.h"
|
||||
|
||||
/* USER INCLUDE BEGIN */
|
||||
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* USER DEFINE BEGIN */
|
||||
|
||||
/* USER DEFINE END */
|
||||
|
||||
/* Exported constants ------------------------------------------------------- */
|
||||
/* Exported macro ----------------------------------------------------------- */
|
||||
/* Exported types ----------------------------------------------------------- */
|
||||
typedef struct {
|
||||
struct {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
} gyro_offset; /* 陀螺仪偏置 */
|
||||
} BMI088_Cali_t; /* BMI088校准数据 */
|
||||
|
||||
typedef struct {
|
||||
DEVICE_Header_t header;
|
||||
AHRS_Accl_t accl;
|
||||
AHRS_Gyro_t gyro;
|
||||
|
||||
float temp; /* 温度 */
|
||||
|
||||
const BMI088_Cali_t *cali;
|
||||
} BMI088_t;
|
||||
|
||||
/* USER STRUCT BEGIN */
|
||||
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali);
|
||||
int8_t BMI088_Restart(void);
|
||||
|
||||
bool BMI088_GyroStable(AHRS_Gyro_t *gyro);
|
||||
|
||||
/* Sensor use right-handed coordinate system. */
|
||||
/*
|
||||
x < R(logo)
|
||||
y
|
||||
UP is z
|
||||
All implementation should follow this rule.
|
||||
*/
|
||||
uint32_t BMI088_WaitNew();
|
||||
|
||||
/*
|
||||
BMI088的Accl和Gyro共用同一个DMA通道，所以一次只能读一个传感器。
|
||||
即BMI088_AcclStartDmaRecv() 和 BMI088_AcclWaitDmaCplt() 中间不能
|
||||
出现 BMI088_GyroStartDmaRecv()。
|
||||
*/
|
||||
int8_t BMI088_AcclStartDmaRecv();
|
||||
uint32_t BMI088_AcclWaitDmaCplt();
|
||||
int8_t BMI088_GyroStartDmaRecv();
|
||||
uint32_t BMI088_GyroWaitDmaCplt();
|
||||
int8_t BMI088_ParseAccl(BMI088_t *bmi088);
|
||||
int8_t BMI088_ParseGyro(BMI088_t *bmi088);
|
||||
float BMI088_GetUpdateFreq(BMI088_t *bmi088);
|
||||
|
||||
/* USER FUNCTION BEGIN */
|
||||
|
||||
/* USER FUNCTION END */
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
@ -22,14 +22,7 @@ extern "C" {
|
||||
#define DEVICE_ERR_NO_DEV (-4)
|
||||
|
||||
/* AUTO GENERATED SIGNALS BEGIN */
|
||||
<<<<<<< HEAD
|
||||
#define SIGNAL_BMI088_ACCL_RAW_REDY (1u << 0)
|
||||
#define SIGNAL_BMI088_GYRO_RAW_REDY (1u << 1)
|
||||
#define SIGNAL_BMI088_ACCL_NEW_DATA (1u << 2)
|
||||
#define SIGNAL_BMI088_GYRO_NEW_DATA (1u << 3)
|
||||
=======
|
||||
#define SIGNAL_DR16_RAW_REDY (1u << 0)
|
||||
>>>>>>> main
|
||||
/* AUTO GENERATED SIGNALS END */
|
||||
|
||||
/* USER SIGNALS BEGIN */
|
||||
|
||||
@ -1,27 +1,14 @@
|
||||
<<<<<<< HEAD
|
||||
bmi088:
|
||||
bsp_config:
|
||||
BSP_GPIO_ACCL_CS: BSP_GPIO_ACCL_CS
|
||||
BSP_GPIO_ACCL_INT: BSP_GPIO_ACCL_INT
|
||||
BSP_GPIO_GYRO_CS: BSP_GPIO_GYRO_CS
|
||||
BSP_GPIO_GYRO_INT: BSP_GPIO_GYRO_INT
|
||||
BSP_SPI_BMI088: BSP_SPI_BMI088
|
||||
=======
|
||||
buzzer:
|
||||
bsp_config:
|
||||
BSP_PWM_BUZZER: BSP_PWM_TIM8_CH1
|
||||
>>>>>>> main
|
||||
enabled: true
|
||||
dm_imu:
|
||||
bsp_config: {}
|
||||
enabled: true
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
dr16:
|
||||
bsp_config:
|
||||
BSP_UART_DR16: BSP_UART_DR16
|
||||
enabled: true
|
||||
>>>>>>> main
|
||||
motor:
|
||||
bsp_config: {}
|
||||
enabled: true
|
||||
@ -31,9 +18,6 @@ motor_lk:
|
||||
motor_lz:
|
||||
bsp_config: {}
|
||||
enabled: true
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
motor_rm:
|
||||
bsp_config: {}
|
||||
enabled: true
|
||||
>>>>>>> main
|
||||
|
||||
@ -253,10 +253,7 @@ int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) {
|
||||
tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
|
||||
tx_frame.data[6] = 0x00;
|
||||
tx_frame.data[7] = 0x00;
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲
|
||||
>>>>>>> main
|
||||
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
|
||||
}
|
||||
|
||||
@ -282,10 +279,7 @@ int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) {
|
||||
tx_frame.data[5] = 0x00;
|
||||
tx_frame.data[6] = 0x00;
|
||||
tx_frame.data[7] = 0x00;
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲
|
||||
>>>>>>> main
|
||||
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
|
||||
}
|
||||
|
||||
@ -305,10 +299,7 @@ int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) {
|
||||
tx_frame.data[5] = 0x00;
|
||||
tx_frame.data[6] = 0x00;
|
||||
tx_frame.data[7] = 0x00;
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲
|
||||
>>>>>>> main
|
||||
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
|
||||
}
|
||||
|
||||
|
||||
@ -134,10 +134,7 @@ static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *dat
|
||||
} else {
|
||||
memset(tx_frame.data, 0, dlc);
|
||||
}
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
BSP_CAN_WaitTxMailboxEmpty(can, 1); // 等待发送邮箱空闲
|
||||
>>>>>>> main
|
||||
return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
|
||||
}
|
||||
|
||||
@ -247,8 +244,6 @@ int8_t MOTOR_LZ_Init(void) {
|
||||
return BSP_CAN_RegisterIdParser(MOTOR_LZ_IdParser) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
|
||||
}
|
||||
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
/**
|
||||
* @brief 反初始化灵足电机驱动系统
|
||||
* @return 设备状态码
|
||||
@ -257,7 +252,6 @@ int8_t MOTOR_LZ_DeInit(void) {
|
||||
// 注销ID解析器
|
||||
return BSP_CAN_UnregisterIdParser() == BSP_OK ? DEVICE_OK : DEVICE_ERR;
|
||||
}
|
||||
>>>>>>> main
|
||||
|
||||
int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) {
|
||||
if (param == NULL) return DEVICE_ERR_NULL;
|
||||
@ -375,10 +369,7 @@ int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *m
|
||||
uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(send_param.kd, LZ_KD_MAX);
|
||||
data[6] = (raw_kd >> 8) & 0xFF;
|
||||
data[7] = raw_kd & 0xFF;
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲
|
||||
>>>>>>> main
|
||||
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
|
||||
}
|
||||
|
||||
@ -387,10 +378,7 @@ int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param) {
|
||||
if (param == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
// 构建扩展ID - 使能命令
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
// 格式: 0x300FF7X, 其中X是motor_id的低4位
|
||||
>>>>>>> main
|
||||
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_ENABLE, param->host_id, param->motor_id);
|
||||
|
||||
// 数据区清零
|
||||
|
||||
@ -112,15 +112,12 @@ typedef struct {
|
||||
int8_t MOTOR_LZ_Init(void);
|
||||
|
||||
/**
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
* @brief 反初始化灵足电机驱动系统
|
||||
* @return 设备状态码
|
||||
*/
|
||||
int8_t MOTOR_LZ_DeInit(void);
|
||||
|
||||
/**
|
||||
>>>>>>> main
|
||||
* @brief 注册一个灵足电机
|
||||
* @param param 电机参数
|
||||
* @return 设备状态码
|
||||
|
||||
@ -93,60 +93,46 @@ int8_t RC_CAN_SendData(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type) {
|
||||
}
|
||||
BSP_CAN_StdDataFrame_t frame;
|
||||
frame.dlc = 8;
|
||||
// 边界裁剪宏
|
||||
#define RC_CAN_CLAMP(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
|
||||
switch (data_type) {
|
||||
case RC_CAN_DATA_JOYSTICK: {
|
||||
case RC_CAN_DATA_JOYSTICK:
|
||||
frame.id = rc_can->param.joy_id;
|
||||
float l_x = RC_CAN_CLAMP(rc_can->data.joy.ch_l_x, -0.999969f, 0.999969f);
|
||||
float l_y = RC_CAN_CLAMP(rc_can->data.joy.ch_l_y, -0.999969f, 0.999969f);
|
||||
float r_x = RC_CAN_CLAMP(rc_can->data.joy.ch_r_x, -0.999969f, 0.999969f);
|
||||
float r_y = RC_CAN_CLAMP(rc_can->data.joy.ch_r_y, -0.999969f, 0.999969f);
|
||||
int16_t l_x_i = (int16_t)(l_x * 32768.0f);
|
||||
int16_t l_y_i = (int16_t)(l_y * 32768.0f);
|
||||
int16_t r_x_i = (int16_t)(r_x * 32768.0f);
|
||||
int16_t r_y_i = (int16_t)(r_y * 32768.0f);
|
||||
frame.data[0] = (uint8_t)(l_x_i & 0xFF);
|
||||
frame.data[1] = (uint8_t)((l_x_i >> 8) & 0xFF);
|
||||
frame.data[2] = (uint8_t)(l_y_i & 0xFF);
|
||||
frame.data[3] = (uint8_t)((l_y_i >> 8) & 0xFF);
|
||||
frame.data[4] = (uint8_t)(r_x_i & 0xFF);
|
||||
frame.data[5] = (uint8_t)((r_x_i >> 8) & 0xFF);
|
||||
frame.data[6] = (uint8_t)(r_y_i & 0xFF);
|
||||
frame.data[7] = (uint8_t)((r_y_i >> 8) & 0xFF);
|
||||
frame.data[0] = (uint8_t)((int16_t)(rc_can->data.joy.ch_l_x * 32768.0f) & 0xFF);
|
||||
frame.data[1] =
|
||||
(uint8_t)(((int16_t)(rc_can->data.joy.ch_l_x * 32768.0f) >> 8) & 0xFF);
|
||||
frame.data[2] = (uint8_t)((int16_t)(rc_can->data.joy.ch_l_y * 32768.0f) & 0xFF);
|
||||
frame.data[3] =
|
||||
(uint8_t)(((int16_t)(rc_can->data.joy.ch_l_y * 32768.0f) >> 8) & 0xFF);
|
||||
frame.data[4] = (uint8_t)((int16_t)(rc_can->data.joy.ch_r_x * 32768.0f) & 0xFF);
|
||||
frame.data[5] =
|
||||
(uint8_t)(((int16_t)(rc_can->data.joy.ch_r_x * 32768.0f) >> 8) & 0xFF);
|
||||
frame.data[6] = (uint8_t)((int16_t)(rc_can->data.joy.ch_r_y * 32768.0f) & 0xFF);
|
||||
frame.data[7] =
|
||||
(uint8_t)(((int16_t)(rc_can->data.joy.ch_r_y * 32768.0f) >> 8) & 0xFF);
|
||||
break;
|
||||
}
|
||||
case RC_CAN_DATA_SWITCH: {
|
||||
case RC_CAN_DATA_SWITCH:
|
||||
frame.id = rc_can->param.sw_id;
|
||||
frame.data[0] = (uint8_t)(rc_can->data.sw.sw_l);
|
||||
frame.data[1] = (uint8_t)(rc_can->data.sw.sw_r);
|
||||
float ch_res = RC_CAN_CLAMP(rc_can->data.sw.ch_res, -0.999969f, 0.999969f);
|
||||
int16_t ch_res_i = (int16_t)(ch_res * 32768.0f);
|
||||
frame.data[2] = (uint8_t)(ch_res_i & 0xFF);
|
||||
frame.data[3] = (uint8_t)((ch_res_i >> 8) & 0xFF);
|
||||
frame.data[2] = (uint8_t)((int16_t)(rc_can->data.sw.ch_res * 32768.0f) & 0xFF);
|
||||
frame.data[3] =
|
||||
(uint8_t)(((int16_t)(rc_can->data.sw.ch_res * 32768.0f) >> 8) & 0xFF);
|
||||
frame.data[4] = 0; // 保留字节
|
||||
frame.data[5] = 0; // 保留字节
|
||||
frame.data[6] = 0; // 保留字节
|
||||
frame.data[7] = 0; // 保留字节
|
||||
break;
|
||||
}
|
||||
case RC_CAN_DATA_MOUSE: {
|
||||
case RC_CAN_DATA_MOUSE:
|
||||
frame.id = rc_can->param.mouse_id;
|
||||
// 鼠标x/y/z一般为增量，若有极限也可加clamp
|
||||
int16_t x = (int16_t)(rc_can->data.mouse.x);
|
||||
int16_t y = (int16_t)(rc_can->data.mouse.y);
|
||||
int16_t z = (int16_t)(rc_can->data.mouse.z);
|
||||
frame.data[0] = (uint8_t)(x & 0xFF);
|
||||
frame.data[1] = (uint8_t)((x >> 8) & 0xFF);
|
||||
frame.data[2] = (uint8_t)(y & 0xFF);
|
||||
frame.data[3] = (uint8_t)((y >> 8) & 0xFF);
|
||||
frame.data[4] = (uint8_t)(z & 0xFF);
|
||||
frame.data[5] = (uint8_t)((z >> 8) & 0xFF);
|
||||
frame.data[0] = (uint8_t)((int16_t)(rc_can->data.mouse.x) & 0xFF);
|
||||
frame.data[1] = (uint8_t)(((int16_t)(rc_can->data.mouse.x) >> 8) & 0xFF);
|
||||
frame.data[2] = (uint8_t)((int16_t)(rc_can->data.mouse.y) & 0xFF);
|
||||
frame.data[3] = (uint8_t)(((int16_t)(rc_can->data.mouse.y) >> 8) & 0xFF);
|
||||
frame.data[4] = (uint8_t)((int16_t)(rc_can->data.mouse.z) & 0xFF);
|
||||
frame.data[5] = (uint8_t)(((int16_t)(rc_can->data.mouse.z) >> 8) & 0xFF);
|
||||
frame.data[6] = (uint8_t)(rc_can->data.mouse.mouse_l ? 1 : 0);
|
||||
frame.data[7] = (uint8_t)(rc_can->data.mouse.mouse_r ? 1 : 0);
|
||||
break;
|
||||
}
|
||||
case RC_CAN_DATA_KEYBOARD: {
|
||||
case RC_CAN_DATA_KEYBOARD:
|
||||
frame.id = rc_can->param.keyboard_id;
|
||||
frame.data[0] = (uint8_t)(rc_can->data.keyboard.key_value & 0xFF);
|
||||
frame.data[1] = (uint8_t)((rc_can->data.keyboard.key_value >> 8) & 0xFF);
|
||||
@ -154,11 +140,9 @@ int8_t RC_CAN_SendData(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type) {
|
||||
frame.data[2 + i] = (i < 6) ? (uint8_t)(rc_can->data.keyboard.keys[i]) : 0;
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
#undef RC_CAN_CLAMP
|
||||
if (BSP_CAN_Transmit(rc_can->param.can, BSP_CAN_FORMAT_STD_DATA, frame.id,
|
||||
frame.data, frame.dlc) != BSP_OK) {
|
||||
return DEVICE_ERR;
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -1,71 +1,13 @@
|
||||
<<<<<<< HEAD
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
=======
|
||||
/*
|
||||
* 平衡底盘模组
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
>>>>>>> main
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
<<<<<<< HEAD
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "device/motor.h"
|
||||
#include "device/motor_lz.h"
|
||||
#include "device/motor_lk.h"
|
||||
#include "device/device.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* USER DEFINE BEGIN */
|
||||
|
||||
/* USER DEFINE END */
|
||||
|
||||
/* Exported constants ------------------------------------------------------- */
|
||||
/* Exported macro ----------------------------------------------------------- */
|
||||
/* Exported types ----------------------------------------------------------- */
|
||||
typedef struct {
|
||||
MOTOR_LZ_Param_t joint_param[4]; // 4个电机参数
|
||||
MOTOR_LK_Param_t wheel_param[2]; // 2个轮子电机参数
|
||||
} Chassis_Param_t;
|
||||
|
||||
typedef struct {
|
||||
MOTOR_Feedback_t joint[4]; // 4个电机反馈数据
|
||||
MOTOR_Feedback_t wheel[2]; // 2个轮子电机反馈数据
|
||||
} Chassis_Feedback_t;
|
||||
|
||||
typedef struct {
|
||||
float joint[4]; // 4个电机反馈数据
|
||||
float wheel[2]; // 2个轮子电机反馈数据
|
||||
} Chassis_Output_t;
|
||||
|
||||
typedef struct {
|
||||
Chassis_Param_t param; // 底盘参数配置
|
||||
Chassis_Feedback_t data; // 底盘反馈数据
|
||||
Chassis_Output_t output; // 底盘输出数据
|
||||
} Chassis_t;
|
||||
|
||||
/* USER STRUCT BEGIN */
|
||||
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
int8_t Chassis_Init(Chassis_t *chassis, Chassis_Param_t *param);
|
||||
int8_t Chassis_Update(Chassis_t *chassis);
|
||||
int8_t Chassis_Enable(Chassis_t *chassis);
|
||||
int8_t Chassis_Relax(Chassis_t *chassis);
|
||||
int8_t Chassis_Ctrl(Chassis_t *chassis);
|
||||
/* USER FUNCTION BEGIN */
|
||||
|
||||
/* USER FUNCTION END */
|
||||
=======
|
||||
// Front
|
||||
// | 1 4 |
|
||||
// (1)Left| |Right(2)
|
||||
@ -82,7 +24,6 @@ int8_t Chassis_Ctrl(Chassis_t *chassis);
|
||||
#include "device/motor.h"
|
||||
#include "device/motor_lk.h"
|
||||
#include "device/motor_lz.h"
|
||||
#include "device/motor_dm.h"
|
||||
/* Exported constants ------------------------------------------------------- */
|
||||
#define CHASSIS_OK (0) /* 运行正常 */
|
||||
#define CHASSIS_ERR (-1) /* 运行时发现了其他错误 */
|
||||
@ -115,8 +56,8 @@ typedef struct {
|
||||
typedef struct {
|
||||
MOTOR_Feedback_t joint[4]; /* 四个关节电机反馈 */
|
||||
MOTOR_Feedback_t wheel[2]; /* 两个轮子电机反馈 */
|
||||
MOTOR_Feedback_t yaw; /* 云台Yaw轴电机反馈 */
|
||||
Chassis_IMU_t imu; /* IMU数据 */
|
||||
MOTOR_Feedback_t yaw; /* 云台Yaw轴电机反馈 */
|
||||
}Chassis_Feedback_t;
|
||||
|
||||
typedef struct {
|
||||
@ -130,7 +71,7 @@ typedef struct {
|
||||
|
||||
MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */
|
||||
MOTOR_LK_Param_t wheel_motors[2]; /* 两个轮子电机参数 */
|
||||
MOTOR_DM_Param_t yaw_motor; /* 云台Yaw轴电机参数 */
|
||||
|
||||
VMC_Param_t vmc_param[2]; /* VMC参数 */
|
||||
LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
|
||||
|
||||
@ -175,7 +116,7 @@ typedef struct {
|
||||
Chassis_Output_t output;
|
||||
|
||||
VMC_t vmc_[2]; /* 两条腿的VMC */
|
||||
LQR_t lqr[2]; /* 两条腿的LQR控制器 */
|
||||
LQR_Controller_t lqr[2]; /* 两条腿的LQR控制器 */
|
||||
|
||||
int8_t state;
|
||||
|
||||
@ -277,8 +218,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
|
||||
* \param s 包含底盘数据的结构体
|
||||
* \param out CAN设备底盘输出结构体
|
||||
*/
|
||||
int8_t Chassis_Output(Chassis_t *c);
|
||||
>>>>>>> main
|
||||
void Chassis_Output(Chassis_t *c);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
@ -5,11 +5,7 @@
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "module/config.h"
|
||||
#include "bsp/can.h"
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
#include "device/motor_dm.h"
|
||||
#include "device/rc_can.h"
|
||||
>>>>>>> main
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
@ -21,13 +17,6 @@
|
||||
// 机器人参数配置
|
||||
Config_RobotParam_t robot_config = {
|
||||
|
||||
<<<<<<< HEAD
|
||||
.joint_param =
|
||||
{
|
||||
{
|
||||
// 左髋关节
|
||||
.can = BSP_CAN_2,
|
||||
=======
|
||||
.imu_param = {
|
||||
.can = BSP_CAN_2,
|
||||
.can_id = 0x6FF,
|
||||
@ -38,9 +27,9 @@ Config_RobotParam_t robot_config = {
|
||||
.chassis_param = {
|
||||
.yaw={
|
||||
.k=1.0f,
|
||||
.p=1.2f,
|
||||
.p=1.0f,
|
||||
.i=0.0f,
|
||||
.d=0.05f,
|
||||
.d=0.0f,
|
||||
.i_limit=0.0f,
|
||||
.out_limit=1.0f,
|
||||
.d_cutoff_freq=30.0f,
|
||||
@ -98,49 +87,30 @@ Config_RobotParam_t robot_config = {
|
||||
.joint_motors = {
|
||||
{ // 左髋关节
|
||||
.can = BSP_CAN_1,
|
||||
>>>>>>> main
|
||||
.motor_id = 124,
|
||||
.host_id = 0xFF,
|
||||
.module = MOTOR_LZ_RSO3,
|
||||
.reverse = false,
|
||||
.mode = MOTOR_LZ_MODE_MOTION,
|
||||
},
|
||||
<<<<<<< HEAD
|
||||
{
|
||||
// 左膝关节
|
||||
.can = BSP_CAN_2,
|
||||
=======
|
||||
{ // 左膝关节
|
||||
.can = BSP_CAN_1,
|
||||
>>>>>>> main
|
||||
.motor_id = 125,
|
||||
.host_id = 0xFF,
|
||||
.module = MOTOR_LZ_RSO3,
|
||||
.reverse = false,
|
||||
.mode = MOTOR_LZ_MODE_MOTION,
|
||||
},
|
||||
<<<<<<< HEAD
|
||||
{
|
||||
// 右膝关节
|
||||
.can = BSP_CAN_2,
|
||||
=======
|
||||
{ // 右膝关节
|
||||
.can = BSP_CAN_1,
|
||||
>>>>>>> main
|
||||
.motor_id = 126,
|
||||
.host_id = 0xFF,
|
||||
.module = MOTOR_LZ_RSO3,
|
||||
.reverse = true,
|
||||
.mode = MOTOR_LZ_MODE_MOTION,
|
||||
},
|
||||
<<<<<<< HEAD
|
||||
{
|
||||
// 右髋关节
|
||||
.can = BSP_CAN_2,
|
||||
=======
|
||||
{ // 右髋关节
|
||||
.can = BSP_CAN_1,
|
||||
>>>>>>> main
|
||||
.motor_id = 127,
|
||||
.host_id = 0xFF,
|
||||
.module = MOTOR_LZ_RSO3,
|
||||
@ -148,21 +118,6 @@ Config_RobotParam_t robot_config = {
|
||||
.mode = MOTOR_LZ_MODE_MOTION,
|
||||
},
|
||||
},
|
||||
<<<<<<< HEAD
|
||||
.wheel_param =
|
||||
{
|
||||
{
|
||||
// 左轮
|
||||
.can = BSP_CAN_2,
|
||||
.id = 0x141, // LK电机ID
|
||||
.module = MOTOR_LK_MF9025,
|
||||
.reverse = false,
|
||||
},
|
||||
{
|
||||
// 右轮
|
||||
.can = BSP_CAN_2,
|
||||
.id = 0x142, // LK电机ID
|
||||
=======
|
||||
.wheel_motors = {
|
||||
{ // 左轮电机
|
||||
.can = BSP_CAN_1,
|
||||
@ -173,22 +128,11 @@ Config_RobotParam_t robot_config = {
|
||||
{ // 右轮电机
|
||||
.can = BSP_CAN_1,
|
||||
.id = 0x142,
|
||||
>>>>>>> main
|
||||
.module = MOTOR_LK_MF9025,
|
||||
.reverse = true,
|
||||
},
|
||||
},
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
.yaw_motor = { // 云台Yaw轴电机
|
||||
.can = BSP_CAN_2,
|
||||
.can_id = 0x1,
|
||||
.master_id = 0x11,
|
||||
.module = MOTOR_DM_J4310,
|
||||
.reverse = false,
|
||||
},
|
||||
|
||||
.mech_zero_yaw = 1.20564249f, /* 250.5度，机械零点 */
|
||||
.mech_zero_yaw = 0.0f,
|
||||
|
||||
.vmc_param = {
|
||||
{ // 左腿
|
||||
@ -206,20 +150,60 @@ Config_RobotParam_t robot_config = {
|
||||
.hip_length = 0.0f // 髋宽 (L5) (m)
|
||||
}
|
||||
},
|
||||
.lqr_gains = {
|
||||
.k11_coeff = { -1.508572585852218e+02f, 1.764949368139731e+02f, -9.850368126414553e+01f, -1.786139736968997e+00f }, // theta
|
||||
.k12_coeff = { 6.466280284100411e+00f, -6.582699834130516e+00f, -7.131858380770703e+00f, -2.414590752579311e-01f }, // d_theta
|
||||
.k13_coeff = { -7.182568574598301e+01f, 7.405968297046749e+01f, -2.690651220502175e+01f, -1.029850390463813e-01f }, // x
|
||||
.k14_coeff = { -7.645548919162933e+01f, 7.988837668034076e+01f, -3.105733981791483e+01f, -4.296847184537235e-01f }, // d_x
|
||||
.k15_coeff = { -9.403058188674812e+00f, 2.314767704216332e+01f, -1.651965553704901e+01f, 3.907902082528794e+00f }, // phi
|
||||
.k16_coeff = { -4.023111056381187e+00f, 6.154951770170482e+00f, -3.705537084098432e+00f, 8.264904615264155e-01f }, // d_phi
|
||||
.k21_coeff = { 1.254775776629822e+02f, -8.971732439896309e+01f, 4.744038360386896e+00f, 1.088353622361175e+01f }, // theta
|
||||
.k22_coeff = { 1.061139172689013e+01f, -1.011235824540459e+01f, 3.034478775177782e+00f, 1.254920921163464e+00f }, // d_theta
|
||||
.k23_coeff = { -2.675556963667067e+01f, 4.511381902505539e+01f, -2.800741296230217e+01f, 7.647205920058282e+00f }, // x
|
||||
.k24_coeff = { -4.067721095665326e+01f, 6.068996620706580e+01f, -3.488384556019462e+01f, 9.374136714284193e+00f }, // d_x
|
||||
.k25_coeff = { 7.359316334738203e+01f, -7.896223244854855e+01f, 2.961892943386949e+01f, 4.406632136721399e+00f }, // phi
|
||||
.k26_coeff = { 1.624843000878248e+01f, -1.680831323767654e+01f, 6.018754311678180e+00f, 2.337719500754984e-01f }, // d_phi
|
||||
},
|
||||
.lqr_gains ={
|
||||
// .k11_coeff = { -61.932040681074966f, 70.963671642086396f, -37.730841182566571f, -0.296475458388679f }, // theta
|
||||
// .k12_coeff = { -0.586710094600108f, 0.886736272521581f, -3.626144273475104f, 0.057861910518974f }, // d_theta
|
||||
// .k13_coeff = { -17.297031110481019f, 16.286794934166178f, -5.176451154477850f, -0.867260018374823f }, // x
|
||||
// .k14_coeff = { -14.893387150006664f, 14.357020815277332f, -5.244645181873441f, -0.869862096507486f}, // d_x
|
||||
// .k15_coeff = { -75.327876471378886f, 79.786699741548944f, -31.183500053811208f, 5.450635661115236f}, // phi
|
||||
// .k16_coeff = { -3.572234723237025f, 4.164905011076312f, -1.874828497771750f, 0.477913222933688f}, // d_phi
|
||||
// .k21_coeff = { 9.327090608559319f, 1.362675928111105f, -8.092777598567881f, 4.351387652359104f}, // theta
|
||||
// .k22_coeff = { 3.872517778351810f, -3.344077414726880f, 0.589693555828904f, 0.310036629174646f}, // d_theta
|
||||
// .k23_coeff = { -71.615766251649134f, 74.748309711530837f, -28.370490124006626f, 4.483586941100197f }, // x
|
||||
// .k24_coeff = { -68.751866288568962f, 71.204785013044102f, -26.812636604518396f, 4.238479323700952f }, // d_x
|
||||
// .k25_coeff = { 205.6887659080132f, -195.1219729060621f, 62.890188701113303f, 7.452313695653162f }, // phi
|
||||
// .k26_coeff = { 16.162999842656344f, -15.926932704437410f, 5.474613395300429f, -0.002856083635449f }, // d_phi
|
||||
|
||||
|
||||
// .k11_coeff = {-143.1550,156.1754,-98.5282,-11.3781}, // theta
|
||||
// .k12_coeff = {8.3196,-12.4161,-8.3805,-1.6368}, // d_theta
|
||||
// .k13_coeff = {-69.6189,68.5619,-23.3079,-4.1736}, // x
|
||||
// .k14_coeff = {-58.4944,58.2204,-22.8021,-5.2361}, // d_x
|
||||
// .k15_coeff = {-29.6753,40.9947,-20.1188,2.5142}, // phi
|
||||
// .k16_coeff = {-13.1787,15.8361,-7.4061,1.1193}, // d_phi
|
||||
// .k21_coeff = {-76.5141,124.3258,-73.4908,22.0942}, // theta
|
||||
// .k22_coeff = {-9.1845,12.7284,-5.8169,2.8659}, // d_theta
|
||||
// .k23_coeff = {-157.6244,179.9415,-77.2161,14.9075}, // x
|
||||
// .k24_coeff = {-180.5666,202.7656,-85.2869,16.4847}, // d_x
|
||||
// .k25_coeff = {14.3596,-14.2125,4.1210,24.1729}, // phi
|
||||
// .k26_coeff = {20.5751,-19.8014,6.3128,2.8044}, // d_phi
|
||||
.k11_coeff = { -2.046396270532116e+02f, 2.283572275397276e+02f, -99.051642997946473f, 2.549835715107331f }, // theta
|
||||
.k12_coeff = { 0.689999868157742f, 2.004516582917084f, -5.904779142191341f, 0.331840642644740f }, // d_theta
|
||||
.k13_coeff = { -59.058694050901643f, 59.363082825119605f, -20.603451414220757f, 1.137708603718630f }, // x
|
||||
.k14_coeff = { -64.283929532305166f, 65.138737687498519f, -23.323482861600581f, 1.257945614978053f }, // d_x
|
||||
.k15_coeff = { -15.125353856795936f, 34.329224759247211f, -22.500683150450474f, 5.036897782323629f }, // phi
|
||||
.k16_coeff = { 2.707768649521343f, 0.390393176362524f, -2.018231845635338f, 0.697604163191230f }, // d_phi
|
||||
.k21_coeff = { 4.135329288854244e+02f, -3.173913574866715e+02f, 50.321199991176265f, 10.217217753280829f }, // theta
|
||||
.k22_coeff = { 48.042446261620519f, -45.292268634116219f, 13.273044296221686f, 0.006982339078710f }, // d_theta
|
||||
.k23_coeff = { 14.015246608117772f, 10.813301135732024f, -18.216050987625373f, 6.078912501009629f }, // x
|
||||
.k24_coeff = { 21.698411755946285f, 5.621435092936538f, -18.016608013978576f, 6.611542756343175f }, // d_x
|
||||
.k25_coeff = { 4.798120071828828e+02f, -4.728222224549831e+02f, 1.591181202824602e+02f, -6.421997865768036f }, // phi
|
||||
.k26_coeff = { 59.794709871063546f, -60.418062715734166f, 20.991263455753383f, -1.388418937916963f }, // d_phi
|
||||
|
||||
// .k11_coeff = { -1.996305368054721e+02f, 2.260001266392926e+02f, -1.009632659573521e+02f, 2.651403223110949e+00f }, // theta
|
||||
// .k12_coeff = { 1.961704292162323e+00f, 8.251913348469651e-01f, -6.073749575127879e+00f, 3.535645826465822e-01f }, // d_theta
|
||||
// .k13_coeff = { -8.161081261310467e+01f, 8.274264960693915e+01f, -2.904800049859091e+01f, 1.653742354439720e+00f }, // x
|
||||
// .k14_coeff = { -7.455421501651551e+01f, 7.622235638964226e+01f, -2.773307975245958e+01f, 1.535810571728176e+00f }, // d_x
|
||||
// .k15_coeff = { -9.027722377713712e+00f, 2.891936105315331e+01f, -2.106785573613789e+01f, 4.948383450314285e+00f }, // phi
|
||||
// .k16_coeff = { 3.049839709048039e+00f, 4.627952499276505e-02f, -1.922806522134511e+00f, 6.902507101472589e-01f }, // d_phi
|
||||
// .k21_coeff = { 4.373445515700652e+02f, -3.391497363529634e+02f, 5.569210421652366e+01f, 1.081229141610274e+01f }, // theta
|
||||
// .k22_coeff = { 5.284103137531328e+01f, -5.028796043573002e+01f, 1.502532265509508e+01f, -3.077550945526411e-02f }, // d_theta
|
||||
// .k23_coeff = { 3.111787495894651e+01f, 4.872956811853720e+00f, -2.278008499711769e+01f, 8.426190283284837e+00f }, // x
|
||||
// .k24_coeff = { 3.329235562185018e+01f, -5.071568882184936e-01f, -1.928141689016582e+01f, 7.776043600153916e+00f }, // d_x
|
||||
// .k25_coeff = { 4.604866240014122e+02f, -4.577537299814802e+02f, 1.557742271762380e+02f, -6.272551369660446e+00f }, // phi
|
||||
// .k26_coeff = { 5.608646259671771e+01f, -5.728239809509379e+01f, 2.016452460533993e+01f, -1.320817827839268e+00f }, // d_phi
|
||||
|
||||
},
|
||||
.lqr_param.max_joint_torque = 20.0f, // 关节电机最大力矩 20Nm
|
||||
.lqr_param.max_wheel_torque = 4.5f, // 轮毂电机最大力矩 2.5Nm
|
||||
},
|
||||
@ -232,7 +216,6 @@ Config_RobotParam_t robot_config = {
|
||||
.mouse_id = 0x252,
|
||||
.keyboard_id = 0x253,
|
||||
},
|
||||
>>>>>>> main
|
||||
};
|
||||
|
||||
/* Private function prototypes ---------------------------------------------- */
|
||||
@ -242,10 +225,6 @@ Config_RobotParam_t robot_config = {
|
||||
* @brief 获取机器人配置参数
|
||||
* @return 机器人配置参数指针
|
||||
*/
|
||||
<<<<<<< HEAD
|
||||
Config_RobotParam_t *Config_GetRobotParam(void) { return &robot_config; }
|
||||
=======
|
||||
Config_RobotParam_t* Config_GetRobotParam(void) {
|
||||
return &robot_config;
|
||||
}
|
||||
>>>>>>> main
|
||||
}
|
||||
@ -9,13 +9,6 @@ extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
<<<<<<< HEAD
|
||||
#include "device/motor_lz.h"
|
||||
#include "device/motor_lk.h"
|
||||
typedef struct {
|
||||
MOTOR_LZ_Param_t joint_param[4];
|
||||
MOTOR_LK_Param_t wheel_param[2];
|
||||
=======
|
||||
#include "device/dm_imu.h"
|
||||
#include "device/motor_lz.h"
|
||||
#include "device/motor_lk.h"
|
||||
@ -26,7 +19,6 @@ typedef struct {
|
||||
DM_IMU_Param_t imu_param;
|
||||
Chassis_Params_t chassis_param;
|
||||
RC_CAN_Param_t rc_can_param;
|
||||
>>>>>>> main
|
||||
} Config_RobotParam_t;
|
||||
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
|
||||
@ -1,95 +0,0 @@
|
||||
/*
|
||||
imu Task
|
||||
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "bsp/pwm.h"
|
||||
#include "component/ahrs.h"
|
||||
#include "component/pid.h"
|
||||
#include "device/bmi088.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
|
||||
BMI088_t bmi088;
|
||||
|
||||
AHRS_t gimbal_ahrs;
|
||||
AHRS_Magn_t magn;
|
||||
AHRS_Eulr_t eulr_to_send;
|
||||
|
||||
KPID_t imu_temp_ctrl_pid;
|
||||
|
||||
BMI088_Cali_t cali_bmi088= {
|
||||
.gyro_offset = {0.0f,0.0f,0.0f},
|
||||
};
|
||||
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
void Task_imu(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
|
||||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / IMU_FREQ;
|
||||
|
||||
osDelay(IMU_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
BMI088_Init(&bmi088,&cali_bmi088);
|
||||
AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
|
||||
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
BMI088_WaitNew();
|
||||
BMI088_AcclStartDmaRecv();
|
||||
BMI088_AcclWaitDmaCplt();
|
||||
|
||||
BMI088_GyroStartDmaRecv();
|
||||
BMI088_GyroWaitDmaCplt();
|
||||
|
||||
/* 锁住RTOS内核防止数据解析过程中断，造成错误 */
|
||||
osKernelLock();
|
||||
/* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */
|
||||
BMI088_ParseAccl(&bmi088);
|
||||
/* 扩大加速度数据10倍，并交换x和y */
|
||||
float temp_x = bmi088.accl.x * 10.0f;
|
||||
float temp_y = bmi088.accl.y * 10.0f;
|
||||
bmi088.accl.x = temp_y;
|
||||
bmi088.accl.y = -temp_x;
|
||||
bmi088.accl.z *= 10.0f;
|
||||
|
||||
BMI088_ParseGyro(&bmi088);
|
||||
/* 交换陀螺仪x和y */
|
||||
float temp_gyro_x = bmi088.gyro.x;
|
||||
bmi088.gyro.x = bmi088.gyro.y;
|
||||
bmi088.gyro.y = -temp_gyro_x;
|
||||
// IST8310_Parse(&ist8310);
|
||||
|
||||
/* 根据设备接收到的数据进行姿态解析 */
|
||||
AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
|
||||
|
||||
/* 根据解析出来的四元数计算欧拉角 */
|
||||
AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
|
||||
/* 交换pit和rol */
|
||||
float temp_rol = eulr_to_send.rol;
|
||||
eulr_to_send.rol = eulr_to_send.pit;
|
||||
eulr_to_send.pit = temp_rol;
|
||||
osKernelUnlock();
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
}
|
||||
@ -4,25 +4,12 @@
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
<<<<<<< HEAD
|
||||
#include "cmsis_os2.h"
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "bsp/mm.h"
|
||||
#include "bsp/pwm.h"
|
||||
#include "bsp/gpio.h"
|
||||
#include "component/ahrs.h"
|
||||
#include "component/pid.h"
|
||||
#include "device/bmi088.h"
|
||||
#include "task/user_task.h"
|
||||
=======
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "bsp/can.h"
|
||||
#include "device/dm_imu.h"
|
||||
#include "module/config.h"
|
||||
#include "module/balance_chassis.h"
|
||||
>>>>>>> main
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
@ -30,56 +17,8 @@
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
<<<<<<< HEAD
|
||||
BMI088_t bmi088;
|
||||
|
||||
AHRS_t gimbal_ahrs;
|
||||
AHRS_Magn_t magn;
|
||||
AHRS_Eulr_t eulr_to_send;
|
||||
|
||||
KPID_t imu_temp_ctrl_pid;
|
||||
|
||||
BMI088_Cali_t cali_bmi088= {
|
||||
.gyro_offset = {0.00379243772f,0.00133061118f,-0.00154866849f},
|
||||
};
|
||||
|
||||
static const KPID_Params_t imu_temp_ctrl_pid_param = {
|
||||
.k = 0.3f,
|
||||
.p = 1.0f,
|
||||
.i = 0.01f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
};
|
||||
|
||||
/* 校准相关变量 */
|
||||
typedef enum {
|
||||
CALIB_IDLE, // 空闲状态
|
||||
CALIB_RUNNING, // 正在校准
|
||||
CALIB_DONE // 校准完成
|
||||
} CalibState_t;
|
||||
|
||||
static CalibState_t calib_state = CALIB_IDLE;
|
||||
static uint16_t calib_count = 0;
|
||||
static struct {
|
||||
float x_sum;
|
||||
float y_sum;
|
||||
float z_sum;
|
||||
} gyro_sum = {0.00341676874f, 0.000505680335f, -0.00134522165f};
|
||||
static void start_gyro_calibration(void) {
|
||||
if (calib_state == CALIB_IDLE) {
|
||||
calib_state = CALIB_RUNNING;
|
||||
calib_count = 0;
|
||||
gyro_sum.x_sum = 0.0f;
|
||||
gyro_sum.y_sum = 0.0f;
|
||||
gyro_sum.z_sum = 0.0f;
|
||||
}
|
||||
}
|
||||
#define CALIB_SAMPLES 5000 // 校准采样数量
|
||||
=======
|
||||
DM_IMU_t dm_imu;
|
||||
Chassis_IMU_t chassis_imu_send;
|
||||
>>>>>>> main
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -95,109 +34,14 @@ void Task_atti_esti(void *argument) {
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
<<<<<<< HEAD
|
||||
BMI088_Init(&bmi088,&cali_bmi088);
|
||||
AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
|
||||
PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D,
|
||||
1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
|
||||
BSP_PWM_Start(BSP_PWM_IMU_HEAT_PWM);
|
||||
|
||||
/* 注册按钮回调函数并启用中断 */
|
||||
BSP_GPIO_RegisterCallback(BSP_GPIO_USER_KEY, start_gyro_calibration);
|
||||
BSP_GPIO_EnableIRQ(BSP_GPIO_USER_KEY);
|
||||
=======
|
||||
BSP_CAN_Init();
|
||||
// 初始化DM IMU设备
|
||||
DM_IMU_Init(&dm_imu, &Config_GetRobotParam()->imu_param);
|
||||
>>>>>>> main
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
<<<<<<< HEAD
|
||||
BMI088_WaitNew();
|
||||
BMI088_AcclStartDmaRecv();
|
||||
BMI088_AcclWaitDmaCplt();
|
||||
|
||||
BMI088_GyroStartDmaRecv();
|
||||
BMI088_GyroWaitDmaCplt();
|
||||
|
||||
/* 锁住RTOS内核防止数据解析过程中断，造成错误 */
|
||||
osKernelLock();
|
||||
/* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */
|
||||
BMI088_ParseAccl(&bmi088);
|
||||
BMI088_ParseGyro(&bmi088);
|
||||
// IST8310_Parse(&ist8310);
|
||||
|
||||
/* 陀螺仪校准处理 */
|
||||
if (calib_state == CALIB_RUNNING) {
|
||||
/* 累加陀螺仪数据用于计算零偏 */
|
||||
gyro_sum.x_sum += bmi088.gyro.x;
|
||||
gyro_sum.y_sum += bmi088.gyro.y;
|
||||
gyro_sum.z_sum += bmi088.gyro.z;
|
||||
calib_count++;
|
||||
|
||||
/* 达到采样数量后计算平均值并更新零偏 */
|
||||
if (calib_count >= CALIB_SAMPLES) {
|
||||
/* 计算平均值作为零偏 */
|
||||
cali_bmi088.gyro_offset.x = gyro_sum.x_sum / CALIB_SAMPLES;
|
||||
cali_bmi088.gyro_offset.y = gyro_sum.y_sum / CALIB_SAMPLES;
|
||||
cali_bmi088.gyro_offset.z = gyro_sum.z_sum / CALIB_SAMPLES;
|
||||
|
||||
/* 校准完成，重置为空闲状态以便下次校准 */
|
||||
calib_state = CALIB_IDLE;
|
||||
|
||||
/* 重新初始化BMI088以应用新的校准数据 */
|
||||
BMI088_Init(&bmi088, &cali_bmi088);
|
||||
AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
|
||||
}
|
||||
}
|
||||
|
||||
/* 只有在非校准状态下才进行正常的姿态解析 */
|
||||
if (calib_state != CALIB_RUNNING) {
|
||||
/* 根据设备接收到的数据进行姿态解析 */
|
||||
AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
|
||||
|
||||
/* 根据解析出来的四元数计算欧拉角 */
|
||||
AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
|
||||
}
|
||||
|
||||
osKernelUnlock();
|
||||
|
||||
/* 创建修改后的数据副本用于发送到消息队列 */
|
||||
AHRS_Accl_t accl_modified;
|
||||
AHRS_Gyro_t gyro_modified;
|
||||
AHRS_Eulr_t eulr_modified;
|
||||
|
||||
/* 加速度数据：x和y互换，y取负值，全部乘10 */
|
||||
accl_modified.x = bmi088.accl.y * 10.0f;
|
||||
accl_modified.y = -bmi088.accl.x * 10.0f;
|
||||
accl_modified.z = bmi088.accl.z * 10.0f;
|
||||
|
||||
|
||||
/* 陀螺仪数据：x和y互换，y取负值 */
|
||||
gyro_modified.x = bmi088.gyro.y;
|
||||
gyro_modified.y = -bmi088.gyro.x;
|
||||
gyro_modified.z = bmi088.gyro.z;
|
||||
|
||||
/* 欧拉角数据：y取负值 */
|
||||
eulr_modified.yaw = eulr_to_send.yaw;
|
||||
eulr_modified.pit = -eulr_to_send.pit;
|
||||
eulr_modified.rol = eulr_to_send.rol;
|
||||
|
||||
osMessageQueueReset(task_runtime.msgq.imu.accl);
|
||||
osMessageQueueReset(task_runtime.msgq.imu.gyro);
|
||||
osMessageQueueReset(task_runtime.msgq.imu.eulr);
|
||||
osMessageQueueReset(task_runtime.msgq.imu.quat);
|
||||
osMessageQueuePut(task_runtime.msgq.imu.accl, &accl_modified, 0, 0);
|
||||
osMessageQueuePut(task_runtime.msgq.imu.gyro, &gyro_modified, 0, 0);
|
||||
osMessageQueuePut(task_runtime.msgq.imu.eulr, &eulr_modified, 0, 0);
|
||||
osMessageQueuePut(task_runtime.msgq.imu.quat, &gimbal_ahrs.quat, 0, 0);
|
||||
|
||||
BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 40.0f, bmi088.temp, 0.0f, 0.0f));
|
||||
|
||||
=======
|
||||
|
||||
if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) {
|
||||
osMessageQueueReset(task_runtime.msgq.chassis_imu); // 重置消息队列，防止阻塞
|
||||
@ -206,7 +50,6 @@ void Task_atti_esti(void *argument) {
|
||||
chassis_imu_send.euler = dm_imu.data.euler;
|
||||
osMessageQueuePut(task_runtime.msgq.chassis_imu, &chassis_imu_send, 0, 0); // 非阻塞发送IMU数据
|
||||
}
|
||||
>>>>>>> main
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
@ -6,12 +6,8 @@
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
<<<<<<< HEAD
|
||||
|
||||
=======
|
||||
#include "bsp/pwm.h"
|
||||
#include <math.h>
|
||||
>>>>>>> main
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
@ -35,26 +31,18 @@ void Task_blink(void *argument) {
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
<<<<<<< HEAD
|
||||
|
||||
=======
|
||||
BSP_PWM_Stop(BSP_PWM_LED_R);
|
||||
BSP_PWM_Stop(BSP_PWM_LED_B);
|
||||
BSP_PWM_SetComp(BSP_PWM_LED_G, 0.0f);
|
||||
BSP_PWM_Start(BSP_PWM_LED_G);
|
||||
>>>>>>> main
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
<<<<<<< HEAD
|
||||
|
||||
=======
|
||||
// 呼吸灯 - 基于tick的正弦波
|
||||
float duty = (sinf(tick * 0.003f) + 1.0f) * 0.5f; // 0到1之间的正弦波，加快频率
|
||||
BSP_PWM_SetComp(BSP_PWM_LED_G, duty);
|
||||
>>>>>>> main
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
@ -1,11 +1,7 @@
|
||||
- delay: 0
|
||||
description: ''
|
||||
freq_control: true
|
||||
<<<<<<< HEAD
|
||||
frequency: 500.0
|
||||
=======
|
||||
frequency: 100.0
|
||||
>>>>>>> main
|
||||
function: Task_blink
|
||||
name: blink
|
||||
stack: 256
|
||||
@ -13,23 +9,12 @@
|
||||
description: ''
|
||||
freq_control: true
|
||||
frequency: 500.0
|
||||
<<<<<<< HEAD
|
||||
function: Task_imu
|
||||
name: imu
|
||||
=======
|
||||
function: Task_rc
|
||||
name: rc
|
||||
>>>>>>> main
|
||||
stack: 256
|
||||
- delay: 0
|
||||
description: ''
|
||||
freq_control: true
|
||||
<<<<<<< HEAD
|
||||
frequency: 500.0
|
||||
function: Task_ctrl_chassis
|
||||
name: ctrl_chassis
|
||||
stack: 256
|
||||
=======
|
||||
frequency: 1000.0
|
||||
function: Task_atti_esti
|
||||
name: atti_esti
|
||||
@ -41,16 +26,10 @@
|
||||
function: Task_ctrl_chassis
|
||||
name: ctrl_chassis
|
||||
stack: 512
|
||||
>>>>>>> main
|
||||
- delay: 0
|
||||
description: ''
|
||||
freq_control: true
|
||||
frequency: 500.0
|
||||
<<<<<<< HEAD
|
||||
function: Task_atti_esti
|
||||
name: atti_esti
|
||||
=======
|
||||
function: Task_ctrl_gimbal
|
||||
name: ctrl_gimbal
|
||||
>>>>>>> main
|
||||
stack: 256
|
||||
|
||||
@ -6,17 +6,12 @@
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
<<<<<<< HEAD
|
||||
#include "module/balance_chassis.h"
|
||||
#include "module/config.h"
|
||||
=======
|
||||
#include "bsp/can.h"
|
||||
#include "device/motor_lk.h"
|
||||
#include "device/motor_lz.h"
|
||||
#include "module/config.h"
|
||||
#include "module/balance_chassis.h"
|
||||
|
||||
>>>>>>> main
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
@ -24,9 +19,6 @@
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
<<<<<<< HEAD
|
||||
Chassis_t chassis; // 底盘实例
|
||||
=======
|
||||
Chassis_t chassis;
|
||||
Chassis_CMD_t chassis_cmd = {
|
||||
.mode = CHASSIS_MODE_RECOVER, // 改为RECOVER模式，让电机先启动
|
||||
@ -39,7 +31,6 @@ Chassis_CMD_t chassis_cmd = {
|
||||
};
|
||||
Chassis_IMU_t chassis_imu;
|
||||
Chassis_t chassis1;
|
||||
>>>>>>> main
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -55,51 +46,15 @@ void Task_ctrl_chassis(void *argument) {
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
<<<<<<< HEAD
|
||||
|
||||
// 使用配置参数初始化底盘
|
||||
Config_RobotParam_t *robot_param = Config_GetRobotParam();
|
||||
Chassis_Param_t chassis_param = {
|
||||
.joint_param = {
|
||||
robot_param->joint_param[0],
|
||||
robot_param->joint_param[1],
|
||||
robot_param->joint_param[2],
|
||||
robot_param->joint_param[3]
|
||||
},
|
||||
.wheel_param = {
|
||||
robot_param->wheel_param[0],
|
||||
robot_param->wheel_param[1]
|
||||
}
|
||||
};
|
||||
|
||||
// 初始化底盘
|
||||
if (Chassis_Init(&chassis, &chassis_param) != DEVICE_OK) {
|
||||
// 初始化失败处理
|
||||
return;
|
||||
}
|
||||
|
||||
// 使能底盘
|
||||
Chassis_Enable(&chassis);
|
||||
|
||||
=======
|
||||
|
||||
Chassis_Init(&chassis, &Config_GetRobotParam()->chassis_param, CTRL_CHASSIS_FREQ);
|
||||
|
||||
|
||||
>>>>>>> main
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
<<<<<<< HEAD
|
||||
|
||||
// 更新底盘数据
|
||||
Chassis_Update(&chassis);
|
||||
|
||||
// 处理底盘控制（包括CAN通信）
|
||||
Chassis_Ctrl(&chassis);
|
||||
=======
|
||||
if (osMessageQueueGet(task_runtime.msgq.chassis_imu, &chassis_imu, NULL, 0) == osOK) {
|
||||
chassis.feedback.imu = chassis_imu;
|
||||
}
|
||||
@ -110,7 +65,6 @@ void Task_ctrl_chassis(void *argument) {
|
||||
Chassis_UpdateFeedback(&chassis);
|
||||
Chassis_Control(&chassis, &chassis_cmd);
|
||||
|
||||
>>>>>>> main
|
||||
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
|
||||
169
User/task/imu.c
169
User/task/imu.c
@ -1,169 +0,0 @@
|
||||
/*
|
||||
imu Task
|
||||
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "component/user_math.h"
|
||||
#include "bsp/can.h"
|
||||
#include "bsp/time.h"
|
||||
#include "component/ahrs.h"
|
||||
#include <string.h>
|
||||
#include <stdint.h>
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
/* AHRS数据CAN ID定义 */
|
||||
#define CAN_ID_AHRS_ACCL 0x301 /* 加速度计数据 */
|
||||
#define CAN_ID_AHRS_GYRO 0x302 /* 陀螺仪数据 */
|
||||
#define CAN_ID_AHRS_EULR 0x303 /* 欧拉角数据 */
|
||||
#define CAN_ID_AHRS_QUAT 0x304 /* 四元数数据 */
|
||||
|
||||
/* 数据范围定义 */
|
||||
#define ACCEL_CAN_MAX (58.8f)
|
||||
#define ACCEL_CAN_MIN (-58.8f)
|
||||
#define GYRO_CAN_MAX (34.88f)
|
||||
#define GYRO_CAN_MIN (-34.88f)
|
||||
#define PITCH_CAN_MAX (M_PI_2) /* π/2 弧度 ≈ 90° */
|
||||
#define PITCH_CAN_MIN (-M_PI_2) /* -π/2 弧度 ≈ -90° */
|
||||
#define ROLL_CAN_MAX (M_PI) /* π 弧度 ≈ 180° */
|
||||
#define ROLL_CAN_MIN (-M_PI) /* -π 弧度 ≈ -180° */
|
||||
#define YAW_CAN_MAX (M_PI) /* π 弧度 ≈ 180° */
|
||||
#define YAW_CAN_MIN (-M_PI) /* -π 弧度 ≈ -180° */
|
||||
#define QUATERNION_MIN (-1.0f)
|
||||
#define QUATERNION_MAX (1.0f)
|
||||
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* 数据映射宏：将浮点值映射到16位整数范围 */
|
||||
#define MAP_TO_INT16(val, min, max) \
|
||||
((int16_t)(((val) - (min)) / ((max) - (min)) * 65535.0f - 32768.0f))
|
||||
|
||||
/* 限制值在指定范围内 */
|
||||
#define CLAMP(val, min, max) \
|
||||
(((val) < (min)) ? (min) : (((val) > (max)) ? (max) : (val)))
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
AHRS_Accl_t accl;
|
||||
AHRS_Gyro_t gyro;
|
||||
AHRS_Eulr_t eulr;
|
||||
AHRS_Quaternion_t quat;
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
void Task_imu(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
|
||||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / IMU_FREQ;
|
||||
|
||||
osDelay(IMU_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
BSP_CAN_Init();
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
/* 获取加速度计数据并通过CAN发送 - 每轴使用16位编码 */
|
||||
if (osMessageQueueGet(task_runtime.msgq.imu.accl, &accl, NULL, 0) == osOK) {
|
||||
BSP_CAN_StdDataFrame_t accl_frame;
|
||||
accl_frame.id = CAN_ID_AHRS_ACCL;
|
||||
accl_frame.dlc = 6; /* 3轴 × 2字节 = 6字节 */
|
||||
|
||||
/* 限制并映射加速度数据到16位整数 */
|
||||
float ax = CLAMP(accl.x, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
|
||||
float ay = CLAMP(accl.y, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
|
||||
float az = CLAMP(accl.z, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
|
||||
|
||||
int16_t ax_int = MAP_TO_INT16(ax, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
|
||||
int16_t ay_int = MAP_TO_INT16(ay, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
|
||||
int16_t az_int = MAP_TO_INT16(az, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
|
||||
|
||||
memcpy(&accl_frame.data[0], &ax_int, sizeof(int16_t));
|
||||
memcpy(&accl_frame.data[2], &ay_int, sizeof(int16_t));
|
||||
memcpy(&accl_frame.data[4], &az_int, sizeof(int16_t));
|
||||
|
||||
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame);
|
||||
}
|
||||
|
||||
/* 获取陀螺仪数据并通过CAN发送 - 每轴使用16位编码 */
|
||||
if (osMessageQueueGet(task_runtime.msgq.imu.gyro, &gyro, NULL, 0) == osOK) {
|
||||
BSP_CAN_StdDataFrame_t gyro_frame;
|
||||
gyro_frame.id = CAN_ID_AHRS_GYRO;
|
||||
gyro_frame.dlc = 6; /* 3轴 × 2字节 = 6字节 */
|
||||
|
||||
/* 限制并映射陀螺仪数据到16位整数 */
|
||||
float gx = CLAMP(gyro.x, GYRO_CAN_MIN, GYRO_CAN_MAX);
|
||||
float gy = CLAMP(gyro.y, GYRO_CAN_MIN, GYRO_CAN_MAX);
|
||||
float gz = CLAMP(gyro.z, GYRO_CAN_MIN, GYRO_CAN_MAX);
|
||||
|
||||
int16_t gx_int = MAP_TO_INT16(gx, GYRO_CAN_MIN, GYRO_CAN_MAX);
|
||||
int16_t gy_int = MAP_TO_INT16(gy, GYRO_CAN_MIN, GYRO_CAN_MAX);
|
||||
int16_t gz_int = MAP_TO_INT16(gz, GYRO_CAN_MIN, GYRO_CAN_MAX);
|
||||
|
||||
memcpy(&gyro_frame.data[0], &gx_int, sizeof(int16_t));
|
||||
memcpy(&gyro_frame.data[2], &gy_int, sizeof(int16_t));
|
||||
memcpy(&gyro_frame.data[4], &gz_int, sizeof(int16_t));
|
||||
|
||||
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame);
|
||||
}
|
||||
|
||||
/* 获取欧拉角数据并通过CAN发送 - 每角使用16位编码 */
|
||||
if (osMessageQueueGet(task_runtime.msgq.imu.eulr, &eulr, NULL, 0) == osOK) {
|
||||
BSP_CAN_StdDataFrame_t eulr_frame;
|
||||
eulr_frame.id = CAN_ID_AHRS_EULR;
|
||||
eulr_frame.dlc = 6; /* 3个角度 × 2字节 = 6字节 */
|
||||
|
||||
/* 限制并映射欧拉角数据到16位整数 */
|
||||
float yaw = CLAMP(eulr.yaw, YAW_CAN_MIN, YAW_CAN_MAX);
|
||||
float pitch = CLAMP(eulr.pit, PITCH_CAN_MIN, PITCH_CAN_MAX);
|
||||
float roll = CLAMP(eulr.rol, ROLL_CAN_MIN, ROLL_CAN_MAX);
|
||||
|
||||
int16_t yaw_int = MAP_TO_INT16(yaw, YAW_CAN_MIN, YAW_CAN_MAX);
|
||||
int16_t pitch_int = MAP_TO_INT16(pitch, PITCH_CAN_MIN, PITCH_CAN_MAX);
|
||||
int16_t roll_int = MAP_TO_INT16(roll, ROLL_CAN_MIN, ROLL_CAN_MAX);
|
||||
|
||||
memcpy(&eulr_frame.data[0], &yaw_int, sizeof(int16_t));
|
||||
memcpy(&eulr_frame.data[2], &pitch_int, sizeof(int16_t));
|
||||
memcpy(&eulr_frame.data[4], &roll_int, sizeof(int16_t));
|
||||
|
||||
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &eulr_frame);
|
||||
}
|
||||
|
||||
/* 获取四元数数据并通过CAN发送 - 每个分量使用16位编码 */
|
||||
if (osMessageQueueGet(task_runtime.msgq.imu.quat, &quat, NULL, 0) == osOK) {
|
||||
BSP_CAN_StdDataFrame_t quat_frame;
|
||||
quat_frame.id = CAN_ID_AHRS_QUAT;
|
||||
quat_frame.dlc = 8; /* 4个四元数分量 × 2字节 = 8字节 */
|
||||
|
||||
/* 限制并映射四元数到16位整数 */
|
||||
float q0 = CLAMP(quat.q0, QUATERNION_MIN, QUATERNION_MAX);
|
||||
float q1 = CLAMP(quat.q1, QUATERNION_MIN, QUATERNION_MAX);
|
||||
float q2 = CLAMP(quat.q2, QUATERNION_MIN, QUATERNION_MAX);
|
||||
float q3 = CLAMP(quat.q3, QUATERNION_MIN, QUATERNION_MAX);
|
||||
|
||||
int16_t q0_int = MAP_TO_INT16(q0, QUATERNION_MIN, QUATERNION_MAX);
|
||||
int16_t q1_int = MAP_TO_INT16(q1, QUATERNION_MIN, QUATERNION_MAX);
|
||||
int16_t q2_int = MAP_TO_INT16(q2, QUATERNION_MIN, QUATERNION_MAX);
|
||||
int16_t q3_int = MAP_TO_INT16(q3, QUATERNION_MIN, QUATERNION_MAX);
|
||||
|
||||
memcpy(&quat_frame.data[0], &q0_int, sizeof(int16_t));
|
||||
memcpy(&quat_frame.data[2], &q1_int, sizeof(int16_t));
|
||||
memcpy(&quat_frame.data[4], &q2_int, sizeof(int16_t));
|
||||
memcpy(&quat_frame.data[6], &q3_int, sizeof(int16_t));
|
||||
|
||||
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &quat_frame);
|
||||
}
|
||||
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
}
|
||||
@ -8,13 +8,9 @@
|
||||
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "component/ahrs.h"
|
||||
<<<<<<< HEAD
|
||||
/* USER INCLUDE END */
|
||||
=======
|
||||
#include "module/config.h"
|
||||
#include "module/balance_chassis.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
>>>>>>> main
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
@ -37,29 +33,16 @@ void Task_Init(void *argument) {
|
||||
|
||||
/* 创建任务线程 */
|
||||
task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
|
||||
<<<<<<< HEAD
|
||||
task_runtime.thread.imu = osThreadNew(Task_imu, NULL, &attr_imu);
|
||||
task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
|
||||
task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti);
|
||||
=======
|
||||
task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
|
||||
task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti);
|
||||
task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
|
||||
task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal);
|
||||
>>>>>>> main
|
||||
|
||||
// 创建消息队列
|
||||
/* USER MESSAGE BEGIN */
|
||||
task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
|
||||
<<<<<<< HEAD
|
||||
task_runtime.msgq.imu.accl = osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL);
|
||||
task_runtime.msgq.imu.gyro = osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
|
||||
task_runtime.msgq.imu.eulr = osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
|
||||
task_runtime.msgq.imu.quat = osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
|
||||
=======
|
||||
task_runtime.msgq.chassis_imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL);
|
||||
task_runtime.msgq.Chassis_cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
|
||||
>>>>>>> main
|
||||
/* USER MESSAGE END */
|
||||
|
||||
osKernelUnlock(); // 解锁内核
|
||||
|
||||
@ -83,7 +83,7 @@ void Task_rc(void *argument) {
|
||||
cmd_to_chassis.mode = CHASSIS_MODE_RECOVER;
|
||||
break;
|
||||
case RC_CAN_SW_DOWN: // 下位
|
||||
cmd_to_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
|
||||
cmd_to_chassis.mode = CHASSIS_MODE_WHELL_BALANCE;
|
||||
break;
|
||||
default:
|
||||
cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
|
||||
|
||||
@ -14,18 +14,8 @@ const osThreadAttr_t attr_blink = {
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 256 * 4,
|
||||
};
|
||||
<<<<<<< HEAD
|
||||
const osThreadAttr_t attr_imu = {
|
||||
.name = "imu",
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 256 * 4,
|
||||
};
|
||||
const osThreadAttr_t attr_ctrl_chassis = {
|
||||
.name = "ctrl_chassis",
|
||||
=======
|
||||
const osThreadAttr_t attr_rc = {
|
||||
.name = "rc",
|
||||
>>>>>>> main
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 256 * 4,
|
||||
};
|
||||
@ -33,8 +23,6 @@ const osThreadAttr_t attr_atti_esti = {
|
||||
.name = "atti_esti",
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 256 * 4,
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
};
|
||||
const osThreadAttr_t attr_ctrl_chassis = {
|
||||
.name = "ctrl_chassis",
|
||||
@ -45,5 +33,4 @@ const osThreadAttr_t attr_ctrl_gimbal = {
|
||||
.name = "ctrl_gimbal",
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 256 * 4,
|
||||
>>>>>>> main
|
||||
};
|
||||
@ -13,32 +13,19 @@ extern "C" {
|
||||
/* USER INCLUDE END */
|
||||
/* Exported constants ------------------------------------------------------- */
|
||||
/* 任务运行频率 */
|
||||
<<<<<<< HEAD
|
||||
#define BLINK_FREQ (500.0)
|
||||
#define IMU_FREQ (500.0)
|
||||
#define CTRL_CHASSIS_FREQ (500.0)
|
||||
#define ATTI_ESTI_FREQ (500.0)
|
||||
=======
|
||||
#define BLINK_FREQ (100.0)
|
||||
#define RC_FREQ (500.0)
|
||||
#define ATTI_ESTI_FREQ (1000.0)
|
||||
#define CTRL_CHASSIS_FREQ (500.0)
|
||||
#define CTRL_GIMBAL_FREQ (500.0)
|
||||
>>>>>>> main
|
||||
|
||||
/* 任务初始化延时ms */
|
||||
#define TASK_INIT_DELAY (100u)
|
||||
#define BLINK_INIT_DELAY (0)
|
||||
<<<<<<< HEAD
|
||||
#define IMU_INIT_DELAY (0)
|
||||
#define CTRL_CHASSIS_INIT_DELAY (0)
|
||||
#define ATTI_ESTI_INIT_DELAY (0)
|
||||
=======
|
||||
#define RC_INIT_DELAY (0)
|
||||
#define ATTI_ESTI_INIT_DELAY (0)
|
||||
#define CTRL_CHASSIS_INIT_DELAY (500)
|
||||
#define CTRL_GIMBAL_INIT_DELAY (0)
|
||||
>>>>>>> main
|
||||
|
||||
/* Exported defines --------------------------------------------------------- */
|
||||
/* Exported macro ----------------------------------------------------------- */
|
||||
@ -49,34 +36,19 @@ typedef struct {
|
||||
/* 各任务，也可以叫做线程 */
|
||||
struct {
|
||||
osThreadId_t blink;
|
||||
<<<<<<< HEAD
|
||||
osThreadId_t imu;
|
||||
osThreadId_t ctrl_chassis;
|
||||
osThreadId_t atti_esti;
|
||||
=======
|
||||
osThreadId_t rc;
|
||||
osThreadId_t atti_esti;
|
||||
osThreadId_t ctrl_chassis;
|
||||
osThreadId_t ctrl_gimbal;
|
||||
>>>>>>> main
|
||||
} thread;
|
||||
|
||||
/* USER MESSAGE BEGIN */
|
||||
struct {
|
||||
osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */
|
||||
<<<<<<< HEAD
|
||||
struct {
|
||||
osMessageQueueId_t accl;
|
||||
osMessageQueueId_t gyro;
|
||||
osMessageQueueId_t eulr;
|
||||
osMessageQueueId_t quat;
|
||||
}imu;
|
||||
=======
|
||||
|
||||
osMessageQueueId_t chassis_imu;
|
||||
osMessageQueueId_t Chassis_cmd;
|
||||
|
||||
>>>>>>> main
|
||||
} msgq;
|
||||
/* USER MESSAGE END */
|
||||
|
||||
@ -94,46 +66,28 @@ typedef struct {
|
||||
/* 各任务的stack使用 */
|
||||
struct {
|
||||
UBaseType_t blink;
|
||||
<<<<<<< HEAD
|
||||
UBaseType_t imu;
|
||||
UBaseType_t ctrl_chassis;
|
||||
UBaseType_t atti_esti;
|
||||
=======
|
||||
UBaseType_t rc;
|
||||
UBaseType_t atti_esti;
|
||||
UBaseType_t ctrl_chassis;
|
||||
UBaseType_t ctrl_gimbal;
|
||||
>>>>>>> main
|
||||
} stack_water_mark;
|
||||
|
||||
/* 各任务运行频率 */
|
||||
struct {
|
||||
float blink;
|
||||
<<<<<<< HEAD
|
||||
float imu;
|
||||
float ctrl_chassis;
|
||||
float atti_esti;
|
||||
=======
|
||||
float rc;
|
||||
float atti_esti;
|
||||
float ctrl_chassis;
|
||||
float ctrl_gimbal;
|
||||
>>>>>>> main
|
||||
} freq;
|
||||
|
||||
/* 任务最近运行时间 */
|
||||
struct {
|
||||
float blink;
|
||||
<<<<<<< HEAD
|
||||
float imu;
|
||||
float ctrl_chassis;
|
||||
float atti_esti;
|
||||
=======
|
||||
float rc;
|
||||
float atti_esti;
|
||||
float ctrl_chassis;
|
||||
float ctrl_gimbal;
|
||||
>>>>>>> main
|
||||
} last_up_time;
|
||||
|
||||
} Task_Runtime_t;
|
||||
@ -144,30 +98,18 @@ extern Task_Runtime_t task_runtime;
|
||||
/* 初始化任务句柄 */
|
||||
extern const osThreadAttr_t attr_init;
|
||||
extern const osThreadAttr_t attr_blink;
|
||||
<<<<<<< HEAD
|
||||
extern const osThreadAttr_t attr_imu;
|
||||
extern const osThreadAttr_t attr_ctrl_chassis;
|
||||
extern const osThreadAttr_t attr_atti_esti;
|
||||
=======
|
||||
extern const osThreadAttr_t attr_rc;
|
||||
extern const osThreadAttr_t attr_atti_esti;
|
||||
extern const osThreadAttr_t attr_ctrl_chassis;
|
||||
extern const osThreadAttr_t attr_ctrl_gimbal;
|
||||
>>>>>>> main
|
||||
|
||||
/* 任务函数声明 */
|
||||
void Task_Init(void *argument);
|
||||
void Task_blink(void *argument);
|
||||
<<<<<<< HEAD
|
||||
void Task_imu(void *argument);
|
||||
void Task_ctrl_chassis(void *argument);
|
||||
void Task_atti_esti(void *argument);
|
||||
=======
|
||||
void Task_rc(void *argument);
|
||||
void Task_atti_esti(void *argument);
|
||||
void Task_ctrl_chassis(void *argument);
|
||||
void Task_ctrl_gimbal(void *argument);
|
||||
>>>>>>> main
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
@ -1,100 +0,0 @@
|
||||
# Balance Chassis Module 移植完成
|
||||
|
||||
## 概述
|
||||
|
||||
已将 `ctrl_chassis` 任务中的底盘控制代码成功移植到 `balance_chassis` 模块中，实现了模块化的底盘控制系统。
|
||||
|
||||
## 主要功能
|
||||
|
||||
### 1. 电机支持
|
||||
- **关节电机**: 4个LZ电机，用于关节控制
|
||||
- **轮子电机**: 2个LK电机，用于轮子控制
|
||||
|
||||
### 2. CAN通信协议
|
||||
|
||||
#### 控制命令ID（接收）
|
||||
- `ID 121`: 使能4个关节电机
|
||||
- `ID 122`: 关节电机运控模式控制（8字节力矩数据）
|
||||
- `ID 128`: 左轮电机控制命令（直接转发格式）
|
||||
- `ID 129`: 右轮电机控制命令（直接转发格式）
|
||||
|
||||
#### 反馈数据ID（发送）
|
||||
- `ID 124-127`: 关节电机反馈数据
|
||||
- 转矩电流(2字节) + 位置(3字节) + 速度(3字节)
|
||||
- `ID 130-131`: 轮子电机反馈数据
|
||||
- 角度(2字节) + 速度(2字节) + 力矩电流(2字节) + 编码器(2字节)
|
||||
|
||||
### 3. 控制逻辑
|
||||
|
||||
#### 关节电机控制
|
||||
- 如果收到控制命令，执行相应的运控模式控制
|
||||
- 如果没有控制命令，电机进入松弛模式（relax）
|
||||
|
||||
#### 轮子电机控制
|
||||
- 支持CAN命令直接转发到电机
|
||||
- 如果没有控制命令，电机进入松弛模式（relax）
|
||||
- 始终保持通信连接
|
||||
|
||||
### 4. 模块接口
|
||||
|
||||
```c
|
||||
// 底盘初始化
|
||||
int8_t Chassis_Init(Chassis_t *chassis, Chassis_Param_t *param);
|
||||
|
||||
// 更新底盘数据
|
||||
int8_t Chassis_Update(Chassis_t *chassis);
|
||||
|
||||
// 使能底盘
|
||||
int8_t Chassis_Enable(Chassis_t *chassis);
|
||||
|
||||
// 底盘松弛
|
||||
int8_t Chassis_Relax(Chassis_t *chassis);
|
||||
|
||||
// 底盘控制处理（包括CAN通信）
|
||||
int8_t Chassis_Ctrl(Chassis_t *chassis);
|
||||
```
|
||||
|
||||
## 使用方法
|
||||
|
||||
### 1. 配置电机参数
|
||||
|
||||
在 `config.c` 中已配置：
|
||||
- 4个关节电机（LZ电机）
|
||||
- 2个轮子电机（LK电机）
|
||||
|
||||
### 2. 任务集成
|
||||
|
||||
`ctrl_chassis` 任务已简化为：
|
||||
```c
|
||||
// 初始化底盘
|
||||
Chassis_Init(&chassis, &chassis_param);
|
||||
Chassis_Enable(&chassis);
|
||||
|
||||
// 主循环
|
||||
while(1) {
|
||||
Chassis_Update(&chassis); // 更新数据
|
||||
Chassis_Ctrl(&chassis); // 处理控制和通信
|
||||
}
|
||||
```
|
||||
|
||||
## 特性优势
|
||||
|
||||
1. **模块化设计**: 底盘功能封装在独立模块中
|
||||
2. **统一接口**: 提供清晰的API接口
|
||||
3. **CAN转发**: 支持轮子电机命令直接转发
|
||||
4. **通信保持**: 确保电机通信不断
|
||||
5. **故障安全**: 无控制命令时自动松弛
|
||||
|
||||
## 文件结构
|
||||
|
||||
```
|
||||
User/
|
||||
├── module/
|
||||
│ ├── balance_chassis.h # 底盘模块头文件
|
||||
│ ├── balance_chassis.c # 底盘模块实现
|
||||
│ └── config.c # 电机参数配置（已更新）
|
||||
└── task/
|
||||
└── ctrl_chassis.c # 底盘控制任务（已简化）
|
||||
```
|
||||
|
||||
移植完成，所有功能已验证可用！
|
||||
@ -26,11 +26,7 @@ set(TARGET_FLAGS "-mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard ")
|
||||
|
||||
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${TARGET_FLAGS}")
|
||||
set(CMAKE_ASM_FLAGS "${CMAKE_C_FLAGS} -x assembler-with-cpp -MMD -MP")
|
||||
<<<<<<< HEAD
|
||||
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -fdata-sections -ffunction-sections")
|
||||
=======
|
||||
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wextra -Wpedantic -fdata-sections -ffunction-sections")
|
||||
>>>>>>> main
|
||||
|
||||
set(CMAKE_C_FLAGS_DEBUG "-O0 -g3")
|
||||
set(CMAKE_C_FLAGS_RELEASE "-Os -g0")
|
||||
@ -39,14 +35,6 @@ set(CMAKE_CXX_FLAGS_RELEASE "-Os -g0")
|
||||
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_C_FLAGS} -fno-rtti -fno-exceptions -fno-threadsafe-statics")
|
||||
|
||||
<<<<<<< HEAD
|
||||
set(CMAKE_EXE_LINKER_FLAGS "${TARGET_FLAGS}")
|
||||
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -T \"${CMAKE_SOURCE_DIR}/STM32F407XX_FLASH.ld\"")
|
||||
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} --specs=nano.specs")
|
||||
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-Map=${CMAKE_PROJECT_NAME}.map -Wl,--gc-sections")
|
||||
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--print-memory-usage")
|
||||
set(TOOLCHAIN_LINK_LIBRARIES "m")
|
||||
=======
|
||||
set(CMAKE_C_LINK_FLAGS "${TARGET_FLAGS}")
|
||||
set(CMAKE_C_LINK_FLAGS "${CMAKE_C_LINK_FLAGS} -T \"${CMAKE_SOURCE_DIR}/STM32F407XX_FLASH.ld\"")
|
||||
set(CMAKE_C_LINK_FLAGS "${CMAKE_C_LINK_FLAGS} --specs=nano.specs")
|
||||
@ -54,5 +42,4 @@ set(CMAKE_C_LINK_FLAGS "${CMAKE_C_LINK_FLAGS} -Wl,-Map=${CMAKE_PROJECT_NAME}.map
|
||||
set(CMAKE_C_LINK_FLAGS "${CMAKE_C_LINK_FLAGS} -Wl,--start-group -lc -lm -Wl,--end-group")
|
||||
set(CMAKE_C_LINK_FLAGS "${CMAKE_C_LINK_FLAGS} -Wl,--print-memory-usage")
|
||||
|
||||
set(CMAKE_CXX_LINK_FLAGS "${CMAKE_C_LINK_FLAGS} -Wl,--start-group -lstdc++ -lsupc++ -Wl,--end-group")
|
||||
>>>>>>> main
|
||||
set(CMAKE_CXX_LINK_FLAGS "${CMAKE_C_LINK_FLAGS} -Wl,--start-group -lstdc++ -lsupc++ -Wl,--end-group")
|
||||
@ -1,662 +0,0 @@
|
||||
#include "mod_wheelleg_chassis.hpp"
|
||||
|
||||
#include <cmath>
|
||||
#include <tuple>
|
||||
|
||||
using namespace Module;
|
||||
Wheelleg::Wheelleg(Param& param)
|
||||
: param_(param),
|
||||
roll_pid_(param.roll_pid_param, 333.0f),
|
||||
yaw_pid_(param.yaw_pid_param, 333.0f),
|
||||
yaccl_pid_(param.yaccl_pid_param, 333.0f),
|
||||
lowfilter_(333.0f, 50.0f),
|
||||
acclfilter_(333.0f, 30.0f),
|
||||
manfilter_(param.manfilter_param),
|
||||
|
||||
ctrl_lock_(true) {
|
||||
memset(&(this->cmd_), 0, sizeof(this->cmd_));
|
||||
|
||||
this->hip_motor_.at(0) =
|
||||
new Device::MitMotor(param.hip_motor_param.at(0), "hip_left_front");
|
||||
this->hip_motor_.at(1) =
|
||||
new Device::MitMotor(param.hip_motor_param.at(1), "hip_left_back");
|
||||
this->hip_motor_.at(2) =
|
||||
new Device::MitMotor(param.hip_motor_param.at(2), "hip_right_front");
|
||||
this->hip_motor_.at(3) =
|
||||
new Device::MitMotor(param.hip_motor_param.at(3), "hip_left_back");
|
||||
|
||||
this->wheel_motor_.at(0) =
|
||||
new Device::RMMotor(param.wheel_motor_param.at(0), "wheel_left");
|
||||
this->wheel_motor_.at(1) =
|
||||
new Device::RMMotor(param.wheel_motor_param.at(1), "wheel_right");
|
||||
|
||||
this->leglength_pid_.at(0) =
|
||||
new Component::PID(param.leglength_pid_param.at(0), 333.0f);
|
||||
this->leglength_pid_.at(1) =
|
||||
new Component::PID(param.leglength_pid_param.at(1), 333.0f);
|
||||
|
||||
this->theta_pid_.at(0) =
|
||||
new Component::PID(param.theta_pid_param.at(0), 333.0f);
|
||||
this->theta_pid_.at(1) =
|
||||
new Component::PID(param.theta_pid_param.at(1), 333.0f);
|
||||
|
||||
this->tp_pid_.at(0) = new Component::PID(param.Tp_pid_param.at(0), 333.0);
|
||||
this->tp_pid_.at(1) = new Component::PID(param.Tp_pid_param.at(1), 333.0);
|
||||
|
||||
this->leg_.at(0) = new Component::VMC(param_.leg_param.at(0), 333.0f);
|
||||
this->leg_.at(1) = new Component::VMC(param_.leg_param.at(1), 333.0f);
|
||||
|
||||
auto event_callback = [](Wheelleg::ChassisEvent event, Wheelleg* chassis) {
|
||||
chassis->ctrl_lock_.Wait(UINT32_MAX);
|
||||
switch (event) {
|
||||
case SET_MODE_RELAX:
|
||||
chassis->SetMode(RELAX);
|
||||
break;
|
||||
case SET_MODE_STAND:
|
||||
chassis->SetMode(STAND);
|
||||
break;
|
||||
case SET_MODE_ROTOR:
|
||||
chassis->SetMode(ROTOR);
|
||||
break;
|
||||
case SET_MODE_RESET:
|
||||
chassis->SetMode(RESET);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
chassis->ctrl_lock_.Post();
|
||||
};
|
||||
|
||||
Component::CMD::RegisterEvent<Wheelleg*, ChassisEvent>(
|
||||
event_callback, this, this->param_.EVENT_MAP);
|
||||
|
||||
auto chassis_thread = [](Wheelleg* chassis) {
|
||||
auto cmd_sub =
|
||||
Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
|
||||
auto eulr_sub =
|
||||
Message::Subscriber<Component::Type::Eulr>("chassis_imu_eulr");
|
||||
auto gyro_sub =
|
||||
Message::Subscriber<Component::Type::Vector3>("chassis_gyro");
|
||||
|
||||
auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
|
||||
|
||||
auto accl_sub =
|
||||
Message::Subscriber<Component::Type::Vector3>("chassis_imu_accl_abs");
|
||||
// auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
|
||||
uint32_t last_online_time = bsp_time_get_ms();
|
||||
|
||||
while (1) {
|
||||
eulr_sub.DumpData(chassis->eulr_); /* imu */
|
||||
gyro_sub.DumpData(chassis->gyro_); /* imu */
|
||||
accl_sub.DumpData(chassis->accl_);
|
||||
|
||||
yaw_sub.DumpData(chassis->yaw_);
|
||||
cmd_sub.DumpData(chassis->cmd_);
|
||||
// yaw_sub.DumpData(chassis->yaw_);
|
||||
|
||||
chassis->ctrl_lock_.Wait(UINT32_MAX);
|
||||
chassis->MotorSetAble();
|
||||
chassis->UpdateFeedback();
|
||||
chassis->Calculate();
|
||||
chassis->Control();
|
||||
chassis->ctrl_lock_.Post();
|
||||
|
||||
/* 运行结束，等待下一次唤醒 */
|
||||
chassis->thread_.SleepUntil(3, last_online_time);
|
||||
}
|
||||
};
|
||||
this->thread_.Create(chassis_thread, this, "chassis_thread", 1536,
|
||||
System::Thread::MEDIUM);
|
||||
}
|
||||
|
||||
void Wheelleg::MotorSetAble() {
|
||||
if (this->hip_motor_flag_ == 0) {
|
||||
this->hip_motor_[0]->Relax();
|
||||
this->hip_motor_[1]->Relax();
|
||||
this->hip_motor_[2]->Relax();
|
||||
this->hip_motor_[3]->Relax();
|
||||
this->dm_motor_flag_ = 0;
|
||||
}
|
||||
|
||||
else {
|
||||
if (this->dm_motor_flag_ == 0) {
|
||||
for (int i = 0; i < 5; i++) {
|
||||
this->hip_motor_[0]->Enable();
|
||||
}
|
||||
for (int i = 0; i < 5; i++) {
|
||||
this->hip_motor_[1]->Enable();
|
||||
}
|
||||
for (int i = 0; i < 5; i++) {
|
||||
this->hip_motor_[2]->Enable();
|
||||
}
|
||||
for (int i = 0; i < 5; i++) {
|
||||
this->hip_motor_[3]->Enable();
|
||||
}
|
||||
|
||||
this->dm_motor_flag_ = 1;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
void Wheelleg::UpdateFeedback() {
|
||||
this->now_ = bsp_time_get();
|
||||
this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
|
||||
this->last_wakeup_ = this->now_;
|
||||
|
||||
this->wheel_motor_[0]->Update();
|
||||
this->wheel_motor_[1]->Update();
|
||||
|
||||
this->leg_argu_[0].phi4_ =
|
||||
this->hip_motor_[0]->GetAngle() -
|
||||
this->param_.wheel_param.mech_zero[0]; // 前关节角度
|
||||
this->leg_argu_[0].phi1_ =
|
||||
this->hip_motor_[1]->GetAngle() -
|
||||
this->param_.wheel_param.mech_zero[1]; // 后关节角度
|
||||
|
||||
this->leg_argu_[1].phi4_ =
|
||||
(-this->hip_motor_[2]->GetAngle() +
|
||||
this->param_.wheel_param.mech_zero[3]); // 前关节角度
|
||||
if (leg_argu_[1].phi4_ > M_PI) {
|
||||
this->leg_argu_[1].phi4_ = this->leg_argu_[1].phi4_ - 2 * M_PI;
|
||||
}
|
||||
|
||||
this->leg_argu_[1].phi1_ =
|
||||
(-this->hip_motor_[3]->GetAngle() +
|
||||
this->param_.wheel_param.mech_zero[2]); // 后关节角度
|
||||
if (leg_argu_[1].phi1_ > M_PI) {
|
||||
this->leg_argu_[1].phi1_ = this->leg_argu_[1].phi1_ - 2 * M_PI;
|
||||
}
|
||||
|
||||
this->pit_ = this->eulr_.pit;
|
||||
if (this->pit_ > M_PI) {
|
||||
this->pit_ = this->eulr_.pit - 2 * M_PI;
|
||||
}
|
||||
|
||||
/* 注意极性 */
|
||||
std::tuple<float, float, float, float> result0 =
|
||||
this->leg_[0]->VMCsolve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
|
||||
this->pit_, -this->gyro_.x, this->dt_);
|
||||
this->leg_argu_[0].L0 = std::get<0>(result0);
|
||||
this->leg_argu_[0].d_L0 = std::get<1>(result0);
|
||||
this->leg_argu_[0].theta = -std::get<2>(result0);
|
||||
this->leg_argu_[0].d_theta = -std::get<3>(result0);
|
||||
|
||||
if (leg_argu_[0].theta > M_PI) {
|
||||
leg_argu_[0].theta = leg_argu_[0].theta - 2 * M_PI;
|
||||
}
|
||||
if (leg_argu_[0].theta < -M_PI) {
|
||||
leg_argu_[0].theta = leg_argu_[0].theta + 2 * M_PI;
|
||||
}
|
||||
|
||||
std::tuple<float, float, float, float> result1 =
|
||||
this->leg_[1]->VMCsolve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
|
||||
this->pit_, -this->gyro_.x, this->dt_);
|
||||
this->leg_argu_[1].L0 = std::get<0>(result1);
|
||||
this->leg_argu_[1].d_L0 = std::get<1>(result1);
|
||||
this->leg_argu_[1].theta = -std::get<2>(result1);
|
||||
this->leg_argu_[1].d_theta = -std::get<3>(result1);
|
||||
|
||||
if (leg_argu_[1].theta > M_PI) {
|
||||
leg_argu_[1].theta = leg_argu_[1].theta - 2 * M_PI;
|
||||
}
|
||||
if (leg_argu_[1].theta < -M_PI) {
|
||||
leg_argu_[1].theta = leg_argu_[1].theta + 2 * M_PI;
|
||||
}
|
||||
|
||||
/* 轮子转速近似于编码器速度 由此推测机体速度 */
|
||||
this->leg_argu_[0].single_x_dot =
|
||||
-wheel_motor_[0]->GetSpeed() * 2 * M_PI *
|
||||
(this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
|
||||
leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
|
||||
leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta);
|
||||
|
||||
this->leg_argu_[1].single_x_dot =
|
||||
wheel_motor_[1]->GetSpeed() * 2 * M_PI *
|
||||
(this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
|
||||
leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
|
||||
leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta);
|
||||
|
||||
this->move_argu_.last_x_dot = this->move_argu_.x_dot;
|
||||
this->move_argu_.x_dot =
|
||||
|
||||
(this->leg_argu_[0].single_x_dot + this->leg_argu_[1].single_x_dot) / 2;
|
||||
this->move_argu_.x_dot =
|
||||
(-wheel_motor_[0]->GetSpeed() + wheel_motor_[1]->GetSpeed()) * M_PI *
|
||||
this->param_.wheel_param.wheel_radius / 60.f / 15.765;
|
||||
this->move_argu_.x = this->move_argu_.x_dot * dt_ + move_argu_.x;
|
||||
|
||||
this->move_argu_.delta_speed =
|
||||
lowfilter_.Apply((move_argu_.x_dot - move_argu_.last_x_dot) / dt_);
|
||||
|
||||
this->move_argu_.accl = acclfilter_.Apply(this->accl_.y * 9.8f);
|
||||
|
||||
if (now_ > 1000000) {
|
||||
this->move_argu_.x_dot_hat =
|
||||
manfilter_.comp_filter(move_argu_.x_dot, move_argu_.delta_speed,
|
||||
this->move_argu_.last_x_dot, accl_.y * 9.8f,
|
||||
dt_) -
|
||||
((leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
|
||||
leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta)) +
|
||||
(leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
|
||||
leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta))) /
|
||||
2;
|
||||
this->move_argu_.xhat = dt_ * move_argu_.x_dot_hat + move_argu_.xhat;
|
||||
}
|
||||
}
|
||||
|
||||
void Wheelleg::Calculate() {
|
||||
switch (this->mode_) {
|
||||
case Wheelleg::RELAX:
|
||||
// if (fabs(move_argu_.target_dot_x - cmd_.y * 1.5f) > 0.005) {
|
||||
// if (move_argu_.target_dot_x > cmd_.y * 1.5f) {
|
||||
// move_argu_.target_dot_x -= 0.004;
|
||||
// }
|
||||
// if (move_argu_.target_dot_x < cmd_.y * 1.5f) {
|
||||
// move_argu_.target_dot_x += 0.004;
|
||||
// }
|
||||
// } else {
|
||||
// move_argu_.target_dot_x = cmd_.y * 1.5f;
|
||||
// }
|
||||
// move_argu_.target_x = move_argu_.target_dot_x * dt_ +
|
||||
// move_argu_.target_x;
|
||||
move_argu_.target_x = 0;
|
||||
move_argu_.target_dot_x = 0;
|
||||
break;
|
||||
case Wheelleg::STAND:
|
||||
|
||||
/* 0.002为最大加速度 即500hz*0.002 梯度式递增减 */
|
||||
if (fabs(move_argu_.target_dot_x -
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
|
||||
if (move_argu_.target_dot_x >
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
|
||||
move_argu_.target_dot_x -= 0.004;
|
||||
}
|
||||
if (move_argu_.target_dot_x <
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
|
||||
move_argu_.target_dot_x += 0.004;
|
||||
}
|
||||
} else {
|
||||
move_argu_.target_dot_x =
|
||||
cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
|
||||
}
|
||||
move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
|
||||
|
||||
this->move_argu_.target_yaw = 0.0f;
|
||||
|
||||
/*双零点*/
|
||||
if (this->yaw_ > M_PI_2) {
|
||||
this->move_argu_.target_yaw = 3.14158f;
|
||||
}
|
||||
if (this->yaw_ < -M_PI_2) {
|
||||
this->move_argu_.target_yaw = 3.14158f;
|
||||
}
|
||||
break;
|
||||
|
||||
case Wheelleg::ROTOR:
|
||||
if (fabs(move_argu_.target_dot_x -
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
|
||||
if (move_argu_.target_dot_x >
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
|
||||
move_argu_.target_dot_x -= 0.004;
|
||||
}
|
||||
if (move_argu_.target_dot_x <
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
|
||||
move_argu_.target_dot_x += 0.004;
|
||||
}
|
||||
} else {
|
||||
move_argu_.target_dot_x =
|
||||
cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
|
||||
}
|
||||
move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
|
||||
this->move_argu_.target_yaw = this->yaw_ + 0.01;
|
||||
|
||||
break;
|
||||
// move_argu_.target_dot_x = cmd_.x;
|
||||
// move_argu_.target_x =
|
||||
// move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
|
||||
// // move_argu_.target_dot_x = sqrtf(cmd_.x * cmd_.x + cmd_.y *
|
||||
// cmd_.y);
|
||||
// // move_argu_.x_dot =
|
||||
// // move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
|
||||
// // move_argu_.target_yaw = atan2(cmd_.x, cmd_.y);
|
||||
// break;
|
||||
|
||||
case Wheelleg::RESET:
|
||||
this->move_argu_.target_dot_x = 0;
|
||||
move_argu_.target_x = 0;
|
||||
this->move_argu_.target_yaw = this->eulr_.yaw;
|
||||
this->move_argu_.xhat = 0;
|
||||
|
||||
// move_argu_.target_yaw - atan2(cmd_.x, cmd_.y);
|
||||
// if ((fabs(move_argu_.target_yaw) - fabs(eulr_.yaw)) > M_PI / 2) {
|
||||
// this->move_argu_.target_dot_x = -this->move_argu_.target_dot_x;
|
||||
// }
|
||||
break;
|
||||
|
||||
default:
|
||||
XB_ASSERT(false);
|
||||
return;
|
||||
}
|
||||
|
||||
leg_argu_[0].Fn = leg_[0]->GndDetector(leg_argu_[0].Tp, leg_argu_[0].F0);
|
||||
onground0_flag_ = false;
|
||||
if (leg_argu_[0].Fn > 30) {
|
||||
onground0_flag_ = true;
|
||||
}
|
||||
leg_argu_[1].Fn = leg_[1]->GndDetector(leg_argu_[1].Tp, leg_argu_[1].F0);
|
||||
onground1_flag_ = false;
|
||||
if (leg_argu_[1].Fn > 30) {
|
||||
onground1_flag_ = true;
|
||||
}
|
||||
std::tuple<float, float> result3;
|
||||
std::tuple<float, float> result4;
|
||||
|
||||
switch (this->mode_) {
|
||||
case Wheelleg::RELAX:
|
||||
case Wheelleg::ROTOR:
|
||||
case Wheelleg::STAND:
|
||||
|
||||
for (int i = 0; i < 12; i++) {
|
||||
leg_argu_[0].LQR_K[i] = this->leg_[0]->LQR_K_calc(
|
||||
&this->param_.wheel_param.K_Poly_Coefficient_L[i][0],
|
||||
leg_argu_[0].L0);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 12; i++) {
|
||||
leg_argu_[1].LQR_K[i] = this->leg_[1]->LQR_K_calc(
|
||||
&this->param_.wheel_param.K_Poly_Coefficient_R[i][0],
|
||||
leg_argu_[1].L0);
|
||||
}
|
||||
if (now_ > 1000000)
|
||||
if (fabs(move_argu_.target_dot_x) > 0.5 ||
|
||||
fabs(move_argu_.target_dot_x - move_argu_.x_dot_hat) > 0.7 ||
|
||||
((!onground0_flag_) & (!onground1_flag_))) {
|
||||
leg_argu_[0].LQR_K[2] = 0;
|
||||
leg_argu_[1].LQR_K[2] = 0;
|
||||
this->move_argu_.xhat = 0;
|
||||
move_argu_.target_x = 0;
|
||||
}
|
||||
|
||||
if (onground0_flag_) {
|
||||
leg_argu_[0].Tw =
|
||||
(leg_argu_[0].LQR_K[0] *
|
||||
(-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
|
||||
leg_argu_[0].LQR_K[1] * (-leg_argu_[0].d_theta) +
|
||||
leg_argu_[0].LQR_K[2] *
|
||||
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
|
||||
leg_argu_[0].LQR_K[3] *
|
||||
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
|
||||
leg_argu_[0].LQR_K[4] * (-this->pit_ + 0.16) +
|
||||
leg_argu_[0].LQR_K[5] * (-this->gyro_.x + 0.0f));
|
||||
leg_argu_[0].Tp =
|
||||
(leg_argu_[0].LQR_K[6] *
|
||||
(-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
|
||||
leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta) +
|
||||
leg_argu_[0].LQR_K[8] *
|
||||
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
|
||||
leg_argu_[0].LQR_K[9] *
|
||||
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
|
||||
leg_argu_[0].LQR_K[10] * (-this->pit_ + 0.16) +
|
||||
leg_argu_[0].LQR_K[11] * (-this->gyro_.x + 0.0f));
|
||||
} else {
|
||||
leg_argu_[0].Tw = 0;
|
||||
|
||||
leg_argu_[0].Tp =
|
||||
(leg_argu_[0].LQR_K[6] * (-leg_argu_[0].theta + 0.0f) +
|
||||
leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta + 0.0f));
|
||||
}
|
||||
if (onground1_flag_) {
|
||||
leg_argu_[1].Tw =
|
||||
(leg_argu_[1].LQR_K[0] *
|
||||
(-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
|
||||
leg_argu_[1].LQR_K[1] * (-leg_argu_[1].d_theta + 0.0f) +
|
||||
leg_argu_[1].LQR_K[2] *
|
||||
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
|
||||
leg_argu_[1].LQR_K[3] *
|
||||
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
|
||||
leg_argu_[1].LQR_K[4] * (-this->pit_ + 0.16) +
|
||||
leg_argu_[1].LQR_K[5] * (-this->gyro_.x + 0.0f));
|
||||
leg_argu_[1].Tp =
|
||||
(leg_argu_[1].LQR_K[6] *
|
||||
(-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
|
||||
leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f) +
|
||||
leg_argu_[1].LQR_K[8] *
|
||||
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
|
||||
leg_argu_[1].LQR_K[9] *
|
||||
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
|
||||
leg_argu_[1].LQR_K[10] * (-this->pit_ + 0.16) +
|
||||
leg_argu_[1].LQR_K[11] * (-this->gyro_.x + 0.0f));
|
||||
} else {
|
||||
leg_argu_[1].Tw = 0;
|
||||
leg_argu_[1].Tp =
|
||||
(leg_argu_[1].LQR_K[6] * (-leg_argu_[1].theta + 0.0f) +
|
||||
leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f));
|
||||
}
|
||||
|
||||
this->leg_argu_[0].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
|
||||
this->param_.wheel_param.static_L0[0] +
|
||||
+roll_pid_.Calculate(0, eulr_.rol, dt_);
|
||||
this->leg_argu_[1].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
|
||||
this->param_.wheel_param.static_L0[1] -
|
||||
roll_pid_.Calculate(0, eulr_.rol, dt_);
|
||||
|
||||
leg_argu_[0].F0 =
|
||||
leg_argu_[0].Tp * sinf(leg_argu_[0].theta) +
|
||||
this->param_.wheel_param.static_F0[0] +
|
||||
leglength_pid_.at(0)->Calculate(this->leg_argu_[0].Delat_L0,
|
||||
this->leg_argu_[0].L0, this->dt_);
|
||||
leg_argu_[1].F0 =
|
||||
leg_argu_[1].Tp * sinf(leg_argu_[1].theta) +
|
||||
this->param_.wheel_param.static_F0[1] +
|
||||
leglength_pid_.at(1)->Calculate(this->leg_argu_[1].Delat_L0,
|
||||
this->leg_argu_[1].L0, this->dt_);
|
||||
|
||||
this->leg_argu_[0].Delta_Tp =
|
||||
leg_argu_[0].L0 * 10.0f *
|
||||
tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
|
||||
this->leg_argu_[0].theta, this->dt_);
|
||||
this->leg_argu_[1].Delta_Tp =
|
||||
-leg_argu_[1].L0 * 10.0f *
|
||||
tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
|
||||
this->leg_argu_[0].theta, this->dt_);
|
||||
|
||||
result3 = leg_[0]->VMCinserve(
|
||||
-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
|
||||
-leg_argu_[0].Tp - this->leg_argu_[0].Delta_Tp, leg_argu_[0].F0);
|
||||
this->leg_argu_[0].T1 = std::get<0>(result3);
|
||||
this->leg_argu_[0].T2 = std::get<1>(result3);
|
||||
|
||||
result4 = leg_[1]->VMCinserve(
|
||||
-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
|
||||
-leg_argu_[1].Tp - this->leg_argu_[1].Delta_Tp, leg_argu_[1].F0);
|
||||
this->leg_argu_[1].T1 = -std::get<0>(result4);
|
||||
this->leg_argu_[1].T2 = -std::get<1>(result4);
|
||||
|
||||
if (onground0_flag_ & onground1_flag_) {
|
||||
move_argu_.yaw_force =
|
||||
-this->yaw_pid_.Calculate(move_argu_.target_yaw, this->yaw_, dt_);
|
||||
} else {
|
||||
move_argu_.yaw_force = 0;
|
||||
}
|
||||
/*3508不带减速箱是Tw*3.2f*/
|
||||
/*2006带减速是Tw/1.8f*/
|
||||
/* 3508带15.7减速箱是Tw/4.9256 */
|
||||
|
||||
this->wheel_motor_out_[0] =
|
||||
this->leg_argu_[0].Tw / 4.9256f - move_argu_.yaw_force;
|
||||
|
||||
this->wheel_motor_out_[1] =
|
||||
this->leg_argu_[1].Tw / 4.9256f + move_argu_.yaw_force;
|
||||
|
||||
this->hip_motor_out_[0] = this->leg_argu_[0].T1;
|
||||
this->hip_motor_out_[1] = this->leg_argu_[0].T2;
|
||||
this->hip_motor_out_[2] = this->leg_argu_[1].T1;
|
||||
this->hip_motor_out_[3] = this->leg_argu_[1].T2;
|
||||
|
||||
this->hip_motor_flag_ = 1;
|
||||
break;
|
||||
|
||||
case Wheelleg::RESET:
|
||||
if (fabs(pit_) > M_PI / 2 || fabs(leg_argu_[0].theta) > 1.57 ||
|
||||
fabs(leg_argu_[1].theta) > 1.57) {
|
||||
leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
|
||||
|
||||
if (fabs(pit_) > M_PI / 2) {
|
||||
if ((leg_argu_[0].theta - leg_argu_[1].theta) > 0.03) {
|
||||
leg_argu_[1].target_theta = leg_argu_[1].theta + 0.001;
|
||||
}
|
||||
if ((leg_argu_[0].theta - leg_argu_[1].theta) < -0.03) {
|
||||
leg_argu_[1].target_theta = leg_argu_[1].theta - 0.001;
|
||||
}
|
||||
leg_argu_[0].F0 = 10 * leglength_pid_.at(0)->Calculate(
|
||||
0.65f, this->leg_argu_[0].L0, this->dt_);
|
||||
leg_argu_[1].F0 = 10 * leglength_pid_.at(1)->Calculate(
|
||||
0.65f, this->leg_argu_[1].L0, this->dt_);
|
||||
}
|
||||
if (fabs(leg_argu_[0].theta) < 1.57) {
|
||||
leg_argu_[1].target_theta = leg_argu_[1].theta + cmd_.y / 1000;
|
||||
leg_argu_[0].target_theta = leg_argu_[0].theta;
|
||||
}
|
||||
|
||||
if (fabs(leg_argu_[1].theta) < 1.57) {
|
||||
leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
|
||||
leg_argu_[1].target_theta = leg_argu_[1].theta;
|
||||
}
|
||||
|
||||
if (leg_argu_[0].target_theta > M_PI) {
|
||||
leg_argu_[0].target_theta -= 2 * M_PI;
|
||||
}
|
||||
if (leg_argu_[0].target_theta < -M_PI) {
|
||||
leg_argu_[0].target_theta += 2 * M_PI;
|
||||
}
|
||||
if (leg_argu_[1].target_theta > M_PI) {
|
||||
leg_argu_[1].target_theta -= 2 * M_PI;
|
||||
}
|
||||
if (leg_argu_[1].target_theta < -M_PI) {
|
||||
leg_argu_[1].target_theta += 2 * M_PI;
|
||||
}
|
||||
leg_argu_[0].Tp =
|
||||
500 * this->theta_pid_[0]->Calculate(leg_argu_[0].target_theta,
|
||||
leg_argu_[0].theta, dt_);
|
||||
leg_argu_[1].Tp =
|
||||
500 * this->theta_pid_[1]->Calculate(leg_argu_[1].target_theta,
|
||||
leg_argu_[1].theta, dt_);
|
||||
|
||||
} else {
|
||||
leg_argu_[0].F0 = 3 * leglength_pid_.at(0)->Calculate(
|
||||
0.10f, this->leg_argu_[0].L0, this->dt_);
|
||||
leg_argu_[1].F0 = 3 * leglength_pid_.at(1)->Calculate(
|
||||
0.10f, this->leg_argu_[1].L0, this->dt_);
|
||||
|
||||
if ((this->leg_argu_[0].L0 < 0.20) && (this->leg_argu_[1].L0 < 0.20)) {
|
||||
leg_argu_[0].Tp = 5.5 * this->theta_pid_[0]->Calculate(
|
||||
0.1, leg_argu_[0].theta, dt_);
|
||||
leg_argu_[1].Tp = 5.5 * this->theta_pid_[1]->Calculate(
|
||||
0.1, leg_argu_[1].theta, dt_);
|
||||
clampf(&leg_argu_[0].Tp, -10, 10);
|
||||
clampf(&leg_argu_[1].Tp, -10, 10);
|
||||
} else {
|
||||
leg_argu_[0].Tp = 0;
|
||||
leg_argu_[1].Tp = 0;
|
||||
}
|
||||
}
|
||||
|
||||
result3 = leg_[0]->VMCinserve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
|
||||
-leg_argu_[0].Tp, leg_argu_[0].F0);
|
||||
this->leg_argu_[0].T1 = std::get<0>(result3);
|
||||
this->leg_argu_[0].T2 = std::get<1>(result3);
|
||||
|
||||
result4 = leg_[1]->VMCinserve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
|
||||
-leg_argu_[1].Tp, leg_argu_[1].F0);
|
||||
this->leg_argu_[1].T1 = -std::get<0>(result4);
|
||||
this->leg_argu_[1].T2 = -std::get<1>(result4);
|
||||
|
||||
this->hip_motor_out_[0] = this->leg_argu_[0].T1;
|
||||
this->hip_motor_out_[1] = this->leg_argu_[0].T2;
|
||||
this->hip_motor_out_[2] = this->leg_argu_[1].T1;
|
||||
this->hip_motor_out_[3] = this->leg_argu_[1].T2;
|
||||
|
||||
this->hip_motor_flag_ = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Wheelleg::Control() {
|
||||
clampf(&wheel_motor_out_[0], -0.8f, 0.8f);
|
||||
clampf(&wheel_motor_out_[1], -0.8f, 0.8f);
|
||||
clampf(&hip_motor_out_[0], -20.0f, 20.0f);
|
||||
clampf(&hip_motor_out_[1], -20.0f, 20.0f);
|
||||
clampf(&hip_motor_out_[2], -20.0f, 20.0f);
|
||||
clampf(&hip_motor_out_[3], -20.0f, 20.0f);
|
||||
|
||||
// if (fabs(wheel_motor_[0]->GetSpeed()) > 5000 ||
|
||||
// fabs(wheel_motor_[1]->GetSpeed()) > 5000) {
|
||||
// wheel_motor_out_[0] = 0;
|
||||
// wheel_motor_out_[1] = 0;
|
||||
|
||||
// if (fabs(this->pit_) > 0.5f) {
|
||||
// this->hip_motor_flag_ = 0;
|
||||
// }
|
||||
// }
|
||||
|
||||
switch (this->mode_) {
|
||||
case Wheelleg::RELAX:
|
||||
|
||||
this->wheel_motor_[0]->Relax();
|
||||
this->wheel_motor_[1]->Relax();
|
||||
hip_motor_flag_ = 0;
|
||||
hip_motor_[0]->SetMit(0.0f);
|
||||
hip_motor_[1]->SetMit(0.0f);
|
||||
hip_motor_[2]->SetMit(0.0f);
|
||||
hip_motor_[3]->SetMit(0.0f);
|
||||
break;
|
||||
|
||||
case Wheelleg::STAND:
|
||||
case Wheelleg::ROTOR:
|
||||
// this->wheel_motor_[0]->Relax();
|
||||
// this->wheel_motor_[1]->Relax();
|
||||
this->wheel_motor_[0]->Control(-wheel_motor_out_[0]);
|
||||
this->wheel_motor_[1]->Control(wheel_motor_out_[1]);
|
||||
hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
|
||||
hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
|
||||
hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
|
||||
hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
|
||||
// hip_motor_[0]->SetMit(0.0f);
|
||||
// hip_motor_[1]->SetMit(0.0f);
|
||||
// hip_motor_[2]->SetMit(0.0f);
|
||||
// hip_motor_[3]->SetMit(0.0f);
|
||||
break;
|
||||
|
||||
case Wheelleg::RESET:
|
||||
|
||||
this->wheel_motor_[0]->Relax();
|
||||
this->wheel_motor_[1]->Relax();
|
||||
|
||||
hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
|
||||
hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
|
||||
|
||||
hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
|
||||
hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Wheelleg::SetMode(Wheelleg::Mode mode) {
|
||||
if (mode == this->mode_) {
|
||||
return;
|
||||
}
|
||||
|
||||
this->leg_[0]->Reset();
|
||||
this->leg_[1]->Reset();
|
||||
move_argu_.x = 0;
|
||||
move_argu_.x_dot = 0;
|
||||
move_argu_.last_x_dot = 0;
|
||||
move_argu_.target_x = move_argu_.xhat;
|
||||
move_argu_.target_yaw = this->eulr_.yaw;
|
||||
move_argu_.target_dot_x = 0;
|
||||
move_argu_.xhat = 0;
|
||||
move_argu_.x_dot_hat = 0;
|
||||
this->manfilter_.reset_comp();
|
||||
this->mode_ = mode;
|
||||
}
|
||||
@ -48,8 +48,8 @@ function K = get_k_length(leg_length)
|
||||
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
|
||||
B=double(B);
|
||||
|
||||
Q=diag([700 100 2000 1500 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
|
||||
R=[140 0;0 50]; %T Tp
|
||||
Q=diag([700 1 5000 100 3000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
|
||||
R=[240 0;0 25]; %T Tp
|
||||
|
||||
K=lqr(A,B,Q,R);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user