robofish/rm_balance
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

换keil

Browse Source
This commit is contained in:
Robofish 2026-01-10 21:35:57 +08:00
parent 20b386a53b
commit 7bb87644be
19 changed files with 4411 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
.mxproject
View File

File diff suppressed because one or more lines are too long

5
CMakeLists.txt
View File

@ -62,8 +62,10 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/component/error_detect.c User/component/error_detect.c
User/component/filter.c User/component/filter.c
User/component/freertos_cli.c User/component/freertos_cli.c
User/component/lqr.c
User/component/pid.c User/component/pid.c
User/component/user_math.c User/component/user_math.c
User/component/vmc.c
# User/device sources # User/device sources
User/device/bmi088.c User/device/bmi088.c
@ -74,8 +76,9 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/device/motor_lk.c User/device/motor_lk.c
User/device/motor_lz.c User/device/motor_lz.c
User/device/motor_rm.c User/device/motor_rm.c
# User/module sources # User/module sources
User/module/balance_chassis.c
User/module/config.c User/module/config.c
User/module/shoot.c User/module/shoot.c

2
CtrBoard-H7_ALL.ioc
View File

@ -503,7 +503,7 @@ ProjectManager.ProjectName=CtrBoard-H7_ALL
ProjectManager.ProjectStructure= ProjectManager.ProjectStructure=
ProjectManager.RegisterCallBack= ProjectManager.RegisterCallBack=
ProjectManager.StackSize=0x2000 ProjectManager.StackSize=0x2000
ProjectManager.TargetToolchain=CMake ProjectManager.TargetToolchain=MDK-ARM V5.32
ProjectManager.ToolChainLocation= ProjectManager.ToolChainLocation=
ProjectManager.UAScriptAfterPath= ProjectManager.UAScriptAfterPath=
ProjectManager.UAScriptBeforePath= ProjectManager.UAScriptBeforePath=

171
MDK-ARM/CtrBoard-H7_ALL.uvoptx Normal file
View File

@ -0,0 +1,171 @@
<?xml version="1.0" encoding="UTF-8"?>
<ProjectOpt xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="project_optx.xsd">
<SchemaVersion>1.0</SchemaVersion>
<Header>### uVision Project, (C) Keil Software</Header>
<Extensions>
<cExt>*.c</cExt>
<aExt>*.s*; *.src; *.a*</aExt>
<oExt>*.obj; *.o</oExt>
<lExt>*.lib</lExt>
<tExt>*.txt; *.h; *.inc</tExt>
<pExt>*.plm</pExt>
<CppX>*.cpp</CppX>
<nMigrate>0</nMigrate>
</Extensions>
<DaveTm>
<dwLowDateTime>0</dwLowDateTime>
<dwHighDateTime>0</dwHighDateTime>
</DaveTm>
<Target>
<TargetName>CtrBoard-H7_ALL</TargetName>
<ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName>
<TargetOption>
<CLKADS>24000000</CLKADS>
<OPTTT>
<gFlags>1</gFlags>
<BeepAtEnd>1</BeepAtEnd>
<RunSim>0</RunSim>
<RunTarget>1</RunTarget>
<RunAbUc>0</RunAbUc>
</OPTTT>
<OPTHX>
<HexSelection>1</HexSelection>
<FlashByte>65535</FlashByte>
<HexRangeLowAddress>0</HexRangeLowAddress>
<HexRangeHighAddress>0</HexRangeHighAddress>
<HexOffset>0</HexOffset>
</OPTHX>
<OPTLEX>
<PageWidth>79</PageWidth>
<PageLength>66</PageLength>
<TabStop>8</TabStop>
<ListingPath />
</OPTLEX>
<ListingPage>
<CreateCListing>1</CreateCListing>
<CreateAListing>1</CreateAListing>
<CreateLListing>1</CreateLListing>
<CreateIListing>0</CreateIListing>
<AsmCond>1</AsmCond>
<AsmSymb>1</AsmSymb>
<AsmXref>0</AsmXref>
<CCond>1</CCond>
<CCode>0</CCode>
<CListInc>0</CListInc>
<CSymb>0</CSymb>
<LinkerCodeListing>0</LinkerCodeListing>
</ListingPage>
<OPTXL>
<LMap>1</LMap>
<LComments>1</LComments>
<LGenerateSymbols>1</LGenerateSymbols>
<LLibSym>1</LLibSym>
<LLines>1</LLines>
<LLocSym>1</LLocSym>
<LPubSym>1</LPubSym>
<LXref>0</LXref>
<LExpSel>0</LExpSel>
</OPTXL>
<OPTFL>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<IsCurrentTarget>1</IsCurrentTarget>
</OPTFL>
<CpuCode>18</CpuCode>
<DebugOpt>
<uSim>0</uSim>
<uTrg>1</uTrg>
<sLdApp>1</sLdApp>
<sGomain>1</sGomain>
<sRbreak>1</sRbreak>
<sRwatch>1</sRwatch>
<sRmem>1</sRmem>
<sRfunc>1</sRfunc>
<sRbox>1</sRbox>
<tLdApp>1</tLdApp>
<tGomain>1</tGomain>
<tRbreak>1</tRbreak>
<tRwatch>1</tRwatch>
<tRmem>1</tRmem>
<tRfunc>1</tRfunc>
<tRbox>1</tRbox>
<tRtrace>1</tRtrace>
<sRSysVw>1</sRSysVw>
<tRSysVw>1</tRSysVw>
<sRunDeb>0</sRunDeb>
<sLrtime>0</sLrtime>
<bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf>
<nTsel>6</nTsel>
<sDll />
<sDllPa />
<sDlgDll />
<sDlgPa />
<sIfile />
<tDll />
<tDllPa />
<tDlgDll />
<tDlgPa />
<tIfile />
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
</DebugOpt>
<TargetDriverDllRegistry>
<SetRegEntry>
<Number>0</Number>
<Key>ST-LINKIII-KEIL_SWO</Key>
<Name>-U-O142 -O2254 -S0 -C0 -N00("ARM CoreSight SW-DP") -D00(5BA02477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VG$Flash\STM32H72x-73x_1024.FLM)</Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
<Key />
<Name />
</SetRegEntry>
</TargetDriverDllRegistry>
<Breakpoint />
<Tracepoint>
<THDelay>0</THDelay>
</Tracepoint>
<DebugFlag>
<trace>0</trace>
<periodic>1</periodic>
<aLwin>1</aLwin>
<aCover>0</aCover>
<aSer1>0</aSer1>
<aSer2>0</aSer2>
<aPa>0</aPa>
<viewmode>1</viewmode>
<vrSel>0</vrSel>
<aSym>0</aSym>
<aTbox>0</aTbox>
<AscS1>0</AscS1>
<AscS2>0</AscS2>
<AscS3>0</AscS3>
<aSer3>0</aSer3>
<eProf>0</eProf>
<aLa>0</aLa>
<aPa1>0</aPa1>
<AscS4>0</AscS4>
<aSer4>0</aSer4>
<StkLoc>1</StkLoc>
<TrcWin>0</TrcWin>
<newCpu>0</newCpu>
<uProt>0</uProt>
</DebugFlag>
<LintExecutable />
<LintConfigFile />
<bLintAuto>0</bLintAuto>
<bAutoGenD>0</bAutoGenD>
<LntExFlags>0</LntExFlags>
<pMisraName />
<pszMrule />
<pSingCmds />
<pMultCmds />
<pMisraNamep />
<pszMrulep />
<pSingCmdsp />
<pMultCmdsp />
</TargetOption>
</Target>
</ProjectOpt>

692
MDK-ARM/CtrBoard-H7_ALL.uvprojx Normal file
View File

@ -0,0 +1,692 @@
<?xml version="1.0" encoding="UTF-8"?>
<Project xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" noNamespaceSchemaLocation="project_projx.xsd">
<SchemaVersion>2.1</SchemaVersion>
<Header>### uVision Project, (C) Keil Software</Header>
<Targets>
<Target>
<TargetName>CtrBoard-H7_ALL</TargetName>
<ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName>
<TargetOption>
<TargetCommonOption>
<Device>STM32H723VGTx</Device>
<Vendor>STMicroelectronics</Vendor>
<Cpu>IRAM(0x20000000-0x2001FFFF) IRAM2(0x24000000-0x2404FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(12000000) FPU3(DFPU) CPUTYPE("Cortex-M7") ELITTLE TZ</Cpu>
<FlashUtilSpec />
<StartupFile />
<FlashDriverDll />
<DeviceId>0</DeviceId>
<RegisterFile />
<MemoryEnv />
<Cmp />
<Asm />
<Linker />
<OHString />
<InfinionOptionDll />
<SLE66CMisc />
<SLE66AMisc />
<SLE66LinkerMisc />
<SFDFile />
<bCustSvd>0</bCustSvd>
<UseEnv>0</UseEnv>
<BinPath />
<IncludePath />
<LibPath />
<RegisterFilePath />
<DBRegisterFilePath />
<TargetStatus>
<Error>0</Error>
<ExitCodeStop>0</ExitCodeStop>
<ButtonStop>0</ButtonStop>
<NotGenerated>0</NotGenerated>
<InvalidFlash>1</InvalidFlash>
</TargetStatus>
<OutputDirectory>CtrBoard-H7_ALL\</OutputDirectory>
<OutputName>CtrBoard-H7_ALL</OutputName>
<CreateExecutable>1</CreateExecutable>
<CreateLib>0</CreateLib>
<CreateHexFile>1</CreateHexFile>
<DebugInformation>1</DebugInformation>
<BrowseInformation>1</BrowseInformation>
<ListingPath>./CtrBoard-H7_ALL/</ListingPath>
<HexFormatSelection>1</HexFormatSelection>
<Merge32K>0</Merge32K>
<CreateBatchFile>0</CreateBatchFile>
<BeforeCompile>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>0</RunUserProg2>
<UserProg1Name />
<UserProg2Name />
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopU1X>0</nStopU1X>
<nStopU2X>0</nStopU2X>
</BeforeCompile>
<BeforeMake>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>0</RunUserProg2>
<UserProg1Name />
<UserProg2Name />
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopB1X>0</nStopB1X>
<nStopB2X>0</nStopB2X>
</BeforeMake>
<AfterMake>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>1</RunUserProg2>
<UserProg1Name />
<UserProg2Name />
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopA1X>0</nStopA1X>
<nStopA2X>0</nStopA2X>
</AfterMake>
<SelectedForBatchBuild>1</SelectedForBatchBuild>
<SVCSIdString />
</TargetCommonOption>
<CommonProperty>
<UseCPPCompiler>0</UseCPPCompiler>
<RVCTCodeConst>0</RVCTCodeConst>
<RVCTZI>0</RVCTZI>
<RVCTOtherData>0</RVCTOtherData>
<ModuleSelection>0</ModuleSelection>
<IncludeInBuild>1</IncludeInBuild>
<AlwaysBuild>0</AlwaysBuild>
<GenerateAssemblyFile>0</GenerateAssemblyFile>
<AssembleAssemblyFile>0</AssembleAssemblyFile>
<PublicsOnly>0</PublicsOnly>
<StopOnExitCode>3</StopOnExitCode>
<CustomArgument />
<IncludeLibraryModules />
<ComprImg>0</ComprImg>
</CommonProperty>
<DllOption>
<SimDllName>SARMCM3.DLL</SimDllName>
<SimDllArguments>-REMAP -MPU</SimDllArguments>
<SimDlgDll>DCM.DLL</SimDlgDll>
<SimDlgDllArguments>-pCM7</SimDlgDllArguments>
<TargetDllName>SARMCM3.DLL</TargetDllName>
<TargetDllArguments>-MPU</TargetDllArguments>
<TargetDlgDll>TCM.DLL</TargetDlgDll>
<TargetDlgDllArguments>-pCM7</TargetDlgDllArguments>
</DllOption>
<DebugOption>
<OPTHX>
<HexSelection>1</HexSelection>
<HexRangeLowAddress>0</HexRangeLowAddress>
<HexRangeHighAddress>0</HexRangeHighAddress>
<HexOffset>0</HexOffset>
<Oh166RecLen>16</Oh166RecLen>
</OPTHX>
</DebugOption>
<Utilities>
<Flash1>
<UseTargetDll>1</UseTargetDll>
<UseExternalTool>0</UseExternalTool>
<RunIndependent>0</RunIndependent>
<UpdateFlashBeforeDebugging>1</UpdateFlashBeforeDebugging>
<Capability>1</Capability>
<DriverSelection>4101</DriverSelection>
</Flash1>
<bUseTDR>1</bUseTDR>
<Flash2>BIN\UL2V8M.DLL</Flash2>
<Flash3 />
<Flash4 />
<pFcarmOut />
<pFcarmGrp />
<pFcArmRoot />
<FcArmLst>0</FcArmLst>
</Utilities>
<TargetArmAds>
<ArmAdsMisc>
<GenerateListings>0</GenerateListings>
<asHll>1</asHll>
<asAsm>1</asAsm>
<asMacX>1</asMacX>
<asSyms>1</asSyms>
<asFals>1</asFals>
<asDbgD>1</asDbgD>
<asForm>1</asForm>
<ldLst>0</ldLst>
<ldmm>1</ldmm>
<ldXref>1</ldXref>
<BigEnd>0</BigEnd>
<AdsALst>1</AdsALst>
<AdsACrf>1</AdsACrf>
<AdsANop>0</AdsANop>
<AdsANot>0</AdsANot>
<AdsLLst>1</AdsLLst>
<AdsLmap>1</AdsLmap>
<AdsLcgr>1</AdsLcgr>
<AdsLsym>1</AdsLsym>
<AdsLszi>1</AdsLszi>
<AdsLtoi>1</AdsLtoi>
<AdsLsun>1</AdsLsun>
<AdsLven>1</AdsLven>
<AdsLsxf>1</AdsLsxf>
<RvctClst>0</RvctClst>
<GenPPlst>0</GenPPlst>
<AdsCpuType>"Cortex-M7"</AdsCpuType>
<RvctDeviceName />
<mOS>0</mOS>
<uocRom>0</uocRom>
<uocRam>0</uocRam>
<hadIROM>1</hadIROM>
<hadIRAM>1</hadIRAM>
<hadXRAM>0</hadXRAM>
<uocXRam>0</uocXRam>
<RvdsVP>3</RvdsVP>
<RvdsMve>0</RvdsMve>
<RvdsCdeCp>0</RvdsCdeCp>
<hadIRAM2>0</hadIRAM2>
<hadIROM2>1</hadIROM2>
<StupSel>8</StupSel>
<useUlib>0</useUlib>
<EndSel>0</EndSel>
<uLtcg>0</uLtcg>
<nSecure>0</nSecure>
<RoSelD>3</RoSelD>
<RwSelD>4</RwSelD>
<CodeSel>0</CodeSel>
<OptFeed>0</OptFeed>
<NoZi1>0</NoZi1>
<NoZi2>0</NoZi2>
<NoZi3>0</NoZi3>
<NoZi4>0</NoZi4>
<NoZi5>0</NoZi5>
<Ro1Chk>0</Ro1Chk>
<Ro2Chk>0</Ro2Chk>
<Ro3Chk>0</Ro3Chk>
<Ir1Chk>1</Ir1Chk>
<Ir2Chk>1</Ir2Chk>
<Ra1Chk>0</Ra1Chk>
<Ra2Chk>0</Ra2Chk>
<Ra3Chk>0</Ra3Chk>
<Im1Chk>1</Im1Chk>
<Im2Chk>1</Im2Chk>
<OnChipMemories>
<Ocm1>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm1>
<Ocm2>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm2>
<Ocm3>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm3>
<Ocm4>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm4>
<Ocm5>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm5>
<Ocm6>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm6>
<IRAM>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</IRAM>
<IROM>
<Type>1</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</IROM>
<XRAM>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</XRAM>
<OCR_RVCT1>
<Type>1</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT1>
<OCR_RVCT2>
<Type>1</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT2>
<OCR_RVCT3>
<Type>1</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT3>
<OCR_RVCT4>
<Type>1</Type>
<StartAddress />
<Size />
</OCR_RVCT4>
<OCR_RVCT5>
<Type>1</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT5>
<OCR_RVCT6>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT6>
<OCR_RVCT7>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT7>
<OCR_RVCT8>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT8>
<OCR_RVCT9>
<Type>0</Type>
<StartAddress />
<Size />
</OCR_RVCT9>
<OCR_RVCT10>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT10>
</OnChipMemories>
<RvctStartVector />
</ArmAdsMisc>
<Cads>
<interw>1</interw>
<Optim>4</Optim>
<oTime>0</oTime>
<SplitLS>0</SplitLS>
<OneElfS>1</OneElfS>
<Strict>0</Strict>
<EnumInt>0</EnumInt>
<PlainCh>0</PlainCh>
<Ropi>0</Ropi>
<Rwpi>0</Rwpi>
<wLevel>2</wLevel>
<uThumb>0</uThumb>
<uSurpInc>0</uSurpInc>
<uC99>1</uC99>
<uGnu>1</uGnu>
<useXO>0</useXO>
<v6Lang>5</v6Lang>
<v6LangP>3</v6LangP>
<vShortEn>1</vShortEn>
<vShortWch>1</vShortWch>
<v6Lto>0</v6Lto>
<v6WtE>0</v6WtE>
<v6Rtti>0</v6Rtti>
<VariousControls>
<MiscControls />
<Define>USE_PWR_LDO_SUPPLY,USE_HAL_DRIVER,STM32H723xx</Define>
<Undefine />
<IncludePath>../Core/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32H7xx/Include;../Drivers/CMSIS/Include</IncludePath>
</VariousControls>
</Cads>
<Aads>
<interw>1</interw>
<Ropi>0</Ropi>
<Rwpi>0</Rwpi>
<thumb>0</thumb>
<SplitLS>0</SplitLS>
<SwStkChk>0</SwStkChk>
<NoWarn>0</NoWarn>
<uSurpInc>0</uSurpInc>
<useXO>0</useXO>
<ClangAsOpt>1</ClangAsOpt>
<VariousControls>
<MiscControls />
<Define />
<Undefine />
<IncludePath>../Core/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32H7xx/Include;../Drivers/CMSIS/Include</IncludePath>
</VariousControls>
</Aads>
<LDads>
<umfTarg>1</umfTarg>
<Ropi>0</Ropi>
<Rwpi>0</Rwpi>
<noStLib>0</noStLib>
<RepFail>1</RepFail>
<useFile>0</useFile>
<TextAddressRange />
<DataAddressRange />
<pXoBase />
<ScatterFile />
<IncludeLibs />
<IncludeLibsPath />
<Misc />
<LinkerInputFile />
<DisabledWarnings />
</LDads>
</TargetArmAds>
</TargetOption>
<Groups>
<Group>
<GroupName>Application/MDK-ARM</GroupName>
<Files>
<File>
<FileName>startup_stm32h723xx.s</FileName>
<FileType>2</FileType>
<FilePath>startup_stm32h723xx.s</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>Application/User/Core</GroupName>
<Files>
<File>
<FileName>main.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/main.c</FilePath>
</File>
<File>
<FileName>gpio.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/gpio.c</FilePath>
</File>
<File>
<FileName>freertos.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/freertos.c</FilePath>
</File>
<File>
<FileName>adc.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/adc.c</FilePath>
</File>
<File>
<FileName>dma.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/dma.c</FilePath>
</File>
<File>
<FileName>fdcan.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/fdcan.c</FilePath>
</File>
<File>
<FileName>octospi.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/octospi.c</FilePath>
</File>
<File>
<FileName>spi.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/spi.c</FilePath>
</File>
<File>
<FileName>tim.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/tim.c</FilePath>
</File>
<File>
<FileName>usart.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/usart.c</FilePath>
</File>
<File>
<FileName>usb_otg.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/usb_otg.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_it.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/stm32h7xx_it.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_msp.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/stm32h7xx_hal_msp.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_timebase_tim.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/stm32h7xx_hal_timebase_tim.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>Drivers/STM32H7xx_HAL_Driver</GroupName>
<Files>
<File>
<FileName>stm32h7xx_hal_tim.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_tim_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_adc.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_adc.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_adc_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_adc_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_rcc.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_rcc.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_rcc_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_rcc_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_flash.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_flash.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_flash_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_flash_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_gpio.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_gpio.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_hsem.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_hsem.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_dma.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_dma.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_dma_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_dma_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_mdma.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_mdma.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_pwr.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_pwr.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_pwr_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_pwr_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_cortex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cortex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_i2c.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_i2c.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_i2c_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_i2c_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_exti.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_exti.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_fdcan.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_fdcan.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_ospi.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_ospi.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_spi.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_spi.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_spi_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_spi_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_uart.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_uart_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_pcd.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_pcd.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_pcd_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_pcd_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_ll_usb.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_usb.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>Drivers/CMSIS</GroupName>
<Files>
<File>
<FileName>system_stm32h7xx.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/system_stm32h7xx.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>Middlewares/FreeRTOS</GroupName>
<Files>
<File>
<FileName>croutine.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/croutine.c</FilePath>
</File>
<File>
<FileName>event_groups.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/event_groups.c</FilePath>
</File>
<File>
<FileName>list.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/list.c</FilePath>
</File>
<File>
<FileName>queue.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/queue.c</FilePath>
</File>
<File>
<FileName>stream_buffer.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c</FilePath>
</File>
<File>
<FileName>tasks.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/tasks.c</FilePath>
</File>
<File>
<FileName>timers.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/timers.c</FilePath>
</File>
<File>
<FileName>cmsis_os2.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c</FilePath>
</File>
<File>
<FileName>heap_4.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c</FilePath>
</File>
<File>
<FileName>port.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F/port.c</FilePath>
</File>
</Files>
</Group>
</Groups>
</Target>
</Targets>
<RTE>
<apis />
<components>
<component Cclass="CMSIS" Cgroup="CORE" Cvendor="ARM" Cversion="4.3.0" condition="CMSIS Core">
<package name="CMSIS" schemaVersion="1.3" url="http://www.keil.com/pack/" vendor="ARM" version="4.5.0" />
<targetInfos>
<targetInfo name="CtrBoard-H7_ALL" />
</targetInfos>
</component>
</components>
<files />
</RTE>
</Project>

619
MDK-ARM/startup_stm32h723xx.s Normal file
View File

@ -0,0 +1,619 @@
;********************************************************************************
;* File Name : startup_stm32h723xx.s
;* @author MCD Application Team
;* Description : STM32H7xx devices vector table for MDK-ARM toolchain.
;* This module performs:
;* - Set the initial SP
;* - Set the initial PC == Reset_Handler
;* - Set the vector table entries with the exceptions ISR address
;* - Branches to __main in the C library (which eventually
;* calls main()).
;* After Reset the Cortex-M processor is in Thread mode,
;* priority is Privileged, and the Stack is set to Main.
;* <<< Use Configuration Wizard in Context Menu >>>
;******************************************************************************
;* @attention
;*
;* Copyright (c) 2019 STMicroelectronics.
;* All rights reserved.
;*
;* This software is licensed under terms that can be found in the LICENSE file
;* in the root directory of this software component.
;* If no LICENSE file comes with this software, it is provided AS-IS.
;*
;*******************************************************************************
; Amount of memory (in bytes) allocated for Stack
; Tailor this value to your application needs
; <h> Stack Configuration
; <o> Stack Size (in Bytes) <0x0-0xFFFFFFFF:8>
; </h>
Stack_Size EQU 0x2000
AREA STACK, NOINIT, READWRITE, ALIGN=3
Stack_Mem SPACE Stack_Size
__initial_sp
; <h> Heap Configuration
; <o> Heap Size (in Bytes) <0x0-0xFFFFFFFF:8>
; </h>
Heap_Size EQU 0x1000
AREA HEAP, NOINIT, READWRITE, ALIGN=3
__heap_base
Heap_Mem SPACE Heap_Size
__heap_limit
PRESERVE8
THUMB
; Vector Table Mapped to Address 0 at Reset
AREA RESET, DATA, READONLY
EXPORT __Vectors
EXPORT __Vectors_End
EXPORT __Vectors_Size
__Vectors DCD __initial_sp ; Top of Stack
DCD Reset_Handler ; Reset Handler
DCD NMI_Handler ; NMI Handler
DCD HardFault_Handler ; Hard Fault Handler
DCD MemManage_Handler ; MPU Fault Handler
DCD BusFault_Handler ; Bus Fault Handler
DCD UsageFault_Handler ; Usage Fault Handler
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD SVC_Handler ; SVCall Handler
DCD DebugMon_Handler ; Debug Monitor Handler
DCD 0 ; Reserved
DCD PendSV_Handler ; PendSV Handler
DCD SysTick_Handler ; SysTick Handler
; External Interrupts
DCD WWDG_IRQHandler ; Window WatchDog interrupt ( wwdg1_it)
DCD PVD_AVD_IRQHandler ; PVD/AVD through EXTI Line detection
DCD TAMP_STAMP_IRQHandler ; Tamper and TimeStamps through the EXTI line
DCD RTC_WKUP_IRQHandler ; RTC Wakeup through the EXTI line
DCD FLASH_IRQHandler ; FLASH
DCD RCC_IRQHandler ; RCC
DCD EXTI0_IRQHandler ; EXTI Line0
DCD EXTI1_IRQHandler ; EXTI Line1
DCD EXTI2_IRQHandler ; EXTI Line2
DCD EXTI3_IRQHandler ; EXTI Line3
DCD EXTI4_IRQHandler ; EXTI Line4
DCD DMA1_Stream0_IRQHandler ; DMA1 Stream 0
DCD DMA1_Stream1_IRQHandler ; DMA1 Stream 1
DCD DMA1_Stream2_IRQHandler ; DMA1 Stream 2
DCD DMA1_Stream3_IRQHandler ; DMA1 Stream 3
DCD DMA1_Stream4_IRQHandler ; DMA1 Stream 4
DCD DMA1_Stream5_IRQHandler ; DMA1 Stream 5
DCD DMA1_Stream6_IRQHandler ; DMA1 Stream 6
DCD ADC_IRQHandler ; ADC1, ADC2
DCD FDCAN1_IT0_IRQHandler ; FDCAN1 interrupt line 0
DCD FDCAN2_IT0_IRQHandler ; FDCAN2 interrupt line 0
DCD FDCAN1_IT1_IRQHandler ; FDCAN1 interrupt line 1
DCD FDCAN2_IT1_IRQHandler ; FDCAN2 interrupt line 1
DCD EXTI9_5_IRQHandler ; External Line[9:5]s
DCD TIM1_BRK_IRQHandler ; TIM1 Break interrupt
DCD TIM1_UP_IRQHandler ; TIM1 Update Interrupt
DCD TIM1_TRG_COM_IRQHandler ; TIM1 Trigger and Commutation Interrupt
DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare
DCD TIM2_IRQHandler ; TIM2
DCD TIM3_IRQHandler ; TIM3
DCD TIM4_IRQHandler ; TIM4
DCD I2C1_EV_IRQHandler ; I2C1 Event
DCD I2C1_ER_IRQHandler ; I2C1 Error
DCD I2C2_EV_IRQHandler ; I2C2 Event
DCD I2C2_ER_IRQHandler ; I2C2 Error
DCD SPI1_IRQHandler ; SPI1
DCD SPI2_IRQHandler ; SPI2
DCD USART1_IRQHandler ; USART1
DCD USART2_IRQHandler ; USART2
DCD USART3_IRQHandler ; USART3
DCD EXTI15_10_IRQHandler ; External Line[15:10]
DCD RTC_Alarm_IRQHandler ; RTC Alarm (A and B) through EXTI Line
DCD 0 ; Reserved
DCD TIM8_BRK_TIM12_IRQHandler ; TIM8 Break Interrupt and TIM12 global interrupt
DCD TIM8_UP_TIM13_IRQHandler ; TIM8 Update Interrupt and TIM13 global interrupt
DCD TIM8_TRG_COM_TIM14_IRQHandler ; TIM8 Trigger and Commutation Interrupt and TIM14 global interrupt
DCD TIM8_CC_IRQHandler ; TIM8 Capture Compare Interrupt
DCD DMA1_Stream7_IRQHandler ; DMA1 Stream7
DCD FMC_IRQHandler ; FMC
DCD SDMMC1_IRQHandler ; SDMMC1
DCD TIM5_IRQHandler ; TIM5
DCD SPI3_IRQHandler ; SPI3
DCD UART4_IRQHandler ; UART4
DCD UART5_IRQHandler ; UART5
DCD TIM6_DAC_IRQHandler ; TIM6 and DAC1&2 underrun errors
DCD TIM7_IRQHandler ; TIM7
DCD DMA2_Stream0_IRQHandler ; DMA2 Stream 0
DCD DMA2_Stream1_IRQHandler ; DMA2 Stream 1
DCD DMA2_Stream2_IRQHandler ; DMA2 Stream 2
DCD DMA2_Stream3_IRQHandler ; DMA2 Stream 3
DCD DMA2_Stream4_IRQHandler ; DMA2 Stream 4
DCD ETH_IRQHandler ; Ethernet
DCD ETH_WKUP_IRQHandler ; Ethernet Wakeup through EXTI line
DCD FDCAN_CAL_IRQHandler ; FDCAN calibration unit interrupt
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD DMA2_Stream5_IRQHandler ; DMA2 Stream 5
DCD DMA2_Stream6_IRQHandler ; DMA2 Stream 6
DCD DMA2_Stream7_IRQHandler ; DMA2 Stream 7
DCD USART6_IRQHandler ; USART6
DCD I2C3_EV_IRQHandler ; I2C3 event
DCD I2C3_ER_IRQHandler ; I2C3 error
DCD OTG_HS_EP1_OUT_IRQHandler ; USB OTG HS End Point 1 Out
DCD OTG_HS_EP1_IN_IRQHandler ; USB OTG HS End Point 1 In
DCD OTG_HS_WKUP_IRQHandler ; USB OTG HS Wakeup through EXTI
DCD OTG_HS_IRQHandler ; USB OTG HS
DCD DCMI_PSSI_IRQHandler ; DCMI, PSSI
DCD 0 ; Reserved
DCD RNG_IRQHandler ; Rng
DCD FPU_IRQHandler ; FPU
DCD UART7_IRQHandler ; UART7
DCD UART8_IRQHandler ; UART8
DCD SPI4_IRQHandler ; SPI4
DCD SPI5_IRQHandler ; SPI5
DCD SPI6_IRQHandler ; SPI6
DCD SAI1_IRQHandler ; SAI1
DCD LTDC_IRQHandler ; LTDC
DCD LTDC_ER_IRQHandler ; LTDC error
DCD DMA2D_IRQHandler ; DMA2D
DCD 0 ; Reserved
DCD OCTOSPI1_IRQHandler ; OCTOSPI1
DCD LPTIM1_IRQHandler ; LPTIM1
DCD CEC_IRQHandler ; HDMI_CEC
DCD I2C4_EV_IRQHandler ; I2C4 Event
DCD I2C4_ER_IRQHandler ; I2C4 Error
DCD SPDIF_RX_IRQHandler ; SPDIF_RX
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD DMAMUX1_OVR_IRQHandler ; DMAMUX1 Overrun interrupt
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD DFSDM1_FLT0_IRQHandler ; DFSDM Filter0 Interrupt
DCD DFSDM1_FLT1_IRQHandler ; DFSDM Filter1 Interrupt
DCD DFSDM1_FLT2_IRQHandler ; DFSDM Filter2 Interrupt
DCD DFSDM1_FLT3_IRQHandler ; DFSDM Filter3 Interrupt
DCD 0 ; Reserved
DCD SWPMI1_IRQHandler ; Serial Wire Interface 1 global interrupt
DCD TIM15_IRQHandler ; TIM15 global Interrupt
DCD TIM16_IRQHandler ; TIM16 global Interrupt
DCD TIM17_IRQHandler ; TIM17 global Interrupt
DCD MDIOS_WKUP_IRQHandler ; MDIOS Wakeup Interrupt
DCD MDIOS_IRQHandler ; MDIOS global Interrupt
DCD 0 ; Reserved
DCD MDMA_IRQHandler ; MDMA global Interrupt
DCD 0 ; Reserved
DCD SDMMC2_IRQHandler ; SDMMC2 global Interrupt
DCD HSEM1_IRQHandler ; HSEM1 global Interrupt
DCD 0 ; Reserved
DCD ADC3_IRQHandler ; ADC3 global Interrupt
DCD DMAMUX2_OVR_IRQHandler ; DMAMUX Overrun interrupt
DCD BDMA_Channel0_IRQHandler ; BDMA Channel 0 global Interrupt
DCD BDMA_Channel1_IRQHandler ; BDMA Channel 1 global Interrupt
DCD BDMA_Channel2_IRQHandler ; BDMA Channel 2 global Interrupt
DCD BDMA_Channel3_IRQHandler ; BDMA Channel 3 global Interrupt
DCD BDMA_Channel4_IRQHandler ; BDMA Channel 4 global Interrupt
DCD BDMA_Channel5_IRQHandler ; BDMA Channel 5 global Interrupt
DCD BDMA_Channel6_IRQHandler ; BDMA Channel 6 global Interrupt
DCD BDMA_Channel7_IRQHandler ; BDMA Channel 7 global Interrupt
DCD COMP1_IRQHandler ; COMP1 global Interrupt
DCD LPTIM2_IRQHandler ; LP TIM2 global interrupt
DCD LPTIM3_IRQHandler ; LP TIM3 global interrupt
DCD LPTIM4_IRQHandler ; LP TIM4 global interrupt
DCD LPTIM5_IRQHandler ; LP TIM5 global interrupt
DCD LPUART1_IRQHandler ; LP UART1 interrupt
DCD 0 ; Reserved
DCD CRS_IRQHandler ; Clock Recovery Global Interrupt
DCD ECC_IRQHandler ; ECC diagnostic Global Interrupt
DCD SAI4_IRQHandler ; SAI4 global interrupt
DCD DTS_IRQHandler ; DTS interrupt
DCD 0 ; Reserved
DCD WAKEUP_PIN_IRQHandler ; Interrupt for all 6 wake-up pins
DCD OCTOSPI2_IRQHandler ; OCTOSPI2 Interrupt
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD FMAC_IRQHandler ; FMAC Interrupt
DCD CORDIC_IRQHandler ; CORDIC Interrupt
DCD UART9_IRQHandler ; UART9 Interrupt
DCD USART10_IRQHandler ; UART10 Interrupt
DCD I2C5_EV_IRQHandler ; I2C5 Event Interrupt
DCD I2C5_ER_IRQHandler ; I2C5 Error Interrupt
DCD FDCAN3_IT0_IRQHandler ; FDCAN3 interrupt line 0
DCD FDCAN3_IT1_IRQHandler ; FDCAN3 interrupt line 1
DCD TIM23_IRQHandler ; TIM23 global interrupt
DCD TIM24_IRQHandler ; TIM24 global interrupt
__Vectors_End
__Vectors_Size EQU __Vectors_End - __Vectors
AREA |.text|, CODE, READONLY
; Reset handler
Reset_Handler PROC
EXPORT Reset_Handler [WEAK]
IMPORT ExitRun0Mode
IMPORT SystemInit
IMPORT __main
LDR R0, =ExitRun0Mode
BLX R0
LDR R0, =SystemInit
BLX R0
LDR R0, =__main
BX R0
ENDP
; Dummy Exception Handlers (infinite loops which can be modified)
NMI_Handler PROC
EXPORT NMI_Handler [WEAK]
B .
ENDP
HardFault_Handler\
PROC
EXPORT HardFault_Handler [WEAK]
B .
ENDP
MemManage_Handler\
PROC
EXPORT MemManage_Handler [WEAK]
B .
ENDP
BusFault_Handler\
PROC
EXPORT BusFault_Handler [WEAK]
B .
ENDP
UsageFault_Handler\
PROC
EXPORT UsageFault_Handler [WEAK]
B .
ENDP
SVC_Handler PROC
EXPORT SVC_Handler [WEAK]
B .
ENDP
DebugMon_Handler\
PROC
EXPORT DebugMon_Handler [WEAK]
B .
ENDP
PendSV_Handler PROC
EXPORT PendSV_Handler [WEAK]
B .
ENDP
SysTick_Handler PROC
EXPORT SysTick_Handler [WEAK]
B .
ENDP
Default_Handler PROC
EXPORT WWDG_IRQHandler [WEAK]
EXPORT PVD_AVD_IRQHandler [WEAK]
EXPORT TAMP_STAMP_IRQHandler [WEAK]
EXPORT RTC_WKUP_IRQHandler [WEAK]
EXPORT FLASH_IRQHandler [WEAK]
EXPORT RCC_IRQHandler [WEAK]
EXPORT EXTI0_IRQHandler [WEAK]
EXPORT EXTI1_IRQHandler [WEAK]
EXPORT EXTI2_IRQHandler [WEAK]
EXPORT EXTI3_IRQHandler [WEAK]
EXPORT EXTI4_IRQHandler [WEAK]
EXPORT DMA1_Stream0_IRQHandler [WEAK]
EXPORT DMA1_Stream1_IRQHandler [WEAK]
EXPORT DMA1_Stream2_IRQHandler [WEAK]
EXPORT DMA1_Stream3_IRQHandler [WEAK]
EXPORT DMA1_Stream4_IRQHandler [WEAK]
EXPORT DMA1_Stream5_IRQHandler [WEAK]
EXPORT DMA1_Stream6_IRQHandler [WEAK]
EXPORT DMA1_Stream7_IRQHandler [WEAK]
EXPORT ADC_IRQHandler [WEAK]
EXPORT FDCAN1_IT0_IRQHandler [WEAK]
EXPORT FDCAN2_IT0_IRQHandler [WEAK]
EXPORT FDCAN1_IT1_IRQHandler [WEAK]
EXPORT FDCAN2_IT1_IRQHandler [WEAK]
EXPORT EXTI9_5_IRQHandler [WEAK]
EXPORT TIM1_BRK_IRQHandler [WEAK]
EXPORT TIM1_UP_IRQHandler [WEAK]
EXPORT TIM1_TRG_COM_IRQHandler [WEAK]
EXPORT TIM1_CC_IRQHandler [WEAK]
EXPORT TIM2_IRQHandler [WEAK]
EXPORT TIM3_IRQHandler [WEAK]
EXPORT TIM4_IRQHandler [WEAK]
EXPORT I2C1_EV_IRQHandler [WEAK]
EXPORT I2C1_ER_IRQHandler [WEAK]
EXPORT I2C2_EV_IRQHandler [WEAK]
EXPORT I2C2_ER_IRQHandler [WEAK]
EXPORT SPI1_IRQHandler [WEAK]
EXPORT SPI2_IRQHandler [WEAK]
EXPORT USART1_IRQHandler [WEAK]
EXPORT USART2_IRQHandler [WEAK]
EXPORT USART3_IRQHandler [WEAK]
EXPORT EXTI15_10_IRQHandler [WEAK]
EXPORT RTC_Alarm_IRQHandler [WEAK]
EXPORT TIM8_BRK_TIM12_IRQHandler [WEAK]
EXPORT TIM8_UP_TIM13_IRQHandler [WEAK]
EXPORT TIM8_TRG_COM_TIM14_IRQHandler [WEAK]
EXPORT TIM8_CC_IRQHandler [WEAK]
EXPORT DMA1_Stream7_IRQHandler [WEAK]
EXPORT FMC_IRQHandler [WEAK]
EXPORT SDMMC1_IRQHandler [WEAK]
EXPORT TIM5_IRQHandler [WEAK]
EXPORT SPI3_IRQHandler [WEAK]
EXPORT UART4_IRQHandler [WEAK]
EXPORT UART5_IRQHandler [WEAK]
EXPORT TIM6_DAC_IRQHandler [WEAK]
EXPORT TIM7_IRQHandler [WEAK]
EXPORT DMA2_Stream0_IRQHandler [WEAK]
EXPORT DMA2_Stream1_IRQHandler [WEAK]
EXPORT DMA2_Stream2_IRQHandler [WEAK]
EXPORT DMA2_Stream3_IRQHandler [WEAK]
EXPORT DMA2_Stream4_IRQHandler [WEAK]
EXPORT ETH_IRQHandler [WEAK]
EXPORT ETH_WKUP_IRQHandler [WEAK]
EXPORT FDCAN_CAL_IRQHandler [WEAK]
EXPORT DMA2_Stream5_IRQHandler [WEAK]
EXPORT DMA2_Stream6_IRQHandler [WEAK]
EXPORT DMA2_Stream7_IRQHandler [WEAK]
EXPORT USART6_IRQHandler [WEAK]
EXPORT I2C3_EV_IRQHandler [WEAK]
EXPORT I2C3_ER_IRQHandler [WEAK]
EXPORT OTG_HS_EP1_OUT_IRQHandler [WEAK]
EXPORT OTG_HS_EP1_IN_IRQHandler [WEAK]
EXPORT OTG_HS_WKUP_IRQHandler [WEAK]
EXPORT OTG_HS_IRQHandler [WEAK]
EXPORT DCMI_PSSI_IRQHandler [WEAK]
EXPORT RNG_IRQHandler [WEAK]
EXPORT FPU_IRQHandler [WEAK]
EXPORT UART7_IRQHandler [WEAK]
EXPORT UART8_IRQHandler [WEAK]
EXPORT SPI4_IRQHandler [WEAK]
EXPORT SPI5_IRQHandler [WEAK]
EXPORT SPI6_IRQHandler [WEAK]
EXPORT SAI1_IRQHandler [WEAK]
EXPORT LTDC_IRQHandler [WEAK]
EXPORT LTDC_ER_IRQHandler [WEAK]
EXPORT DMA2D_IRQHandler [WEAK]
EXPORT OCTOSPI1_IRQHandler [WEAK]
EXPORT LPTIM1_IRQHandler [WEAK]
EXPORT CEC_IRQHandler [WEAK]
EXPORT I2C4_EV_IRQHandler [WEAK]
EXPORT I2C4_ER_IRQHandler [WEAK]
EXPORT SPDIF_RX_IRQHandler [WEAK]
EXPORT DMAMUX1_OVR_IRQHandler [WEAK]
EXPORT DFSDM1_FLT0_IRQHandler [WEAK]
EXPORT DFSDM1_FLT1_IRQHandler [WEAK]
EXPORT DFSDM1_FLT2_IRQHandler [WEAK]
EXPORT DFSDM1_FLT3_IRQHandler [WEAK]
EXPORT SWPMI1_IRQHandler [WEAK]
EXPORT TIM15_IRQHandler [WEAK]
EXPORT TIM16_IRQHandler [WEAK]
EXPORT TIM17_IRQHandler [WEAK]
EXPORT MDIOS_WKUP_IRQHandler [WEAK]
EXPORT MDIOS_IRQHandler [WEAK]
EXPORT MDMA_IRQHandler [WEAK]
EXPORT SDMMC2_IRQHandler [WEAK]
EXPORT HSEM1_IRQHandler [WEAK]
EXPORT ADC3_IRQHandler [WEAK]
EXPORT DMAMUX2_OVR_IRQHandler [WEAK]
EXPORT BDMA_Channel0_IRQHandler [WEAK]
EXPORT BDMA_Channel1_IRQHandler [WEAK]
EXPORT BDMA_Channel2_IRQHandler [WEAK]
EXPORT BDMA_Channel3_IRQHandler [WEAK]
EXPORT BDMA_Channel4_IRQHandler [WEAK]
EXPORT BDMA_Channel5_IRQHandler [WEAK]
EXPORT BDMA_Channel6_IRQHandler [WEAK]
EXPORT BDMA_Channel7_IRQHandler [WEAK]
EXPORT COMP1_IRQHandler [WEAK]
EXPORT LPTIM2_IRQHandler [WEAK]
EXPORT LPTIM3_IRQHandler [WEAK]
EXPORT LPTIM4_IRQHandler [WEAK]
EXPORT LPTIM5_IRQHandler [WEAK]
EXPORT LPUART1_IRQHandler [WEAK]
EXPORT CRS_IRQHandler [WEAK]
EXPORT ECC_IRQHandler [WEAK]
EXPORT SAI4_IRQHandler [WEAK]
EXPORT DTS_IRQHandler [WEAK]
EXPORT WAKEUP_PIN_IRQHandler [WEAK]
EXPORT OCTOSPI2_IRQHandler [WEAK]
EXPORT FMAC_IRQHandler [WEAK]
EXPORT CORDIC_IRQHandler [WEAK]
EXPORT UART9_IRQHandler [WEAK]
EXPORT USART10_IRQHandler [WEAK]
EXPORT I2C5_EV_IRQHandler [WEAK]
EXPORT I2C5_ER_IRQHandler [WEAK]
EXPORT FDCAN3_IT0_IRQHandler [WEAK]
EXPORT FDCAN3_IT1_IRQHandler [WEAK]
EXPORT TIM23_IRQHandler [WEAK]
EXPORT TIM24_IRQHandler [WEAK]
WWDG_IRQHandler
PVD_AVD_IRQHandler
TAMP_STAMP_IRQHandler
RTC_WKUP_IRQHandler
FLASH_IRQHandler
RCC_IRQHandler
EXTI0_IRQHandler
EXTI1_IRQHandler
EXTI2_IRQHandler
EXTI3_IRQHandler
EXTI4_IRQHandler
DMA1_Stream0_IRQHandler
DMA1_Stream1_IRQHandler
DMA1_Stream2_IRQHandler
DMA1_Stream3_IRQHandler
DMA1_Stream4_IRQHandler
DMA1_Stream5_IRQHandler
DMA1_Stream6_IRQHandler
ADC_IRQHandler
FDCAN1_IT0_IRQHandler
FDCAN2_IT0_IRQHandler
FDCAN1_IT1_IRQHandler
FDCAN2_IT1_IRQHandler
EXTI9_5_IRQHandler
TIM1_BRK_IRQHandler
TIM1_UP_IRQHandler
TIM1_TRG_COM_IRQHandler
TIM1_CC_IRQHandler
TIM2_IRQHandler
TIM3_IRQHandler
TIM4_IRQHandler
I2C1_EV_IRQHandler
I2C1_ER_IRQHandler
I2C2_EV_IRQHandler
I2C2_ER_IRQHandler
SPI1_IRQHandler
SPI2_IRQHandler
USART1_IRQHandler
USART2_IRQHandler
USART3_IRQHandler
EXTI15_10_IRQHandler
RTC_Alarm_IRQHandler
TIM8_BRK_TIM12_IRQHandler
TIM8_UP_TIM13_IRQHandler
TIM8_TRG_COM_TIM14_IRQHandler
TIM8_CC_IRQHandler
DMA1_Stream7_IRQHandler
FMC_IRQHandler
SDMMC1_IRQHandler
TIM5_IRQHandler
SPI3_IRQHandler
UART4_IRQHandler
UART5_IRQHandler
TIM6_DAC_IRQHandler
TIM7_IRQHandler
DMA2_Stream0_IRQHandler
DMA2_Stream1_IRQHandler
DMA2_Stream2_IRQHandler
DMA2_Stream3_IRQHandler
DMA2_Stream4_IRQHandler
ETH_IRQHandler
ETH_WKUP_IRQHandler
FDCAN_CAL_IRQHandler
DMA2_Stream5_IRQHandler
DMA2_Stream6_IRQHandler
DMA2_Stream7_IRQHandler
USART6_IRQHandler
I2C3_EV_IRQHandler
I2C3_ER_IRQHandler
OTG_HS_EP1_OUT_IRQHandler
OTG_HS_EP1_IN_IRQHandler
OTG_HS_WKUP_IRQHandler
OTG_HS_IRQHandler
DCMI_PSSI_IRQHandler
RNG_IRQHandler
FPU_IRQHandler
UART7_IRQHandler
UART8_IRQHandler
SPI4_IRQHandler
SPI5_IRQHandler
SPI6_IRQHandler
SAI1_IRQHandler
LTDC_IRQHandler
LTDC_ER_IRQHandler
DMA2D_IRQHandler
OCTOSPI1_IRQHandler
LPTIM1_IRQHandler
CEC_IRQHandler
I2C4_EV_IRQHandler
I2C4_ER_IRQHandler
SPDIF_RX_IRQHandler
DMAMUX1_OVR_IRQHandler
DFSDM1_FLT0_IRQHandler
DFSDM1_FLT1_IRQHandler
DFSDM1_FLT2_IRQHandler
DFSDM1_FLT3_IRQHandler
SWPMI1_IRQHandler
TIM15_IRQHandler
TIM16_IRQHandler
TIM17_IRQHandler
MDIOS_WKUP_IRQHandler
MDIOS_IRQHandler
MDMA_IRQHandler
SDMMC2_IRQHandler
HSEM1_IRQHandler
ADC3_IRQHandler
DMAMUX2_OVR_IRQHandler
BDMA_Channel0_IRQHandler
BDMA_Channel1_IRQHandler
BDMA_Channel2_IRQHandler
BDMA_Channel3_IRQHandler
BDMA_Channel4_IRQHandler
BDMA_Channel5_IRQHandler
BDMA_Channel6_IRQHandler
BDMA_Channel7_IRQHandler
COMP1_IRQHandler
LPTIM2_IRQHandler
LPTIM3_IRQHandler
LPTIM4_IRQHandler
LPTIM5_IRQHandler
LPUART1_IRQHandler
CRS_IRQHandler
ECC_IRQHandler
SAI4_IRQHandler
DTS_IRQHandler
WAKEUP_PIN_IRQHandler
OCTOSPI2_IRQHandler
FMAC_IRQHandler
CORDIC_IRQHandler
UART9_IRQHandler
USART10_IRQHandler
I2C5_EV_IRQHandler
I2C5_ER_IRQHandler
FDCAN3_IT0_IRQHandler
FDCAN3_IT1_IRQHandler
TIM23_IRQHandler
TIM24_IRQHandler
B .
ENDP
ALIGN
;*******************************************************************************
; User Stack and Heap initialization
;*******************************************************************************
IF :DEF:__MICROLIB
EXPORT __initial_sp
EXPORT __heap_base
EXPORT __heap_limit
ELSE
IMPORT __use_two_region_memory
EXPORT __user_initial_stackheap
__user_initial_stackheap
LDR R0, = Heap_Mem
LDR R1, =(Stack_Mem + Stack_Size)
LDR R2, = (Heap_Mem + Heap_Size)
LDR R3, = Stack_Mem
BX LR
ALIGN
ENDIF
END

775
Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F/port.c vendored Normal file
View File

@ -0,0 +1,775 @@
/*
* FreeRTOS Kernel V10.3.1
* Copyright (C) 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of
* this software and associated documentation files (the "Software"), to deal in
* the Software without restriction, including without limitation the rights to
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
* the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*
* http://www.FreeRTOS.org
* http://aws.amazon.com/freertos
*
* 1 tab == 4 spaces!
*/
/*-----------------------------------------------------------
* Implementation of functions defined in portable.h for the ARM CM4F port.
*----------------------------------------------------------*/
/* Scheduler includes. */
#include "FreeRTOS.h"
#include "task.h"
#ifndef __VFP_FP__
#error This port can only be used when the project options are configured to enable hardware floating point support.
#endif
#ifndef configSYSTICK_CLOCK_HZ
#define configSYSTICK_CLOCK_HZ configCPU_CLOCK_HZ
/* Ensure the SysTick is clocked at the same frequency as the core. */
#define portNVIC_SYSTICK_CLK_BIT ( 1UL << 2UL )
#else
/* The way the SysTick is clocked is not modified in case it is not the same
as the core. */
#define portNVIC_SYSTICK_CLK_BIT ( 0 )
#endif
/* Constants required to manipulate the core. Registers first... */
#define portNVIC_SYSTICK_CTRL_REG ( * ( ( volatile uint32_t * ) 0xe000e010 ) )
#define portNVIC_SYSTICK_LOAD_REG ( * ( ( volatile uint32_t * ) 0xe000e014 ) )
#define portNVIC_SYSTICK_CURRENT_VALUE_REG ( * ( ( volatile uint32_t * ) 0xe000e018 ) )
#define portNVIC_SYSPRI2_REG ( * ( ( volatile uint32_t * ) 0xe000ed20 ) )
/* ...then bits in the registers. */
#define portNVIC_SYSTICK_INT_BIT ( 1UL << 1UL )
#define portNVIC_SYSTICK_ENABLE_BIT ( 1UL << 0UL )
#define portNVIC_SYSTICK_COUNT_FLAG_BIT ( 1UL << 16UL )
#define portNVIC_PENDSVCLEAR_BIT ( 1UL << 27UL )
#define portNVIC_PEND_SYSTICK_CLEAR_BIT ( 1UL << 25UL )
/* Constants used to detect a Cortex-M7 r0p1 core, which should use the ARM_CM7
r0p1 port. */
#define portCPUID ( * ( ( volatile uint32_t * ) 0xE000ed00 ) )
#define portCORTEX_M7_r0p1_ID ( 0x410FC271UL )
#define portCORTEX_M7_r0p0_ID ( 0x410FC270UL )
#define portNVIC_PENDSV_PRI ( ( ( uint32_t ) configKERNEL_INTERRUPT_PRIORITY ) << 16UL )
#define portNVIC_SYSTICK_PRI ( ( ( uint32_t ) configKERNEL_INTERRUPT_PRIORITY ) << 24UL )
/* Constants required to check the validity of an interrupt priority. */
#define portFIRST_USER_INTERRUPT_NUMBER ( 16 )
#define portNVIC_IP_REGISTERS_OFFSET_16 ( 0xE000E3F0 )
#define portAIRCR_REG ( * ( ( volatile uint32_t * ) 0xE000ED0C ) )
#define portMAX_8_BIT_VALUE ( ( uint8_t ) 0xff )
#define portTOP_BIT_OF_BYTE ( ( uint8_t ) 0x80 )
#define portMAX_PRIGROUP_BITS ( ( uint8_t ) 7 )
#define portPRIORITY_GROUP_MASK ( 0x07UL << 8UL )
#define portPRIGROUP_SHIFT ( 8UL )
/* Masks off all bits but the VECTACTIVE bits in the ICSR register. */
#define portVECTACTIVE_MASK ( 0xFFUL )
/* Constants required to manipulate the VFP. */
#define portFPCCR ( ( volatile uint32_t * ) 0xe000ef34 ) /* Floating point context control register. */
#define portASPEN_AND_LSPEN_BITS ( 0x3UL << 30UL )
/* Constants required to set up the initial stack. */
#define portINITIAL_XPSR ( 0x01000000 )
#define portINITIAL_EXC_RETURN ( 0xfffffffd )
/* The systick is a 24-bit counter. */
#define portMAX_24_BIT_NUMBER ( 0xffffffUL )
/* For strict compliance with the Cortex-M spec the task start address should
have bit-0 clear, as it is loaded into the PC on exit from an ISR. */
#define portSTART_ADDRESS_MASK ( ( StackType_t ) 0xfffffffeUL )
/* A fiddle factor to estimate the number of SysTick counts that would have
occurred while the SysTick counter is stopped during tickless idle
calculations. */
#define portMISSED_COUNTS_FACTOR ( 45UL )
/* Let the user override the pre-loading of the initial LR with the address of
prvTaskExitError() in case it messes up unwinding of the stack in the
debugger. */
#ifdef configTASK_RETURN_ADDRESS
#define portTASK_RETURN_ADDRESS configTASK_RETURN_ADDRESS
#else
#define portTASK_RETURN_ADDRESS prvTaskExitError
#endif
/*
* Setup the timer to generate the tick interrupts. The implementation in this
* file is weak to allow application writers to change the timer used to
* generate the tick interrupt.
*/
void vPortSetupTimerInterrupt( void );
/*
* Exception handlers.
*/
void xPortPendSVHandler( void ) __attribute__ (( naked ));
void xPortSysTickHandler( void );
void vPortSVCHandler( void ) __attribute__ (( naked ));
/*
* Start first task is a separate function so it can be tested in isolation.
*/
static void prvPortStartFirstTask( void ) __attribute__ (( naked ));
/*
* Function to enable the VFP.
*/
static void vPortEnableVFP( void ) __attribute__ (( naked ));
/*
* Used to catch tasks that attempt to return from their implementing function.
*/
static void prvTaskExitError( void );
/*-----------------------------------------------------------*/
/* Each task maintains its own interrupt status in the critical nesting
variable. */
static UBaseType_t uxCriticalNesting = 0xaaaaaaaa;
/*
* The number of SysTick increments that make up one tick period.
*/
#if( configUSE_TICKLESS_IDLE == 1 )
static uint32_t ulTimerCountsForOneTick = 0;
#endif /* configUSE_TICKLESS_IDLE */
/*
* The maximum number of tick periods that can be suppressed is limited by the
* 24 bit resolution of the SysTick timer.
*/
#if( configUSE_TICKLESS_IDLE == 1 )
static uint32_t xMaximumPossibleSuppressedTicks = 0;
#endif /* configUSE_TICKLESS_IDLE */
/*
* Compensate for the CPU cycles that pass while the SysTick is stopped (low
* power functionality only.
*/
#if( configUSE_TICKLESS_IDLE == 1 )
static uint32_t ulStoppedTimerCompensation = 0;
#endif /* configUSE_TICKLESS_IDLE */
/*
* Used by the portASSERT_IF_INTERRUPT_PRIORITY_INVALID() macro to ensure
* FreeRTOS API functions are not called from interrupts that have been assigned
* a priority above configMAX_SYSCALL_INTERRUPT_PRIORITY.
*/
#if( configASSERT_DEFINED == 1 )
static uint8_t ucMaxSysCallPriority = 0;
static uint32_t ulMaxPRIGROUPValue = 0;
static const volatile uint8_t * const pcInterruptPriorityRegisters = ( const volatile uint8_t * const ) portNVIC_IP_REGISTERS_OFFSET_16;
#endif /* configASSERT_DEFINED */
/*-----------------------------------------------------------*/
/*
* See header file for description.
*/
StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters )
{
/* Simulate the stack frame as it would be created by a context switch
interrupt. */
/* Offset added to account for the way the MCU uses the stack on entry/exit
of interrupts, and to ensure alignment. */
pxTopOfStack--;
*pxTopOfStack = portINITIAL_XPSR; /* xPSR */
pxTopOfStack--;
*pxTopOfStack = ( ( StackType_t ) pxCode ) & portSTART_ADDRESS_MASK; /* PC */
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) portTASK_RETURN_ADDRESS; /* LR */
/* Save code space by skipping register initialisation. */
pxTopOfStack -= 5; /* R12, R3, R2 and R1. */
*pxTopOfStack = ( StackType_t ) pvParameters; /* R0 */
/* A save method is being used that requires each task to maintain its
own exec return value. */
pxTopOfStack--;
*pxTopOfStack = portINITIAL_EXC_RETURN;
pxTopOfStack -= 8; /* R11, R10, R9, R8, R7, R6, R5 and R4. */
return pxTopOfStack;
}
/*-----------------------------------------------------------*/
static void prvTaskExitError( void )
{
volatile uint32_t ulDummy = 0;
/* A function that implements a task must not exit or attempt to return to
its caller as there is nothing to return to. If a task wants to exit it
should instead call vTaskDelete( NULL ).
Artificially force an assert() to be triggered if configASSERT() is
defined, then stop here so application writers can catch the error. */
configASSERT( uxCriticalNesting == ~0UL );
portDISABLE_INTERRUPTS();
while( ulDummy == 0 )
{
/* This file calls prvTaskExitError() after the scheduler has been
started to remove a compiler warning about the function being defined
but never called. ulDummy is used purely to quieten other warnings
about code appearing after this function is called - making ulDummy
volatile makes the compiler think the function could return and
therefore not output an 'unreachable code' warning for code that appears
after it. */
}
}
/*-----------------------------------------------------------*/
void vPortSVCHandler( void )
{
__asm volatile (
" ldr r3, pxCurrentTCBConst2 \n" /* Restore the context. */
" ldr r1, [r3] \n" /* Use pxCurrentTCBConst to get the pxCurrentTCB address. */
" ldr r0, [r1] \n" /* The first item in pxCurrentTCB is the task top of stack. */
" ldmia r0!, {r4-r11, r14} \n" /* Pop the registers that are not automatically saved on exception entry and the critical nesting count. */
" msr psp, r0 \n" /* Restore the task stack pointer. */
" isb \n"
" mov r0, #0 \n"
" msr basepri, r0 \n"
" bx r14 \n"
" \n"
" .align 4 \n"
"pxCurrentTCBConst2: .word pxCurrentTCB \n"
);
}
/*-----------------------------------------------------------*/
static void prvPortStartFirstTask( void )
{
/* Start the first task. This also clears the bit that indicates the FPU is
in use in case the FPU was used before the scheduler was started - which
would otherwise result in the unnecessary leaving of space in the SVC stack
for lazy saving of FPU registers. */
__asm volatile(
" ldr r0, =0xE000ED08 \n" /* Use the NVIC offset register to locate the stack. */
" ldr r0, [r0] \n"
" ldr r0, [r0] \n"
" msr msp, r0 \n" /* Set the msp back to the start of the stack. */
" mov r0, #0 \n" /* Clear the bit that indicates the FPU is in use, see comment above. */
" msr control, r0 \n"
" cpsie i \n" /* Globally enable interrupts. */
" cpsie f \n"
" dsb \n"
" isb \n"
" svc 0 \n" /* System call to start first task. */
" nop \n"
);
}
/*-----------------------------------------------------------*/
/*
* See header file for description.
*/
BaseType_t xPortStartScheduler( void )
{
/* configMAX_SYSCALL_INTERRUPT_PRIORITY must not be set to 0.
See http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html */
configASSERT( configMAX_SYSCALL_INTERRUPT_PRIORITY );
/* This port can be used on all revisions of the Cortex-M7 core other than
the r0p1 parts. r0p1 parts should use the port from the
/source/portable/GCC/ARM_CM7/r0p1 directory. */
configASSERT( portCPUID != portCORTEX_M7_r0p1_ID );
configASSERT( portCPUID != portCORTEX_M7_r0p0_ID );
#if( configASSERT_DEFINED == 1 )
{
volatile uint32_t ulOriginalPriority;
volatile uint8_t * const pucFirstUserPriorityRegister = ( volatile uint8_t * const ) ( portNVIC_IP_REGISTERS_OFFSET_16 + portFIRST_USER_INTERRUPT_NUMBER );
volatile uint8_t ucMaxPriorityValue;
/* Determine the maximum priority from which ISR safe FreeRTOS API
functions can be called. ISR safe functions are those that end in
"FromISR". FreeRTOS maintains separate thread and ISR API functions to
ensure interrupt entry is as fast and simple as possible.
Save the interrupt priority value that is about to be clobbered. */
ulOriginalPriority = *pucFirstUserPriorityRegister;
/* Determine the number of priority bits available. First write to all
possible bits. */
*pucFirstUserPriorityRegister = portMAX_8_BIT_VALUE;
/* Read the value back to see how many bits stuck. */
ucMaxPriorityValue = *pucFirstUserPriorityRegister;
/* Use the same mask on the maximum system call priority. */
ucMaxSysCallPriority = configMAX_SYSCALL_INTERRUPT_PRIORITY & ucMaxPriorityValue;
/* Calculate the maximum acceptable priority group value for the number
of bits read back. */
ulMaxPRIGROUPValue = portMAX_PRIGROUP_BITS;
while( ( ucMaxPriorityValue & portTOP_BIT_OF_BYTE ) == portTOP_BIT_OF_BYTE )
{
ulMaxPRIGROUPValue--;
ucMaxPriorityValue <<= ( uint8_t ) 0x01;
}
#ifdef __NVIC_PRIO_BITS
{
/* Check the CMSIS configuration that defines the number of
priority bits matches the number of priority bits actually queried
from the hardware. */
configASSERT( ( portMAX_PRIGROUP_BITS - ulMaxPRIGROUPValue ) == __NVIC_PRIO_BITS );
}
#endif
#ifdef configPRIO_BITS
{
/* Check the FreeRTOS configuration that defines the number of
priority bits matches the number of priority bits actually queried
from the hardware. */
configASSERT( ( portMAX_PRIGROUP_BITS - ulMaxPRIGROUPValue ) == configPRIO_BITS );
}
#endif
/* Shift the priority group value back to its position within the AIRCR
register. */
ulMaxPRIGROUPValue <<= portPRIGROUP_SHIFT;
ulMaxPRIGROUPValue &= portPRIORITY_GROUP_MASK;
/* Restore the clobbered interrupt priority register to its original
value. */
*pucFirstUserPriorityRegister = ulOriginalPriority;
}
#endif /* conifgASSERT_DEFINED */
/* Make PendSV and SysTick the lowest priority interrupts. */
portNVIC_SYSPRI2_REG |= portNVIC_PENDSV_PRI;
portNVIC_SYSPRI2_REG |= portNVIC_SYSTICK_PRI;
/* Start the timer that generates the tick ISR. Interrupts are disabled
here already. */
vPortSetupTimerInterrupt();
/* Initialise the critical nesting count ready for the first task. */
uxCriticalNesting = 0;
/* Ensure the VFP is enabled - it should be anyway. */
vPortEnableVFP();
/* Lazy save always. */
*( portFPCCR ) |= portASPEN_AND_LSPEN_BITS;
/* Start the first task. */
prvPortStartFirstTask();
/* Should never get here as the tasks will now be executing! Call the task
exit error function to prevent compiler warnings about a static function
not being called in the case that the application writer overrides this
functionality by defining configTASK_RETURN_ADDRESS. Call
vTaskSwitchContext() so link time optimisation does not remove the
symbol. */
vTaskSwitchContext();
prvTaskExitError();
/* Should not get here! */
return 0;
}
/*-----------------------------------------------------------*/
void vPortEndScheduler( void )
{
/* Not implemented in ports where there is nothing to return to.
Artificially force an assert. */
configASSERT( uxCriticalNesting == 1000UL );
}
/*-----------------------------------------------------------*/
void vPortEnterCritical( void )
{
portDISABLE_INTERRUPTS();
uxCriticalNesting++;
/* This is not the interrupt safe version of the enter critical function so
assert() if it is being called from an interrupt context. Only API
functions that end in "FromISR" can be used in an interrupt. Only assert if
the critical nesting count is 1 to protect against recursive calls if the
assert function also uses a critical section. */
if( uxCriticalNesting == 1 )
{
configASSERT( ( portNVIC_INT_CTRL_REG & portVECTACTIVE_MASK ) == 0 );
}
}
/*-----------------------------------------------------------*/
void vPortExitCritical( void )
{
configASSERT( uxCriticalNesting );
uxCriticalNesting--;
if( uxCriticalNesting == 0 )
{
portENABLE_INTERRUPTS();
}
}
/*-----------------------------------------------------------*/
void xPortPendSVHandler( void )
{
/* This is a naked function. */
__asm volatile
(
" mrs r0, psp \n"
" isb \n"
" \n"
" ldr r3, pxCurrentTCBConst \n" /* Get the location of the current TCB. */
" ldr r2, [r3] \n"
" \n"
" tst r14, #0x10 \n" /* Is the task using the FPU context? If so, push high vfp registers. */
" it eq \n"
" vstmdbeq r0!, {s16-s31} \n"
" \n"
" stmdb r0!, {r4-r11, r14} \n" /* Save the core registers. */
" str r0, [r2] \n" /* Save the new top of stack into the first member of the TCB. */
" \n"
" stmdb sp!, {r0, r3} \n"
" mov r0, %0 \n"
" msr basepri, r0 \n"
" dsb \n"
" isb \n"
" bl vTaskSwitchContext \n"
" mov r0, #0 \n"
" msr basepri, r0 \n"
" ldmia sp!, {r0, r3} \n"
" \n"
" ldr r1, [r3] \n" /* The first item in pxCurrentTCB is the task top of stack. */
" ldr r0, [r1] \n"
" \n"
" ldmia r0!, {r4-r11, r14} \n" /* Pop the core registers. */
" \n"
" tst r14, #0x10 \n" /* Is the task using the FPU context? If so, pop the high vfp registers too. */
" it eq \n"
" vldmiaeq r0!, {s16-s31} \n"
" \n"
" msr psp, r0 \n"
" isb \n"
" \n"
#ifdef WORKAROUND_PMU_CM001 /* XMC4000 specific errata workaround. */
#if WORKAROUND_PMU_CM001 == 1
" push { r14 } \n"
" pop { pc } \n"
#endif
#endif
" \n"
" bx r14 \n"
" \n"
" .align 4 \n"
"pxCurrentTCBConst: .word pxCurrentTCB \n"
::"i"(configMAX_SYSCALL_INTERRUPT_PRIORITY)
);
}
/*-----------------------------------------------------------*/
void xPortSysTickHandler( void )
{
/* The SysTick runs at the lowest interrupt priority, so when this interrupt
executes all interrupts must be unmasked. There is therefore no need to
save and then restore the interrupt mask value as its value is already
known. */
portDISABLE_INTERRUPTS();
{
/* Increment the RTOS tick. */
if( xTaskIncrementTick() != pdFALSE )
{
/* A context switch is required. Context switching is performed in
the PendSV interrupt. Pend the PendSV interrupt. */
portNVIC_INT_CTRL_REG = portNVIC_PENDSVSET_BIT;
}
}
portENABLE_INTERRUPTS();
}
/*-----------------------------------------------------------*/
#if( configUSE_TICKLESS_IDLE == 1 )
__attribute__((weak)) void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime )
{
uint32_t ulReloadValue, ulCompleteTickPeriods, ulCompletedSysTickDecrements;
TickType_t xModifiableIdleTime;
/* Make sure the SysTick reload value does not overflow the counter. */
if( xExpectedIdleTime > xMaximumPossibleSuppressedTicks )
{
xExpectedIdleTime = xMaximumPossibleSuppressedTicks;
}
/* Stop the SysTick momentarily. The time the SysTick is stopped for
is accounted for as best it can be, but using the tickless mode will
inevitably result in some tiny drift of the time maintained by the
kernel with respect to calendar time. */
portNVIC_SYSTICK_CTRL_REG &= ~portNVIC_SYSTICK_ENABLE_BIT;
/* Calculate the reload value required to wait xExpectedIdleTime
tick periods. -1 is used because this code will execute part way
through one of the tick periods. */
ulReloadValue = portNVIC_SYSTICK_CURRENT_VALUE_REG + ( ulTimerCountsForOneTick * ( xExpectedIdleTime - 1UL ) );
if( ulReloadValue > ulStoppedTimerCompensation )
{
ulReloadValue -= ulStoppedTimerCompensation;
}
/* Enter a critical section but don't use the taskENTER_CRITICAL()
method as that will mask interrupts that should exit sleep mode. */
__asm volatile( "cpsid i" ::: "memory" );
__asm volatile( "dsb" );
__asm volatile( "isb" );
/* If a context switch is pending or a task is waiting for the scheduler
to be unsuspended then abandon the low power entry. */
if( eTaskConfirmSleepModeStatus() == eAbortSleep )
{
/* Restart from whatever is left in the count register to complete
this tick period. */
portNVIC_SYSTICK_LOAD_REG = portNVIC_SYSTICK_CURRENT_VALUE_REG;
/* Restart SysTick. */
portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT;
/* Reset the reload register to the value required for normal tick
periods. */
portNVIC_SYSTICK_LOAD_REG = ulTimerCountsForOneTick - 1UL;
/* Re-enable interrupts - see comments above the cpsid instruction()
above. */
__asm volatile( "cpsie i" ::: "memory" );
}
else
{
/* Set the new reload value. */
portNVIC_SYSTICK_LOAD_REG = ulReloadValue;
/* Clear the SysTick count flag and set the count value back to
zero. */
portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL;
/* Restart SysTick. */
portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT;
/* Sleep until something happens. configPRE_SLEEP_PROCESSING() can
set its parameter to 0 to indicate that its implementation contains
its own wait for interrupt or wait for event instruction, and so wfi
should not be executed again. However, the original expected idle
time variable must remain unmodified, so a copy is taken. */
xModifiableIdleTime = xExpectedIdleTime;
configPRE_SLEEP_PROCESSING( xModifiableIdleTime );
if( xModifiableIdleTime > 0 )
{
__asm volatile( "dsb" ::: "memory" );
__asm volatile( "wfi" );
__asm volatile( "isb" );
}
configPOST_SLEEP_PROCESSING( xExpectedIdleTime );
/* Re-enable interrupts to allow the interrupt that brought the MCU
out of sleep mode to execute immediately. see comments above
__disable_interrupt() call above. */
__asm volatile( "cpsie i" ::: "memory" );
__asm volatile( "dsb" );
__asm volatile( "isb" );
/* Disable interrupts again because the clock is about to be stopped
and interrupts that execute while the clock is stopped will increase
any slippage between the time maintained by the RTOS and calendar
time. */
__asm volatile( "cpsid i" ::: "memory" );
__asm volatile( "dsb" );
__asm volatile( "isb" );
/* Disable the SysTick clock without reading the
portNVIC_SYSTICK_CTRL_REG register to ensure the
portNVIC_SYSTICK_COUNT_FLAG_BIT is not cleared if it is set. Again,
the time the SysTick is stopped for is accounted for as best it can
be, but using the tickless mode will inevitably result in some tiny
drift of the time maintained by the kernel with respect to calendar
time*/
portNVIC_SYSTICK_CTRL_REG = ( portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT );
/* Determine if the SysTick clock has already counted to zero and
been set back to the current reload value (the reload back being
correct for the entire expected idle time) or if the SysTick is yet
to count to zero (in which case an interrupt other than the SysTick
must have brought the system out of sleep mode). */
if( ( portNVIC_SYSTICK_CTRL_REG & portNVIC_SYSTICK_COUNT_FLAG_BIT ) != 0 )
{
uint32_t ulCalculatedLoadValue;
/* The tick interrupt is already pending, and the SysTick count
reloaded with ulReloadValue. Reset the
portNVIC_SYSTICK_LOAD_REG with whatever remains of this tick
period. */
ulCalculatedLoadValue = ( ulTimerCountsForOneTick - 1UL ) - ( ulReloadValue - portNVIC_SYSTICK_CURRENT_VALUE_REG );
/* Don't allow a tiny value, or values that have somehow
underflowed because the post sleep hook did something
that took too long. */
if( ( ulCalculatedLoadValue < ulStoppedTimerCompensation ) || ( ulCalculatedLoadValue > ulTimerCountsForOneTick ) )
{
ulCalculatedLoadValue = ( ulTimerCountsForOneTick - 1UL );
}
portNVIC_SYSTICK_LOAD_REG = ulCalculatedLoadValue;
/* As the pending tick will be processed as soon as this
function exits, the tick value maintained by the tick is stepped
forward by one less than the time spent waiting. */
ulCompleteTickPeriods = xExpectedIdleTime - 1UL;
}
else
{
/* Something other than the tick interrupt ended the sleep.
Work out how long the sleep lasted rounded to complete tick
periods (not the ulReload value which accounted for part
ticks). */
ulCompletedSysTickDecrements = ( xExpectedIdleTime * ulTimerCountsForOneTick ) - portNVIC_SYSTICK_CURRENT_VALUE_REG;
/* How many complete tick periods passed while the processor
was waiting? */
ulCompleteTickPeriods = ulCompletedSysTickDecrements / ulTimerCountsForOneTick;
/* The reload value is set to whatever fraction of a single tick
period remains. */
portNVIC_SYSTICK_LOAD_REG = ( ( ulCompleteTickPeriods + 1UL ) * ulTimerCountsForOneTick ) - ulCompletedSysTickDecrements;
}
/* Restart SysTick so it runs from portNVIC_SYSTICK_LOAD_REG
again, then set portNVIC_SYSTICK_LOAD_REG back to its standard
value. */
portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL;
portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT;
vTaskStepTick( ulCompleteTickPeriods );
portNVIC_SYSTICK_LOAD_REG = ulTimerCountsForOneTick - 1UL;
/* Exit with interrupts enabled. */
__asm volatile( "cpsie i" ::: "memory" );
}
}
#endif /* #if configUSE_TICKLESS_IDLE */
/*-----------------------------------------------------------*/
/*
* Setup the systick timer to generate the tick interrupts at the required
* frequency.
*/
__attribute__(( weak )) void vPortSetupTimerInterrupt( void )
{
/* Calculate the constants required to configure the tick interrupt. */
#if( configUSE_TICKLESS_IDLE == 1 )
{
ulTimerCountsForOneTick = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ );
xMaximumPossibleSuppressedTicks = portMAX_24_BIT_NUMBER / ulTimerCountsForOneTick;
ulStoppedTimerCompensation = portMISSED_COUNTS_FACTOR / ( configCPU_CLOCK_HZ / configSYSTICK_CLOCK_HZ );
}
#endif /* configUSE_TICKLESS_IDLE */
/* Stop and clear the SysTick. */
portNVIC_SYSTICK_CTRL_REG = 0UL;
portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL;
/* Configure SysTick to interrupt at the requested rate. */
portNVIC_SYSTICK_LOAD_REG = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ) - 1UL;
portNVIC_SYSTICK_CTRL_REG = ( portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT | portNVIC_SYSTICK_ENABLE_BIT );
}
/*-----------------------------------------------------------*/
/* This is a naked function. */
static void vPortEnableVFP( void )
{
__asm volatile
(
" ldr.w r0, =0xE000ED88 \n" /* The FPU enable bits are in the CPACR. */
" ldr r1, [r0] \n"
" \n"
" orr r1, r1, #( 0xf << 20 ) \n" /* Enable CP10 and CP11 coprocessors, then save back. */
" str r1, [r0] \n"
" bx r14 "
);
}
/*-----------------------------------------------------------*/
#if( configASSERT_DEFINED == 1 )
void vPortValidateInterruptPriority( void )
{
uint32_t ulCurrentInterrupt;
uint8_t ucCurrentPriority;
/* Obtain the number of the currently executing interrupt. */
__asm volatile( "mrs %0, ipsr" : "=r"( ulCurrentInterrupt ) :: "memory" );
/* Is the interrupt number a user defined interrupt? */
if( ulCurrentInterrupt >= portFIRST_USER_INTERRUPT_NUMBER )
{
/* Look up the interrupt's priority. */
ucCurrentPriority = pcInterruptPriorityRegisters[ ulCurrentInterrupt ];
/* The following assertion will fail if a service routine (ISR) for
an interrupt that has been assigned a priority above
configMAX_SYSCALL_INTERRUPT_PRIORITY calls an ISR safe FreeRTOS API
function. ISR safe FreeRTOS API functions must *only* be called
from interrupts that have been assigned a priority at or below
configMAX_SYSCALL_INTERRUPT_PRIORITY.
Numerically low interrupt priority numbers represent logically high
interrupt priorities, therefore the priority of the interrupt must
be set to a value equal to or numerically *higher* than
configMAX_SYSCALL_INTERRUPT_PRIORITY.
Interrupts that use the FreeRTOS API must not be left at their
default priority of zero as that is the highest possible priority,
which is guaranteed to be above configMAX_SYSCALL_INTERRUPT_PRIORITY,
and therefore also guaranteed to be invalid.
FreeRTOS maintains separate thread and ISR API functions to ensure
interrupt entry is as fast and simple as possible.
The following links provide detailed information:
http://www.freertos.org/RTOS-Cortex-M3-M4.html
http://www.freertos.org/FAQHelp.html */
configASSERT( ucCurrentPriority >= ucMaxSysCallPriority );
}
/* Priority grouping: The interrupt controller (NVIC) allows the bits
that define each interrupt's priority to be split between bits that
define the interrupt's pre-emption priority bits and bits that define
the interrupt's sub-priority. For simplicity all bits must be defined
to be pre-emption priority bits. The following assertion will fail if
this is not the case (if some bits represent a sub-priority).
If the application only uses CMSIS libraries for interrupt
configuration then the correct setting can be achieved on all Cortex-M
devices by calling NVIC_SetPriorityGrouping( 0 ); before starting the
scheduler. Note however that some vendor specific peripheral libraries
assume a non-zero priority group setting, in which cases using a value
of zero will result in unpredictable behaviour. */
configASSERT( ( portAIRCR_REG & portPRIORITY_GROUP_MASK ) <= ulMaxPRIGROUPValue );
}
#endif /* configASSERT_DEFINED */

243
Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F/portmacro.h vendored Normal file
View File

@ -0,0 +1,243 @@
/*
* FreeRTOS Kernel V10.3.1
* Copyright (C) 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of
* this software and associated documentation files (the "Software"), to deal in
* the Software without restriction, including without limitation the rights to
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
* the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*
* http://www.FreeRTOS.org
* http://aws.amazon.com/freertos
*
* 1 tab == 4 spaces!
*/
#ifndef PORTMACRO_H
#define PORTMACRO_H
#ifdef __cplusplus
extern "C" {
#endif
/*-----------------------------------------------------------
* Port specific definitions.
*
* The settings in this file configure FreeRTOS correctly for the
* given hardware and compiler.
*
* These settings should not be altered.
*-----------------------------------------------------------
*/
/* Type definitions. */
#define portCHAR char
#define portFLOAT float
#define portDOUBLE double
#define portLONG long
#define portSHORT short
#define portSTACK_TYPE uint32_t
#define portBASE_TYPE long
typedef portSTACK_TYPE StackType_t;
typedef long BaseType_t;
typedef unsigned long UBaseType_t;
#if( configUSE_16_BIT_TICKS == 1 )
typedef uint16_t TickType_t;
#define portMAX_DELAY ( TickType_t ) 0xffff
#else
typedef uint32_t TickType_t;
#define portMAX_DELAY ( TickType_t ) 0xffffffffUL
/* 32-bit tick type on a 32-bit architecture, so reads of the tick count do
not need to be guarded with a critical section. */
#define portTICK_TYPE_IS_ATOMIC 1
#endif
/*-----------------------------------------------------------*/
/* Architecture specifics. */
#define portSTACK_GROWTH ( -1 )
#define portTICK_PERIOD_MS ( ( TickType_t ) 1000 / configTICK_RATE_HZ )
#define portBYTE_ALIGNMENT 8
/*-----------------------------------------------------------*/
/* Scheduler utilities. */
#define portYIELD() \
{ \
/* Set a PendSV to request a context switch. */ \
portNVIC_INT_CTRL_REG = portNVIC_PENDSVSET_BIT; \
\
/* Barriers are normally not required but do ensure the code is completely \
within the specified behaviour for the architecture. */ \
__asm volatile( "dsb" ::: "memory" ); \
__asm volatile( "isb" ); \
}
#define portNVIC_INT_CTRL_REG ( * ( ( volatile uint32_t * ) 0xe000ed04 ) )
#define portNVIC_PENDSVSET_BIT ( 1UL << 28UL )
#define portEND_SWITCHING_ISR( xSwitchRequired ) if( xSwitchRequired != pdFALSE ) portYIELD()
#define portYIELD_FROM_ISR( x ) portEND_SWITCHING_ISR( x )
/*-----------------------------------------------------------*/
/* Critical section management. */
extern void vPortEnterCritical( void );
extern void vPortExitCritical( void );
#define portSET_INTERRUPT_MASK_FROM_ISR() ulPortRaiseBASEPRI()
#define portCLEAR_INTERRUPT_MASK_FROM_ISR(x) vPortSetBASEPRI(x)
#define portDISABLE_INTERRUPTS() vPortRaiseBASEPRI()
#define portENABLE_INTERRUPTS() vPortSetBASEPRI(0)
#define portENTER_CRITICAL() vPortEnterCritical()
#define portEXIT_CRITICAL() vPortExitCritical()
/*-----------------------------------------------------------*/
/* Task function macros as described on the FreeRTOS.org WEB site. These are
not necessary for to use this port. They are defined so the common demo files
(which build with all the ports) will build. */
#define portTASK_FUNCTION_PROTO( vFunction, pvParameters ) void vFunction( void *pvParameters )
#define portTASK_FUNCTION( vFunction, pvParameters ) void vFunction( void *pvParameters )
/*-----------------------------------------------------------*/
/* Tickless idle/low power functionality. */
#ifndef portSUPPRESS_TICKS_AND_SLEEP
extern void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime );
#define portSUPPRESS_TICKS_AND_SLEEP( xExpectedIdleTime ) vPortSuppressTicksAndSleep( xExpectedIdleTime )
#endif
/*-----------------------------------------------------------*/
/* Architecture specific optimisations. */
#ifndef configUSE_PORT_OPTIMISED_TASK_SELECTION
#define configUSE_PORT_OPTIMISED_TASK_SELECTION 1
#endif
#if configUSE_PORT_OPTIMISED_TASK_SELECTION == 1
/* Generic helper function. */
__attribute__( ( always_inline ) ) static inline uint8_t ucPortCountLeadingZeros( uint32_t ulBitmap )
{
uint8_t ucReturn;
__asm volatile ( "clz %0, %1" : "=r" ( ucReturn ) : "r" ( ulBitmap ) : "memory" );
return ucReturn;
}
/* Check the configuration. */
#if( configMAX_PRIORITIES > 32 )
#error configUSE_PORT_OPTIMISED_TASK_SELECTION can only be set to 1 when configMAX_PRIORITIES is less than or equal to 32. It is very rare that a system requires more than 10 to 15 difference priorities as tasks that share a priority will time slice.
#endif
/* Store/clear the ready priorities in a bit map. */
#define portRECORD_READY_PRIORITY( uxPriority, uxReadyPriorities ) ( uxReadyPriorities ) |= ( 1UL << ( uxPriority ) )
#define portRESET_READY_PRIORITY( uxPriority, uxReadyPriorities ) ( uxReadyPriorities ) &= ~( 1UL << ( uxPriority ) )
/*-----------------------------------------------------------*/
#define portGET_HIGHEST_PRIORITY( uxTopPriority, uxReadyPriorities ) uxTopPriority = ( 31UL - ( uint32_t ) ucPortCountLeadingZeros( ( uxReadyPriorities ) ) )
#endif /* configUSE_PORT_OPTIMISED_TASK_SELECTION */
/*-----------------------------------------------------------*/
#ifdef configASSERT
void vPortValidateInterruptPriority( void );
#define portASSERT_IF_INTERRUPT_PRIORITY_INVALID() vPortValidateInterruptPriority()
#endif
/* portNOP() is not required by this port. */
#define portNOP()
#define portINLINE __inline
#ifndef portFORCE_INLINE
#define portFORCE_INLINE inline __attribute__(( always_inline))
#endif
portFORCE_INLINE static BaseType_t xPortIsInsideInterrupt( void )
{
uint32_t ulCurrentInterrupt;
BaseType_t xReturn;
/* Obtain the number of the currently executing interrupt. */
__asm volatile( "mrs %0, ipsr" : "=r"( ulCurrentInterrupt ) :: "memory" );
if( ulCurrentInterrupt == 0 )
{
xReturn = pdFALSE;
}
else
{
xReturn = pdTRUE;
}
return xReturn;
}
/*-----------------------------------------------------------*/
portFORCE_INLINE static void vPortRaiseBASEPRI( void )
{
uint32_t ulNewBASEPRI;
__asm volatile
(
" mov %0, %1 \n" \
" msr basepri, %0 \n" \
" isb \n" \
" dsb \n" \
:"=r" (ulNewBASEPRI) : "i" ( configMAX_SYSCALL_INTERRUPT_PRIORITY ) : "memory"
);
}
/*-----------------------------------------------------------*/
portFORCE_INLINE static uint32_t ulPortRaiseBASEPRI( void )
{
uint32_t ulOriginalBASEPRI, ulNewBASEPRI;
__asm volatile
(
" mrs %0, basepri \n" \
" mov %1, %2 \n" \
" msr basepri, %1 \n" \
" isb \n" \
" dsb \n" \
:"=r" (ulOriginalBASEPRI), "=r" (ulNewBASEPRI) : "i" ( configMAX_SYSCALL_INTERRUPT_PRIORITY ) : "memory"
);
/* This return will not be reached but is necessary to prevent compiler
warnings. */
return ulOriginalBASEPRI;
}
/*-----------------------------------------------------------*/
portFORCE_INLINE static void vPortSetBASEPRI( uint32_t ulNewMaskValue )
{
__asm volatile
(
" msr basepri, %0 " :: "r" ( ulNewMaskValue ) : "memory"
);
}
/*-----------------------------------------------------------*/
#define portMEMORY_BARRIER() __asm volatile( "" ::: "memory" )
#ifdef __cplusplus
}
#endif
#endif /* PORTMACRO_H */

167
User/component/lqr.c Normal file
View File

@ -0,0 +1,167 @@
/*
* LQR控制器实现：单腿建模
*/
#include "lqr.h"
#include <string.h>
#include <stddef.h>
/* Private macros ----------------------------------------------------------- */
#define LQR_LIMIT(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
/* Private variables -------------------------------------------------------- */
/* Private function prototypes ---------------------------------------------- */
static int8_t LQR_CalculateErrorState(LQR_t *lqr);
static float LQR_PolynomialCalc(const float *coeff, float leg_length);
/* Public functions --------------------------------------------------------- */
int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix) {
if (lqr == NULL || gain_matrix == NULL) {
return -1;
}
/* 清零结构体 */
memset(lqr, 0, sizeof(LQR_t));
/* 设置增益矩阵 */
lqr->gain_matrix = gain_matrix;
/* 初始化当前腿长为中等值 */
lqr->current_leg_length = 0.25f;
/* 计算初始增益矩阵 */
LQR_CalculateGainMatrix(lqr, lqr->current_leg_length);
lqr->initialized = true;
return 0;
}
int8_t LQR_UpdateState(LQR_t *lqr, const LQR_State_t *current_state) {
if (lqr == NULL || current_state == NULL) {
return -1;
}
/* 更新当前状态 */
lqr->current_state = *current_state;
/* 计算状态误差 */
return LQR_CalculateErrorState(lqr);
}
int8_t LQR_SetTargetState(LQR_t *lqr, const LQR_State_t *target_state) {
if (lqr == NULL || target_state == NULL) {
return -1;
}
lqr->target_state = *target_state;
/* 重新计算状态误差 */
return LQR_CalculateErrorState(lqr);
}
int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length) {
if (lqr == NULL || lqr->gain_matrix == NULL) {
return -1;
}
/* 限制腿长范围 */
leg_length = LQR_LIMIT(leg_length, 0.1f, 0.4f);
lqr->current_leg_length = leg_length;
/* 使用多项式拟合计算当前增益矩阵 */
lqr->K_matrix[0][0] = LQR_PolynomialCalc(lqr->gain_matrix->k11_coeff, leg_length); /* K11: theta */
lqr->K_matrix[0][1] = LQR_PolynomialCalc(lqr->gain_matrix->k12_coeff, leg_length); /* K12: d_theta */
lqr->K_matrix[0][2] = LQR_PolynomialCalc(lqr->gain_matrix->k13_coeff, leg_length); /* K13: x */
lqr->K_matrix[0][3] = LQR_PolynomialCalc(lqr->gain_matrix->k14_coeff, leg_length); /* K14: d_x */
lqr->K_matrix[0][4] = LQR_PolynomialCalc(lqr->gain_matrix->k15_coeff, leg_length); /* K15: phi */
lqr->K_matrix[0][5] = LQR_PolynomialCalc(lqr->gain_matrix->k16_coeff, leg_length); /* K16: d_phi */
lqr->K_matrix[1][0] = LQR_PolynomialCalc(lqr->gain_matrix->k21_coeff, leg_length); /* K21: theta */
lqr->K_matrix[1][1] = LQR_PolynomialCalc(lqr->gain_matrix->k22_coeff, leg_length); /* K22: d_theta */
lqr->K_matrix[1][2] = LQR_PolynomialCalc(lqr->gain_matrix->k23_coeff, leg_length); /* K23: x */
lqr->K_matrix[1][3] = LQR_PolynomialCalc(lqr->gain_matrix->k24_coeff, leg_length); /* K24: d_x */
lqr->K_matrix[1][4] = LQR_PolynomialCalc(lqr->gain_matrix->k25_coeff, leg_length); /* K25: phi */
lqr->K_matrix[1][5] = LQR_PolynomialCalc(lqr->gain_matrix->k26_coeff, leg_length); /* K26: d_phi */
return 0;
}
int8_t LQR_Control(LQR_t *lqr) {
if (lqr == NULL || !lqr->initialized) {
return -1;
}
/* LQR控制律: u = -K * x_error
* 其中 u = [T; Tp], x_error = x_current - x_target
*/
/* 计算轮毂力矩T */
lqr->control_output.T = -(
lqr->K_matrix[0][0] * lqr->error_state.theta +
lqr->K_matrix[0][1] * lqr->error_state.d_theta +
lqr->K_matrix[0][2] * lqr->error_state.x +
lqr->K_matrix[0][3] * lqr->error_state.d_x +
lqr->K_matrix[0][4] * lqr->error_state.phi +
lqr->K_matrix[0][5] * lqr->error_state.d_phi
);
/* 计算髋关节力矩Tp */
lqr->control_output.Tp = -(
lqr->K_matrix[1][0] * lqr->error_state.theta +
lqr->K_matrix[1][1] * lqr->error_state.d_theta +
lqr->K_matrix[1][2] * lqr->error_state.x +
lqr->K_matrix[1][3] * lqr->error_state.d_x +
lqr->K_matrix[1][4] * lqr->error_state.phi +
lqr->K_matrix[1][5] * lqr->error_state.d_phi
);
return 0;
}
int8_t LQR_Reset(LQR_t *lqr) {
if (lqr == NULL) {
return -1;
}
/* 清零状态和输出 */
memset(&lqr->current_state, 0, sizeof(LQR_State_t));
memset(&lqr->target_state, 0, sizeof(LQR_State_t));
memset(&lqr->error_state, 0, sizeof(LQR_State_t));
memset(&lqr->control_output, 0, sizeof(LQR_Output_t));
return 0;
}
/* Private functions -------------------------------------------------------- */
static int8_t LQR_CalculateErrorState(LQR_t *lqr) {
if (lqr == NULL) {
return -1;
}
/* 计算状态误差 */
lqr->error_state.theta = lqr->current_state.theta - lqr->target_state.theta;
lqr->error_state.d_theta = lqr->current_state.d_theta - lqr->target_state.d_theta;
lqr->error_state.x = lqr->current_state.x - lqr->target_state.x;
lqr->error_state.d_x = lqr->current_state.d_x - lqr->target_state.d_x;
lqr->error_state.phi = lqr->current_state.phi - lqr->target_state.phi;
lqr->error_state.d_phi = lqr->current_state.d_phi - lqr->target_state.d_phi;
return 0;
}
static float LQR_PolynomialCalc(const float *coeff, float leg_length) {
if (coeff == NULL) {
return 0.0f;
}
/* 计算3阶多项式: coeff[0]*L^3 + coeff[1]*L^2 + coeff[2]*L + coeff[3] */
float L = leg_length;
float L2 = L * L;
float L3 = L2 * L;
return coeff[0] * L3 + coeff[1] * L2 + coeff[2] * L + coeff[3];
}

134
User/component/lqr.h Normal file
View File

@ -0,0 +1,134 @@
/*
* LQR控制器
* 用于轮腿平衡机器人的线性二次调节器控制：单腿建模
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
/* Exported constants ------------------------------------------------------- */
#define LQR_STATE_DIM 6 /* 状态向量维度: theta, d_theta, x, d_x, phi, d_phi */
#define LQR_INPUT_DIM 2 /* 输入向量维度: T(轮毂力矩), Tp(髋关节力矩) */
#define LQR_POLY_ORDER 4 /* 多项式拟合阶数 */
/* Exported types ----------------------------------------------------------- */
typedef struct {
float theta; /* 摆杆角度 */
float d_theta; /* 摆杆角速度 */
float x; /* 驱动轮位移 */
float d_x; /* 驱动轮速度 */
float phi; /* 机体俯仰角 */
float d_phi; /* 机体俯仰角速度 */
} LQR_State_t;
typedef struct {
float T; /* 轮毂力矩 (N·m) */
float Tp; /* 髋关节力矩 (N·m) */
} LQR_Output_t;
typedef struct {
/* K矩阵第一行(轮毂力矩T对应的增益) */
float k11_coeff[LQR_POLY_ORDER]; /* K(1,1): theta */
float k12_coeff[LQR_POLY_ORDER]; /* K(1,2): d_theta */
float k13_coeff[LQR_POLY_ORDER]; /* K(1,3): x */
float k14_coeff[LQR_POLY_ORDER]; /* K(1,4): d_x */
float k15_coeff[LQR_POLY_ORDER]; /* K(1,5): phi */
float k16_coeff[LQR_POLY_ORDER]; /* K(1,6): d_phi */
/* K矩阵第二行(髋关节力矩Tp对应的增益) */
float k21_coeff[LQR_POLY_ORDER]; /* K(2,1): theta */
float k22_coeff[LQR_POLY_ORDER]; /* K(2,2): d_theta */
float k23_coeff[LQR_POLY_ORDER]; /* K(2,3): x */
float k24_coeff[LQR_POLY_ORDER]; /* K(2,4): d_x */
float k25_coeff[LQR_POLY_ORDER]; /* K(2,5): phi */
float k26_coeff[LQR_POLY_ORDER]; /* K(2,6): d_phi */
} LQR_GainMatrix_t;
/**
* @brief LQR控制器主结构体
*/
typedef struct {
LQR_GainMatrix_t *gain_matrix; /* 增益矩阵参数指针 */
LQR_State_t current_state; /* 当前状态 */
LQR_State_t target_state; /* 目标状态 */
LQR_State_t error_state; /* 状态误差 */
LQR_Output_t control_output; /* 控制输出 */
/* 运行时变量 */
float current_leg_length; /* 当前腿长 */
float K_matrix[LQR_INPUT_DIM][LQR_STATE_DIM]; /* 当前增益矩阵 */
bool initialized; /* 初始化标志 */
} LQR_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化LQR控制器
*
* @param lqr LQR控制器结构体指针
* @param gain_matrix 增益矩阵参数指针
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix);
/**
* @brief 更新当前状态
*
* @param lqr LQR控制器结构体指针
* @param state 当前状态
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_UpdateState(LQR_t *lqr, const LQR_State_t *current_state);
/**
* @brief 设置目标状态
*
* @param lqr LQR控制器结构体指针
* @param target_state 目标状态
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_SetTargetState(LQR_t *lqr, const LQR_State_t *target_state);
/**
* @brief 根据当前腿长计算增益矩阵
*
* @param lqr LQR控制器结构体指针
* @param leg_length 当前腿长 (m)
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length);
/**
* @brief 执行LQR控制计算
*
* @param lqr LQR控制器结构体指针
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_Control(LQR_t *lqr);
/**
* @brief 重置LQR控制器
*
* @param lqr LQR控制器结构体指针
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_Reset(LQR_t *lqr);
#ifdef __cplusplus
}
#endif

383
User/component/vmc.c Normal file
View File

@ -0,0 +1,383 @@
/*
* VMC虚拟模型控制器实现
*
* 本文件实现了轮腿机器人的VMC (Virtual Model Control) 虚拟模型控制算法
* 主要功能包括:
* 1. 五连杆机构的正逆运动学解算
* 2. 虚拟力到关节力矩的映射
* 3. 地面接触检测
* 4. 等效摆动杆模型转换
*
* 参考文献:
* - 韭菜的菜 知乎: 平衡步兵控制系统设计
* - 上交轮腿电控开源方案 (2023)
*/
#include "vmc.h"
#include <string.h>
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
#define VMC_EPSILON (1e-6f) // 数值计算精度
#define VMC_MAX_ITER (10) // 最大迭代次数
/* Private macro ------------------------------------------------------------ */
/**
* @brief 限制数值范围
*/
#define VMC_CLAMP(val, min, max) ((val) < (min) ? (min) : ((val) > (max) ? (max) : (val)))
/**
* @brief 安全开方
*/
#define VMC_SAFE_SQRT(x) (((x) > 0) ? sqrtf(x) : 0.0f)
/* Private variables -------------------------------------------------------- */
/* Private function prototypes ---------------------------------------------- */
static int8_t VMC_ValidateParams(const VMC_Param_t *param);
static void VMC_UpdateKinematics(VMC_t *vmc, float phi1, float phi4);
static int8_t VMC_SolveClosedLoop(VMC_t *vmc);
static float VMC_ComputeNumericDerivative(float current, float previous, float dt);
/* Exported functions ------------------------------------------------------- */
/**
* @brief 初始化VMC控制器
*/
int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq) {
if (vmc == NULL || param == NULL || sample_freq <= 0) {
return -1;
}
// 复制参数
memcpy(&vmc->param, param, sizeof(VMC_Param_t));
// 设置控制周期
vmc->dt = 1.0f / sample_freq;
// 重置状态
VMC_Reset(vmc);
vmc->initialized = true;
return 0;
}
/**
* @brief VMC五连杆正解算
*
* 通过髋关节角度和机体姿态，计算足端位置和等效摆动杆参数
*
* 坐标系定义:
* - x轴: 机体前进方向为正
* - y轴: 竖直向下为正
* - 角度: 顺时针为正
*/
int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, float body_pitch_rate) {
if (vmc == NULL || !vmc->initialized) {
return -1;
}
body_pitch = -body_pitch; // 机体俯仰角取反
VMC_Leg_t *leg = &vmc->leg;
// 保存历史值
leg->last_phi0 = leg->phi0;
leg->last_L0 = leg->L0;
leg->last_d_L0 = leg->d_L0;
leg->last_d_theta = leg->d_theta;
// 更新关节角度
leg->phi1 = phi1;
leg->phi4 = phi4;
// 更新运动学状态
VMC_UpdateKinematics(vmc, phi1, phi4);
// 求解闭环运动学
if (VMC_SolveClosedLoop(vmc) != 0) {
return -1;
}
// 计算足端坐标
leg->foot_x = leg->XC - vmc->param.hip_length / 2.0f;
leg->foot_y = leg->YC;
// 计算等效摆动杆参数
leg->L0 = VMC_SAFE_SQRT(leg->foot_x * leg->foot_x + leg->foot_y * leg->foot_y);
leg->phi0 = atan2f(leg->foot_y, leg->foot_x);
// 计算等效摆动杆角度(相对于机体坐标系)
leg->alpha = VMC_PI_2 - leg->phi0;
leg->theta = VMC_PI - (VMC_PI_2 + body_pitch - leg->phi0);
// 角度归一化
VMC_ANGLE_NORMALIZE(leg->theta);
VMC_ANGLE_NORMALIZE(leg->phi0);
// 计算角速度和长度变化率
leg->d_phi0 = VMC_ComputeNumericDerivative(leg->phi0, leg->last_phi0, vmc->dt);
leg->d_alpha = -leg->d_phi0;
leg->d_theta = body_pitch_rate + leg->d_phi0;
leg->d_L0 = VMC_ComputeNumericDerivative(leg->L0, leg->last_L0, vmc->dt);
// 计算角加速度
leg->dd_theta = VMC_ComputeNumericDerivative(leg->d_theta, leg->last_d_theta, vmc->dt);
VMC_GroundContactDetection(vmc); // 更新地面接触状态
return 0;
}
/**
* @brief VMC五连杆逆解算(力矩分配)
*
* 根据期望的虚拟力和力矩，通过雅可比矩阵计算关节力矩
*/
int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual) {
if (vmc == NULL || !vmc->initialized) {
return -1;
}
// 保存虚拟力和力矩
vmc->leg.F_virtual = F_virtual;
vmc->leg.T_virtual = -T_virtual;
// 计算雅可比矩阵
if (VMC_ComputeJacobian(vmc) != 0) {
return -1;
}
VMC_Leg_t *leg = &vmc->leg;
// 通过雅可比转置计算关节力矩
// tau = J^T * F_virtual
leg->tau_hip_rear = leg->J11 * vmc->leg.F_virtual + leg->J12 * vmc->leg.T_virtual;
leg->tau_hip_front = leg->J21 * vmc->leg.F_virtual + leg->J22 * vmc->leg.T_virtual;
return 0;
}
/**
* @brief 地面接触检测
*
* 基于虚拟力和腿部状态估计地面法向力
*/
float VMC_GroundContactDetection(VMC_t *vmc) {
if (vmc == NULL || !vmc->initialized) {
return 0.0f;
}
VMC_Leg_t *leg = &vmc->leg;
// 计算地面法向力
// Fn = F0*cos(theta) + Tp*sin(theta)/L0 + mg
leg->Fn = leg->F_virtual * cosf(leg->theta) +
leg->T_virtual * sinf(leg->theta) / leg->L0 +
10.0f; // 添加轮子重力，假设轮子质量约1kg
// 地面接触判断
leg->is_ground_contact = (leg->Fn > 20.0f); // 10N阈值
return leg->Fn;
}
/**
* @brief 获取足端位置
*/
int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y) {
if (vmc == NULL || !vmc->initialized || x == NULL || y == NULL) {
return -1;
}
*x = vmc->leg.foot_x;
*y = vmc->leg.foot_y;
return 0;
}
/**
* @brief 获取等效摆动杆参数
*/
int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle) {
if (vmc == NULL || !vmc->initialized) {
return -1;
}
if (length) *length = vmc->leg.L0;
if (angle) *angle = vmc->leg.theta;
if (d_length) *d_length = vmc->leg.d_L0;
if (d_angle) *d_angle = vmc->leg.d_theta;
return 0;
}
/**
* @brief 获取关节输出力矩
*/
int8_t VMC_GetJointTorques(const VMC_t *vmc, float *tau_front, float *tau_rear) {
if (vmc == NULL || !vmc->initialized || tau_front == NULL || tau_rear == NULL) {
return -1;
}
*tau_front = vmc->leg.tau_hip_front;
*tau_rear = vmc->leg.tau_hip_rear;
return 0;
}
/**
* @brief 重置VMC控制器状态
*/
void VMC_Reset(VMC_t *vmc) {
if (vmc == NULL) {
return;
}
// 清零腿部状态
memset(&vmc->leg, 0, sizeof(VMC_Leg_t));
// 设置初始值
vmc->leg.L0 = 0.15f; // 默认腿长15cm
vmc->leg.theta = 0.0f;
}
/**
* @brief 设置虚拟力和力矩
*/
void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual) {
if (vmc == NULL || !vmc->initialized) {
return;
}
vmc->leg.F_virtual = F_virtual;
vmc->leg.T_virtual = T_virtual;
}
/**
* @brief 计算雅可比矩阵
*
* 根据当前关节配置计算从虚拟力到关节力矩的雅可比矩阵
*/
int8_t VMC_ComputeJacobian(VMC_t *vmc) {
if (vmc == NULL || !vmc->initialized) {
return -1;
}
VMC_Leg_t *leg = &vmc->leg;
// 检查分母不为零
float sin_diff = sinf(leg->phi3 - leg->phi2);
if (fabsf(sin_diff) < VMC_EPSILON) {
return -1; // 奇异配置
}
// 计算雅可比矩阵元素
// J11: 后髋关节到支撑力的雅可比
leg->J11 = (vmc->param.leg_1 * sinf(leg->phi0 - leg->phi3) *
sinf(leg->phi1 - leg->phi2)) / sin_diff;
// J12: 后髋关节到摆动力矩的雅可比
leg->J12 = (vmc->param.leg_1 * cosf(leg->phi0 - leg->phi3) *
sinf(leg->phi1 - leg->phi2)) / (leg->L0 * sin_diff);
// J21: 前髋关节到支撑力的雅可比
leg->J21 = (vmc->param.leg_4 * sinf(leg->phi0 - leg->phi2) *
sinf(leg->phi3 - leg->phi4)) / sin_diff;
// J22: 前髋关节到摆动力矩的雅可比
leg->J22 = (vmc->param.leg_4 * cosf(leg->phi0 - leg->phi2) *
sinf(leg->phi3 - leg->phi4)) / (leg->L0 * sin_diff);
return 0;
}
/* Private functions -------------------------------------------------------- */
/**
* @brief 验证VMC参数有效性
*/
static int8_t VMC_ValidateParams(const VMC_Param_t *param) {
if (param->hip_length <= 0 || param->leg_1 <= 0 || param->leg_2 <= 0 ||
param->leg_3 <= 0 || param->leg_4 <= 0 || param->wheel_radius <= 0) {
return -1;
}
// 检查腿部几何约束
if (param->leg_2 + param->leg_3 <= param->leg_1 + param->leg_4) {
return -1; // 不满足闭环几何约束
}
return 0;
}
/**
* @brief 更新基本运动学参数
*/
static void VMC_UpdateKinematics(VMC_t *vmc, float phi1, float phi4) {
VMC_Leg_t *leg = &vmc->leg;
// 计算关键点坐标
// 点B (后髋关节末端)
leg->XB = vmc->param.leg_1 * cosf(phi1);
leg->YB = vmc->param.leg_1 * sinf(phi1);
// 点D (前髋关节末端)
leg->XD = vmc->param.hip_length + vmc->param.leg_4 * cosf(phi4);
leg->YD = vmc->param.leg_4 * sinf(phi4);
// 计算BD连杆长度
float dx = leg->XD - leg->XB;
float dy = leg->YD - leg->YB;
leg->lBD = VMC_SAFE_SQRT(dx * dx + dy * dy);
}
/**
* @brief 求解闭环运动学方程
*
* 根据两个髋关节角度，求解中间关节角度
*/
static int8_t VMC_SolveClosedLoop(VMC_t *vmc) {
VMC_Leg_t *leg = &vmc->leg;
// 使用余弦定理求解phi2
leg->A0 = 2 * vmc->param.leg_2 * (leg->XD - leg->XB);
leg->B0 = 2 * vmc->param.leg_2 * (leg->YD - leg->YB);
leg->C0 = vmc->param.leg_2 * vmc->param.leg_2 +
leg->lBD * leg->lBD -
vmc->param.leg_3 * vmc->param.leg_3;
// 检查判别式
float discriminant = leg->A0 * leg->A0 + leg->B0 * leg->B0 - leg->C0 * leg->C0;
if (discriminant < 0) {
return -1; // 无解
}
float sqrt_discriminant = VMC_SAFE_SQRT(discriminant);
// 计算phi2 (选择合适的解)
leg->phi2 = 2 * atan2f(leg->B0 + sqrt_discriminant, leg->A0 + leg->C0);
// 计算phi3
leg->phi3 = atan2f(leg->YB - leg->YD + vmc->param.leg_2 * sinf(leg->phi2),
leg->XB - leg->XD + vmc->param.leg_2 * cosf(leg->phi2));
// 计算足端坐标点C
leg->XC = leg->XB + vmc->param.leg_2 * cosf(leg->phi2);
leg->YC = leg->YB + vmc->param.leg_2 * sinf(leg->phi2);
return 0;
}
/**
* @brief 计算数值微分
*/
static float VMC_ComputeNumericDerivative(float current, float previous, float dt) {
if (dt <= 0) {
return 0.0f;
}
return (current - previous) / dt;
}

195
User/component/vmc.h Normal file
View File

@ -0,0 +1,195 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
/* Exported types ----------------------------------------------------------- */
/**
* @brief VMC虚拟模型控制参数结构体
*/
typedef struct {
float hip_length; // 髋关节间距
float leg_1; // 大腿前端长度 (L1)
float leg_2; // 大腿后端长度 (L2)
float leg_3; // 小腿长度 (L3)
float leg_4; // 小腿前端长度 (L4)
float wheel_radius; // 轮子半径
float wheel_mass; // 轮子质量
} VMC_Param_t;
/**
* @brief VMC腿部运动学状态结构体
*/
typedef struct {
// 关节角度
float phi1; // 后髋关节角度 (rad)
float phi2; // 大腿后端角度 (rad)
float phi3; // 小腿角度 (rad)
float phi4; // 前髋关节角度 (rad)
// 足端坐标
float foot_x; // 足端x坐标
float foot_y; // 足端y坐标
// 等效摆动杆参数
float L0; // 等效摆动杆长度
float d_L0; // 等效摆动杆长度变化率
float theta; // 等效摆动杆角度
float d_theta; // 等效摆动杆角速度
float dd_theta; // 等效摆动杆角加速度
// 虚拟力和力矩
float F_virtual; // 虚拟支撑力
float T_virtual; // 虚拟摆动力矩
// 雅可比矩阵元素
float J11, J12, J21, J22;
// 输出力矩
float tau_hip_front; // 前髋关节输出力矩
float tau_hip_rear; // 后髋关节输出力矩
// 内部计算变量
float XB, YB, XC, YC, XD, YD; // 各关键点坐标
float lBD; // BD连杆长度
float A0, B0, C0; // 运动学计算中间变量
float phi0; // 足端极角
float alpha; // 等效摆动杆与竖直方向夹角
float d_phi0; // 足端极角变化率
float d_alpha; // alpha角变化率
// 历史值（用于数值微分）
float last_phi0;
float last_L0;
float last_d_L0;
float last_d_theta;
// 地面接触检测
float Fn; // 地面法向力
bool is_ground_contact; // 地面接触标志
} VMC_Leg_t;
/**
* @brief VMC控制器结构体
*/
typedef struct {
VMC_Param_t param; // VMC参数
VMC_Leg_t leg; // 腿部状态
float dt; // 控制周期
bool initialized; // 初始化标志
} VMC_t;
/* Exported constants ------------------------------------------------------- */
#define VMC_PI_2 (1.5707963267948966f)
#define VMC_PI (3.1415926535897932f)
#define VMC_2PI (6.2831853071795865f)
/* Exported macros ---------------------------------------------------------- */
/**
* @brief 角度范围限制到[-PI, PI]
*/
#define VMC_ANGLE_NORMALIZE(angle) do { \
while((angle) > VMC_PI) (angle) -= VMC_2PI; \
while((angle) < -VMC_PI) (angle) += VMC_2PI; \
} while(0)
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化VMC控制器
* @param vmc VMC控制器实例
* @param param VMC参数
* @param sample_freq 采样频率 (Hz)
* @return 0:成功, -1:失败
*/
int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq);
/**
* @brief VMC五连杆正解算
* @param vmc VMC控制器实例
* @param phi1 后髋关节角度 (rad)
* @param phi4 前髋关节角度 (rad)
* @param body_pitch 机体pitch角 (rad)
* @param body_pitch_rate 机体pitch角速度 (rad/s)
* @return 0:成功, -1:失败
*/
int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, float body_pitch_rate);
/**
* @brief VMC五连杆逆解算(力矩分配)
* @param vmc VMC控制器实例
* @param F_virtual 期望虚拟支撑力 (N)
* @param T_virtual 期望虚拟摆动力矩 (N*m)
* @return 0:成功, -1:失败
*/
int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual);
/**
* @brief 地面接触检测
* @param vmc VMC控制器实例
* @return 地面法向力 (N)
*/
float VMC_GroundContactDetection(VMC_t *vmc);
/**
* @brief 获取足端位置(直角坐标)
* @param vmc VMC控制器实例
* @param x 足端x坐标输出
* @param y 足端y坐标输出
* @return 0:成功, -1:失败
*/
int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y);
/**
* @brief 获取等效摆动杆参数
* @param vmc VMC控制器实例
* @param length 等效摆动杆长度输出
* @param angle 等效摆动杆角度输出
* @param d_length 等效摆动杆长度变化率输出
* @param d_angle 等效摆动杆角速度输出
* @return 0:成功, -1:失败
*/
int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle);
/**
* @brief 获取关节输出力矩
* @param vmc VMC控制器实例
* @param tau_front 前髋关节力矩输出
* @param tau_rear 后髋关节力矩输出
* @return 0:成功, -1:失败
*/
int8_t VMC_GetJointTorques(const VMC_t *vmc, float *tau_front, float *tau_rear);
/**
* @brief 重置VMC控制器状态
* @param vmc VMC控制器实例
*/
void VMC_Reset(VMC_t *vmc);
/**
* @brief 设置虚拟力和力矩
* @param vmc VMC控制器实例
* @param F_virtual 虚拟支撑力 (N)
* @param T_virtual 虚拟摆动力矩 (N*m)
*/
void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual);
/**
* @brief 计算雅可比矩阵
* @param vmc VMC控制器实例
* @return 0:成功, -1:失败
*/
int8_t VMC_ComputeJacobian(VMC_t *vmc);
#ifdef __cplusplus
}
#endif

710
User/module/balance_chassis.c Normal file
View File

@ -0,0 +1,710 @@
#include "module/balance_chassis.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include "component/filter.h"
#include "component/user_math.h"
#include "device/motor_lz.h"
#include <math.h>
#include <stddef.h>
#include <string.h>
/**
* @brief 使能所有电机
* @param c 底盘结构体指针
* @return
*/
static int8_t Chassis_MotorEnable(Chassis_t *c) {
if (c == NULL)
return CHASSIS_ERR_NULL;
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Param_t *joint_param = &c->param->joint_param[i];
MOTOR_LZ_Enable(joint_param);
}
for (int i = 0; i < 2; i++) {
MOTOR_LK_Param_t *wheel_param = &c->param->wheel_param[i];
MOTOR_LK_MotorOn(wheel_param);
}
return CHASSIS_OK;
}
static int8_t Chassis_MotorRelax(Chassis_t *c) {
if (c == NULL)
return CHASSIS_ERR_NULL;
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Param_t *joint_param = &c->param->joint_param[i];
MOTOR_LZ_Relax(joint_param);
}
for (int i = 0; i < 2; i++) {
MOTOR_LK_Param_t *wheel_param = &c->param->wheel_param[i];
MOTOR_LK_Relax(wheel_param);
}
return CHASSIS_OK;
}
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
if (c == NULL)
return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
if (mode == c->mode)
return CHASSIS_OK; /* 模式未改变直接返回 */
// 特殊处理：从JUMP切换到WHELL_LEG_BALANCE时不重置
// 但从WHELL_LEG_BALANCE切换到JUMP时，需要重置以触发新的跳跃
if (c->mode == CHASSIS_MODE_JUMP && mode == CHASSIS_MODE_WHELL_LEG_BALANCE) {
c->mode = mode;
return CHASSIS_OK;
}
if (c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE && mode == CHASSIS_MODE_JUMP) {
c->mode = mode;
c->state = 0; // 重置状态，确保每次切换模式时都重新初始化
c->jump_time=0; // 重置跳跃时间，切换到JUMP模式时会触发新跳跃
return CHASSIS_OK;
}
Chassis_MotorEnable(c);
PID_Reset(&c->pid.leg_length[0]);
PID_Reset(&c->pid.leg_length[1]);
PID_Reset(&c->pid.yaw);
PID_Reset(&c->pid.roll);
PID_Reset(&c->pid.tp[0]);
PID_Reset(&c->pid.tp[1]);
// 双零点自动选择逻辑（使用user_math的CircleError函数）
float current_yaw = c->feedback.yaw.rotor_abs_angle;
float zero_point_1 = c->param->mech_zero_yaw;
float zero_point_2 = zero_point_1 + M_PI; // 第二个零点，相差180度
// 使用CircleError计算到两个零点的角度差（范围为M_2PI）
float error_to_zero1 = CircleError(zero_point_1, current_yaw, M_2PI);
float error_to_zero2 = CircleError(zero_point_2, current_yaw, M_2PI);
// 选择误差绝对值更小的零点作为目标，并记录是否使用了第二个零点
if (fabsf(error_to_zero1) <= fabsf(error_to_zero2)) {
c->yaw_control.target_yaw = zero_point_1;
c->yaw_control.is_reversed = false; // 使用第一个零点，不反转
} else {
c->yaw_control.target_yaw = zero_point_2;
c->yaw_control.is_reversed = true; // 使用第二个零点，需要反转前后方向
}
// 清空位移
c->chassis_state.position_x = 0.0f;
c->chassis_state.velocity_x = 0.0f;
c->chassis_state.last_velocity_x = 0.0f;
c->chassis_state.target_x = 0.0f;
c->chassis_state.target_velocity_x = 0.0f;
c->chassis_state.last_target_velocity_x = 0.0f;
LQR_Reset(&c->lqr[0]);
LQR_Reset(&c->lqr[1]);
LowPassFilter2p_Reset(&c->filter.joint_out[0], 0.0f);
LowPassFilter2p_Reset(&c->filter.joint_out[1], 0.0f);
LowPassFilter2p_Reset(&c->filter.joint_out[2], 0.0f);
LowPassFilter2p_Reset(&c->filter.joint_out[3], 0.0f);
LowPassFilter2p_Reset(&c->filter.wheel_out[0], 0.0f);
LowPassFilter2p_Reset(&c->filter.wheel_out[1], 0.0f);
c->mode = mode;
c->state = 0; // 重置状态，确保每次切换模式时都重新初始化
c->jump_time=0; // 重置跳跃时间，切换到JUMP模式时会触发新跳跃
c->wz_multi=0.0f;
return CHASSIS_OK;
}
/* 更新机体状态估计 */
static void Chassis_UpdateChassisState(Chassis_t *c) {
if (c == NULL)
return;
// 从轮子编码器估计机体速度 (参考C++代码)
float left_wheel_speed_dps = c->feedback.wheel[0].rotor_speed; // dps (度每秒)
float right_wheel_speed_dps =
c->feedback.wheel[1].rotor_speed; // dps (度每秒)
// 将dps转换为rad/s
float left_wheel_speed = left_wheel_speed_dps * M_PI / 180.0f; // rad/s
float right_wheel_speed = right_wheel_speed_dps * M_PI / 180.0f; // rad/s
float wheel_radius = 0.072f;
float left_wheel_linear_vel = left_wheel_speed * wheel_radius;
float right_wheel_linear_vel = right_wheel_speed * wheel_radius;
// 机体x方向速度 (轮子中心速度)
c->chassis_state.last_velocity_x = c->chassis_state.velocity_x;
c->chassis_state.velocity_x =
(left_wheel_linear_vel + right_wheel_linear_vel) / 2.0f;
// 在跳跃模式的起跳和收腿阶段暂停位移积分，避免轮子空转导致的位移误差
if (!(c->mode == CHASSIS_MODE_JUMP && (c->state == 2 || c->state == 3))) {
// 积分得到位置
c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
}
}
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
if (c == NULL || param == NULL || target_freq <= 0.0f) {
return -1; // 参数错误
}
c->param = param;
c->mode = CHASSIS_MODE_RELAX;
/*初始化can*/
BSP_CAN_Init();
/*初始化和注册所有电机*/
MOTOR_LZ_Init();
// 注册关节电机
for (int i = 0; i < 4; i++) {
if (MOTOR_LZ_Register(&c->param->joint_param[i]) != DEVICE_OK) {
return DEVICE_ERR;
}
}
// 注册轮子电机
for (int i = 0; i < 2; i++) {
if (MOTOR_LK_Register(&c->param->wheel_param[i]) != DEVICE_OK) {
return DEVICE_ERR;
}
}
MOTOR_DM_Register(&c->param->yaw_motor);
/*初始化VMC控制器*/
VMC_Init(&c->vmc_[0], &param->vmc_param[0], target_freq);
VMC_Init(&c->vmc_[1], &param->vmc_param[1], target_freq);
/*初始化pid*/
PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq,
&param->leg_length);
PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq,
&param->leg_length);
PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw);
PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->roll);
PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, &param->tp);
PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, &param->tp);
/*初始化LQR控制器*/
LQR_Init(&c->lqr[0], &param->lqr_gains);
LQR_Init(&c->lqr[1], &param->lqr_gains);
LowPassFilter2p_Init(&c->filter.joint_out[0], target_freq,
param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&c->filter.joint_out[1], target_freq,
param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&c->filter.joint_out[2], target_freq,
param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&c->filter.joint_out[3], target_freq,
param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&c->filter.wheel_out[0], target_freq,
param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&c->filter.wheel_out[1], target_freq,
param->low_pass_cutoff_freq.out);
/*初始化机体状态*/
c->chassis_state.position_x = 0.0f;
c->chassis_state.velocity_x = 0.0f;
c->chassis_state.last_velocity_x = 0.0f;
c->chassis_state.target_x = 0.0f;
c->chassis_state.target_velocity_x = 0.0f;
c->chassis_state.last_target_velocity_x = 0.0f;
/*初始化yaw控制状态*/
c->yaw_control.target_yaw = 0.0f;
c->yaw_control.current_yaw = 0.0f;
c->yaw_control.yaw_force = 0.0f;
c->yaw_control.is_reversed = false;
return CHASSIS_OK;
}
int8_t Chassis_UpdateFeedback(Chassis_t *c) {
if (c == NULL) {
return -1; // 参数错误
}
// 更新所有电机数据
MOTOR_LZ_UpdateAll();
MOTOR_LK_UpdateAll();
// 更新反馈数据
for (int i = 0; i < 4; i++) {
MOTOR_LZ_t* joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_param[i]);
if (joint_motor != NULL) {
c->feedback.joint[i] = joint_motor->motor.feedback;
}
}
for (int i = 0; i < 2; i++) {
MOTOR_LK_t* wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_param[i]);
if (wheel_motor != NULL) {
c->feedback.wheel[i] = wheel_motor->motor.feedback;
}
}
// for (int i = 0; i < 4; i++) {
// c->feedback.joint[i] = virtual_chassis.data.joint[i];
// if (c->feedback.joint[i].rotor_abs_angle > M_PI) {
// c->feedback.joint[i].rotor_abs_angle -= M_2PI;
// }
// c->feedback.joint[i].rotor_abs_angle =
// -c->feedback.joint[i].rotor_abs_angle; // 机械零点调整
// }
// for (int i = 0; i < 2; i++) {
// c->feedback.wheel[i] = virtual_chassis.data.wheel[i];
// }
// c->feedback.imu.accl = virtual_chassis.data.imu.accl;
// c->feedback.imu.gyro = virtual_chassis.data.imu.gyro;
// c->feedback.imu.euler = virtual_chassis.data.imu.euler;
// 更新机体状态估计
Chassis_UpdateChassisState(c);
return 0;
}
int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu) {
if (c == NULL) {
return -1; // 参数错误
}
c->feedback.imu = imu;
return 0;
}
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
if (c == NULL || c_cmd == NULL) {
return CHASSIS_ERR_NULL; // 参数错误
}
c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) /
1000000.0f; /* 计算两次调用的时间间隔，单位秒 */
c->lask_wakeup = BSP_TIME_Get_us();
/*设置底盘模式*/
if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) {
return CHASSIS_ERR_MODE; // 设置模式失败
}
VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle,
c->feedback.joint[1].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle,
c->feedback.joint[2].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
/*根据底盘模式执行不同的控制逻辑*/
switch (c->mode) {
case CHASSIS_MODE_RELAX:
// 放松模式，电机不输出
Chassis_MotorRelax(c);
Chassis_LQRControl(c, c_cmd);
break;
case CHASSIS_MODE_RECOVER: {
float fn, tp;
fn = -20.0f;
tp = 0.0f;
Chassis_LQRControl(c, c_cmd);
VMC_InverseSolve(&c->vmc_[0], fn, tp);
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
VMC_InverseSolve(&c->vmc_[1], fn, tp);
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
c->output.wheel[0] = c_cmd->move_vec.vx * 0.2f;
c->output.wheel[1] = c_cmd->move_vec.vx * 0.2f;
Chassis_Output(c); // 统一输出
} break;
case CHASSIS_MODE_JUMP:
// 跳跃模式状态机
// 状态转换: 0(准备)->1(0.3s)->2(0.2s)->3(0.3s)->0(完成)
// jump_time == 0: 未开始跳跃
// jump_time == 1: 已完成跳跃（不再触发）
// jump_time > 1: 跳跃进行中
// 检测是否需要开始新的跳跃（state为0且jump_time为0）
if (c->state == 0 && c->jump_time == 0) {
// 开始跳跃，进入state 1
c->state = 1;
c->jump_time = BSP_TIME_Get_us();
}
// 只有在跳跃进行中时才处理状态转换（jump_time > 1）
if (c->jump_time > 1) {
// 计算已经过的时间（微秒）
uint64_t elapsed_us = BSP_TIME_Get_us() - c->jump_time;
// 状态转换逻辑
if (c->state == 1 && elapsed_us >= 300000) {
// state 1 保持0.3s后转到state 2
c->state = 2;
c->jump_time = BSP_TIME_Get_us(); // 重置计时
} else if (c->state == 2 && elapsed_us >= 230000) {
// state 2 保持0.25s后转到state 3，确保腿部充分伸展
c->state = 3;
c->jump_time = BSP_TIME_Get_us(); // 重置计时
} else if (c->state == 3 && elapsed_us >= 500000) {
// state 3 保持0.4s后转到state 0，确保收腿到最高
c->state = 0;
c->jump_time = 1; // 设置为1，表示已完成跳跃，不再触发
}
}
// 执行LQR控制（包含PID腿长控制）
Chassis_LQRControl(c, c_cmd);
Chassis_Output(c); // 统一输出
break;
case CHASSIS_MODE_WHELL_LEG_BALANCE:
case CHASSIS_MODE_ROTOR:
// 执行LQR控制（包含PID腿长控制）
Chassis_LQRControl(c, c_cmd);
Chassis_Output(c); // 统一输出
break;
break;
default:
return CHASSIS_ERR_MODE;
}
return CHASSIS_OK;
}
void Chassis_Output(Chassis_t *c) {
if (c == NULL)
return;
c->output.joint[0] =
LowPassFilter2p_Apply(&c->filter.joint_out[0], c->output.joint[0]);
c->output.joint[1] =
LowPassFilter2p_Apply(&c->filter.joint_out[1], c->output.joint[1]);
c->output.joint[2] =
LowPassFilter2p_Apply(&c->filter.joint_out[2], c->output.joint[2]);
c->output.joint[3] =
LowPassFilter2p_Apply(&c->filter.joint_out[3], c->output.joint[3]);
float out[4] = {c->output.joint[0], c->output.joint[1], c->output.joint[2],
c->output.joint[3]};
// Virtual_Chassis_SendJointTorque(&virtual_chassis, out);
c->output.wheel[0] =
LowPassFilter2p_Apply(&c->filter.wheel_out[0], c->output.wheel[0]);
c->output.wheel[1] =
LowPassFilter2p_Apply(&c->filter.wheel_out[1], c->output.wheel[1]);
// Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0],
// c->output.wheel[1]);
// Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f,
// 0.0f); // 暂时不让轮子动
}
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
if (c == NULL || c_cmd == NULL) {
return CHASSIS_ERR_NULL;
}
/* 运动参数（参考C++版本的状态估计） */
static float xhat = 0.0f, x_dot_hat = 0.0f;
float target_dot_x = 0.0f;
float max_acceleration = 2.0f; // 最大加速度限制: 3 m/s²
// 简化的速度估计（后续可以改进为C++版本的复杂滤波）
x_dot_hat = c->chassis_state.velocity_x;
xhat = c->chassis_state.position_x;
// 目标速度设定（原始期望速度）
float raw_vx = c_cmd->move_vec.vx * 2;
// 根据零点选择情况决定是否反转前后方向
float desired_velocity = c->yaw_control.is_reversed ? -raw_vx : raw_vx;
// 加速度限制处理
float velocity_change =
desired_velocity - c->chassis_state.last_target_velocity_x;
float max_velocity_change = max_acceleration * c->dt; // 最大允许的速度变化
// 限制速度变化率（加速度限制）
if (velocity_change > max_velocity_change) {
velocity_change = max_velocity_change;
} else if (velocity_change < -max_velocity_change) {
velocity_change = -max_velocity_change;
}
// 应用加速度限制后的目标速度
target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
c->chassis_state.target_velocity_x = target_dot_x;
c->chassis_state.last_target_velocity_x = target_dot_x;
// 更新目标位置
c->chassis_state.target_x += target_dot_x * c->dt;
/* 分别计算左右腿的LQR控制 */
/* 构建当前状态 */
LQR_State_t current_state = {0};
current_state.theta = c->vmc_[0].leg.theta;
current_state.d_theta = c->vmc_[0].leg.d_theta;
current_state.x = xhat;
current_state.d_x = x_dot_hat;
current_state.phi = -c->feedback.imu.euler.pit;
current_state.d_phi = -c->feedback.imu.gyro.y;
LQR_UpdateState(&c->lqr[0], &current_state);
current_state.theta = c->vmc_[1].leg.theta;
current_state.d_theta = c->vmc_[1].leg.d_theta;
LQR_UpdateState(&c->lqr[1], &current_state);
/* 根据当前腿长更新增益矩阵 */
LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0);
LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0);
/* 构建目标状态 */
LQR_State_t target_state = {0};
// 在跳跃收腿阶段强制摆角目标为0，确保落地时腿部竖直
if (c->mode == CHASSIS_MODE_JUMP && c->state == 3) {
target_state.theta = 0.0f; // 强制摆杆角度为0，确保腿部竖直
} else {
target_state.theta = 0.00; // 目标摆杆角度
}
target_state.d_theta = 0.0f; // 目标摆杆角速度
target_state.phi = 11.9601*pow((0.16f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.16f + c_cmd->height * 0.25f),2) + 3.87083*(0.16f + c_cmd->height * 0.25f) + -0.4154; // 目标俯仰角
target_state.d_phi = 0.0f; // 目标俯仰角速度
target_state.x = c->chassis_state.target_x -2.07411f*(0.16f + c_cmd->height * 0.25f) + 0.41182f;
target_state.d_x = target_dot_x; // 目标速度
if (c->mode != CHASSIS_MODE_ROTOR){
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
// 双零点自动选择逻辑（考虑手动控制偏移，使用CircleError函数）
float manual_offset = c_cmd->move_vec.vy * M_PI_2;
float base_target_1 = c->param->mech_zero_yaw + manual_offset;
float base_target_2 = c->param->mech_zero_yaw + M_PI + manual_offset;
// 使用CircleError计算到两个目标的角度误差
float error_to_target1 = CircleError(base_target_1, c->yaw_control.current_yaw, M_2PI);
float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI);
// 选择误差绝对值更小的目标，并更新反转状态
if (fabsf(error_to_target1) <= fabsf(error_to_target2)) {
c->yaw_control.target_yaw = base_target_1;
c->yaw_control.is_reversed = false; // 使用第一个零点，不反转
} else {
c->yaw_control.target_yaw = base_target_2;
c->yaw_control.is_reversed = true; // 使用第二个零点，需要反转前后方向
}
c->yaw_control.yaw_force =
PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
}else{
// 小陀螺模式：使用速度环控制
c->yaw_control.current_yaw = c->feedback.imu.euler.yaw;
// 渐增旋转速度，最大角速度约6.9 rad/s（约400度/秒）
c->wz_multi += 0.005f;
if (c->wz_multi > 1.2f)
c->wz_multi = 1.2f;
// 目标角速度（rad/s），可根据需要调整最大速度
float target_yaw_velocity = c->wz_multi * 1.0f; // 最大约7.2 rad/s
// 当前角速度反馈来自IMU陀螺仪
float current_yaw_velocity = c->feedback.imu.gyro.z;
// 使用PID控制角速度，输出力矩
c->yaw_control.yaw_force =
PID_Calc(&c->pid.yaw, target_yaw_velocity,
current_yaw_velocity, 0.0f, c->dt);
}
if (c->vmc_[0].leg.is_ground_contact) {
/* 更新LQR状态 */
LQR_SetTargetState(&c->lqr[0], &target_state);
LQR_Control(&c->lqr[0]);
} else {
// 在跳跃收腿阶段强制摆角目标为0，其他情况为0.16f
float non_contact_theta = (c->mode == CHASSIS_MODE_JUMP && c->state == 3) ? 0.0f : 0.16f;
target_state.theta = non_contact_theta; // 非接触时的腿摆角目标
LQR_SetTargetState(&c->lqr[0], &target_state);
c->lqr[0].control_output.T = 0.0f;
c->lqr[0].control_output.Tp =
c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + non_contact_theta) +
c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta + 0.0f);
c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
}
if (c->vmc_[1].leg.is_ground_contact){
LQR_SetTargetState(&c->lqr[1], &target_state);
LQR_Control(&c->lqr[1]);
}else {
// 在跳跃收腿阶段强制摆角目标为0，其他情况为0.16f
float non_contact_theta = (c->mode == CHASSIS_MODE_JUMP && c->state == 3) ? 0.0f : 0.16f;
target_state.theta = non_contact_theta; // 非接触时的腿摆角目标
LQR_SetTargetState(&c->lqr[1], &target_state);
c->lqr[1].control_output.T = 0.0f;
c->lqr[1].control_output.Tp =
c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + non_contact_theta) +
c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta + 0.0f);
c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
}
/* 轮毂力矩输出（参考C++版本的减速比） */
// 在跳跃的起跳和收腿阶段强制关闭轮子输出，防止离地时轮子猛转
if (c->mode == CHASSIS_MODE_JUMP && (c->state == 2 || c->state == 3)) {
c->output.wheel[0] = 0.0f;
c->output.wheel[1] = 0.0f;
} else {
c->output.wheel[0] =
c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force;
c->output.wheel[1] =
c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force;
}
/* 腿长控制和VMC逆解算（使用PID控制） */
float virtual_force[2];
float target_L0[2];
float leg_d_length[2]; // 腿长变化率
/* 横滚角PID补偿计算 */
// 使用PID控制器计算横滚角补偿长度，目标横滚角为0
float roll_compensation_length =
PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol,
c->feedback.imu.gyro.x, c->dt);
// 限制补偿长度范围，防止过度补偿
// 如果任一腿离地，限制补偿长度
if (!c->vmc_[0].leg.is_ground_contact || !c->vmc_[1].leg.is_ground_contact) {
if (roll_compensation_length > 0.02f)
roll_compensation_length = 0.02f;
if (roll_compensation_length < -0.02f)
roll_compensation_length = -0.02f;
} else {
if (roll_compensation_length > 0.05f)
roll_compensation_length = 0.05f;
if (roll_compensation_length < -0.05f)
roll_compensation_length = -0.05f;
}
// 目标腿长设定（包含横滚角补偿）
switch (c->state) {
case 0: // 正常状态，根据高度指令调节腿长
target_L0[0] = 0.16f + c_cmd->height * 0.22f + roll_compensation_length; // 左腿：基础腿长 + 高度调节 + 横滚补偿
target_L0[1] = 0.16f + c_cmd->height * 0.22f - roll_compensation_length; // 右腿：基础腿长 + 高度调节 - 横滚补偿
break;
case 1: // 准备阶段，腿长收缩
target_L0[0] = 0.14f + roll_compensation_length; // 左腿：收缩到较短腿长 + 横滚补偿
target_L0[1] = 0.14f - roll_compensation_length; // 右腿：收缩到较短腿长 - 横滚补偿
break;
case 2: // 起跳阶段，腿长快速伸展
target_L0[0] = 0.65f; // 左腿：伸展到最大腿长（跳跃时不进行横滚补偿）
target_L0[1] = 0.65f; // 右腿：伸展到最大腿长（跳跃时不进行横滚补偿）
break;
case 3: // 收腿阶段，腿长收缩到最高
target_L0[0] = 0.06f; // 左腿：收缩到最短腿长（确保收腿到最高）
target_L0[1] = 0.06f; // 右腿：收缩到最短腿长（确保收腿到最高）
break;
default:
target_L0[0] = 0.16f + c_cmd->height * 0.22f + roll_compensation_length;
target_L0[1] = 0.16f + c_cmd->height * 0.22f - roll_compensation_length;
break;
}
// 腿长限幅：根据跳跃状态调整限制范围
if (c->mode == CHASSIS_MODE_JUMP && c->state == 3) {
// 在跳跃收腿阶段，允许更短的腿长，确保收腿到最高
if (target_L0[0] < 0.06f) target_L0[0] = 0.06f;
if (target_L0[1] < 0.06f) target_L0[1] = 0.06f;
} else {
// 正常情况下的腿长限制：最短0.10，最长0.42m
if (target_L0[0] < 0.10f) target_L0[0] = 0.10f;
if (target_L0[1] < 0.10f) target_L0[1] = 0.10f;
}
if (target_L0[0] > 0.42f) target_L0[0] = 0.42f;
if (target_L0[1] > 0.42f) target_L0[1] = 0.42f;
// 获取腿长变化率
VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL);
/* 左右腿摆角相互补偿（参考C++版本的Delta_Tp机制） */
float Delta_Tp[2];
// 使用tp_pid进行左右腿摆角同步控制
// 左腿补偿力矩：使左腿theta向右腿theta靠拢
Delta_Tp[0] = c->vmc_[0].leg.L0 * 10.0f *
PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta,
c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt);
// 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
Delta_Tp[1] = c->vmc_[1].leg.L0 * 10.0f *
PID_Calc(&c->pid.tp[1], c->vmc_[0].leg.theta,
c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt);
// 左腿控制
{
// 使用PID进行腿长控制
float pid_output = PID_Calc(&c->pid.leg_length[0], target_L0[0],
c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力
virtual_force[0] =
(c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) +
pid_output + 60.0f;
// VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0],
&c->output.joint[1]);
} else {
// VMC失败，设为0力矩
c->output.joint[0] = 0.0f;
c->output.joint[1] = 0.0f;
}
}
// 右腿控制
{
// 使用PID进行腿长控制
float pid_output = PID_Calc(&c->pid.leg_length[1], target_L0[1],
c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力
virtual_force[1] =
(c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) +
pid_output + 65.0f;
// VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],
c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) {
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3],
&c->output.joint[2]);
} else {
// VMC失败，设为0力矩
c->output.joint[2] = 0.0f;
c->output.joint[3] = 0.0f;
}
}
/* 安全限制 */
for (int i = 0; i < 2; i++) {
if (fabsf(c->output.wheel[i]) > 1.0f) {
c->output.wheel[i] = (c->output.wheel[i] > 0) ? 1.0f : -1.0f;
}
}
for (int i = 0; i < 4; i++) {
if (fabsf(c->output.joint[i]) > 20.0f) {
c->output.joint[i] = (c->output.joint[i] > 0) ? 25.0f : -25.0f;
}
}
return CHASSIS_OK;
}

239
User/module/balance_chassis.h Normal file
View File

@ -0,0 +1,239 @@
/*
* 平衡底盘模组
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
// Front
// | 1 4 |
// (1)Left| |Right(2)
// | 2 3 |
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include "component/vmc.h"
#include "component/lqr.h"
#include "component/ahrs.h"
#include "component/filter.h"
#include "component/pid.h"
#include "device/motor.h"
#include "device/motor_lk.h"
#include "device/motor_lz.h"
#include "device/motor_dm.h"
/* Exported constants ------------------------------------------------------- */
#define CHASSIS_OK (0) /* 运行正常 */
#define CHASSIS_ERR (-1) /* 运行时发现了其他错误 */
#define CHASSIS_ERR_NULL (-2) /* 运行时发现NULL指针 */
#define CHASSIS_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */
#define CHASSIS_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
CHASSIS_MODE_RECOVER, /* 复位模式 */
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */
CHASSIS_MODE_JUMP, /* 跳跃模式，底盘跳跃 */
CHASSIS_MODE_ROTOR, /* 小陀螺模式，底盘高速旋转 */
} Chassis_Mode_t;
typedef enum {
CHASSIS_JUMP_STATE_READY, /* 准备跳跃 */
CHASSIS_JUMP_STATE_JUMPING, /* 跳跃中 */
CHASSIS_JUMP_STATE_LANDING, /* 着陆中 */
CHASSIS_JUMP_STATE_END, /* 跳跃结束 */
} Chassis_JumpState_t;
typedef struct {
Chassis_Mode_t mode; /* 底盘模式 */
MoveVector_t move_vec; /* 底盘运动向量 */
float height; /* 目标高度 */
} Chassis_CMD_t;
typedef struct {
AHRS_Accl_t accl; /* IMU加速度计 */
AHRS_Gyro_t gyro; /* IMU陀螺仪 */
AHRS_Eulr_t euler; /* IMU欧拉角 */
}Chassis_IMU_t;
typedef struct {
MOTOR_Feedback_t joint[4]; /* 四个关节电机反馈 */
MOTOR_Feedback_t wheel[2]; /* 两个轮子电机反馈 */
MOTOR_Feedback_t yaw; /* 云台Yaw轴电机反馈 */
Chassis_IMU_t imu; /* IMU数据 */
}Chassis_Feedback_t;
typedef struct {
float joint[4]; /* 四个关节电机输出 */
float wheel[2]; /* 两个轮子电机输出 */
}Chassis_Output_t;
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
typedef struct {
MOTOR_LZ_Param_t joint_param[4]; // 4个电机参数
MOTOR_LK_Param_t wheel_param[2]; // 2个轮子电机参数
MOTOR_DM_Param_t yaw_motor; /* 云台Yaw轴电机参数 */
VMC_Param_t vmc_param[2]; /* VMC参数 */
LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
KPID_Params_t yaw; /* yaw轴PID控制参数，用于控制底盘朝向 */
KPID_Params_t roll; /* roll轴PID控制参数，用于横滚角补偿 */
KPID_Params_t tp; /* 摆力矩PID控制参数，用于控制摆力矩 */
KPID_Params_t leg_length; /* 腿长PID控制参数，用于控制虚拟腿长度 */
KPID_Params_t leg_theta; /* 摆角PID控制参数，用于控制虚拟腿摆角 */
float mech_zero_yaw; /* 机械零点 */
float theta;
float x;
float phi;
/* 低通滤波器截止频率 */
struct {
float in; /* 输入 */
float out; /* 输出 */
} low_pass_cutoff_freq;
} Chassis_Params_t;
/*
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体
* 包含了初始化参数，中间变量，输出变量
*/
typedef struct {
uint64_t lask_wakeup;
float dt;
Chassis_Params_t *param; /* 底盘的参数，用Chassis_Init设定 */
AHRS_Eulr_t *mech_zero;
/* 模块通用 */
Chassis_Mode_t mode; /* 底盘模式 */
/* 反馈信息 */
Chassis_Feedback_t feedback;
/* 控制信息*/
Chassis_Output_t output;
VMC_t vmc_[2]; /* 两条腿的VMC */
LQR_t lqr[2]; /* 两条腿的LQR控制器 */
int8_t state;
uint64_t jump_time;
float angle;
float height;
/* 机体状态估计 */
struct {
float position_x; /* 机体x位置 */
float velocity_x; /* 机体x速度 */
float last_velocity_x; /* 上一次速度用于数值微分 */
float target_x; /* 目标x位置 */
float target_velocity_x; /* 目标速度 */
float last_target_velocity_x; /* 上一次目标速度 */
} chassis_state;
/* yaw控制相关 */
struct {
float target_yaw; /* 目标yaw角度 */
float current_yaw; /* 当前yaw角度 */
float yaw_force; /* yaw轴控制力矩 */
bool is_reversed; /* 是否使用反转的零点（180度零点），影响前后方向 */
} yaw_control;
float wz_multi; /* 小陀螺模式旋转方向 */
/* PID计算的目标值 */
struct {
AHRS_Eulr_t chassis;
} setpoint;
/* 反馈控制用的PID */
struct {
KPID_t yaw; /* 跟随云台用的PID */
KPID_t roll; /* 横滚角控制PID */
KPID_t tp[2];
KPID_t leg_length[2]; /* 两条腿的腿长PID */
KPID_t leg_theta[2]; /* 两条腿的摆角PID */
} pid;
/* 滤波器 */
struct {
LowPassFilter2p_t joint_out[4]; /* 关节输出滤波器 */
LowPassFilter2p_t wheel_out[2]; /* 轮子输出滤波器 */
} filter;
} Chassis_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* \brief 初始化底盘
*
* \param c 包含底盘数据的结构体
* \param param 包含底盘参数的结构体指针
* \param target_freq 任务预期的运行频率
*
* \return 函数运行结果
*/
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq);
/**
* \brief 更新底盘的反馈信息
*
* \param c 包含底盘数据的结构体
* \return 函数运行结果
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c);
/**
* @brief 更新IMU数据
* @param c 包含底盘数据的结构体
* @param imu IMU数据
* @return
*/
int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu);
/**
* \brief 运行底盘控制逻辑
*
* \param c 包含底盘数据的结构体
* \param c_cmd 底盘控制指令
*
* \return 函数运行结果
*/
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd);
/**
* \brief LQR控制逻辑
*
* \param c 包含底盘数据的结构体
* \param c_cmd 底盘控制指令
*
* \return 函数运行结果
*/
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
/**
* \brief 底盘输出值
*
* \param s 包含底盘数据的结构体
* \param out CAN设备底盘输出结构体
*/
void Chassis_Output(Chassis_t *c);
#ifdef __cplusplus
}
#endif

3
User/module/config.h
View File

@ -11,6 +11,7 @@ extern "C" {
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
#include "module/shoot.h" #include "module/shoot.h"
#include "module/balance_chassis.h"
/** /**
* @brief 机器人参数配置结构体 * @brief 机器人参数配置结构体
* @note 在此添加您的配置参数 * @note 在此添加您的配置参数
@ -18,7 +19,7 @@ extern "C" {
typedef struct { typedef struct {
/* USER CODE BEGIN Config_RobotParam */ /* USER CODE BEGIN Config_RobotParam */
Shoot_Params_t shoot_param; Shoot_Params_t shoot_param;
Chassis_Params_t chassis_param;
/* USER CODE END Config_RobotParam */ /* USER CODE END Config_RobotParam */
} Config_RobotParam_t; } Config_RobotParam_t;

10
User/task/atti_esit.c
View File

@ -12,6 +12,7 @@
#include "component/ahrs.h" #include "component/ahrs.h"
#include "component/pid.h" #include "component/pid.h"
#include "device/bmi088.h" #include "device/bmi088.h"
#include "module/balance_chassis.h"
#include "task/user_task.h" #include "task/user_task.h"
/* USER INCLUDE END */ /* USER INCLUDE END */
@ -25,7 +26,7 @@ BMI088_t bmi088;
AHRS_t gimbal_ahrs; AHRS_t gimbal_ahrs;
AHRS_Magn_t magn; AHRS_Magn_t magn;
AHRS_Eulr_t eulr_to_send; AHRS_Eulr_t eulr_to_send;
Chassis_IMU_t imu_for_chassis;
KPID_t imu_temp_ctrl_pid; KPID_t imu_temp_ctrl_pid;
/*默认校准参数*/ /*默认校准参数*/
@ -81,6 +82,13 @@ void Task_atti_esit(void *argument) {
/* 根据解析出来的四元数计算欧拉角 */ /* 根据解析出来的四元数计算欧拉角 */
AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs); AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
imu_for_chassis.accl = bmi088.accl;
imu_for_chassis.gyro = bmi088.gyro;
imu_for_chassis.euler = eulr_to_send;
osMessageQueueReset(
task_runtime.msgq.chassis.imu); // 重置消息队列，防止阻塞
osMessageQueuePut(task_runtime.msgq.chassis.imu, &imu_for_chassis, 0,
0); // 非阻塞发送底盘IMU数据
BSP_PWM_SetComp(BSP_PWM_IMU_HEAT, PID_Calc(&imu_temp_ctrl_pid, 40.5f, BSP_PWM_SetComp(BSP_PWM_IMU_HEAT, PID_Calc(&imu_temp_ctrl_pid, 40.5f,
bmi088.temp, 0.0f, 0.0f)); bmi088.temp, 0.0f, 0.0f));
osKernelUnlock(); osKernelUnlock();

43
User/task/ctrl_chassis.c
View File

@ -7,9 +7,10 @@
#include "task/user_task.h" #include "task/user_task.h"
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "bsp/can.h" #include "bsp/can.h"
#include "device/motor.h"
#include "device/motor_lk.h" #include "device/motor_lk.h"
#include "device/motor_lz.h" #include "device/motor_lz.h"
#include "module/config.h"
#include "module/balance_chassis.h"
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -17,16 +18,17 @@
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */ /* USER STRUCT BEGIN */
MOTOR_LZ_Param_t motor_lz_param = { Chassis_t chassis;
.can = BSP_CAN_3, Chassis_CMD_t chassis_cmd = {
.motor_id = 125, .mode = CHASSIS_MODE_RELAX, // 改为RECOVER模式，让电机先启动
.host_id = 0xFF, .move_vec = {
.module = MOTOR_LZ_RSO3, .vx = 0.0f,
.reverse = false, .vy = 0.0f,
.mode = MOTOR_LZ_MODE_MOTION, .wz = 0.0f,
},
.height = 0.0f,
}; };
Chassis_IMU_t chassis_imu;
float out = 0.0f;
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -42,19 +44,30 @@ void Task_ctrl_chassis(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */ /* USER CODE INIT BEGIN */
BSP_CAN_Init();
MOTOR_LZ_Init(); Chassis_Init(&chassis, &Config_GetRobotParam()->chassis_param, CTRL_CHASSIS_FREQ);
MOTOR_LZ_Register(&motor_lz_param);
/* USER CODE INIT END */ /* USER CODE INIT END */
while (1) { while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */ /* USER CODE BEGIN */
MOTOR_LZ_Enable(&motor_lz_param); if (osMessageQueueGet(task_runtime.msgq.chassis.cmd, &chassis_cmd, NULL, 0) != osOK) {
// 没有新命令，保持上次命令
}
if (osMessageQueueGet(task_runtime.msgq.chassis.imu, &chassis_imu, NULL, 0) == osOK) {
chassis.feedback.imu = chassis_imu;
}
osMessageQueueGet(task_runtime.msgq.chassis.yaw, &chassis.feedback.yaw, NULL, 0);
Chassis_UpdateFeedback(&chassis);
Chassis_Control(&chassis, &chassis_cmd);
/* USER CODE END */ /* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
} }
} }

4
User/task/init.c
View File

@ -8,6 +8,7 @@
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "module/shoot.h" #include "module/shoot.h"
#include "module/balance_chassis.h"
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -42,6 +43,9 @@ void Task_Init(void *argument) {
/* USER MESSAGE BEGIN */ /* USER MESSAGE BEGIN */
task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL); task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
task_runtime.msgq.shoot.shoot_cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL); task_runtime.msgq.shoot.shoot_cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL);
task_runtime.msgq.chassis.imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL);
task_runtime.msgq.chassis.cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
task_runtime.msgq.chassis.yaw = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL);
/* USER MESSAGE END */ /* USER MESSAGE END */
osKernelUnlock(); // 解锁内核 osKernelUnlock(); // 解锁内核

30
User/task/rc.c
View File

@ -8,6 +8,7 @@
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "device/dr16.h" #include "device/dr16.h"
#include "module/shoot.h" #include "module/shoot.h"
#include "module/balance_chassis.h"
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -17,7 +18,7 @@
/* USER STRUCT BEGIN */ /* USER STRUCT BEGIN */
DR16_t dr16; DR16_t dr16;
Shoot_CMD_t for_shoot; Shoot_CMD_t for_shoot;
Chassis_CMD_t cmd_for_chassis;
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -99,7 +100,34 @@ void Task_rc(void *argument) {
// osMessageQueueReset(task_runtime.msgq.gimbal.cmd); // osMessageQueueReset(task_runtime.msgq.gimbal.cmd);
// osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0); // osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0);
switch (dr16.data.sw_l) {
case DR16_SW_UP:
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
break;
case DR16_SW_MID:
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
break;
case DR16_SW_DOWN:
// cmd_for_chassis.mode = CHASSIS_MODE_ROTOR;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
break;
default:
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
break;
}
cmd_for_chassis.move_vec.vx = dr16.data.ch_l_y;
cmd_for_chassis.move_vec.vy = dr16.data.ch_l_x;
cmd_for_chassis.move_vec.wz = dr16.data.ch_r_x;
cmd_for_chassis.height = dr16.data.ch_res;
osMessageQueueReset(
task_runtime.msgq.chassis.cmd); // 重置消息队列，防止阻塞
osMessageQueuePut(task_runtime.msgq.chassis.cmd, &cmd_for_chassis, 0,
0); // 非阻塞发送底盘控制命令
/************************* 发射命令 **************************************/
for_shoot.online = dr16.header.online; for_shoot.online = dr16.header.online;
switch (dr16.data.sw_r) { switch (dr16.data.sw_r) {
case DR16_SW_UP: case DR16_SW_UP: