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

111

Browse Source
This commit is contained in:
Xiaocheng 2026-03-11 22:06:13 +08:00
parent 5f06fe477b
commit d290e6c3a2
7 changed files with 451 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

2
User/device/motor_rm.c
View File

@ -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) { static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) {
switch (module) { switch (module) {
case MOTOR_M2006: return 36.0f; 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; case MOTOR_GM6020: return 1.0f;
default: return 1.0f; default: return 1.0f;
} }

41
User/module/chassis.c
View File

@ -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_3508VPID[i]);
PID_Reset(&c->pid.chassis_6020OmegaPid[i]); PID_Reset(&c->pid.chassis_6020OmegaPid[i]);
PID_Reset(&c->pid.chassis_6020anglePid[i]); PID_Reset(&c->pid.chassis_6020anglePid[i]);
PID_Reset(&c->pid.chassis_3508DAOHANG_pid[i]);
} }
c->mode = mode; 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_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); 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); 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 LowPassFilter2p_Init(&c->filled[0], target_freq, 20.0f); // vx
@ -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_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_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_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); 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) if (c->motorfeedback.rotor_angle6020[i] < 0)
{ {
@ -404,9 +409,9 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
case CHASSIS_MODE_DAOHANG: case CHASSIS_MODE_DAOHANG:
// 导航模式 // 导航模式
c->move_vec.Vx = -c_cmd->Vx/10; c->move_vec.Vx = -c_cmd->Vx/1;
c->move_vec.Vy = c_cmd->Vy/10; c->move_vec.Vy = c_cmd->Vy/1;
c->move_vec.Vw = c_cmd->Vw/10; 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; break;
default: default:
@ -420,6 +425,14 @@ case CHASSIS_MODE_DAOHANG:
Chassis_speed_calculate(c, c_cmd); 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++) for (int i = 0; i < 4; i++)
{ {
float chassis6020_detangle[4]; // 6020解算出的角度 float chassis6020_detangle[4]; // 6020解算出的角度
@ -437,6 +450,26 @@ case CHASSIS_MODE_DAOHANG:
c->motorfeedback.rotor_rpm3508[i], 0.0f, c->dt); 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]); 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; return CHASSIS_OK;
} }

7
User/module/chassis.h
View File

@ -55,6 +55,9 @@ extern "C"
KPID_Params_t C6020Omega_param; KPID_Params_t C6020Omega_param;
KPID_Params_t C6020Angle_param; KPID_Params_t C6020Angle_param;
KPID_Params_t M3508v_param; KPID_Params_t M3508v_param;
KPID_Params_t M3508DAOHANG_param;
KPID_Params_t chassis_follow_gimbalPID; KPID_Params_t chassis_follow_gimbalPID;
MOTOR_RM_Param_t motor_3508_param[4]; // 四个3508电机 MOTOR_RM_Param_t motor_3508_param[4]; // 四个3508电机
@ -113,6 +116,7 @@ extern "C"
{ {
fp32 rotor_rpm3508[4]; fp32 rotor_rpm3508[4];
fp32 rotor_current3508[4]; fp32 rotor_current3508[4];
fp32 rotor_truespeed3508[4];
fp32 rotor_rpm6020[4]; fp32 rotor_rpm6020[4];
fp32 rotor_angle6020[4]; fp32 rotor_angle6020[4];
fp32 rotor_current6020[4]; fp32 rotor_current6020[4];
@ -126,6 +130,9 @@ extern "C"
KPID_t chassis_6020anglePid[4]; KPID_t chassis_6020anglePid[4];
KPID_t chassis_6020OmegaPid[4]; KPID_t chassis_6020OmegaPid[4];
KPID_t chassis_3508VPID[4]; KPID_t chassis_3508VPID[4];
KPID_t chassis_3508DAOHANG_pid[4];
KPID_t chassis_follow_gimbal_pid; KPID_t chassis_follow_gimbal_pid;
} pid; } pid;

12
User/module/config.c
View File

@ -65,6 +65,18 @@ static const Config_Param_t config = {
.d_cutoff_freq = -1.0f, .d_cutoff_freq = -1.0f,
.range = -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 = { .chassis_follow_gimbalPID = {
.k = 0.7f, .k = 0.7f,
.p = 0.5f, .p = 0.5f,

2
User/module/gimbal.c
View File

@ -246,7 +246,7 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
Gimbal_SetMode(g, g_cmd->mode); Gimbal_SetMode(g, g_cmd->mode);
/*处理小yaw轴控制命令*/ /*处理小yaw轴控制命令*/
float delta_yaw_6020 = 20.0f*g_cmd->delta_yaw_6020 * g->dt; float delta_yaw_6020 = 10.0f*g_cmd->delta_yaw_6020 * g->dt;
if(g->param->travel.yaw_6020 > 0) // 有限位才处理 if(g->param->travel.yaw_6020 > 0) // 有限位才处理
{ {