111
This commit is contained in:
parent
5f06fe477b
commit
d290e6c3a2
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
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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
|
||||
@ -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)
|
||||
{
|
||||
@ -404,9 +409,9 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
|
||||
|
||||
case CHASSIS_MODE_DAOHANG:
|
||||
// 导航模式
|
||||
c->move_vec.Vx = -c_cmd->Vx/10;
|
||||
c->move_vec.Vy = c_cmd->Vy/10;
|
||||
c->move_vec.Vw = c_cmd->Vw/10;
|
||||
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:
|
||||
@ -420,6 +425,14 @@ case CHASSIS_MODE_DAOHANG:
|
||||
|
||||
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解算出的角度
|
||||
@ -437,6 +450,26 @@ case CHASSIS_MODE_DAOHANG:
|
||||
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;
|
||||
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -246,7 +246,7 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
|
||||
Gimbal_SetMode(g, g_cmd->mode);
|
||||
|
||||
/*处理小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) // 有限位才处理
|
||||
{
|
||||
|
||||
Loading…
Reference in New Issue
Block a user