xiaocheng/Er_sentry
1
1
Fork 0
Code Issues 1 Pull Requests Actions Packages Projects Releases Wiki Activity

修改了卡顿

Browse Source
This commit is contained in:
Xiaocheng 2026-03-04 16:38:42 +08:00
parent 2377a13dea
commit 87ff7ff05e
40 changed files with 21889 additions and 11877 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15468
MDK-ARM/JLinkLog.txt
View File

File diff suppressed because it is too large Load Diff

91
MDK-ARM/lll0121.uvguix.zhaoruihong
View File

File diff suppressed because one or more lines are too long

BIN
MDK-ARM/lll0121/atti_esti.o Normal file
View File

Binary file not shown.

4
MDK-ARM/lll0121/calc_lib.d
View File

@ -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

BIN
MDK-ARM/lll0121/calc_lib.o
View File

Binary file not shown.

BIN
MDK-ARM/lll0121/can.o
View File

Binary file not shown.

BIN
MDK-ARM/lll0121/chassis.o
View File

Binary file not shown.

BIN
MDK-ARM/lll0121/chassis_1.o
View File

Binary file not shown.

BIN
MDK-ARM/lll0121/cmd.o
View File

Binary file not shown.

3
MDK-ARM/lll0121/cmd_1.d
View File

@ -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 \

BIN
MDK-ARM/lll0121/cmd_1.o
View File

Binary file not shown.

BIN
MDK-ARM/lll0121/config.o
View File

Binary file not shown.

BIN
MDK-ARM/lll0121/gimbal.o
View File

Binary file not shown.

3
MDK-ARM/lll0121/gimbal_1.d
View File

@ -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

BIN
MDK-ARM/lll0121/gimbal_1.o
View File

Binary file not shown.

BIN
MDK-ARM/lll0121/gpio_1.o
View File

Binary file not shown.

3
MDK-ARM/lll0121/init.d
View File

@ -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

BIN
MDK-ARM/lll0121/init.o
View File

Binary file not shown.

BIN
MDK-ARM/lll0121/lll0121.axf Normal file
View File

Binary file not shown.

12
MDK-ARM/lll0121/lll0121.build_log.htm
View File

@ -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>

11187
MDK-ARM/lll0121/lll0121.hex
View File

File diff suppressed because it is too large Load Diff

1426
MDK-ARM/lll0121/lll0121.htm
View File

File diff suppressed because it is too large Load Diff

4213
MDK-ARM/lll0121/lll0121.map
View File

File diff suppressed because it is too large Load Diff

892
MDK-ARM/lll0121/lll0121_lll0121.dep
View File

File diff suppressed because it is too large Load Diff

BIN
MDK-ARM/lll0121/motor_rm.o
View File

Binary file not shown.

BIN
MDK-ARM/lll0121/rc.o
View File

Binary file not shown.

BIN
MDK-ARM/lll0121/remote_control.o
View File

Binary file not shown.

BIN
MDK-ARM/lll0121/shoot.o
View File

Binary file not shown.

16
MDK-ARM/lll0121/shoot_1.d
View File

@ -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

BIN
MDK-ARM/lll0121/shoot_1.o
View File

Binary file not shown.

BIN
MDK-ARM/lll0121/stm32f4xx_it.o
View File

Binary file not shown.

BIN
MDK-ARM/lll0121/uart.o
View File

Binary file not shown.

BIN
MDK-ARM/lll0121/user_math.o
View File

Binary file not shown.

BIN
MDK-ARM/lll0121/user_task.o
View File

Binary file not shown.

361
Ozone.jdebug Normal file
View 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
View 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

9
User/module/chassis.c
View File

@ -338,6 +338,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 +384,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,7 +397,7 @@ 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, 0.0627f ,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;

12
User/module/config.c
View File

@ -115,12 +115,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
},
@ -214,12 +214,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无限位*/

31
User/module/gimbal.c
View File

@ -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;
@ -333,18 +335,17 @@ g->setpoint.eulr.pit = g_cmd->set_pitch;
*/
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 = 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 +359,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,7 +384,7 @@ 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 * 2.5f;
// output_yaw_4310.kp = 0.2f;
output_yaw_4310.kd = 0.1f;
// output_yaw_4310.kd=0.1f;

4
User/task/init.c
View File

@ -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 */