Compare commits
3 Commits
2377a13dea
...
d290e6c3a2
| Author | SHA1 | Date | |
|---|---|---|---|
| d290e6c3a2 | |||
| 5f06fe477b | |||
| 87ff7ff05e |
@ -68,6 +68,7 @@ void CAN2_RX0_IRQHandler(void);
|
||||
void CAN2_RX1_IRQHandler(void);
|
||||
void DMA2_Stream5_IRQHandler(void);
|
||||
void DMA2_Stream6_IRQHandler(void);
|
||||
void DMA2_Stream7_IRQHandler(void);
|
||||
void USART6_IRQHandler(void);
|
||||
/* USER CODE BEGIN EFP */
|
||||
|
||||
|
||||
@ -62,6 +62,9 @@ void MX_DMA_Init(void)
|
||||
/* DMA2_Stream6_IRQn interrupt configuration */
|
||||
HAL_NVIC_SetPriority(DMA2_Stream6_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(DMA2_Stream6_IRQn);
|
||||
/* DMA2_Stream7_IRQn interrupt configuration */
|
||||
HAL_NVIC_SetPriority(DMA2_Stream7_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(DMA2_Stream7_IRQn);
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -63,6 +63,7 @@ extern CAN_HandleTypeDef hcan2;
|
||||
extern DMA_HandleTypeDef hdma_spi1_rx;
|
||||
extern DMA_HandleTypeDef hdma_spi1_tx;
|
||||
extern DMA_HandleTypeDef hdma_usart1_rx;
|
||||
extern DMA_HandleTypeDef hdma_usart1_tx;
|
||||
extern DMA_HandleTypeDef hdma_usart3_rx;
|
||||
extern DMA_HandleTypeDef hdma_usart6_rx;
|
||||
extern DMA_HandleTypeDef hdma_usart6_tx;
|
||||
@ -405,6 +406,20 @@ void DMA2_Stream6_IRQHandler(void)
|
||||
/* USER CODE END DMA2_Stream6_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles DMA2 stream7 global interrupt.
|
||||
*/
|
||||
void DMA2_Stream7_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN DMA2_Stream7_IRQn 0 */
|
||||
|
||||
/* USER CODE END DMA2_Stream7_IRQn 0 */
|
||||
HAL_DMA_IRQHandler(&hdma_usart1_tx);
|
||||
/* USER CODE BEGIN DMA2_Stream7_IRQn 1 */
|
||||
|
||||
/* USER CODE END DMA2_Stream7_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles USART6 global interrupt.
|
||||
*/
|
||||
|
||||
@ -28,6 +28,7 @@ UART_HandleTypeDef huart1;
|
||||
UART_HandleTypeDef huart3;
|
||||
UART_HandleTypeDef huart6;
|
||||
DMA_HandleTypeDef hdma_usart1_rx;
|
||||
DMA_HandleTypeDef hdma_usart1_tx;
|
||||
DMA_HandleTypeDef hdma_usart3_rx;
|
||||
DMA_HandleTypeDef hdma_usart6_rx;
|
||||
DMA_HandleTypeDef hdma_usart6_tx;
|
||||
@ -171,6 +172,24 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
|
||||
|
||||
__HAL_LINKDMA(uartHandle,hdmarx,hdma_usart1_rx);
|
||||
|
||||
/* USART1_TX Init */
|
||||
hdma_usart1_tx.Instance = DMA2_Stream7;
|
||||
hdma_usart1_tx.Init.Channel = DMA_CHANNEL_4;
|
||||
hdma_usart1_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
|
||||
hdma_usart1_tx.Init.PeriphInc = DMA_PINC_DISABLE;
|
||||
hdma_usart1_tx.Init.MemInc = DMA_MINC_ENABLE;
|
||||
hdma_usart1_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
|
||||
hdma_usart1_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
|
||||
hdma_usart1_tx.Init.Mode = DMA_NORMAL;
|
||||
hdma_usart1_tx.Init.Priority = DMA_PRIORITY_LOW;
|
||||
hdma_usart1_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
|
||||
if (HAL_DMA_Init(&hdma_usart1_tx) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
|
||||
__HAL_LINKDMA(uartHandle,hdmatx,hdma_usart1_tx);
|
||||
|
||||
/* USART1 interrupt Init */
|
||||
HAL_NVIC_SetPriority(USART1_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(USART1_IRQn);
|
||||
@ -311,6 +330,7 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
|
||||
|
||||
/* USART1 DMA DeInit */
|
||||
HAL_DMA_DeInit(uartHandle->hdmarx);
|
||||
HAL_DMA_DeInit(uartHandle->hdmatx);
|
||||
|
||||
/* USART1 interrupt Deinit */
|
||||
HAL_NVIC_DisableIRQ(USART1_IRQn);
|
||||
|
||||
15468
MDK-ARM/JLinkLog.txt
15468
MDK-ARM/JLinkLog.txt
File diff suppressed because it is too large
Load Diff
File diff suppressed because one or more lines are too long
BIN
MDK-ARM/lll0121/atti_esti.o
Normal file
BIN
MDK-ARM/lll0121/atti_esti.o
Normal file
Binary file not shown.
@ -1,3 +1,3 @@
|
||||
lll0121/calc_lib.o: ..\User\bsp\calc_lib.c ..\User\bsp\calc_lib.h \
|
||||
..\User\bsp\struct_typedef.h \
|
||||
C:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdint.h
|
||||
C:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdint.h \
|
||||
..\User\bsp\struct_typedef.h
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -1,8 +1,7 @@
|
||||
lll0121/cmd_1.o: ..\User\task\cmd.c \
|
||||
lll0121/cmd_1.o: ..\User\task\cmd.c ..\User\task\user_task.h \
|
||||
..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h \
|
||||
C:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdint.h \
|
||||
C:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stddef.h \
|
||||
..\User\task\user_task.h \
|
||||
..\Middlewares\Third_Party\FreeRTOS\Source\include\FreeRTOS.h \
|
||||
..\Core\Inc\FreeRTOSConfig.h \
|
||||
..\Middlewares\Third_Party\FreeRTOS\Source\include\projdefs.h \
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -54,4 +54,5 @@ lll0121/gimbal_1.o: ..\User\task\gimbal.c ..\User\task\user_task.h \
|
||||
..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os.h \
|
||||
..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h \
|
||||
..\User\module\cmd.h ..\User\module\gimbal.h ..\User\device\motor_dm.h \
|
||||
..\User\module\shoot.h ..\Core\Inc\main.h
|
||||
..\User\module\shoot.h ..\Core\Inc\main.h ..\User\device\ai.h \
|
||||
..\User\component\user_math.h ..\User\device\remote_control.h
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@ -54,4 +54,5 @@ lll0121/init.o: ..\User\task\init.c ..\User\task\user_task.h \
|
||||
..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os.h \
|
||||
..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h \
|
||||
..\User\module\cmd.h ..\User\module\gimbal.h ..\User\device\motor_dm.h \
|
||||
..\User\module\shoot.h ..\Core\Inc\main.h
|
||||
..\User\module\shoot.h ..\Core\Inc\main.h ..\User\device\ai.h \
|
||||
..\User\component\user_math.h ..\User\device\remote_control.h
|
||||
|
||||
Binary file not shown.
BIN
MDK-ARM/lll0121/lll0121.axf
Normal file
BIN
MDK-ARM/lll0121/lll0121.axf
Normal file
Binary file not shown.
@ -22,7 +22,7 @@ Dialog DLL: TCM.DLL V1.48.0.0
|
||||
|
||||
<h2>Project:</h2>
|
||||
D:\STM32CubeMX\103\Er(Sentry)\MDK-ARM\lll0121.uvprojx
|
||||
Project File Date: 12/06/2025
|
||||
Project File Date: 03/02/2026
|
||||
|
||||
<h2>Output:</h2>
|
||||
*** Using Compiler 'V6.16', folder: 'C:\Keil_v5\ARM\ARMCLANG\Bin'
|
||||
@ -34,14 +34,7 @@ Note: source file '..\User\task\chassis.c' - object file renamed from 'lll0121\c
|
||||
Note: source file '..\User\task\cmd.c' - object file renamed from 'lll0121\cmd.o' to 'lll0121\cmd_1.o'.
|
||||
Note: source file '..\User\task\gimbal.c' - object file renamed from 'lll0121\gimbal.o' to 'lll0121\gimbal_1.o'.
|
||||
Note: source file '..\User\task\shoot.c' - object file renamed from 'lll0121\shoot.o' to 'lll0121\shoot_1.o'.
|
||||
compiling dr16.c...
|
||||
../User/task/atti_esti.c(27): error: unknown type name 'IST8310_t'
|
||||
IST8310_t ist8310;
|
||||
^
|
||||
1 error generated.
|
||||
compiling atti_esti.c...
|
||||
compiling rc.c...
|
||||
"lll0121\lll0121.axf" - 1 Error(s), 0 Warning(s).
|
||||
"lll0121\lll0121.axf" - 0 Error(s), 0 Warning(s).
|
||||
|
||||
<h2>Software Packages used:</h2>
|
||||
|
||||
@ -65,7 +58,6 @@ Package Vendor: Keil
|
||||
|
||||
* Component: ARM::CMSIS:CORE:5.4.0
|
||||
Include file: CMSIS\Core\Include\tz_context.h
|
||||
Target not created.
|
||||
Build Time Elapsed: 00:00:01
|
||||
</pre>
|
||||
</body>
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -1,6 +1,9 @@
|
||||
lll0121/shoot_1.o: ..\User\task\shoot.c ..\User\task\user_task.h \
|
||||
..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h \
|
||||
lll0121/shoot_1.o: ..\User\task\shoot.c ..\User\module\cmd.h \
|
||||
..\User\bsp\struct_typedef.h ..\User\device\device.h \
|
||||
C:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdbool.h \
|
||||
C:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdint.h \
|
||||
..\User\task\user_task.h \
|
||||
..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h \
|
||||
C:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stddef.h \
|
||||
..\Middlewares\Third_Party\FreeRTOS\Source\include\FreeRTOS.h \
|
||||
..\Core\Inc\FreeRTOSConfig.h \
|
||||
@ -12,11 +15,9 @@ lll0121/shoot_1.o: ..\User\task\shoot.c ..\User\task\user_task.h \
|
||||
..\Middlewares\Third_Party\FreeRTOS\Source\include\task.h \
|
||||
..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h \
|
||||
..\User\module\config.h ..\User\device\bmi088.h \
|
||||
C:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdbool.h \
|
||||
..\User\component\ahrs.h ..\User\component\user_math.h \
|
||||
C:\Keil_v5\ARM\ARMCLANG\Bin\..\include\float.h \
|
||||
C:\Keil_v5\ARM\ARMCLANG\Bin\..\include\math.h ..\User\device\device.h \
|
||||
..\User\module\chassis.h ..\User\bsp\struct_typedef.h \
|
||||
C:\Keil_v5\ARM\ARMCLANG\Bin\..\include\math.h ..\User\module\chassis.h \
|
||||
..\User\component\filter.h ..\User\component\pid.h \
|
||||
..\User\device\motor_rm.h ..\User\device\motor.h ..\User\bsp\can.h \
|
||||
..\Core\Inc\can.h ..\Core\Inc\main.h \
|
||||
@ -53,5 +54,6 @@ lll0121/shoot_1.o: ..\User\task\shoot.c ..\User\task\user_task.h \
|
||||
..\User\bsp\bsp.h ..\User\bsp\mm.h \
|
||||
..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os.h \
|
||||
..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h \
|
||||
..\User\module\cmd.h ..\User\module\gimbal.h ..\User\device\motor_dm.h \
|
||||
..\User\module\shoot.h ..\Core\Inc\main.h
|
||||
..\User\module\gimbal.h ..\User\device\motor_dm.h \
|
||||
..\User\module\shoot.h ..\Core\Inc\main.h ..\User\device\ai.h \
|
||||
..\User\component\user_math.h ..\User\device\remote_control.h
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
361
Ozone.jdebug
Normal file
361
Ozone.jdebug
Normal file
@ -0,0 +1,361 @@
|
||||
/*********************************************************************
|
||||
* (c) SEGGER Microcontroller GmbH *
|
||||
* The Embedded Experts *
|
||||
* www.segger.com *
|
||||
**********************************************************************
|
||||
|
||||
File :
|
||||
Created : 12. Feb 2026 09:20
|
||||
Ozone Version : V3.40b
|
||||
*/
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnProjectLoad
|
||||
*
|
||||
* Function description
|
||||
* Project load routine. Required.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
void OnProjectLoad (void) {
|
||||
//
|
||||
// Dialog-generated settings
|
||||
//
|
||||
Project.AddPathSubstitute ("D:/STM32CubeMX/103/Er(Sentry)", "$(ProjectDir)");
|
||||
Project.AddPathSubstitute ("d:/stm32cubemx/103/er(sentry)", "$(ProjectDir)");
|
||||
Project.SetDevice ("STM32F407IG");
|
||||
Project.SetHostIF ("USB", "20750713");
|
||||
Project.SetTargetIF ("SWD");
|
||||
Project.SetTIFSpeed ("4 MHz");
|
||||
Project.AddSvdFile ("$(InstallDir)/Config/CPU/Cortex-M4F.svd");
|
||||
//
|
||||
// User settings
|
||||
//
|
||||
File.Open ("$(ProjectDir)/build/Debug/lll0121.elf");
|
||||
}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnStartupComplete
|
||||
*
|
||||
* Function description
|
||||
* Called when program execution has reached/passed
|
||||
* the startup completion point. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void OnStartupComplete (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* TargetReset
|
||||
*
|
||||
* Function description
|
||||
* Replaces the default target device reset routine. Optional.
|
||||
*
|
||||
* Notes
|
||||
* This example demonstrates the usage when
|
||||
* debugging an application in RAM on a Cortex-M target device.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void TargetReset (void) {
|
||||
//
|
||||
// unsigned int SP;
|
||||
// unsigned int PC;
|
||||
// unsigned int VectorTableAddr;
|
||||
//
|
||||
// VectorTableAddr = Elf.GetBaseAddr();
|
||||
// //
|
||||
// // Set up initial stack pointer
|
||||
// //
|
||||
// if (VectorTableAddr != 0xFFFFFFFF) {
|
||||
// SP = Target.ReadU32(VectorTableAddr);
|
||||
// Target.SetReg("SP", SP);
|
||||
// }
|
||||
// //
|
||||
// // Set up entry point PC
|
||||
// //
|
||||
// PC = Elf.GetEntryPointPC();
|
||||
//
|
||||
// if (PC != 0xFFFFFFFF) {
|
||||
// Target.SetReg("PC", PC);
|
||||
// } else if (VectorTableAddr != 0xFFFFFFFF) {
|
||||
// PC = Target.ReadU32(VectorTableAddr + 4);
|
||||
// Target.SetReg("PC", PC);
|
||||
// } else {
|
||||
// Util.Error("Project file error: failed to set entry point PC", 1);
|
||||
// }
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* BeforeTargetReset
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void BeforeTargetReset (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterTargetReset
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
* The default implementation initializes SP and PC to reset values.
|
||||
**
|
||||
**********************************************************************
|
||||
*/
|
||||
void AfterTargetReset (void) {
|
||||
_SetupTarget();
|
||||
}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* DebugStart
|
||||
*
|
||||
* Function description
|
||||
* Replaces the default debug session startup routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void DebugStart (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* TargetConnect
|
||||
*
|
||||
* Function description
|
||||
* Replaces the default target IF connection routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void TargetConnect (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* BeforeTargetConnect
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void BeforeTargetConnect (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterTargetConnect
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void AfterTargetConnect (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* TargetDownload
|
||||
*
|
||||
* Function description
|
||||
* Replaces the default program download routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void TargetDownload (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* BeforeTargetDownload
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void BeforeTargetDownload (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterTargetDownload
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
* The default implementation initializes SP and PC to reset values.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
void AfterTargetDownload (void) {
|
||||
_SetupTarget();
|
||||
}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* BeforeTargetDisconnect
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void BeforeTargetDisconnect (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterTargetDisconnect
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void AfterTargetDisconnect (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterTargetHalt
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void AfterTargetHalt (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* BeforeTargetResume
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void BeforeTargetResume (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnSnapshotLoad
|
||||
*
|
||||
* Function description
|
||||
* Called upon loading a snapshot. Optional.
|
||||
*
|
||||
* Additional information
|
||||
* This function is used to restore the target state in cases
|
||||
* where values cannot simply be written to the target.
|
||||
* Typical use: GPIO clock needs to be enabled, before
|
||||
* GPIO is configured.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void OnSnapshotLoad (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnSnapshotSave
|
||||
*
|
||||
* Function description
|
||||
* Called upon saving a snapshot. Optional.
|
||||
*
|
||||
* Additional information
|
||||
* This function is usually used to save values of the target
|
||||
* state which can either not be trivially read,
|
||||
* or need to be restored in a specific way or order.
|
||||
* Typically use: Memory Mapped Registers,
|
||||
* such as PLL and GPIO configuration.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void OnSnapshotSave (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnError
|
||||
*
|
||||
* Function description
|
||||
* Called when an error ocurred. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void OnError (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterProjectLoad
|
||||
*
|
||||
* Function description
|
||||
* After Project load routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void AfterProjectLoad (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnDebugStartBreakSymbolReached
|
||||
*
|
||||
* Function description
|
||||
* Called when program execution has reached/passed
|
||||
* the symbol to be breaked at during debug start. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void OnDebugStartBreakSymReached (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* _SetupTarget
|
||||
*
|
||||
* Function description
|
||||
* Setup the target.
|
||||
* Called by AfterTargetReset() and AfterTargetDownload().
|
||||
*
|
||||
* Auto-generated function. May be overridden by Ozone.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
void _SetupTarget(void) {
|
||||
unsigned int SP;
|
||||
unsigned int PC;
|
||||
unsigned int VectorTableAddr;
|
||||
|
||||
VectorTableAddr = Elf.GetBaseAddr();
|
||||
//
|
||||
// Set up initial stack pointer
|
||||
//
|
||||
SP = Target.ReadU32(VectorTableAddr);
|
||||
if (SP != 0xFFFFFFFF) {
|
||||
Target.SetReg("SP", SP);
|
||||
}
|
||||
//
|
||||
// Set up entry point PC
|
||||
//
|
||||
PC = Elf.GetEntryPointPC();
|
||||
if (PC != 0xFFFFFFFF) {
|
||||
Target.SetReg("PC", PC);
|
||||
} else {
|
||||
Util.Error("Project script error: failed to set up entry point PC", 1);
|
||||
}
|
||||
}
|
||||
31
Ozone.jdebug.user
Normal file
31
Ozone.jdebug.user
Normal file
@ -0,0 +1,31 @@
|
||||
|
||||
|
||||
OpenDocument="startup_stm32f407xx.s", FilePath="D:/STM32CubeMX/103/Er(Sentry)/startup_stm32f407xx.s", Line=51
|
||||
OpenDocument="shoot.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/shoot.c", Line=0
|
||||
OpenDocument="gimbal.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/gimbal.c", Line=0
|
||||
OpenDocument="cmd.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/cmd.c", Line=0
|
||||
OpenDocument="chassis.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/chassis.c", Line=3
|
||||
OpenDocument="atti_esti.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/atti_esti.c", Line=3
|
||||
OpenDocument="main.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/Core/Src/main.c", Line=62
|
||||
OpenToolbar="Debug", Floating=0, x=0, y=0
|
||||
OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=273, h=589, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
|
||||
OpenWindow="Source Files", DockArea=LEFT, x=0, y=1, w=738, h=286, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=273, h=102, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=0, w=738, h=405, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Data Sampling", DockArea=BOTTOM, x=0, y=0, w=446, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
|
||||
OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1473, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=1, CodePaneShown=1, PinCursor="Cursor Movable", TimePerDiv="1 ns / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="1272;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=1, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1272;-10", CodeGraphLegendShown=1, CodeGraphLegendPosition="1230;5"
|
||||
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
|
||||
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[191;100;100;100;931]
|
||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100]
|
||||
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[100;100;100;100;100;100;100;107;107]
|
||||
TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
|
||||
TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;118;105]
|
||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[171;144;100;302]
|
||||
TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
|
||||
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;351]
|
||||
WatchedExpression="chassis", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="cmd_chassis", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="gimbal", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="cmd_gimbal", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="shoot", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="shoot_cmd", RefreshRate=5, Window=Watched Data 1
|
||||
361
Ozone/ozone.jdebug
Normal file
361
Ozone/ozone.jdebug
Normal file
@ -0,0 +1,361 @@
|
||||
/*********************************************************************
|
||||
* (c) SEGGER Microcontroller GmbH *
|
||||
* The Embedded Experts *
|
||||
* www.segger.com *
|
||||
**********************************************************************
|
||||
|
||||
File :
|
||||
Created : 09. Mar 2026 16:55
|
||||
Ozone Version : V3.40b
|
||||
*/
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnProjectLoad
|
||||
*
|
||||
* Function description
|
||||
* Project load routine. Required.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
void OnProjectLoad (void) {
|
||||
//
|
||||
// Dialog-generated settings
|
||||
//
|
||||
Project.AddPathSubstitute ("D:/STM32CubeMX/103/Er(Sentry)/Ozone", "$(ProjectDir)");
|
||||
Project.AddPathSubstitute ("d:/stm32cubemx/103/er(sentry)/ozone", "$(ProjectDir)");
|
||||
Project.SetDevice ("STM32F407IG");
|
||||
Project.SetHostIF ("USB", "");
|
||||
Project.SetTargetIF ("SWD");
|
||||
Project.SetTIFSpeed ("4 MHz");
|
||||
Project.AddSvdFile ("$(InstallDir)/Config/CPU/Cortex-M4F.svd");
|
||||
//
|
||||
// User settings
|
||||
//
|
||||
File.Open ("D:/STM32CubeMX/103/Er(Sentry)/build/Debug/lll0121.elf");
|
||||
}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnStartupComplete
|
||||
*
|
||||
* Function description
|
||||
* Called when program execution has reached/passed
|
||||
* the startup completion point. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void OnStartupComplete (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* TargetReset
|
||||
*
|
||||
* Function description
|
||||
* Replaces the default target device reset routine. Optional.
|
||||
*
|
||||
* Notes
|
||||
* This example demonstrates the usage when
|
||||
* debugging an application in RAM on a Cortex-M target device.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void TargetReset (void) {
|
||||
//
|
||||
// unsigned int SP;
|
||||
// unsigned int PC;
|
||||
// unsigned int VectorTableAddr;
|
||||
//
|
||||
// VectorTableAddr = Elf.GetBaseAddr();
|
||||
// //
|
||||
// // Set up initial stack pointer
|
||||
// //
|
||||
// if (VectorTableAddr != 0xFFFFFFFF) {
|
||||
// SP = Target.ReadU32(VectorTableAddr);
|
||||
// Target.SetReg("SP", SP);
|
||||
// }
|
||||
// //
|
||||
// // Set up entry point PC
|
||||
// //
|
||||
// PC = Elf.GetEntryPointPC();
|
||||
//
|
||||
// if (PC != 0xFFFFFFFF) {
|
||||
// Target.SetReg("PC", PC);
|
||||
// } else if (VectorTableAddr != 0xFFFFFFFF) {
|
||||
// PC = Target.ReadU32(VectorTableAddr + 4);
|
||||
// Target.SetReg("PC", PC);
|
||||
// } else {
|
||||
// Util.Error("Project file error: failed to set entry point PC", 1);
|
||||
// }
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* BeforeTargetReset
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void BeforeTargetReset (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterTargetReset
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
* The default implementation initializes SP and PC to reset values.
|
||||
**
|
||||
**********************************************************************
|
||||
*/
|
||||
void AfterTargetReset (void) {
|
||||
_SetupTarget();
|
||||
}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* DebugStart
|
||||
*
|
||||
* Function description
|
||||
* Replaces the default debug session startup routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void DebugStart (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* TargetConnect
|
||||
*
|
||||
* Function description
|
||||
* Replaces the default target IF connection routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void TargetConnect (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* BeforeTargetConnect
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void BeforeTargetConnect (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterTargetConnect
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void AfterTargetConnect (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* TargetDownload
|
||||
*
|
||||
* Function description
|
||||
* Replaces the default program download routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void TargetDownload (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* BeforeTargetDownload
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void BeforeTargetDownload (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterTargetDownload
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
* The default implementation initializes SP and PC to reset values.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
void AfterTargetDownload (void) {
|
||||
_SetupTarget();
|
||||
}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* BeforeTargetDisconnect
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void BeforeTargetDisconnect (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterTargetDisconnect
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void AfterTargetDisconnect (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterTargetHalt
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void AfterTargetHalt (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* BeforeTargetResume
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void BeforeTargetResume (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnSnapshotLoad
|
||||
*
|
||||
* Function description
|
||||
* Called upon loading a snapshot. Optional.
|
||||
*
|
||||
* Additional information
|
||||
* This function is used to restore the target state in cases
|
||||
* where values cannot simply be written to the target.
|
||||
* Typical use: GPIO clock needs to be enabled, before
|
||||
* GPIO is configured.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void OnSnapshotLoad (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnSnapshotSave
|
||||
*
|
||||
* Function description
|
||||
* Called upon saving a snapshot. Optional.
|
||||
*
|
||||
* Additional information
|
||||
* This function is usually used to save values of the target
|
||||
* state which can either not be trivially read,
|
||||
* or need to be restored in a specific way or order.
|
||||
* Typically use: Memory Mapped Registers,
|
||||
* such as PLL and GPIO configuration.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void OnSnapshotSave (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnError
|
||||
*
|
||||
* Function description
|
||||
* Called when an error ocurred. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void OnError (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterProjectLoad
|
||||
*
|
||||
* Function description
|
||||
* After Project load routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void AfterProjectLoad (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnDebugStartBreakSymbolReached
|
||||
*
|
||||
* Function description
|
||||
* Called when program execution has reached/passed
|
||||
* the symbol to be breaked at during debug start. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void OnDebugStartBreakSymReached (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* _SetupTarget
|
||||
*
|
||||
* Function description
|
||||
* Setup the target.
|
||||
* Called by AfterTargetReset() and AfterTargetDownload().
|
||||
*
|
||||
* Auto-generated function. May be overridden by Ozone.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
void _SetupTarget(void) {
|
||||
unsigned int SP;
|
||||
unsigned int PC;
|
||||
unsigned int VectorTableAddr;
|
||||
|
||||
VectorTableAddr = Elf.GetBaseAddr();
|
||||
//
|
||||
// Set up initial stack pointer
|
||||
//
|
||||
SP = Target.ReadU32(VectorTableAddr);
|
||||
if (SP != 0xFFFFFFFF) {
|
||||
Target.SetReg("SP", SP);
|
||||
}
|
||||
//
|
||||
// Set up entry point PC
|
||||
//
|
||||
PC = Elf.GetEntryPointPC();
|
||||
if (PC != 0xFFFFFFFF) {
|
||||
Target.SetReg("PC", PC);
|
||||
} else {
|
||||
Util.Error("Project script error: failed to set up entry point PC", 1);
|
||||
}
|
||||
}
|
||||
32
Ozone/ozone.jdebug.user
Normal file
32
Ozone/ozone.jdebug.user
Normal file
@ -0,0 +1,32 @@
|
||||
|
||||
|
||||
OpenDocument="atti_esti.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/atti_esti.c", Line=0
|
||||
OpenDocument="cmd.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/cmd.c", Line=0
|
||||
OpenDocument="startup_stm32f407xx.s", FilePath="D:/STM32CubeMX/103/Er(Sentry)/startup_stm32f407xx.s", Line=51
|
||||
OpenDocument="chassis.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/chassis.c", Line=0
|
||||
OpenDocument="ai.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/ai.c", Line=0
|
||||
OpenDocument="main.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/Core/Src/main.c", Line=62
|
||||
OpenToolbar="Debug", Floating=0, x=0, y=0
|
||||
OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=273, h=589, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
|
||||
OpenWindow="Source Files", DockArea=LEFT, x=0, y=1, w=738, h=313, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=273, h=102, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=0, w=738, h=378, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Data Sampling", DockArea=BOTTOM, x=0, y=0, w=422, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
|
||||
OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1497, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=1, CodePaneShown=1, PinCursor="Cursor Movable", TimePerDiv="1 ns / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="1295;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=1, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1295;-10", CodeGraphLegendShown=1, CodeGraphLegendPosition="1254;5"
|
||||
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
|
||||
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[191;100;100;100;931]
|
||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100]
|
||||
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[100;100;100;100;100;100;100;107;107]
|
||||
TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
|
||||
TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;118;105]
|
||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[210;144;100;263]
|
||||
TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
|
||||
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;351]
|
||||
WatchedExpression="shoot_mcu_package", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="ai", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="mcu", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="chassis", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="cmd_chassis", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="rc_c", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="imu_eulr", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="ai_result", RefreshRate=5, Window=Watched Data 1
|
||||
179
User/device/ai.c
179
User/device/ai.c
@ -4,162 +4,63 @@
|
||||
#include "component/crc16.h"
|
||||
|
||||
|
||||
int8_t AI_Init(AI_t *ai) {
|
||||
if (ai == NULL) return DEVICE_ERR_NULL;
|
||||
// if (inited) return DEVICE_ERR_INITED;
|
||||
// if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_NUC,
|
||||
BSP_UART_RX_CPLT_CB);
|
||||
// inited = true;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int8_t AI_StartReceiving(AI_t *ai) {
|
||||
if (BSP_UART_Receive(BSP_UART_NUC,&ai->RX,sizeof(ai->RX), true)==HAL_OK) {
|
||||
int8_t AI_StartReceiving(PackageAI_t *ai) {
|
||||
if (BSP_UART_Receive(BSP_UART_1,(uint8_t *)ai,sizeof(*ai), true)==HAL_OK) {
|
||||
return DEVICE_OK;}
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
int8_t AI_Get_NUC(AI_t *ai,AI_cmd_t* ai_cmd) {
|
||||
if(ai->RX.head[0]!='M'&&ai->RX.head[1]!='R'){
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
// CRC16_Calc(&ai->RX,sizeof(ai->RX),ai->RX.crc16);
|
||||
if(CRC16_Verify((const uint8_t*)&(ai->RX), sizeof(ai->RX))!=true){
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
ai_cmd->gimbal_t.setpoint.pit=ai->RX.pitch;
|
||||
ai_cmd->gimbal_t.setpoint.yaw=ai->RX.yaw;
|
||||
ai_cmd->mode=ai->RX.mode;
|
||||
ai_cmd->gimbal_t.accl.pit=ai->RX.pitch_acc;
|
||||
ai_cmd->gimbal_t.vel.pit=ai->RX.pitch_vel;
|
||||
ai_cmd->gimbal_t.accl.yaw=ai->RX.yaw_acc;
|
||||
ai_cmd->gimbal_t.vel.yaw=ai->RX.yaw_vel;
|
||||
return DEVICE_OK;
|
||||
int8_t AI_Get_NUC(PackageAI_t *ai,AI_result_t* result) {
|
||||
|
||||
if(ai->id==ID_AI){
|
||||
// if(CRC16_Verify((const uint8_t*)&(ai), sizeof(&ai))==true)
|
||||
// {
|
||||
result->mode=ai->data.mode;
|
||||
result->gimbal_t.setpoint.yaw=ai->data.yaw;
|
||||
result->gimbal_t.vel.yaw=ai->data.yaw_vel;
|
||||
result->gimbal_t.accl.yaw=ai->data.yaw_acc;
|
||||
result->gimbal_t.setpoint.pit=ai->data.pitch;
|
||||
result->gimbal_t.vel.pit=ai->data.pitch_vel;
|
||||
result->gimbal_t.accl.pit=ai->data.pitch_acc;
|
||||
|
||||
result->chassis_t.Vx=ai->data.vx;
|
||||
result->chassis_t.Vy=ai->data.vy;
|
||||
result->chassis_t.Vw=ai->data.wz;
|
||||
// }
|
||||
|
||||
return DEVICE_OK;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int8_t AI_ParseHost(AI_t* ai,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) {
|
||||
ai->TX.head[0]='M';
|
||||
ai->TX.head[1]='R';
|
||||
// ai->TX.mode=2;
|
||||
int8_t MCU_Send(PackageMCU_t* mcu,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) {
|
||||
|
||||
ai->TX.pitch=motor->motor.pitch_4310_motor_feedback.rotor_abs_angle;
|
||||
ai->TX.yaw=motor->motor.yaw_4310_motor_feedback.rotor_abs_angle;
|
||||
ai->TX.pitch_vel=motor->motor.pitch_4310_motor_feedback.rotor_speed;
|
||||
ai->TX.yaw_vel=motor->motor.yaw_4310_motor_feedback.rotor_speed;
|
||||
ai->TX.q[0]=motor->imu.quat.q0;
|
||||
ai->TX.q[1]=motor->imu.quat.q1;
|
||||
ai->TX.q[2]=motor->imu.quat.q2;
|
||||
ai->TX.q[3]=motor->imu.quat.q3;
|
||||
mcu->id=ID_MCU;
|
||||
mcu->data.mode=0;
|
||||
mcu->data.q[0]=motor->imu.quat.q0;
|
||||
mcu->data.q[1]=motor->imu.quat.q1;
|
||||
mcu->data.q[2]=motor->imu.quat.q2;
|
||||
mcu->data.q[3]=motor->imu.quat.q3;
|
||||
mcu->data.yaw=motor->imu.eulr.yaw;
|
||||
mcu->data.yaw_vel=motor->imu.gyro.z;
|
||||
mcu->data.pitch=motor->imu.eulr.rol;
|
||||
mcu->data.pitch_vel=motor->imu.gyro.x;
|
||||
mcu->data.bullet_count=0;
|
||||
mcu->data.bullet_speed=22;
|
||||
|
||||
ai->TX.bullet_count=10;
|
||||
ai->TX.bullet_speed=22;
|
||||
|
||||
ai->TX.crc16=CRC16_Calc(((const uint8_t*)&(ai->TX)),sizeof(ai->TX)-sizeof(uint16_t), CRC16_INIT );
|
||||
if(CRC16_Verify(((const uint8_t*)&(ai->TX)), sizeof(ai->TX))!=true){
|
||||
mcu->crc16=CRC16_Calc(((const uint8_t*)&(mcu)),sizeof(&mcu)-sizeof(uint16_t), CRC16_INIT );
|
||||
if(CRC16_Verify(((const uint8_t*)&(mcu)), sizeof(&mcu))!=true){
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
int8_t AI_StartSend(AI_t *ai) {
|
||||
int8_t MCU_StartSend(PackageMCU_t *mcu) {
|
||||
if(BSP_UART_Transmit(BSP_UART_1, (uint8_t *)mcu, sizeof(*mcu), true)==HAL_OK)
|
||||
return DEVICE_OK;
|
||||
|
||||
if (BSP_UART_Transmit(BSP_UART_NUC,&ai->TX,sizeof(ai->TX), true)==HAL_OK)
|
||||
return DEVICE_OK;
|
||||
// else
|
||||
// return DEVICE_ERR;
|
||||
// } else {
|
||||
// if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
|
||||
// (uint8_t *)&(ai->TX),
|
||||
// sizeof(ai->TX)) == HAL_OK)
|
||||
// return DEVICE_OK;
|
||||
// else
|
||||
// return DEVICE_ERR;
|
||||
// }
|
||||
}
|
||||
|
||||
|
||||
// #include "ai.h"
|
||||
// #include "bsp/uart.h"
|
||||
// #include "component/crc16.h"
|
||||
|
||||
|
||||
|
||||
// // void ai_init(AI_t* ai){
|
||||
// // ai->RX.head[0]="M";
|
||||
// // ai->RX.head[0]="R";
|
||||
// // ai->TX.head[0]="M";
|
||||
// // ai->TX.head[0]="R";
|
||||
// // }
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// int8_t AI_StartReceiving(AI_t *ai) {
|
||||
// if (BSP_UART_Receive(BSP_UART_NUC,&ai->RX,sizeof(ai->RX), false))
|
||||
// return DEVICE_OK;
|
||||
// return DEVICE_ERR;
|
||||
// }
|
||||
|
||||
// int8_t AI_Get_NUC(AI_t *ai,AI_cmd_t* ai_cmd) {
|
||||
// if(ai->RX.head[0]!='M'&&ai->RX.head[1]!='R'){
|
||||
// return DEVICE_ERR;
|
||||
// }
|
||||
// // CRC16_Calc(&ai->RX,sizeof(ai->RX),ai->RX.crc16);
|
||||
// // if(CRC16_Verify((const uint8_t*)&(ai->RX), sizeof(ai->RX.crc16))!=true){
|
||||
// // return DEVICE_ERR;
|
||||
// // }
|
||||
// ai_cmd->gimbal_t.setpoint.pit=ai->RX.pitch;
|
||||
// ai_cmd->gimbal_t.setpoint.yaw=ai->RX.yaw;
|
||||
// ai_cmd->mode=ai->RX.mode;
|
||||
// ai_cmd->gimbal_t.accl.pit=ai->RX.pitch_acc;
|
||||
// ai_cmd->gimbal_t.vel.pit=ai->RX.pitch_vel;
|
||||
// ai_cmd->gimbal_t.accl.yaw=ai->RX.yaw_acc;
|
||||
// ai_cmd->gimbal_t.vel.yaw=ai->RX.yaw_vel;
|
||||
// return DEVICE_OK;
|
||||
// }
|
||||
|
||||
|
||||
// int8_t AI_ParseHost(AI_t *ai,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) {
|
||||
|
||||
// ai->TX.head[0]='M';
|
||||
// ai->TX.head[1]='R';
|
||||
// // ai->TX.mode=2;
|
||||
// ai->TX.pitch=motor->motor.pitch_4310_motor_feedback.rotor_abs_angle;
|
||||
// ai->TX.yaw=motor->motor.yaw_4310_motor_feedback.rotor_abs_angle;
|
||||
// ai->TX.pitch_vel=motor->motor.pitch_4310_motor_feedback.rotor_speed;
|
||||
// ai->TX.yaw_vel=motor->motor.yaw_4310_motor_feedback.rotor_speed;
|
||||
// ai->TX.q[0]=motor->imu.quat.q1;
|
||||
// ai->TX.q[1]=motor->imu.quat.q2;
|
||||
// ai->TX.q[2]=motor->imu.quat.q3;
|
||||
// ai->TX.q[3]=motor->imu.quat.q0;
|
||||
|
||||
// ai->TX.bullet_count=10;
|
||||
// ai->TX.bullet_speed=10;
|
||||
|
||||
// ai->TX.crc16=CRC16_Calc(((const uint8_t*)&(ai->TX)),sizeof(ai->TX)-sizeof(uint16_t),ai->TX.crc16);
|
||||
// if(CRC16_Verify(((const uint8_t*)&(ai->TX)), sizeof(ai->TX))!=true){
|
||||
// return DEVICE_ERR;
|
||||
// }
|
||||
// return DEVICE_OK;
|
||||
// }
|
||||
|
||||
// int8_t AI_StartSend(AI_t *ai) {
|
||||
// // if (ref_update) {
|
||||
// if (BSP_UART_Transmit(BSP_UART_NUC,&ai->TX,sizeof(ai->TX), false))
|
||||
// return DEVICE_OK;
|
||||
// // else
|
||||
// // return DEVICE_ERR;
|
||||
// // } else {
|
||||
// // if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
|
||||
// // (uint8_t *)&(ai->TX),
|
||||
// // sizeof(ai->TX)) == HAL_OK)
|
||||
// // return DEVICE_OK;
|
||||
// // else
|
||||
// // return DEVICE_ERR;
|
||||
// // }
|
||||
// }
|
||||
|
||||
187
User/device/ai.h
187
User/device/ai.h
@ -9,73 +9,162 @@ extern "C" {
|
||||
#include "component\user_math.h"
|
||||
#include "remote_control.h"
|
||||
#include "module/gimbal.h"
|
||||
#include "stdint.h"
|
||||
// struct __attribute__((packed)) GimbalToVision
|
||||
// {
|
||||
// uint8_t head[2];
|
||||
// uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
|
||||
// float q[4]; // wxyz顺序 /q4,q0,q1,q2/
|
||||
// float yaw;
|
||||
// float yaw_vel;
|
||||
// float pitch;
|
||||
// float pitch_vel;
|
||||
// float bullet_speed;
|
||||
// uint16_t bullet_count; // 子弹累计发送次数
|
||||
// uint16_t crc16;
|
||||
// };
|
||||
|
||||
struct __attribute__((packed)) GimbalToVision
|
||||
{
|
||||
uint8_t head[2];
|
||||
uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
|
||||
float q[4]; // wxyz顺序 /q4,q0,q1,q2/
|
||||
float yaw;
|
||||
float yaw_vel;
|
||||
float pitch;
|
||||
float pitch_vel;
|
||||
float bullet_speed;
|
||||
uint16_t bullet_count; // 子弹累计发送次数
|
||||
uint16_t crc16;
|
||||
};
|
||||
// struct __attribute__((packed)) VisionToGimbal
|
||||
// {
|
||||
// uint8_t head[2];
|
||||
// uint8_t mode; // 0: 不控制, 1: 控制云台但不开火，2: 控制云台且开火
|
||||
// float yaw;
|
||||
// float yaw_vel;
|
||||
// float yaw_acc;
|
||||
// float pitch;
|
||||
// float pitch_vel;
|
||||
// float pitch_acc;
|
||||
// uint16_t crc16;
|
||||
// };
|
||||
|
||||
struct __attribute__((packed)) VisionToGimbal
|
||||
// typedef struct __attribute__((packed)) {
|
||||
// uint8_t mode;
|
||||
// struct{
|
||||
// // Gimbal_CMD_t g_cmd;
|
||||
// struct{
|
||||
// float yaw;
|
||||
// float pit;
|
||||
// }setpoint;
|
||||
|
||||
|
||||
// struct{
|
||||
// float pit;
|
||||
// float yaw;
|
||||
// }accl;
|
||||
// struct{
|
||||
// float pit;
|
||||
// float yaw;
|
||||
// }vel;
|
||||
// }gimbal_t;
|
||||
|
||||
// }AI_cmd_t;
|
||||
|
||||
|
||||
// typedef struct __attribute__((packed)) {
|
||||
// struct GimbalToVision TX;
|
||||
// struct VisionToGimbal RX;
|
||||
// }AI_t;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 数据包 ID 定义
|
||||
#define ID_MCU (0xC4) // MCU 数据包
|
||||
#define ID_REF (0xA8) // 裁判系统数据包
|
||||
#define ID_AI (0xB5) // AI 控制数据包
|
||||
|
||||
// MCU 数据结构（MCU -> 上位机）
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t head[2];
|
||||
uint8_t mode; // 0: 不控制, 1: 控制云台但不开火，2: 控制云台且开火
|
||||
float yaw;
|
||||
float yaw_vel;
|
||||
float yaw_acc;
|
||||
float pitch;
|
||||
float pitch_vel;
|
||||
float pitch_acc;
|
||||
uint16_t crc16;
|
||||
};
|
||||
uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
|
||||
float q[4]; // 四元数 wxyz 顺序
|
||||
float yaw; // 偏航角
|
||||
float yaw_vel; // 偏航角速度
|
||||
float pitch; // 俯仰角
|
||||
float pitch_vel; // 俯仰角速度
|
||||
float bullet_speed; // 弹速
|
||||
uint16_t bullet_count; // 子弹累计发送次数
|
||||
} DataMCU_t;
|
||||
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t id; // 数据包 ID: 0xC4
|
||||
DataMCU_t data;
|
||||
uint16_t crc16;
|
||||
} PackageMCU_t;
|
||||
|
||||
// 裁判系统数据结构
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint16_t remain_hp; // 剩余血量
|
||||
uint8_t game_progress : 4; // 比赛进度
|
||||
uint16_t stage_remain_time; // 比赛剩余时间
|
||||
} DataReferee_t;
|
||||
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t id; // 数据包 ID: 0xA8
|
||||
DataReferee_t data;
|
||||
uint16_t crc16;
|
||||
} PackageReferee_t;
|
||||
|
||||
// AI 控制数据结构（上位机 -> MCU）
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t mode; // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
|
||||
float yaw; // 目标偏航角
|
||||
float yaw_vel; // 偏航角速度
|
||||
float yaw_acc; // 偏航角加速度
|
||||
float pitch; // 目标俯仰角
|
||||
float pitch_vel; // 俯仰角速度
|
||||
float pitch_acc; // 俯仰角加速度
|
||||
float vx; // X 方向速度
|
||||
float vy; // Y 方向速度
|
||||
float wz; // Z 方向角速度
|
||||
uint8_t reserved; // 预留字段
|
||||
} DataAI_t;
|
||||
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t id; // 数据包 ID: 0xB5
|
||||
DataAI_t data;
|
||||
uint16_t crc16;
|
||||
} PackageAI_t;
|
||||
|
||||
typedef struct __attribute__((packed)) {
|
||||
uint8_t mode;
|
||||
uint8_t mode; // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
|
||||
struct{
|
||||
// Gimbal_CMD_t g_cmd;
|
||||
struct{
|
||||
float yaw;
|
||||
float pit;
|
||||
float yaw; // 目标偏航角
|
||||
float pit; // 目标俯仰角
|
||||
}setpoint;
|
||||
|
||||
|
||||
struct{
|
||||
float pit;
|
||||
float yaw;
|
||||
float pit; // 俯仰角加速度
|
||||
float yaw; // 偏航角加速度
|
||||
}accl;
|
||||
struct{
|
||||
float pit;
|
||||
float yaw;
|
||||
float pit; // 俯仰角速度
|
||||
float yaw; // 偏航角速度
|
||||
}vel;
|
||||
}gimbal_t;
|
||||
|
||||
}AI_cmd_t;
|
||||
struct{
|
||||
float Vx; // X 方向速度
|
||||
float Vy; // Y 方向速度
|
||||
float Vw; // Z 方向角速度
|
||||
}chassis_t;
|
||||
|
||||
uint8_t reserved; // 预留字段
|
||||
}AI_result_t; //接收到的所有数据，来自NUC
|
||||
|
||||
|
||||
typedef struct __attribute__((packed)) {
|
||||
struct GimbalToVision TX;
|
||||
struct VisionToGimbal RX;
|
||||
}AI_t;
|
||||
|
||||
|
||||
|
||||
int8_t AI_StartReceiving(AI_t *ai) ;
|
||||
|
||||
int8_t AI_Get_NUC(AI_t * ai,AI_cmd_t* ai_cmd) ;
|
||||
|
||||
|
||||
int8_t AI_ParseHost(AI_t * ai,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) ;
|
||||
|
||||
int8_t AI_StartSend(AI_t *ai) ;
|
||||
|
||||
int8_t MCU_Send(PackageMCU_t* mcu,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat);
|
||||
int8_t MCU_StartSend(PackageMCU_t *mcu);
|
||||
int8_t AI_Get_NUC(PackageAI_t *ai,AI_result_t* result);
|
||||
int8_t AI_StartReceiving(PackageAI_t *ai);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -12,11 +12,11 @@
|
||||
|
||||
// /* USER INCLUDE BEGIN */
|
||||
|
||||
// ///* USER INCLUDE END */
|
||||
// // ///* USER INCLUDE END */
|
||||
|
||||
// /* USER DEFINE BEGIN */
|
||||
|
||||
// ///* USER DEFINE END */
|
||||
// // ///* USER DEFINE END */
|
||||
|
||||
// /* Exported constants ------------------------------------------------------- */
|
||||
// /* Exported macro ----------------------------------------------------------- */
|
||||
@ -110,7 +110,7 @@
|
||||
|
||||
// /* USER FUNCTION BEGIN */
|
||||
|
||||
// ///* USER FUNCTION END */
|
||||
// // ///* USER FUNCTION END */
|
||||
|
||||
// #ifdef __cplusplus
|
||||
// }
|
||||
|
||||
@ -74,7 +74,7 @@ static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module
|
||||
static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) {
|
||||
switch (module) {
|
||||
case MOTOR_M2006: return 36.0f;
|
||||
case MOTOR_M3508: return 3591.0f / 187.0f;
|
||||
case MOTOR_M3508: return 268.0f / 17.0f;
|
||||
case MOTOR_GM6020: return 1.0f;
|
||||
default: return 1.0f;
|
||||
}
|
||||
|
||||
@ -73,7 +73,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_mode_t mode ,uint32_t now)
|
||||
|
||||
if (mode == CHASSIS_MODE_ROTOR && c->mode != CHASSIS_MODE_ROTOR) {
|
||||
srand(now);
|
||||
c->wz_multi =1;
|
||||
c->wz_multi =0.1;
|
||||
// c->wz_multi = (rand() % 2) ? -1 : 1;
|
||||
}
|
||||
for (int i = 0; i < 4; i++)
|
||||
@ -81,6 +81,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_mode_t mode ,uint32_t now)
|
||||
PID_Reset(&c->pid.chassis_3508VPID[i]);
|
||||
PID_Reset(&c->pid.chassis_6020OmegaPid[i]);
|
||||
PID_Reset(&c->pid.chassis_6020anglePid[i]);
|
||||
PID_Reset(&c->pid.chassis_3508DAOHANG_pid[i]);
|
||||
}
|
||||
|
||||
c->mode = mode;
|
||||
@ -142,6 +143,10 @@ int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
|
||||
PID_Init(&c->pid.chassis_6020OmegaPid[i], KPID_MODE_CALC_D, target_freq, &c->param->C6020Omega_param);
|
||||
PID_Init(&c->pid.chassis_6020anglePid[i], KPID_MODE_CALC_D, target_freq, &c->param->C6020Angle_param);
|
||||
}
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
PID_Init(&c->pid.chassis_3508DAOHANG_pid[i], KPID_MODE_CALC_D, target_freq, &c->param->M3508DAOHANG_param);
|
||||
}
|
||||
PID_Init(&c->pid.chassis_follow_gimbal_pid, KPID_MODE_CALC_D, target_freq, &c->param->chassis_follow_gimbalPID);
|
||||
|
||||
LowPassFilter2p_Init(&c->filled[0], target_freq, 20.0f); // vx
|
||||
@ -203,7 +208,7 @@ void Chassis_speed_calculate(Chassis_t *c, Chassis_CMD_t *c_cmd)
|
||||
|
||||
case CHASSIS_MODE_ROTOR:
|
||||
case CHASSIS_MODE_FOLLOW_GIMBAL:
|
||||
|
||||
case CHASSIS_MODE_DAOHANG:
|
||||
|
||||
// const double radians = atan(1.0f * 330 / 330);
|
||||
|
||||
@ -320,7 +325,7 @@ int8_t Chassis_update(Chassis_t *c)
|
||||
c->motorfeedback.rotor_rpm6020[i] = (MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor)) / 320);
|
||||
c->motorfeedback.rotor_rpm3508[i] = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_3508_param[i])->motor)) /10000;
|
||||
// c->motorfeedback.rotor_angle6020[i] = MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor));
|
||||
|
||||
c->motorfeedback.rotor_truespeed3508[i]=MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_3508_param[i])->motor))*17.0f/268.0f/60.0f*M_PI*0.116f;
|
||||
c->motorfeedback.rotor_angle6020[i] = fmod(c->motorfeedback.rotor_angle6020[i] - c->motoroffset.MOTOR_OFFSET[i], 360.0);
|
||||
if (c->motorfeedback.rotor_angle6020[i] < 0)
|
||||
{
|
||||
@ -338,6 +343,7 @@ void ChassisrolPrevent(Chassis_t *c)
|
||||
{
|
||||
}
|
||||
|
||||
float beta,cos_beta,sin_beta;
|
||||
int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
|
||||
{
|
||||
|
||||
@ -383,9 +389,9 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
|
||||
case CHASSIS_MODE_ROTOR:
|
||||
// 小陀螺模式
|
||||
|
||||
float beta = (c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle / 180.0f * M_PI) - c->mech_zero_4310; // 云台当前角度转弧度
|
||||
float cos_beta = cosf(beta);
|
||||
float sin_beta = sinf(beta);
|
||||
beta = (c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle / 180.0f * M_PI) - c->mech_zero_4310; // 云台当前角度转弧度
|
||||
cos_beta = cosf(beta);
|
||||
sin_beta = sinf(beta);
|
||||
|
||||
c->move_vec.Vx =cos_beta * c_cmd->x_l - sin_beta * c_cmd->y_l;
|
||||
c->move_vec.Vy =sin_beta* c_cmd->x_l + cos_beta * c_cmd->y_l;
|
||||
@ -396,12 +402,17 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
|
||||
// 跟随云台模式
|
||||
c->move_vec.Vx =-c_cmd->y_l;
|
||||
c->move_vec.Vy =-c_cmd->x_l;
|
||||
c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 2.06f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);
|
||||
c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 1.44215584f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);
|
||||
|
||||
// c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, c->motorfeedback.gimbal_yaw_encoder-2.07694483f ,c->motorfeedback.gimbal_yaw_encoder, 0.0f, c->dt);
|
||||
break;
|
||||
|
||||
|
||||
case CHASSIS_MODE_DAOHANG:
|
||||
// 导航模式
|
||||
c->move_vec.Vx = -c_cmd->Vx/1;
|
||||
c->move_vec.Vy = c_cmd->Vy/1;
|
||||
c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 1.44215584f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);;
|
||||
break;
|
||||
|
||||
default:
|
||||
return CHASSIS_ERR_MODE; /* 未知模式 */
|
||||
@ -414,6 +425,14 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
|
||||
|
||||
Chassis_speed_calculate(c, c_cmd);
|
||||
|
||||
switch (c->mode)
|
||||
{ case CHASSIS_MODE_BREAK:
|
||||
case CHASSIS_MODE_RELAX:
|
||||
case STOP:
|
||||
case RC:
|
||||
case CHASSIS_MODE_FOLLOW_GIMBAL:
|
||||
case CHASSIS_MODE_ROTOR:
|
||||
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
float chassis6020_detangle[4]; // 6020解算出的角度
|
||||
@ -431,6 +450,26 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
|
||||
c->motorfeedback.rotor_rpm3508[i], 0.0f, c->dt);
|
||||
c->out.rotor3508_out[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_3508out[i]);
|
||||
}
|
||||
|
||||
case CHASSIS_MODE_DAOHANG:
|
||||
for(int i=0;i<4;i++)
|
||||
{
|
||||
float chassis6020_detangle[4]; // 6020解算出的角度
|
||||
c->hopemotorout.motor6020_target[i] = c->hopemotorout.rotor6020_jiesuan_2[i];
|
||||
chassis6020_detangle[i] = PID_Calc(&(c->pid.chassis_6020anglePid[i]), c->hopemotorout.motor6020_target[i],
|
||||
c->motorfeedback.rotor_angle6020[i], 0.0f, c->dt);
|
||||
// c->final_out.final_6020out[i] = chassis6020_detangle[i] ; //单环控制就用这个
|
||||
c->final_out.final_6020out[i] = PID_Calc(&(c->pid.chassis_6020OmegaPid[i]), chassis6020_detangle[i],
|
||||
c->motorfeedback.rotor_rpm6020[i], 0.0f, c->dt);
|
||||
|
||||
c->out.rotor6020_out[i] = LowPassFilter2p_Apply(&c->filled[7+i], c->final_out.final_6020out[i]);
|
||||
|
||||
c->hopemotorout.motor3508_target[i] = c->hopemotorout.rotor3508_jiesuan_2[i];
|
||||
c->final_out.final_3508out[i] = PID_Calc(&(c->pid.chassis_3508DAOHANG_pid[i]), c->hopemotorout.motor3508_target[i],
|
||||
c->motorfeedback.rotor_truespeed3508[i], 0.0f, c->dt);
|
||||
c->out.rotor3508_out[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_3508out[i]);
|
||||
}
|
||||
}
|
||||
return CHASSIS_OK;
|
||||
}
|
||||
|
||||
|
||||
@ -55,6 +55,9 @@ extern "C"
|
||||
KPID_Params_t C6020Omega_param;
|
||||
KPID_Params_t C6020Angle_param;
|
||||
KPID_Params_t M3508v_param;
|
||||
|
||||
KPID_Params_t M3508DAOHANG_param;
|
||||
|
||||
KPID_Params_t chassis_follow_gimbalPID;
|
||||
|
||||
MOTOR_RM_Param_t motor_3508_param[4]; // 四个3508电机
|
||||
@ -113,6 +116,7 @@ extern "C"
|
||||
{
|
||||
fp32 rotor_rpm3508[4];
|
||||
fp32 rotor_current3508[4];
|
||||
fp32 rotor_truespeed3508[4];
|
||||
fp32 rotor_rpm6020[4];
|
||||
fp32 rotor_angle6020[4];
|
||||
fp32 rotor_current6020[4];
|
||||
@ -126,6 +130,9 @@ extern "C"
|
||||
KPID_t chassis_6020anglePid[4];
|
||||
KPID_t chassis_6020OmegaPid[4];
|
||||
KPID_t chassis_3508VPID[4];
|
||||
|
||||
KPID_t chassis_3508DAOHANG_pid[4];
|
||||
|
||||
KPID_t chassis_follow_gimbal_pid;
|
||||
} pid;
|
||||
|
||||
|
||||
@ -147,18 +147,22 @@ int8_t CMD_CtrlSet(Chassis_CMD_t *c_cmd, const CMD_RC_t *rc, Gimbal_CMD_t *g_cmd
|
||||
case CMD_SW_UP:
|
||||
g_cmd->ctrl_mode = GIMBAL_MODE_REMOTE;
|
||||
s_cmd->control_mode = SHOOT_REMOTE;
|
||||
// c_cmd->mode = CHASSIS_MODE_FOLLOW_GIMBAL;
|
||||
break;
|
||||
case CMD_SW_MID:
|
||||
g_cmd->ctrl_mode = GIMBAL_MODE_AI;
|
||||
s_cmd->control_mode = SHOOT_AI;
|
||||
// c_cmd->mode = CHASSIS_MODE_FOLLOW_GIMBAL;
|
||||
break;
|
||||
case CMD_SW_DOWN:
|
||||
g_cmd->ctrl_mode = GIMBAL_MODE_AI;
|
||||
s_cmd->control_mode = SHOOT_AI;
|
||||
c_cmd->mode = CHASSIS_MODE_DAOHANG;
|
||||
break;
|
||||
case CMD_SW_ERR:
|
||||
g_cmd->ctrl_mode = GIMBAL_MODE_REMOTE;
|
||||
s_cmd->control_mode = SHOOT_REMOTE;
|
||||
c_cmd->mode = CHASSIS_MODE_FOLLOW_GIMBAL;
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
@ -81,6 +81,7 @@ typedef enum
|
||||
CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */
|
||||
CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */
|
||||
|
||||
CHASSIS_MODE_DAOHANG, /* 导航模式，底盘根据导航数据进行移动 */
|
||||
} Chassis_mode_t;
|
||||
|
||||
typedef struct
|
||||
@ -118,8 +119,12 @@ typedef struct {
|
||||
float delta_pitch_4310;
|
||||
float delta_yaw_6020;
|
||||
GIMBAL_Ctrl_mode_t ctrl_mode;
|
||||
float set_yaw;
|
||||
float set_pitch;
|
||||
float set_yaw; /*自瞄YAW目标值*/
|
||||
float set_pitch;/*自瞄PITCH目标值*/
|
||||
float yaw_vel; /*自瞄YAW角速度*/
|
||||
float yaw_accl; /*自瞄YAW加速度*/
|
||||
float pit_vel; /*自瞄PITCH角速度*/
|
||||
float pit_accl; /*自瞄PITCH加速度*/
|
||||
} Gimbal_CMD_t;
|
||||
|
||||
typedef enum {
|
||||
|
||||
@ -65,6 +65,18 @@ static const Config_Param_t config = {
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f
|
||||
},
|
||||
|
||||
.M3508DAOHANG_param={
|
||||
.k = 1.0f,
|
||||
.p = 2.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f
|
||||
},
|
||||
|
||||
.chassis_follow_gimbalPID = {
|
||||
.k = 0.7f,
|
||||
.p = 0.5f,
|
||||
@ -115,12 +127,12 @@ static const Config_Param_t config = {
|
||||
},
|
||||
|
||||
.pid.pitch_4310_motor_omega = {
|
||||
.k = 0.3f,
|
||||
.p = 0.3f,
|
||||
.k = 0.5f,
|
||||
.p = 0.5f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 1.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 10.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f
|
||||
},
|
||||
@ -174,12 +186,22 @@ static const Config_Param_t config = {
|
||||
// .d_cutoff_freq = 0.0f,
|
||||
// .range = M_2PI
|
||||
|
||||
.k = 2.0f,
|
||||
.p = 7.0f,
|
||||
.i = 0.5f,
|
||||
.d = 1.0f,
|
||||
/*双环pid参数*/
|
||||
// .k = 2.0f,
|
||||
// .p = 7.0f,
|
||||
// .i = 0.5f,
|
||||
// .d = 1.0f,
|
||||
// .i_limit = 1.0f,
|
||||
// .out_limit = 1.0f,
|
||||
// .d_cutoff_freq = 0.0f,
|
||||
// .range = M_2PI
|
||||
|
||||
.k = 1.0f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 10.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = 0.0f,
|
||||
.range = M_2PI
|
||||
},
|
||||
@ -214,12 +236,12 @@ static const Config_Param_t config = {
|
||||
},
|
||||
|
||||
.mech_zero = {
|
||||
.yaw_6020 = 1.31f,
|
||||
.yaw_6020 = 1.31f,/*1.31*/
|
||||
.yaw_4310 = 2.06f, /*大yaw零点*/
|
||||
.pitch_4310 = 0.93f,
|
||||
},
|
||||
.travel = {
|
||||
.yaw_6020 = 1.4f,
|
||||
.yaw_6020 = 1.59f,/*1.59*/
|
||||
.yaw_4310 = -1.0f, /*大yaw无限位*/
|
||||
.pitch_4310 = 0.9f,
|
||||
// .pitch_4310=-1.0f,/*pitch无限位*/
|
||||
@ -227,6 +249,10 @@ static const Config_Param_t config = {
|
||||
.low_pass_cutoff_freq={
|
||||
.out = -1.0f,
|
||||
.gyro = 20.0f,
|
||||
},
|
||||
.K_forward_pid={
|
||||
.K_vel=0.2f,
|
||||
.K_accl=0.1f,
|
||||
}
|
||||
|
||||
},
|
||||
|
||||
@ -177,6 +177,7 @@ int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal)
|
||||
return GIMBAL_OK;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
*更新陀螺仪数据欧拉角的值
|
||||
*/
|
||||
@ -193,9 +194,6 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
|
||||
|
||||
gimbal->feedback.imu.quat = imu->quat;
|
||||
|
||||
gimbal->feedback.imu.eulr.yaw = LowPassFilter2p_Apply(&gimbal->filter_out.gyro_yaw, gimbal->feedback.imu.eulr.yaw);
|
||||
gimbal->feedback.imu.eulr.pit = LowPassFilter2p_Apply(&gimbal->filter_out.gyro_pitch, gimbal->feedback.imu.eulr.pit);
|
||||
gimbal->feedback.imu.eulr.rol = LowPassFilter2p_Apply(&gimbal->filter_out.gyro_rol, gimbal->feedback.imu.eulr.rol);
|
||||
|
||||
|
||||
|
||||
@ -233,7 +231,8 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
|
||||
// float delta_max_pitch;
|
||||
// float delta_min_pitch;
|
||||
/*这三个参数是用来调pitch的*/
|
||||
|
||||
float yaw_omega_set_point, pitch_omega_set_point,yaw_6020_omega_set_point;
|
||||
float motor_imu_offset_6020;
|
||||
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd)
|
||||
{
|
||||
if (g == NULL || g_cmd == NULL)
|
||||
@ -252,10 +251,12 @@ float delta_yaw_6020 = 10.0f*g_cmd->delta_yaw_6020 * g->dt;
|
||||
if(g->param->travel.yaw_6020 > 0) // 有限位才处理
|
||||
{
|
||||
/* 计算当前电机角度与IMU角度的偏差 */
|
||||
float motor_imu_offset_6020 = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle - g->feedback.imu.eulr.yaw;
|
||||
/*float*/
|
||||
motor_imu_offset_6020 = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle - g->feedback.imu.eulr.yaw;
|
||||
/* 处理跨越±π的情况 */
|
||||
if (motor_imu_offset_6020 > M_PI) motor_imu_offset_6020 -= M_2PI;
|
||||
if (motor_imu_offset_6020 < -M_PI) motor_imu_offset_6020 += M_2PI;
|
||||
|
||||
/*计算限位距离*/
|
||||
const float delta_max_6020 = CircleError(g->limit.yaw_6020.max,
|
||||
(g->setpoint.eulr.yaw + motor_imu_offset_6020 + delta_yaw_6020), M_2PI);
|
||||
@ -266,6 +267,7 @@ const float delta_min_6020 = CircleError(g->limit.yaw_6020.min,
|
||||
if(delta_yaw_6020 < delta_min_6020) delta_yaw_6020 = delta_min_6020;
|
||||
}
|
||||
CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw_6020, M_2PI);
|
||||
|
||||
// g->setpoint.eulr.yaw = g->setpoint.NUC_Yaw;
|
||||
|
||||
/*处理大yaw控制命令，软件限位 - 使用电机绝对角度*/
|
||||
@ -304,7 +306,7 @@ if(g->param->travel.pitch_4310 > 0) // 有限位才处理
|
||||
// g->setpoint.eulr.pit = g->setpoint.NUC_Pitch;
|
||||
|
||||
/* 控制相关逻辑 */
|
||||
float yaw_omega_set_point, pitch_omega_set_point,yaw_6020_omega_set_point;
|
||||
|
||||
switch (g->mode) {
|
||||
case GIMBAL_MODE_RELAX:
|
||||
g->out.yaw_4310 = 0.0f;
|
||||
@ -322,29 +324,35 @@ if(g_cmd->ctrl_mode == GIMBAL_MODE_REMOTE){
|
||||
else if (g_cmd->ctrl_mode == GIMBAL_MODE_AI) {
|
||||
g->setpoint.eulr.yaw = g_cmd->set_yaw;
|
||||
g->setpoint.eulr.pit = g_cmd->set_pitch;
|
||||
|
||||
g->setpoint.yaw_vel = g_cmd->yaw_vel;
|
||||
g->setpoint.yaw_accl = g_cmd->yaw_accl;
|
||||
g->setpoint.pit_vel = g_cmd->pit_vel;
|
||||
g->setpoint.pit_accl = g_cmd->pit_accl;
|
||||
}
|
||||
|
||||
yaw_6020_omega_set_point =PID_Calc(&g->pid.yaw_6020_angle,g->setpoint.eulr.yaw,
|
||||
g->feedback.imu.eulr.yaw,0.0f,g->dt);
|
||||
g->out.yaw_6020 = PID_Calc(&g->pid.yaw_6020_omega,yaw_6020_omega_set_point,
|
||||
g->feedback.imu.gyro.z,0.0f,g->dt);
|
||||
// g->out.yaw_6020 = g->pid.yaw_6020_omega,yaw_6020_omega_set_point,
|
||||
// g-PID_Calc(&>feedback.imu.gyro.z,0.0f,g->dt);
|
||||
g->out.yaw_6020 = yaw_6020_omega_set_point+g->param->K_forward_pid.K_vel*g->setpoint.yaw_vel+g->param->K_forward_pid.K_accl*g->setpoint.yaw_accl;
|
||||
/*4310大YAW控制
|
||||
这里是单环控制的，有需要加双环
|
||||
*/
|
||||
|
||||
|
||||
yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310,
|
||||
yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310,
|
||||
g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_6020_motor_feedback.rotor_speed,g->dt);
|
||||
|
||||
|
||||
g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point,
|
||||
g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,0.0f,g->dt);
|
||||
// g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出
|
||||
/*控制云台4310电机 也是单环，但是加了重力补偿函数，可以根据不一样的情况去拟合函数*/
|
||||
pitch_omega_set_point =PID_Calc(&g->pid.pitch_4310_angle,g->setpoint.eulr.pit,
|
||||
g->feedback.imu.eulr.rol,0.0f,g->dt);
|
||||
|
||||
g->out.pitch_4310 = pitch_omega_set_point+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle);
|
||||
g->feedback.imu.eulr.rol,-g->feedback.imu.gyro.y,g->dt);
|
||||
g->out.pitch_4310 = pitch_omega_set_point+g->param->K_forward_pid.K_vel*g->setpoint.pit_vel+g->param->K_forward_pid.K_accl*g->setpoint.pit_accl+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle);
|
||||
// g->out.pitch_4310 = PID_Calc(&g->pid.pitch_4310_omega,8*pitch_omega_set_point,
|
||||
// -g->feedback.imu.gyro.y,0.0f,g->dt)+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle);
|
||||
// g->out.pitch_4310 = pitch_omega_set_point+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle);
|
||||
|
||||
|
||||
|
||||
@ -358,7 +366,9 @@ yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310,
|
||||
|
||||
/* 6020输出滤波 4310个人觉得不是很需要滤波，可以自己加*/
|
||||
// g->out.yaw_6020 = LowPassFilter2p_Apply(&g->filter_out.yaw_6020, g->out.yaw_6020);
|
||||
|
||||
g->out.yaw_6020 = LowPassFilter2p_Apply(&g->filter_out.yaw_6020, g->out.yaw_6020);
|
||||
g->out.pitch_4310 = LowPassFilter2p_Apply(&g->filter_out.pitch_4310, g->out.pitch_4310);
|
||||
g->out.yaw_4310 = LowPassFilter2p_Apply(&g->filter_out.yaw_4310, g->out.yaw_4310);
|
||||
|
||||
}
|
||||
return 0;
|
||||
@ -381,9 +391,9 @@ void Gimbal_Output(Gimbal_t *g)
|
||||
MOTOR_RM_SetOutput(&g->param->yaw_6020_motor, g->out.yaw_6020);
|
||||
|
||||
MOTOR_MIT_Output_t output_yaw_4310 = {0};
|
||||
output_yaw_4310.torque = g->out.yaw_4310 * 2.0f;
|
||||
output_yaw_4310.torque = g->out.yaw_4310 * 3.0f;
|
||||
// output_yaw_4310.kp = 0.2f;
|
||||
output_yaw_4310.kd = 0.1f;
|
||||
// output_yaw_4310.kd = 0.1f;
|
||||
// output_yaw_4310.kd=0.1f;
|
||||
|
||||
MOTOR_MIT_Output_t output_pitch_4310 = {0};
|
||||
|
||||
@ -65,6 +65,12 @@ extern "C"
|
||||
float pitch_4310; /* pitch轴4310机械限位行程 -1表示无限位*/
|
||||
} travel;
|
||||
|
||||
struct {
|
||||
float K_vel; /*前馈速度*/
|
||||
float K_accl; /*前馈加速度*/
|
||||
}K_forward_pid;
|
||||
|
||||
|
||||
} Gimbal_Param_t;
|
||||
|
||||
typedef struct
|
||||
@ -110,6 +116,12 @@ extern "C"
|
||||
AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */
|
||||
float yaw_4310; /* 大yaw电机目标角度 */
|
||||
|
||||
float yaw_vel; /*yaw自瞄角速度*/
|
||||
float yaw_accl; /*yaw自瞄加速度*/
|
||||
|
||||
float pit_vel; /*pitch自瞄角速度*/
|
||||
float pit_accl; /*pitch自瞄加速度*/
|
||||
|
||||
float NUC_Pitch; /* 自瞄用pitch目标角度 */
|
||||
float NUC_Yaw; /* 自瞄用yaw目标角度 */
|
||||
} setpoint;
|
||||
|
||||
@ -15,10 +15,12 @@
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
AI_t ai;
|
||||
AI_cmd_t ai_cmd;
|
||||
PackageAI_t ai;
|
||||
PackageMCU_t mcu;
|
||||
AI_result_t ai_result;
|
||||
AHRS_Quaternion_t quat;
|
||||
Gimbal_feedback_t gimbal_motor;
|
||||
PackageMCU_t shoot_mcu_package; /* 新增的 ai 数据包 主要是给自瞄发送射击数量*/
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -42,14 +44,17 @@ void Task_ai(void *argument) {
|
||||
/* USER CODE BEGIN */
|
||||
osMessageQueueGet(task_runtime.msgq.gimbal.ai.quat, &quat, NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.gimbal.ai.feedback, &gimbal_motor, NULL, 0);
|
||||
AI_ParseHost(&ai,&gimbal_motor,&quat);
|
||||
AI_StartSend(&ai);
|
||||
osMessageQueueGet(task_runtime.msgq.shoot.ai.s_cmd_ai_bool_count, &shoot_mcu_package, NULL, 0);
|
||||
MCU_Send(&mcu,&gimbal_motor,&quat);
|
||||
mcu.data.bullet_count=shoot_mcu_package.data.bullet_count; /* 从 shoot 任务获取射击数量并放入 mcu 数据包中 */
|
||||
MCU_StartSend(&mcu);
|
||||
|
||||
AI_StartReceiving(&ai);
|
||||
AI_Get_NUC(&ai,&ai_cmd);
|
||||
AI_Get_NUC(&ai,&ai_result);
|
||||
osMessageQueueReset(task_runtime.msgq.gimbal.ai.g_cmd);
|
||||
osMessageQueuePut(task_runtime.msgq.gimbal.ai.g_cmd, &ai_cmd, 0, 0);
|
||||
osMessageQueuePut(task_runtime.msgq.shoot.ai.s_cmd, &ai_cmd, 0, 0);
|
||||
osMessageQueuePut(task_runtime.msgq.gimbal.ai.g_cmd, &ai_result, 0, 0);
|
||||
osMessageQueuePut(task_runtime.msgq.shoot.ai.s_cmd, &ai_result, 0, 0);
|
||||
osMessageQueuePut(task_runtime.msgq.navi.c_cmd, &ai_result, 0, 0); /* 将 ai_result 中的射击命令布尔值发送给 shoot 任务 */
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
@ -4,6 +4,8 @@
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "cmsis_os2.h"
|
||||
#include "device/ai.h"
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "module/chassis.h"
|
||||
@ -17,6 +19,7 @@
|
||||
/* USER STRUCT BEGIN */
|
||||
Chassis_t chassis;
|
||||
Chassis_CMD_t cmd_chassis;
|
||||
AI_result_t c_cmd_ai_result; /* 新增的 ai 结果变量 主要是给底盘接收自瞄相关的命令*/
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -41,11 +44,17 @@ chassis_init(&chassis,&Config_GetRobotParam()->chassis,CHASSIS_FREQ);
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
|
||||
osMessageQueueGet(task_runtime.msgq.navi.c_cmd, &c_cmd_ai_result, NULL, 0);
|
||||
|
||||
osMessageQueueGet(task_runtime.msgq.gimbal.yaw6020,&chassis.motorfeedback.gimbal_yaw_encoder,NULL,0);
|
||||
/*接受cmd任务数据*/
|
||||
if(osMessageQueueGet(task_runtime.msgq.chassis.cmd, &cmd_chassis, NULL, 0)==osOK)
|
||||
{
|
||||
if(cmd_chassis.mode == CHASSIS_MODE_DAOHANG){
|
||||
cmd_chassis.Vx = c_cmd_ai_result.chassis_t.Vx;
|
||||
cmd_chassis.Vy = c_cmd_ai_result.chassis_t.Vy;
|
||||
cmd_chassis.Vw = c_cmd_ai_result.chassis_t.Vw;
|
||||
}
|
||||
Chassis_update(&chassis);
|
||||
Chassis_Control(&chassis, &cmd_chassis,tick);
|
||||
}else
|
||||
|
||||
@ -16,10 +16,10 @@
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
Gimbal_CMD_t cmd_gimbal; //遥控器
|
||||
Gimbal_CMD_t cmd_gimbal; //cmd命令关于云台
|
||||
Gimbal_IMU_t gimbal_imu;
|
||||
Gimbal_t gimbal;
|
||||
AI_cmd_t ai_gimbal_cmd;
|
||||
AI_result_t ai_gimbal_result_cmd; /*ai发送自瞄数据和 导航数据*/
|
||||
|
||||
Gimbal_CMD_t final_gimbal_cmd; //最终命令
|
||||
|
||||
@ -47,10 +47,10 @@ void Task_gimbal(void *argument) {
|
||||
osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu,NULL, 0);
|
||||
Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
|
||||
|
||||
osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd,&ai_gimbal_cmd,NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd,&ai_gimbal_result_cmd,NULL, 0);
|
||||
/* ai指令 */
|
||||
// if(osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd, &ai_gimbal_cmd, NULL, 0)==osOK){
|
||||
// if(ai_gimbal_cmd.mode==0){
|
||||
// if(osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd, &ai_gimbal_result_cmd, NULL, 0)==osOK){
|
||||
// if(ai_gimbal_result_cmd.mode==0){
|
||||
// final_gimbal_cmd.set_pitch =gimbal_imu.eulr.rol;
|
||||
// final_gimbal_cmd.set_yaw=gimbal_imu.eulr.yaw;
|
||||
// }
|
||||
@ -64,18 +64,26 @@ osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd,&ai_gimbal_cmd,NULL, 0);
|
||||
|
||||
osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &cmd_gimbal, NULL, 0);
|
||||
|
||||
if(ai_gimbal_cmd.mode==0){
|
||||
if(ai_gimbal_result_cmd.mode==0){
|
||||
cmd_gimbal.set_pitch =gimbal_imu.eulr.pit;
|
||||
cmd_gimbal.set_yaw=gimbal_imu.eulr.yaw;
|
||||
}
|
||||
if(ai_gimbal_cmd.mode==1)
|
||||
if(ai_gimbal_result_cmd.mode==1)
|
||||
{
|
||||
cmd_gimbal.set_pitch=ai_gimbal_cmd.gimbal_t.setpoint.pit;
|
||||
cmd_gimbal.set_yaw=ai_gimbal_cmd.gimbal_t.setpoint.yaw;
|
||||
cmd_gimbal.set_pitch=ai_gimbal_result_cmd.gimbal_t.setpoint.pit;
|
||||
cmd_gimbal.set_yaw=ai_gimbal_result_cmd.gimbal_t.setpoint.yaw;
|
||||
cmd_gimbal.yaw_vel=ai_gimbal_result_cmd.gimbal_t.vel.yaw;
|
||||
cmd_gimbal.yaw_accl=ai_gimbal_result_cmd.gimbal_t.accl.yaw;
|
||||
cmd_gimbal.pit_vel=ai_gimbal_result_cmd.gimbal_t.vel.pit;
|
||||
cmd_gimbal.pit_accl=ai_gimbal_result_cmd.gimbal_t.accl.pit;
|
||||
}
|
||||
if(ai_gimbal_cmd.mode==2){
|
||||
cmd_gimbal.set_pitch=ai_gimbal_cmd.gimbal_t.setpoint.pit;
|
||||
cmd_gimbal.set_yaw=ai_gimbal_cmd.gimbal_t.setpoint.yaw;
|
||||
if(ai_gimbal_result_cmd.mode==2){
|
||||
cmd_gimbal.set_pitch=ai_gimbal_result_cmd.gimbal_t.setpoint.pit;
|
||||
cmd_gimbal.set_yaw=ai_gimbal_result_cmd.gimbal_t.setpoint.yaw;
|
||||
cmd_gimbal.yaw_vel=ai_gimbal_result_cmd.gimbal_t.vel.yaw;
|
||||
cmd_gimbal.yaw_accl=ai_gimbal_result_cmd.gimbal_t.accl.yaw;
|
||||
cmd_gimbal.pit_vel=ai_gimbal_result_cmd.gimbal_t.vel.pit;
|
||||
cmd_gimbal.pit_accl=ai_gimbal_result_cmd.gimbal_t.accl.pit;
|
||||
}
|
||||
Gimbal_UpdateFeedback(&gimbal);
|
||||
|
||||
|
||||
@ -37,8 +37,8 @@ void Task_Init(void *argument) {
|
||||
task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd);
|
||||
task_runtime.thread.gimbal = osThreadNew(Task_gimbal, NULL, &attr_gimbal);
|
||||
task_runtime.thread.shoot = osThreadNew(Task_shoot, NULL, &attr_shoot);
|
||||
task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
|
||||
task_runtime.thread.Task8 = osThreadNew(Task_Task8, NULL, &attr_Task8);
|
||||
task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
|
||||
// task_runtime.thread.Task8 = osThreadNew(Task_Task8, NULL, &attr_Task8);
|
||||
|
||||
// 创建消息队列
|
||||
/* USER MESSAGE BEGIN */
|
||||
@ -53,9 +53,10 @@ void Task_Init(void *argument) {
|
||||
task_runtime.msgq.gimbal.yaw6020 = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL);
|
||||
task_runtime.msgq.shoot.cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t),NULL);
|
||||
task_runtime.msgq.gimbal.ai.feedback = osMessageQueueNew(2u, sizeof(Gimbal_feedback_t),NULL);
|
||||
task_runtime.msgq.gimbal.ai.g_cmd = osMessageQueueNew(2u, sizeof(AI_cmd_t),NULL);
|
||||
task_runtime.msgq.shoot.ai.s_cmd = osMessageQueueNew(2u, sizeof(AI_cmd_t),NULL);
|
||||
|
||||
task_runtime.msgq.gimbal.ai.g_cmd = osMessageQueueNew(2u, sizeof(AI_result_t),NULL);
|
||||
task_runtime.msgq.shoot.ai.s_cmd = osMessageQueueNew(2u, sizeof(AI_result_t),NULL);
|
||||
task_runtime.msgq.shoot.ai.s_cmd_ai_bool_count = osMessageQueueNew(2u, sizeof(PackageMCU_t),NULL);
|
||||
task_runtime.msgq.navi.c_cmd = osMessageQueueNew(2u, sizeof(AI_result_t),NULL); /* 新增的 ai 消息队列 主要是给底盘发送导航相关的命令*/
|
||||
/* USER MESSAGE END */
|
||||
|
||||
osKernelUnlock(); // 解锁内核
|
||||
|
||||
@ -4,6 +4,7 @@
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "cmsis_os2.h"
|
||||
#include "module/cmd.h"
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
@ -19,7 +20,9 @@
|
||||
/* USER STRUCT BEGIN */
|
||||
Shoot_t shoot;
|
||||
Shoot_CMD_t shoot_cmd;
|
||||
AI_cmd_t shoot_ai_cmd;
|
||||
AI_result_t shoot_ai_result_cmd;
|
||||
PackageMCU_t shoot_ai_mcu_package;
|
||||
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -34,43 +37,55 @@ void Task_shoot(void *argument) {
|
||||
osDelay(SHOOT_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
/* USER CODE INIT BEGIN */
|
||||
Shoot_Init(&shoot,&Config_GetRobotParam()->shoot,SHOOT_FREQ);
|
||||
Shoot_SetMode(&shoot,SHOOT_MODE_SINGLE);
|
||||
|
||||
/* USER CODE INIT END */
|
||||
shoot_ai_mcu_package.data.bullet_count = 0;
|
||||
static bool last_fire_state = false;
|
||||
bool current_fire_state = false; // 当前是否需要发射
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
osMessageQueueGet(task_runtime.msgq.shoot.cmd, &shoot_cmd, NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.shoot.ai.s_cmd, &shoot_ai_cmd, NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.shoot.ai.s_cmd, &shoot_ai_result_cmd, NULL, 0);
|
||||
|
||||
// if(shoot_cmd.control_mode==SHOOT_REMOTE)
|
||||
// {
|
||||
// //do nothing，使用遥控器的指令
|
||||
// }
|
||||
// else if(shoot_cmd.control_mode==SHOOT_AI)
|
||||
// {
|
||||
if(shoot_cmd.control_mode==SHOOT_REMOTE)
|
||||
{
|
||||
//do nothing，使用遥控器的指令
|
||||
current_fire_state = shoot_cmd.firecmd;
|
||||
}
|
||||
else if(shoot_cmd.control_mode==SHOOT_AI)
|
||||
{
|
||||
// shoot_cmd.mode = SHOOT_MODE_SINGLE;
|
||||
// if(shoot_ai_cmd.mode==0 || shoot_ai_cmd.mode==1)
|
||||
// if(shoot_ai_result_cmd.mode==0 || shoot_ai_result_cmd.mode==1)
|
||||
// {
|
||||
// shoot_cmd.firecmd=false;
|
||||
// shoot_cmd.ready=true;
|
||||
|
||||
// }
|
||||
// else if(shoot_ai_cmd.mode==2)
|
||||
// else if(shoot_ai_result_cmd.mode==2)
|
||||
// {
|
||||
|
||||
// shoot_cmd.firecmd=true;
|
||||
// shoot_cmd.ready=true;
|
||||
// current_fire_state = true;
|
||||
// }
|
||||
// }
|
||||
current_fire_state = shoot_cmd.firecmd;
|
||||
}
|
||||
|
||||
if(current_fire_state == true && last_fire_state == false)
|
||||
{
|
||||
shoot_ai_mcu_package.data.bullet_count++; /* 每次射击时增加射击数量 */
|
||||
}
|
||||
last_fire_state = current_fire_state;
|
||||
|
||||
Shoot_UpdateFeedback(&shoot);
|
||||
Shoot_SetMode(&shoot,shoot_cmd.mode);
|
||||
Shoot_Control(&shoot,&shoot_cmd);
|
||||
|
||||
osMessageQueueReset(task_runtime.msgq.shoot.ai.s_cmd_ai_bool_count);
|
||||
osMessageQueuePut(task_runtime.msgq.shoot.ai.s_cmd_ai_bool_count, &shoot_ai_mcu_package, 0, 0);
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
@ -102,9 +102,12 @@ typedef struct {
|
||||
struct{
|
||||
|
||||
osMessageQueueId_t s_cmd;
|
||||
osMessageQueueId_t s_cmd_ai_bool_count; /* 新增的 ai 消息队列 主要是给自瞄发送射击数量*/
|
||||
}ai;
|
||||
}shoot;
|
||||
|
||||
struct{
|
||||
osMessageQueueId_t c_cmd; /* 新增的 ai 消息队列 主要是给底盘发送自瞄相关的命令*/
|
||||
}navi;
|
||||
|
||||
} msgq;
|
||||
/* USER MESSAGE END */
|
||||
|
||||
14
lll0121.ioc
14
lll0121.ioc
@ -24,7 +24,8 @@ Dma.Request2=USART3_RX
|
||||
Dma.Request3=USART6_RX
|
||||
Dma.Request4=USART6_TX
|
||||
Dma.Request5=USART1_RX
|
||||
Dma.RequestsNb=6
|
||||
Dma.Request6=USART1_TX
|
||||
Dma.RequestsNb=7
|
||||
Dma.SPI1_RX.0.Direction=DMA_PERIPH_TO_MEMORY
|
||||
Dma.SPI1_RX.0.FIFOMode=DMA_FIFOMODE_DISABLE
|
||||
Dma.SPI1_RX.0.Instance=DMA2_Stream2
|
||||
@ -55,6 +56,16 @@ Dma.USART1_RX.5.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
|
||||
Dma.USART1_RX.5.PeriphInc=DMA_PINC_DISABLE
|
||||
Dma.USART1_RX.5.Priority=DMA_PRIORITY_VERY_HIGH
|
||||
Dma.USART1_RX.5.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode
|
||||
Dma.USART1_TX.6.Direction=DMA_MEMORY_TO_PERIPH
|
||||
Dma.USART1_TX.6.FIFOMode=DMA_FIFOMODE_DISABLE
|
||||
Dma.USART1_TX.6.Instance=DMA2_Stream7
|
||||
Dma.USART1_TX.6.MemDataAlignment=DMA_MDATAALIGN_BYTE
|
||||
Dma.USART1_TX.6.MemInc=DMA_MINC_ENABLE
|
||||
Dma.USART1_TX.6.Mode=DMA_NORMAL
|
||||
Dma.USART1_TX.6.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
|
||||
Dma.USART1_TX.6.PeriphInc=DMA_PINC_DISABLE
|
||||
Dma.USART1_TX.6.Priority=DMA_PRIORITY_LOW
|
||||
Dma.USART1_TX.6.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode
|
||||
Dma.USART3_RX.2.Direction=DMA_PERIPH_TO_MEMORY
|
||||
Dma.USART3_RX.2.FIFOMode=DMA_FIFOMODE_DISABLE
|
||||
Dma.USART3_RX.2.Instance=DMA1_Stream1
|
||||
@ -154,6 +165,7 @@ NVIC.DMA2_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DMA2_Stream3_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DMA2_Stream5_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DMA2_Stream6_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DMA2_Stream7_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||
NVIC.EXTI0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.EXTI4_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
|
||||
Loading…
Reference in New Issue
Block a user