Welcome to mirror list, hosted at ThFree Co, Russian Federation.

github.com/Duet3D/RepRapFirmware.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--.cproject185
-rw-r--r--Maths/closestPointOfApproach2d.wxmxbin0 -> 6856 bytes
-rw-r--r--src/Duet3_V03/Pins_Duet3_V03.cpp62
-rw-r--r--src/Duet3_V03/Pins_Duet3_V03.h319
-rw-r--r--src/Duet3_V05/Pins_Duet3_V05.h6
-rw-r--r--src/Duet3_V06/Pins_Duet3_V06.h2
-rw-r--r--src/DuetM/Pins_DuetM.h5
-rw-r--r--src/DuetNG/Pins_DuetNG.h5
-rw-r--r--src/Fans/FansManager.cpp4
-rw-r--r--src/Fans/FansManager.h2
-rw-r--r--src/FilamentMonitors/PulsedFilamentMonitor.cpp22
-rw-r--r--src/FilamentMonitors/PulsedFilamentMonitor.h1
-rw-r--r--src/GCodes/GCodes.cpp66
-rw-r--r--src/GCodes/GCodes.h4
-rw-r--r--src/GCodes/GCodes4.cpp30
-rw-r--r--src/Hardware/Cache.cpp216
-rw-r--r--src/Hardware/Cache.h54
-rw-r--r--src/Movement/DDA.cpp16
-rw-r--r--src/Movement/DDA.h28
-rw-r--r--src/Movement/DDARing.cpp4
-rw-r--r--src/Movement/Kinematics/ScaraKinematics.cpp4
-rw-r--r--src/Movement/Move.cpp106
-rw-r--r--src/Movement/Move.h24
-rw-r--r--src/Movement/RawMove.cpp3
-rw-r--r--src/Movement/RawMove.h3
-rw-r--r--src/Movement/StepperDrivers/TMC2660.cpp4
-rw-r--r--src/Platform.cpp17
-rw-r--r--src/RepRap.cpp30
-rw-r--r--src/RepRap.h7
-rw-r--r--src/Tasks.cpp144
-rw-r--r--src/Tasks.h31
-rw-r--r--src/Tools/Tool.cpp17
-rw-r--r--src/Tools/Tool.h3
-rw-r--r--src/Version.h2
34 files changed, 487 insertions, 939 deletions
diff --git a/.cproject b/.cproject
index 57a5405b..241354d9 100644
--- a/.cproject
+++ b/.cproject
@@ -171,7 +171,7 @@
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
- <configuration artifactExtension="elf" artifactName="Duet2CombinedFirmware" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.cross.exe.release.516195201.976458850.241502451.170574622" name="Duet2_RTOS" optionalBuildProperties="org.eclipse.cdt.docker.launcher.containerbuild.property.selectedvolumes=,org.eclipse.cdt.docker.launcher.containerbuild.property.volumes=" parent="cdt.managedbuild.config.gnu.cross.exe.release" postannouncebuildStep="Generating binary file" postbuildStep="arm-none-eabi-objcopy -O binary &quot;${workspace_loc:/${ProjName}/${ConfigName}}/${BuildArtifactFileBaseName}.elf&quot; &quot;${workspace_loc:/${ProjName}/${ConfigName}}/${BuildArtifactFileBaseName}.bin&quot; &amp;&amp; crc32appender &quot;${workspace_loc:/${ProjName}/${ConfigName}}/${BuildArtifactFileBaseName}.bin&quot;">
+ <configuration artifactExtension="elf" artifactName="Duet2CombinedFirmware" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.cross.exe.release.516195201.976458850.241502451.170574622" name="Duet2_RTOS" optionalBuildProperties="org.eclipse.cdt.docker.launcher.containerbuild.property.volumes=,org.eclipse.cdt.docker.launcher.containerbuild.property.selectedvolumes=" parent="cdt.managedbuild.config.gnu.cross.exe.release" postannouncebuildStep="Generating binary file" postbuildStep="arm-none-eabi-objcopy -O binary &quot;${workspace_loc:/${ProjName}/${ConfigName}}/${BuildArtifactFileBaseName}.elf&quot; &quot;${workspace_loc:/${ProjName}/${ConfigName}}/${BuildArtifactFileBaseName}.bin&quot; &amp;&amp; crc32appender &quot;${workspace_loc:/${ProjName}/${ConfigName}}/${BuildArtifactFileBaseName}.bin&quot;">
<folderInfo id="cdt.managedbuild.config.gnu.cross.exe.release.516195201.976458850.241502451.170574622." name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.cross.exe.release.435431950" name="Cross GCC" nonInternalBuilderId="cdt.managedbuild.builder.gnu.cross" superClass="cdt.managedbuild.toolchain.gnu.cross.exe.release">
<option id="cdt.managedbuild.option.gnu.cross.path.1881231799" name="Path" superClass="cdt.managedbuild.option.gnu.cross.path" useByScannerDiscovery="false" value="${ArmGccPath}" valueType="string"/>
@@ -466,187 +466,6 @@
</externalSettings>
</storageModule>
</cconfiguration>
- <cconfiguration id="cdt.managedbuild.config.gnu.cross.exe.release.516195201.976458850.241502451.1275216290.274082366.1645191116.1852610203">
- <storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.config.gnu.cross.exe.release.516195201.976458850.241502451.1275216290.274082366.1645191116.1852610203" moduleId="org.eclipse.cdt.core.settings" name="Duet3_V03">
- <macros>
- <stringMacro name="LinkFlags2" type="VALUE_TEXT" value="-Wl,--end-group -lm"/>
- <stringMacro name="LinkFlags1" type="VALUE_TEXT" value="-mthumb -Wl,--cref -Wl,--check-sections -Wl,--gc-sections -Wl,--entry=Reset_Handler -Wl,--unresolved-symbols=report-all -Wl,--warn-common -Wl,--warn-section-align -Wl,--warn-unresolved-symbols -Wl,--start-group"/>
- <stringMacro name="CoreName" type="VALUE_TEXT" value="CoreNG"/>
- </macros>
- <externalSettings/>
- <extensions>
- <extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
- <extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
- <extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
- <extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
- <extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
- <extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
- </extensions>
- </storageModule>
- <storageModule moduleId="cdtBuildSystem" version="4.0.0">
- <configuration artifactExtension="elf" artifactName="Duet3Firmware_MBP03" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.cross.exe.release.516195201.976458850.241502451.1275216290.274082366.1645191116.1852610203" name="Duet3_V03" optionalBuildProperties="org.eclipse.cdt.docker.launcher.containerbuild.property.selectedvolumes=,org.eclipse.cdt.docker.launcher.containerbuild.property.volumes=" parent="cdt.managedbuild.config.gnu.cross.exe.release" postannouncebuildStep="Generating binary file" postbuildStep="arm-none-eabi-objcopy -O binary &quot;${workspace_loc:/${ProjName}/${ConfigName}}/${BuildArtifactFileBaseName}.elf&quot; &quot;${workspace_loc:/${ProjName}/${ConfigName}}/${BuildArtifactFileBaseName}.bin&quot; &amp;&amp; crc32appender &quot;${workspace_loc:/${ProjName}/${ConfigName}}/${BuildArtifactFileBaseName}.bin&quot;">
- <folderInfo id="cdt.managedbuild.config.gnu.cross.exe.release.516195201.976458850.241502451.1275216290.274082366.1645191116.1852610203." name="/" resourcePath="">
- <toolChain id="cdt.managedbuild.toolchain.gnu.cross.exe.release.5959303" name="Cross GCC" superClass="cdt.managedbuild.toolchain.gnu.cross.exe.release">
- <option id="cdt.managedbuild.option.gnu.cross.path.163568524" name="Path" superClass="cdt.managedbuild.option.gnu.cross.path" useByScannerDiscovery="false" value="${ArmGccPath}" valueType="string"/>
- <option id="cdt.managedbuild.option.gnu.cross.prefix.308380201" name="Prefix" superClass="cdt.managedbuild.option.gnu.cross.prefix" useByScannerDiscovery="false" value="arm-none-eabi-" valueType="string"/>
- <targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.targetPlatform.gnu.cross.1827418537" isAbstract="false" osList="all" superClass="cdt.managedbuild.targetPlatform.gnu.cross"/>
- <builder buildPath="${workspace_loc:/RepRapFirmware}/Release" id="cdt.managedbuild.builder.gnu.cross.1828958094" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="cdt.managedbuild.builder.gnu.cross"/>
- <tool id="cdt.managedbuild.tool.gnu.cross.assembler.254554862" name="Cross GCC Assembler" superClass="cdt.managedbuild.tool.gnu.cross.assembler">
- <option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.both.asm.option.include.paths.568962193" name="Include paths (-I)" superClass="gnu.both.asm.option.include.paths" valueType="includePath">
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/RRFLibraries}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/FreeRTOS}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/CoreNG}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/CANlib}&quot;"/>
- </option>
- <inputType id="cdt.managedbuild.tool.gnu.assembler.input.1960931313" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
- </tool>
- <tool commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${INPUTS}" id="cdt.managedbuild.tool.gnu.cross.c.compiler.1113168108" name="Cross GCC Compiler" superClass="cdt.managedbuild.tool.gnu.cross.c.compiler">
- <option defaultValue="gnu.c.optimization.level.most" id="gnu.c.compiler.option.optimization.level.1099089925" name="Optimization Level" superClass="gnu.c.compiler.option.optimization.level" useByScannerDiscovery="false" value="gnu.c.optimization.level.more" valueType="enumerated"/>
- <option id="gnu.c.compiler.option.debugging.level.2018591546" name="Debug Level" superClass="gnu.c.compiler.option.debugging.level" useByScannerDiscovery="false" value="gnu.c.debugging.level.default" valueType="enumerated"/>
- <option id="gnu.c.compiler.option.misc.verbose.1767118602" name="Verbose (-v)" superClass="gnu.c.compiler.option.misc.verbose" useByScannerDiscovery="false" value="false" valueType="boolean"/>
- <option id="gnu.c.compiler.option.misc.other.2139236731" name="Other flags" superClass="gnu.c.compiler.option.misc.other" useByScannerDiscovery="false" value="-c -mcpu=cortex-m7 -mthumb -mfpu=fpv5-d16 -mfloat-abi=hard -ffunction-sections -fdata-sections -nostdlib -Wundef -Wdouble-promotion -fsingle-precision-constant &quot;-Wa,-ahl=$*.s&quot;" valueType="string"/>
- <option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.c.compiler.option.include.paths.1736169055" name="Include paths (-I)" superClass="gnu.c.compiler.option.include.paths" useByScannerDiscovery="false" valueType="includePath">
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/RRFLibraries}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/FreeRTOS}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/CoreNG}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/CANlib}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/cores/arduino}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/libraries/Storage}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/common/utils}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/common/services/clock}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/common/services/ioport}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/sam/components/ethernet_phy/ksz8081rna}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/sam/drivers/hsmci}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/sam/drivers/gmac}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/sam/drivers/pmc}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/sam/drivers/rstc}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/sam/utils}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/sam/utils/cmsis/same70/include}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/sam/utils/header_files}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/sam/utils/preprocessor}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/thirdparty/CMSIS/Include}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/variants/same70}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/src/Networking/LwipEthernet}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/src/Networking/LwipEthernet/Lwip/src/include}&quot;"/>
- </option>
- <option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.c.compiler.option.preprocessor.def.symbols.316510289" name="Defined symbols (-D)" superClass="gnu.c.compiler.option.preprocessor.def.symbols" useByScannerDiscovery="false" valueType="definedSymbols">
- <listOptionValue builtIn="false" value="__SAME70Q20B__"/>
- <listOptionValue builtIn="false" value="RTOS"/>
- <listOptionValue builtIn="false" value="DUET3_V03"/>
- <listOptionValue builtIn="false" value="LWIP_GMAC_TASK=0"/>
- </option>
- <option id="gnu.c.compiler.option.dialect.std.133148413" name="Language standard" superClass="gnu.c.compiler.option.dialect.std" useByScannerDiscovery="true" value="gnu.c.compiler.dialect.default" valueType="enumerated"/>
- <option id="gnu.c.compiler.option.dialect.flags.489815237" name="Other dialect flags" superClass="gnu.c.compiler.option.dialect.flags" useByScannerDiscovery="true" value="-std=gnu99" valueType="string"/>
- <inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.477491021" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
- </tool>
- <tool id="cdt.managedbuild.tool.gnu.cross.c.linker.1392905935" name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker"/>
- <tool id="cdt.managedbuild.tool.gnu.cross.archiver.733829747" name="Cross GCC Archiver" superClass="cdt.managedbuild.tool.gnu.cross.archiver"/>
- <tool command="gcc" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${LinkFlags1} &quot;${workspace_loc:/${CoreName}}/SAME70/cores/arduino/syscalls.o&quot; ${INPUTS} ${LinkFlags2}" id="cdt.managedbuild.tool.gnu.cross.cpp.linker.1019421301" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
- <option id="gnu.cpp.link.option.nostdlibs.1609488418" name="No startup or default libs (-nostdlib)" superClass="gnu.cpp.link.option.nostdlibs" useByScannerDiscovery="false" value="false" valueType="boolean"/>
- <option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.link.option.paths.579521002" name="Library search path (-L)" superClass="gnu.cpp.link.option.paths" useByScannerDiscovery="false" valueType="libPaths">
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/FreeRTOS/SAME70}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/CoreNG/SAME70_DUET3V03}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/CANlib/Release}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/RRFLibraries/SAME70_RTOS}&quot;"/>
- </option>
- <option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.link.option.libs.1783235457" name="Libraries (-l)" superClass="gnu.cpp.link.option.libs" useByScannerDiscovery="false" valueType="libs">
- <listOptionValue builtIn="false" srcPrefixMapping="" srcRootPath="" value="RRFLibraries"/>
- <listOptionValue builtIn="false" srcPrefixMapping="" srcRootPath="" value="FreeRTOS"/>
- <listOptionValue builtIn="false" srcPrefixMapping="" srcRootPath="" value="CoreNG"/>
- <listOptionValue builtIn="false" srcPrefixMapping="" srcRootPath="" value="CANlib"/>
- <listOptionValue builtIn="false" value="${CoreName}"/>
- </option>
- <option id="gnu.cpp.link.option.flags.2100826215" name="Linker flags" superClass="gnu.cpp.link.option.flags" useByScannerDiscovery="false" value="-Os --specs=nano.specs -Wl,--gc-sections -Wl,--fatal-warnings -mcpu=cortex-m7 -mfpu=fpv5-d16 -mfloat-abi=hard -T&quot;${workspace_loc:/${CoreName}/variants/same70/linker_scripts/gcc/flash.ld}&quot; -Wl,-Map,&quot;${workspace_loc:/${ProjName}/${ConfigName}}/${BuildArtifactFileBaseName}.map&quot;" valueType="string"/>
- <inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.1623645672" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
- <additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
- <additionalInput kind="additionalinput" paths="$(LIBS)"/>
- </inputType>
- </tool>
- <tool command="g++" id="cdt.managedbuild.tool.gnu.cross.cpp.compiler.1243015316" name="Cross G++ Compiler" superClass="cdt.managedbuild.tool.gnu.cross.cpp.compiler">
- <option id="gnu.cpp.compiler.option.optimization.level.744605431" name="Optimization Level" superClass="gnu.cpp.compiler.option.optimization.level" useByScannerDiscovery="false" value="gnu.cpp.compiler.optimization.level.more" valueType="enumerated"/>
- <option id="gnu.cpp.compiler.option.debugging.level.1247470383" name="Debug Level" superClass="gnu.cpp.compiler.option.debugging.level" useByScannerDiscovery="false" value="gnu.cpp.compiler.debugging.level.default" valueType="enumerated"/>
- <option id="gnu.cpp.compiler.option.other.verbose.1130347909" name="Verbose (-v)" superClass="gnu.cpp.compiler.option.other.verbose" useByScannerDiscovery="false" value="false" valueType="boolean"/>
- <option id="gnu.cpp.compiler.option.other.other.1453375027" name="Other flags" superClass="gnu.cpp.compiler.option.other.other" useByScannerDiscovery="false" value="-c -mcpu=cortex-m7 -mthumb -mfpu=fpv5-d16 -mfloat-abi=hard -ffunction-sections -fdata-sections -fno-threadsafe-statics -fno-rtti -fno-exceptions -nostdlib -Wundef -Wdouble-promotion -fsingle-precision-constant &quot;-Wa,-ahl=$*.s&quot;" valueType="string"/>
- <option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.compiler.option.include.paths.633679617" name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths" useByScannerDiscovery="false" valueType="includePath">
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/cores/arduino}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/libraries/Flash}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/libraries/SharedSpi}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/libraries/Storage}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/libraries/Wire}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/common/utils}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/common/services/clock}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/common/services/ioport}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/sam/components/ethernet_phy/ksz8081rna}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/sam/drivers}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/sam/drivers/gmac}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/sam/services/flash_efc}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/sam/utils}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/sam/utils/cmsis/same70/include}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/sam/utils/header_files}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/sam/utils/preprocessor}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/asf/thirdparty/CMSIS/Include}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${CoreName}/variants/same70}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/src}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/src/Networking}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/src/Networking/LwipEthernet}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/src/Networking/LwipEthernet/Lwip/src/include}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/DuetWiFiSocketServer/src/include}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/FreeRTOS/src/include}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/FreeRTOS/src/portable/GCC/ARM_CM7/r0p1}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/RRFLibraries/src}&quot;"/>
- <listOptionValue builtIn="false" value="&quot;${workspace_loc:/CANlib/src}&quot;"/>
- </option>
- <option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.compiler.option.preprocessor.def.315745995" name="Defined symbols (-D)" superClass="gnu.cpp.compiler.option.preprocessor.def" useByScannerDiscovery="false" valueType="definedSymbols">
- <listOptionValue builtIn="false" value="__SAME70Q20B__"/>
- <listOptionValue builtIn="false" value="RTOS"/>
- <listOptionValue builtIn="false" value="DUET3_V03"/>
- <listOptionValue builtIn="false" value="LWIP_GMAC_TASK=0"/>
- <listOptionValue builtIn="false" value="_XOPEN_SOURCE"/>
- </option>
- <option id="gnu.cpp.compiler.option.dialect.std.815297330" name="Language standard" superClass="gnu.cpp.compiler.option.dialect.std" useByScannerDiscovery="true" value="gnu.cpp.compiler.dialect.default" valueType="enumerated"/>
- <option id="gnu.cpp.compiler.option.dialect.flags.1471520961" name="Other dialect flags" superClass="gnu.cpp.compiler.option.dialect.flags" useByScannerDiscovery="true" value="-std=gnu++17" valueType="string"/>
- <inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.2035099195" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
- </tool>
- </toolChain>
- </folderInfo>
- <sourceEntries>
- <entry excluding="src/Duet3_V06|src/Linux|src/Duet3_V05|src/SAME70xpld|src/Networking/LwipEthernet/Lwip/src/apps/snmp|src/Duet|src/Networking/LwipEthernet/Lwip/src/apps/httpd|/src/Networking/LwipEthernet/Lwip/test|src/DuetNG|src/Networking/LwipEthernet/Lwip/src/apps/tftp|src/Networking/W5500Ethernet|src/Alligator|src/Networking/LwipEthernet/Lwip/src/netif/ppp|src/Networking/LwipEthernet/Lwip/src/apps/lwiperf|src/Networking/LwipEthernet/Lwip/src/apps/sntp|src/Display|src/Pccb|/src/Networking/LwipEthernet/Lwip/src/apps/mqtt|src/DuetM|src/RADDS|src/Networking/LwipEthernet/Lwip/doc" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
- </sourceEntries>
- </configuration>
- </storageModule>
- <storageModule moduleId="org.eclipse.cdt.core.externalSettings">
- <externalSettings containerId="CANlib;cdt.managedbuild.config.gnu.cross.lib.release.1485070058" factoryId="org.eclipse.cdt.core.cfg.export.settings.sipplier">
- <externalSetting>
- <entry flags="VALUE_WORKSPACE_PATH" kind="includePath" name="/CANlib"/>
- <entry flags="VALUE_WORKSPACE_PATH" kind="libraryPath" name="/CANlib/Release"/>
- <entry flags="RESOLVED" kind="libraryFile" name="CANlib" srcPrefixMapping="" srcRootPath=""/>
- </externalSetting>
- </externalSettings>
- <externalSettings containerId="CoreNG;cdt.managedbuild.config.gnu.cross.lib.release.897729483.161018050.906601308.290747685" factoryId="org.eclipse.cdt.core.cfg.export.settings.sipplier">
- <externalSetting>
- <entry flags="VALUE_WORKSPACE_PATH" kind="includePath" name="/CoreNG"/>
- <entry flags="VALUE_WORKSPACE_PATH" kind="libraryPath" name="/CoreNG/SAME70_DUET3V03"/>
- <entry flags="RESOLVED" kind="libraryFile" name="CoreNG" srcPrefixMapping="" srcRootPath=""/>
- </externalSetting>
- </externalSettings>
- <externalSettings containerId="FreeRTOS;cdt.managedbuild.config.gnu.cross.exe.release.487753828.694459458" factoryId="org.eclipse.cdt.core.cfg.export.settings.sipplier">
- <externalSetting>
- <entry flags="VALUE_WORKSPACE_PATH" kind="includePath" name="/FreeRTOS"/>
- <entry flags="VALUE_WORKSPACE_PATH" kind="libraryPath" name="/FreeRTOS/SAME70"/>
- <entry flags="RESOLVED" kind="libraryFile" name="FreeRTOS" srcPrefixMapping="" srcRootPath=""/>
- </externalSetting>
- </externalSettings>
- <externalSettings containerId="RRFLibraries;cdt.managedbuild.config.gnu.cross.lib.release.1693990866.1913177425" factoryId="org.eclipse.cdt.core.cfg.export.settings.sipplier">
- <externalSetting>
- <entry flags="VALUE_WORKSPACE_PATH" kind="includePath" name="/RRFLibraries"/>
- <entry flags="VALUE_WORKSPACE_PATH" kind="libraryPath" name="/RRFLibraries/SAME70"/>
- <entry flags="RESOLVED" kind="libraryFile" name="RRFLibraries" srcPrefixMapping="" srcRootPath=""/>
- </externalSetting>
- </externalSettings>
- </storageModule>
- </cconfiguration>
<cconfiguration id="cdt.managedbuild.config.gnu.cross.exe.release.516195201.976458850.241502451.170574622.649587786.1798324396">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.config.gnu.cross.exe.release.516195201.976458850.241502451.170574622.649587786.1798324396" moduleId="org.eclipse.cdt.core.settings" name="PCCB_08">
<macros>
@@ -1490,7 +1309,7 @@
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
- <configuration artifactExtension="elf" artifactName="Duet3Firmware_MB6HC" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.cross.exe.release.516195201.976458850.241502451.1275216290.274082366.1645191116.1852610203.289083307.712841925" name="Duet3_V06" optionalBuildProperties="org.eclipse.cdt.docker.launcher.containerbuild.property.selectedvolumes=,org.eclipse.cdt.docker.launcher.containerbuild.property.volumes=" parent="cdt.managedbuild.config.gnu.cross.exe.release" postannouncebuildStep="Generating binary file" postbuildStep="arm-none-eabi-objcopy -O binary &quot;${workspace_loc:/${ProjName}/${ConfigName}}/${BuildArtifactFileBaseName}.elf&quot; &quot;${workspace_loc:/${ProjName}/${ConfigName}}/${BuildArtifactFileBaseName}.bin&quot; &amp;&amp; crc32appender &quot;${workspace_loc:/${ProjName}/${ConfigName}}/${BuildArtifactFileBaseName}.bin&quot;">
+ <configuration artifactExtension="elf" artifactName="Duet3Firmware_MB6HC" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.cross.exe.release.516195201.976458850.241502451.1275216290.274082366.1645191116.1852610203.289083307.712841925" name="Duet3_V06" optionalBuildProperties="org.eclipse.cdt.docker.launcher.containerbuild.property.volumes=,org.eclipse.cdt.docker.launcher.containerbuild.property.selectedvolumes=" parent="cdt.managedbuild.config.gnu.cross.exe.release" postannouncebuildStep="Generating binary file" postbuildStep="arm-none-eabi-objcopy -O binary &quot;${workspace_loc:/${ProjName}/${ConfigName}}/${BuildArtifactFileBaseName}.elf&quot; &quot;${workspace_loc:/${ProjName}/${ConfigName}}/${BuildArtifactFileBaseName}.bin&quot; &amp;&amp; crc32appender &quot;${workspace_loc:/${ProjName}/${ConfigName}}/${BuildArtifactFileBaseName}.bin&quot;">
<folderInfo id="cdt.managedbuild.config.gnu.cross.exe.release.516195201.976458850.241502451.1275216290.274082366.1645191116.1852610203.289083307.712841925." name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.cross.exe.release.537248487" name="Cross GCC" superClass="cdt.managedbuild.toolchain.gnu.cross.exe.release">
<option id="cdt.managedbuild.option.gnu.cross.path.582108797" name="Path" superClass="cdt.managedbuild.option.gnu.cross.path" useByScannerDiscovery="false" value="${ArmGccPath}" valueType="string"/>
diff --git a/Maths/closestPointOfApproach2d.wxmx b/Maths/closestPointOfApproach2d.wxmx
new file mode 100644
index 00000000..2604a547
--- /dev/null
+++ b/Maths/closestPointOfApproach2d.wxmx
Binary files differ
diff --git a/src/Duet3_V03/Pins_Duet3_V03.cpp b/src/Duet3_V03/Pins_Duet3_V03.cpp
deleted file mode 100644
index d7f5325b..00000000
--- a/src/Duet3_V03/Pins_Duet3_V03.cpp
+++ /dev/null
@@ -1,62 +0,0 @@
-/*
- * Pins_DuetNG.cpp
- *
- * Created on: 31 Mar 2019
- * Author: David
- */
-
-#include "RepRapFirmware.h"
-
-// Hardware-dependent pins functions
-
-// Function to look up a pin name pass back the corresponding index into the pin table
-// On this platform, the mapping from pin names to pins is fixed, so this is a simple lookup
-bool LookupPinName(const char*pn, LogicalPin& lpin, bool& hardwareInverted)
-{
- if (StringEqualsIgnoreCase(pn, NoPinName))
- {
- lpin = NoLogicalPin;
- hardwareInverted = false;
- return true;
- }
-
- for (size_t lp = 0; lp < ARRAY_SIZE(PinTable); ++lp)
- {
- const char *q = PinTable[lp].names;
- while (*q != 0)
- {
- // Try the next alias in the list of names for this pin
- const char *p = pn;
- bool hwInverted = (*q == '!');
- if (hwInverted)
- {
- ++q;
- }
- while (*q != ',' && *q != 0 && *p == *q)
- {
- ++p;
- ++q;
- }
- if (*p == 0 && (*q == 0 || *q == ','))
- {
- // Found a match
- lpin = (LogicalPin)lp;
- hardwareInverted = hwInverted;
- return true;
- }
-
- // Skip to the start of the next alias
- while (*q != 0 && *q != ',')
- {
- ++q;
- }
- if (*q == ',')
- {
- ++q;
- }
- }
- }
- return false;
-}
-
-// End
diff --git a/src/Duet3_V03/Pins_Duet3_V03.h b/src/Duet3_V03/Pins_Duet3_V03.h
deleted file mode 100644
index fdc327a3..00000000
--- a/src/Duet3_V03/Pins_Duet3_V03.h
+++ /dev/null
@@ -1,319 +0,0 @@
-#ifndef PINS_SAME70_H__
-#define PINS_SAME70_H__
-
-#define BOARD_SHORT_NAME "MBP03"
-#define FIRMWARE_NAME "RepRapFirmware for Duet 3 v0.3"
-#define DEFAULT_BOARD_TYPE BoardType::Duet3
-const size_t NumFirmwareUpdateModules = 4; // 3 modules, plus one for manual upload to WiFi module (module 2 not used)
-
-#define IAP_FIRMWARE_FILE "Duet3Firmware_" BOARD_SHORT_NAME ".bin"
-#define WIFI_FIRMWARE_FILE "DuetWiFiServer.bin"
-#define IAP_UPDATE_FILE "Duet3iap_sd_" BOARD_SHORT_NAME ".bin"
-
-// Features definition
-#define HAS_LWIP_NETWORKING 1
-#define HAS_WIFI_NETWORKING 1
-#define HAS_LINUX_INTERFACE 0
-#define HAS_CPU_TEMP_SENSOR 1
-#define HAS_MASS_STORAGE 1
-#define HAS_HIGH_SPEED_SD 1
-
-#define SUPPORT_TMC51xx 1
-#define TMC51xx_USES_USART 1
-
-#define SUPPORT_CAN_EXPANSION 1
-#define HAS_VOLTAGE_MONITOR 1
-#define ENFORCE_MAX_VIN 0
-#define HAS_VREF_MONITOR 1
-
-#define SUPPORT_INKJET 0 // set nonzero to support inkjet control
-#define SUPPORT_ROLAND 0 // set nonzero to support Roland mill
-#define SUPPORT_SCANNER 0 // set zero to disable support for FreeLSS scanners
-#define SUPPORT_LASER 1 // support laser cutters and engravers using G1 S parameter
-#define SUPPORT_IOBITS 1 // set to support P parameter in G0/G1 commands
-#define SUPPORT_DHT_SENSOR 1 // set nonzero to support DHT temperature/humidity sensors
-#define SUPPORT_WORKPLACE_COORDINATES 1 // set nonzero to support G10 L2 and G53..59
-#define SUPPORT_OBJECT_MODEL 1
-#define SUPPORT_FTP 1
-#define SUPPORT_TELNET 1
-#define SUPPORT_ASYNC_MOVES 1
-#define ALLOCATE_DEFAULT_PORTS 0
-
-#define USE_CACHE 0 // Cache controller disabled for now
-
-#define NO_EXTRUDER_ENDSTOPS 1 // Temporary!!!
-
-// The physical capabilities of the machine
-
-constexpr size_t NumDirectDrivers = 6; // The maximum number of drives supported by the electronics inc. direct expansion
-constexpr size_t MaxSmartDrivers = 6; // The maximum number of direct smart drivers
-constexpr size_t MaxCanDrivers = 18;
-constexpr size_t MaxCanBoards = 18;
-
-constexpr float MaxTmc5160Current = 3200.0; // The maximum current we allow the TMC5160/5161 drivers to be set to
-
-constexpr size_t MaxSensorsInSystem = 64;
-typedef uint64_t SensorsBitmap;
-
-constexpr size_t MaxHeaters = 12;
-constexpr size_t NumExtraHeaterProtections = 8; // The number of extra heater protection instances
-constexpr size_t NumThermistorInputs = 3; // The TEMP_2 input is unusable, so 3 not 4
-constexpr size_t NumTmcDriversSenseChannels = 1;
-
-constexpr size_t MaxZProbes = 4;
-constexpr size_t MaxGpioPorts = 12;
-
-constexpr size_t MinAxes = 3; // The minimum and default number of axes
-constexpr size_t MaxAxes = 9; // The maximum number of movement axes in the machine, usually just X, Y and Z, <= DRIVES
-constexpr size_t MaxDriversPerAxis = 5; // The maximum number of stepper drivers assigned to one axis
-
-constexpr size_t MaxExtruders = 16; // The maximum number of extruders
-constexpr size_t NumDefaultExtruders = 3; // The number of drivers that we configure as extruders by default
-
-constexpr size_t MaxHeatersPerTool = 4;
-constexpr size_t MaxExtrudersPerTool = 6;
-
-constexpr size_t NUM_SERIAL_CHANNELS = 2; // The number of serial IO channels not counting the WiFi serial connection (USB and one auxiliary UART)
-#define SERIAL_MAIN_DEVICE SerialUSB
-#define SERIAL_AUX_DEVICE Serial
-#define SERIAL_WIFI_DEVICE Serial1
-
-constexpr Pin UsbVBusPin = PortCPin(21); // Pin used to monitor VBUS on USB port
-
-// Drivers
-
-constexpr Pin STEP_PINS[NumDirectDrivers] = { PortCPin(18), PortCPin(16), PortCPin(28), PortCPin(01), PortCPin(04), PortCPin(9) };
-constexpr Pin DIRECTION_PINS[NumDirectDrivers] = { PortBPin(05), PortDPin(10), PortAPin(04), PortAPin(22), PortCPin(03), PortDPin(14) };
-constexpr Pin DIAG_PINS[NumDirectDrivers] = { PortDPin(19), PortCPin(17), PortDPin(13), PortCPin(02), PortDPin(31), PortCPin(10) };
-
-// Pin assignments etc. using USART1 in SPI mode
-constexpr Pin GlobalTmc51xxEnablePin = PortAPin(9); // The pin that drives ENN of all TMC drivers
-constexpr Pin GlobalTmc51xxCSPin = PortDPin(17); // The pin that drives CS of all TMC drivers
-Usart * const USART_TMC51xx = USART1;
-constexpr uint32_t ID_TMC51xx_SPI = ID_USART1;
-constexpr IRQn TMC51xx_SPI_IRQn = USART1_IRQn;
-#define TMC51xx_SPI_Handler USART1_Handler
-
-// These next two are #defines to avoid the need to #include DmacManager.h here
-#define TMC51xx_DmaTxPerid ((uint32_t)DmaTrigSource::usart1tx)
-#define TMC51xx_DmaRxPerid ((uint32_t)DmaTrigSource::usart1rx)
-
-constexpr Pin TMC51xxMosiPin = PortBPin(4);
-constexpr Pin TMC51xxMisoPin = PortAPin(21);
-constexpr Pin TMC51xxSclkPin = PortAPin(23);
-
-constexpr size_t NumPwmOutputs = 11; // number of heater/fan/servo outputs
-constexpr size_t NumInputOutputs = 9; // number of connectors we have for endstops, filament sensors, Z probes etc.
-
-// Thermistor/PT1000 inputs. The TEMP2 pin is left out because we had to reassign it to CAN.
-constexpr Pin TEMP_SENSE_PINS[NumThermistorInputs] = { PortBPin(3), PortCPin(15), PortCPin(30) }; // Thermistor/PT1000 pins
-constexpr Pin VssaSensePin = PortAPin(20);
-constexpr Pin VrefSensePin = PortEPin(0);
-
-// Thermistor series resistor value in Ohms
-constexpr float DefaultThermistorSeriesR = 2200.0;
-constexpr float MinVrefLoadR = (DefaultThermistorSeriesR / 4) * 4700.0/((DefaultThermistorSeriesR / 4) + 4700.0);
- // there are 4 temperature sensing channels and a 4K7 load resistor
-// Digital pins the SPI temperature sensors have their select lines tied to
-constexpr Pin SpiTempSensorCsPins[] = { PortDPin(16), PortDPin(15), PortDPin(27), PortCPin(22) };
-
-// Pin that controls the ATX power on/off
-constexpr Pin ATX_POWER_PIN = PortAPin(10);
-
-// Analogue pin numbers
-constexpr Pin PowerMonitorVinDetectPin = PortCPin(13);
-constexpr float PowerMonitorVoltageRange = 11.0 * 3.3; // We use an 11:1 voltage divider (TBD)
-
-// Digital pin number to turn the IR LED on (high) or off (low), also controls the DIAG LED
-constexpr Pin DiagPin = PortCPin(20);
-
-// Cooling fans
-constexpr size_t NumTotalFans = 12;
-
-// SD cards
-constexpr size_t NumSdCards = 2;
-constexpr Pin SdCardDetectPins[NumSdCards] = { PortAPin(6), NoPin };
-constexpr Pin SdWriteProtectPins[NumSdCards] = { NoPin, NoPin };
-constexpr Pin SdSpiCSPins[1] = { PortDPin(24) };
-constexpr uint32_t ExpectedSdCardSpeed = 25000000;
-
-// Ethernet
-constexpr Pin PhyInterruptPin = PortCPin(6);
-constexpr Pin PhyResetPin = PortDPin(11);
-
-// Enum to represent allowed types of pin access
-// We don't have a separate bit for servo, because Duet PWM-capable ports can be used for servos if they are on the Duet main board
-enum class PinCapability: uint8_t
-{
- // Individual capabilities
- read = 1,
- ain = 2,
- write = 4,
- pwm = 8,
-
- // Combinations
- ainr = 1|2,
- rw = 1|4,
- wpwm = 4|8,
- rwpwm = 1|4|8,
- ainrw = 1|2|4,
- ainrwpwm = 1|2|4|8
-};
-
-constexpr inline PinCapability operator|(PinCapability a, PinCapability b)
-{
- return (PinCapability)((uint8_t)a | (uint8_t)b);
-}
-
-// Struct to represent a pin that can be assigned to various functions
-// This can be varied to suit the hardware. It is a struct not a class so that it can be direct initialised in read-only memory.
-struct PinEntry
-{
- Pin GetPin() const { return pin; }
- PinCapability GetCapability() const { return cap; }
- const char* GetNames() const { return names; }
-
- Pin pin;
- PinCapability cap;
- const char *names;
-};
-
-// List of assignable pins and their mapping from names to MPU ports. This is indexed by logical pin number.
-// The names must match user input that has been concerted to lowercase and had _ and - characters stripped out.
-// Aliases are separate by the , character.
-// If a pin name is prefixed by ! then this means the pin is hardware inverted. The same pin may have names for both the inverted and non-inverted cases,
-// for example the inverted heater pins on the expansion connector are available as non-inverted servo pins on a DueX.
-constexpr PinEntry PinTable[] =
-{
- // Output connectors
- { PortAPin(7), PinCapability::wpwm, "out0" },
- { PortAPin(24), PinCapability::wpwm, "out1" },
- { PortAPin(16), PinCapability::wpwm, "out2" },
- { PortAPin(11), PinCapability::wpwm, "out3" },
- { PortAPin(15), PinCapability::wpwm, "out4" },
- { PortCPin(5), PinCapability::wpwm, "out5" },
- { PortAPin(8), PinCapability::wpwm, "out6" },
- { PortCPin(11), PinCapability::wpwm, "out7" },
- { PortCPin(8), PinCapability::wpwm, "out8" },
- { PortAPin(0), PinCapability::wpwm, "out9" },
- { PortCPin(23), PinCapability::wpwm, "out10,servo" },
- { PortAPin(10), PinCapability::write, "pson" },
-
- // Tacho inputs associated with outputs 4-6
- { PortCPin(7), PinCapability::read, "out4.tach" },
- { PortDPin(23), PinCapability::read, "out5.tach" },
- { PortAPin(1), PinCapability::read, "out6.tach" },
-
- // IO connector inputs
- //TODO some have ain capability too
- { PortDPin(30), PinCapability::read, "io0.in" },
- { PortEPin(4), PinCapability::read, "io1.in" },
- { PortAPin(18), PinCapability::read, "io2.in" },
- { PortEPin(5), PinCapability::read, "io3.in" },
- { PortAPin(17), PinCapability::read, "io4.in" },
- { PortAPin(19), PinCapability::read, "io5.in" },
- { PortCPin(31), PinCapability::read, "io6.in" },
- { PortCPin(0), PinCapability::read, "io7.in" },
- { PortEPin(3), PinCapability::read, "io8.in" },
-
- // IO connector outputs
- //TODO some have PWM capability too
- { PortBPin(7), PinCapability::write, "io0.out" },
- { PortBPin(6), PinCapability::write, "io1.out" },
- { PortCPin(14), PinCapability::write, "io2.out" },
- { PortAPin(3), PinCapability::write, "io3.out" },
- { PortAPin(2), PinCapability::write, "io4.out" },
- { PortEPin(2), PinCapability::write, "io5.out" },
- { PortAPin(12), PinCapability::write, "io6.out" },
- { PortCPin(29), PinCapability::write, "io7.out" },
- { PortEPin(1), PinCapability::write, "io8.out" },
-
- // Thermistor inputs
- { PortBPin(3), PinCapability::ainr, "temp0" },
- { PortCPin(15), PinCapability::ainr, "temp1" },
- // temp2 is unusable because we had to reassign it to CAN
- { PortCPin(30), PinCapability::ainr, "temp3" },
-
- // Misc
- { PortDPin(16), PinCapability::rw, "spi.cs0" },
- { PortDPin(15), PinCapability::rw, "spi.cs1" },
- { PortDPin(27), PinCapability::rw, "spi.cs2" },
- { PortCPin(22), PinCapability::rw, "spi.cs3" },
- { PortDPin(24), PinCapability::rw, "spi.cs4" }
-};
-
-constexpr unsigned int NumNamedPins = ARRAY_SIZE(PinTable);
-
-// Function to look up a pin name pass back the corresponding index into the pin table
-bool LookupPinName(const char *pn, LogicalPin& lpin, bool& hardwareInverted);
-
-// SAME70 Flash locations
-// These are designed to work with 1Mbyte flash processors as well as 2Mbyte
-// We can only erase complete 128kb sectors on the SAME70, so we allow 128Kb for IAP
-constexpr uint32_t IAP_FLASH_START = 0x004E0000;
-constexpr uint32_t IAP_FLASH_END = 0x004FFFFF;
-
-// Duet pin numbers to control the WiFi interface
-constexpr Pin EspResetPin = PortAPin(5); // Low on this in holds the WiFi module in reset (ESP_RESET)
-constexpr Pin EspDataReadyPin = PortCPin(19); // Input from the WiFi module indicating that it wants to transfer data (ESP GPIO0)
-constexpr Pin SamTfrReadyPin = PortAPin(29); // Output from the SAM to the WiFi module indicating we can accept a data transfer (ESP GPIO4 via 7474)
-constexpr Pin SamCsPin = PortBPin(2); // SPI NPCS pin, input from WiFi module
-Spi * const EspSpi = SPI0;
-
-// Timer allocation
-// Network timer is timer 4 aka TC1 channel1
-#define NETWORK_TC (TC1)
-#define NETWORK_TC_CHAN (1)
-#define NETWORK_TC_IRQN TC4_IRQn
-#define NETWORK_TC_HANDLER TC4_Handler
-#define NETWORK_TC_ID ID_TC4
-
-// Step timer is timer 2 aka TC0 channel 2
-#define STEP_TC (TC0)
-#define STEP_TC_CHAN (2)
-#define STEP_TC_IRQN TC2_IRQn
-#define STEP_TC_HANDLER TC2_Handler
-#define STEP_TC_ID ID_TC2
-
-// DMA channel allocation
-constexpr uint8_t DmacChanHsmci = 0; // this is hard coded in the ASF HSMCI driver
-constexpr uint8_t DmacChanWiFiTx = 1;
-constexpr uint8_t DmacChanWiFiRx = 2;
-constexpr uint8_t DmacChanTmcTx = 3;
-constexpr uint8_t DmacChanTmcRx = 4;
-
-constexpr size_t NumDmaChannelsUsed = 5;
-
-namespace StepPins
-{
- // *** These next three functions must use the same bit assignments in the drivers bitmap ***
- // Each stepper driver must be assigned one bit in a 32-bit word, in such a way that multiple drivers can be stepped efficiently
- // and more or less simultaneously by doing parallel writes to several bits in one or more output ports.
- // All our step pins are on port C, so the bitmap is just the map of step bits in port C.
-
- // Calculate the step bit for a driver. This doesn't need to be fast. It must return 0 if the driver is remote.
- static inline uint32_t CalcDriverBitmap(size_t driver)
- {
- return (driver < NumDirectDrivers)
- ? g_APinDescription[STEP_PINS[driver]].ulPin
- : 0;
- }
-
- // Set the specified step pins high
- // This needs to be as fast as possible, so we do a parallel write to the port(s).
- // We rely on only those port bits that are step pins being set in the PIO_OWSR register of each port
- static inline void StepDriversHigh(uint32_t driverMap)
- {
- PIOC->PIO_ODSR = driverMap; // on Duet 3 all step pins are on port C
- }
-
- // Set all step pins low
- // This needs to be as fast as possible, so we do a parallel write to the port(s).
- // We rely on only those port bits that are step pins being set in the PIO_OWSR register of each port
- static inline void StepDriversLow()
- {
- PIOC->PIO_ODSR = 0; // on Duet 3 all step pins are on port C
- }
-}
-
-#endif
diff --git a/src/Duet3_V05/Pins_Duet3_V05.h b/src/Duet3_V05/Pins_Duet3_V05.h
index d73ee2fe..bf95bb54 100644
--- a/src/Duet3_V05/Pins_Duet3_V05.h
+++ b/src/Duet3_V05/Pins_Duet3_V05.h
@@ -75,6 +75,8 @@ constexpr size_t MaxAxesPlusExtruders = 20; // May be <= MaxAxes + MaxExtruder
constexpr size_t MaxHeatersPerTool = 4;
constexpr size_t MaxExtrudersPerTool = 6;
+constexpr size_t MaxFans = 16;
+
constexpr size_t NUM_SERIAL_CHANNELS = 2; // The number of serial IO channels not counting the WiFi serial connection (USB and one auxiliary UART)
#define SERIAL_MAIN_DEVICE SerialUSB
#define SERIAL_AUX_DEVICE Serial
@@ -103,7 +105,6 @@ constexpr Pin TMC51xxMosiPin = PortBPin(4);
constexpr Pin TMC51xxMisoPin = PortAPin(21);
constexpr Pin TMC51xxSclkPin = PortAPin(23);
-
// Thermistor/PT1000 inputs
constexpr Pin TEMP_SENSE_PINS[NumThermistorInputs] = { PortCPin(31), PortCPin(15), PortCPin(29), PortCPin(30) }; // Thermistor/PT1000 pins
constexpr Pin VssaSensePin = PortCPin(13);
@@ -126,9 +127,6 @@ constexpr float PowerMonitorVoltageRange = 11.0 * 3.3; // we use an 11:1 vo
// Digital pin number to turn the IR LED on (high) or off (low), also controls the DIAG LED
constexpr Pin DiagPin = PortCPin(20);
-// Cooling fans
-constexpr size_t NumTotalFans = 12;
-
// SD cards
constexpr size_t NumSdCards = 1; // actually 0 cards, but 0 probably won't work yet
constexpr Pin SdCardDetectPins[1] = { NoPin };
diff --git a/src/Duet3_V06/Pins_Duet3_V06.h b/src/Duet3_V06/Pins_Duet3_V06.h
index 0c02a334..12f9e178 100644
--- a/src/Duet3_V06/Pins_Duet3_V06.h
+++ b/src/Duet3_V06/Pins_Duet3_V06.h
@@ -79,7 +79,7 @@ constexpr size_t MaxAxesPlusExtruders = 20; // May be <= MaxAxes + MaxExtruder
constexpr size_t MaxHeatersPerTool = 4;
constexpr size_t MaxExtrudersPerTool = 6;
-constexpr size_t NumTotalFans = 12;
+constexpr size_t MaxFans = 16;
constexpr size_t NUM_SERIAL_CHANNELS = 2; // The number of serial IO channels not counting the WiFi serial connection (USB and one auxiliary UART)
#define SERIAL_MAIN_DEVICE SerialUSB
diff --git a/src/DuetM/Pins_DuetM.h b/src/DuetM/Pins_DuetM.h
index b29b5665..8de48911 100644
--- a/src/DuetM/Pins_DuetM.h
+++ b/src/DuetM/Pins_DuetM.h
@@ -72,6 +72,8 @@ constexpr size_t MaxAxesPlusExtruders = 7;
constexpr size_t MaxHeatersPerTool = 2;
constexpr size_t MaxExtrudersPerTool = 4;
+constexpr size_t MaxFans = 6;
+
constexpr size_t NUM_SERIAL_CHANNELS = 2; // The number of serial IO channels (USB and one auxiliary UART)
#define SERIAL_MAIN_DEVICE SerialUSB
#define SERIAL_AUX_DEVICE Serial
@@ -140,9 +142,6 @@ constexpr Pin Z_PROBE_PIN = PortCPin(15); // Z probe analog input
constexpr Pin Z_PROBE_MOD_PIN = PortCPin(26);
constexpr Pin DiagPin = Z_PROBE_MOD_PIN;
-// Cooling fans
-constexpr size_t NumTotalFans = 4;
-
// SD cards
constexpr size_t NumSdCards = 2;
constexpr Pin SdCardDetectPins[NumSdCards] = { PortCPin(8), NoPin };
diff --git a/src/DuetNG/Pins_DuetNG.h b/src/DuetNG/Pins_DuetNG.h
index 59330072..eec18f4b 100644
--- a/src/DuetNG/Pins_DuetNG.h
+++ b/src/DuetNG/Pins_DuetNG.h
@@ -73,6 +73,8 @@ constexpr size_t MaxAxesPlusExtruders = 12;
constexpr size_t MaxHeatersPerTool = 8;
constexpr size_t MaxExtrudersPerTool = 8;
+constexpr size_t MaxFans = 12;
+
constexpr size_t NUM_SERIAL_CHANNELS = 2; // The number of serial IO channels not counting the WiFi serial connection (USB and one auxiliary UART)
#define SERIAL_MAIN_DEVICE SerialUSB
#define SERIAL_AUX_DEVICE Serial
@@ -160,9 +162,6 @@ constexpr Pin Z_PROBE_PIN = PortCPin(1); // AFE1_AD4/PC1 Z probe analog
constexpr Pin Z_PROBE_MOD_PIN = PortCPin(2);
constexpr Pin DiagPin = Z_PROBE_MOD_PIN;
-// Cooling fans
-constexpr size_t NumTotalFans = 12;
-
// SD cards
constexpr size_t NumSdCards = 2;
constexpr Pin SdCardDetectPins[NumSdCards] = { PortCPin(21), NoPin };
diff --git a/src/Fans/FansManager.cpp b/src/Fans/FansManager.cpp
index a0fc0583..ecfe7d66 100644
--- a/src/Fans/FansManager.cpp
+++ b/src/Fans/FansManager.cpp
@@ -78,7 +78,7 @@ bool FansManager::WriteFanSettings(FileStore *f) const
{
ReadLocker lock(fansLock);
bool ok = true;
- for (size_t fanNum = 0; ok && fanNum < NumTotalFans; ++fanNum)
+ for (size_t fanNum = 0; ok && fanNum < MaxFans; ++fanNum)
{
ok = fans[fanNum] == nullptr || fans[fanNum]->WriteSettings(f, fanNum);
}
@@ -90,7 +90,7 @@ bool FansManager::WriteFanSettings(FileStore *f) const
// This is called by M950 to create a fan or change its PWM frequency
GCodeResult FansManager::ConfigureFanPort(uint32_t fanNum, GCodeBuffer& gb, const StringRef& reply)
{
- if (fanNum < NumTotalFans)
+ if (fanNum < MaxFans)
{
const bool seenPin = gb.Seen('C');
if (seenPin)
diff --git a/src/Fans/FansManager.h b/src/Fans/FansManager.h
index 7b7ab89b..db213ee0 100644
--- a/src/Fans/FansManager.h
+++ b/src/Fans/FansManager.h
@@ -48,7 +48,7 @@ private:
LocalFan *CreateLocalFan(uint32_t fanNum, const char *pinNames, PwmFrequency freq, const StringRef& reply);
mutable ReadWriteLock fansLock;
- Fan *fans[NumTotalFans];
+ Fan *fans[MaxFans];
};
#endif /* SRC_FANS_FANSMANAGER_H_ */
diff --git a/src/FilamentMonitors/PulsedFilamentMonitor.cpp b/src/FilamentMonitors/PulsedFilamentMonitor.cpp
index 38917cbe..4a9101a3 100644
--- a/src/FilamentMonitors/PulsedFilamentMonitor.cpp
+++ b/src/FilamentMonitors/PulsedFilamentMonitor.cpp
@@ -96,7 +96,6 @@ bool PulsedFilamentMonitor::Configure(GCodeBuffer& gb, const StringRef& reply, b
}
else
{
- reply.catf("current position %.1f, ", (double)GetCurrentPosition());
if (calibrationStarted && fabsf(totalMovementMeasured) > 1.0 && totalExtrusionCommanded > 20.0)
{
const float measuredMmPerPulse = totalExtrusionCommanded/totalMovementMeasured;
@@ -161,12 +160,6 @@ void PulsedFilamentMonitor::Poll()
}
}
-// Return the current wheel angle
-float PulsedFilamentMonitor::GetCurrentPosition() const
-{
- return (float)sensorValue;
-}
-
// Call the following at intervals to check the status. This is only called when extrusion is in progress or imminent.
// 'filamentConsumed' is the net amount of extrusion since the last call to this function.
// 'isPrinting' is true unless a non-printing extruder move was in progress
@@ -282,20 +275,7 @@ void PulsedFilamentMonitor::Diagnostics(MessageType mtype, unsigned int extruder
{
Poll();
const char* const statusText = (samplesReceived < 2) ? "no data received" : "ok";
- reprap.GetPlatform().MessageF(mtype, "Extruder %u sensor: position %.2f, %s, ", extruder, (double)GetCurrentPosition(), statusText);
- if (calibrationStarted && fabsf(totalMovementMeasured) > 1.0 && totalExtrusionCommanded > 20.0)
- {
- const float measuredMmPerRev = totalExtrusionCommanded/totalMovementMeasured;
- const float normalRatio = 1.0/measuredMmPerRev;
- const int measuredPosTolerance = lrintf(100.0 * (((normalRatio > 0.0) ? maxMovementRatio : minMovementRatio) - normalRatio)/normalRatio);
- const int measuredNegTolerance = lrintf(100.0 * (normalRatio - ((normalRatio > 0.0) ? minMovementRatio : maxMovementRatio))/normalRatio);
- reprap.GetPlatform().MessageF(mtype,"measured sensitivity %.3fmm/pulse +%d%% -%d%%\n",
- (double)measuredMmPerRev, measuredPosTolerance, measuredNegTolerance);
- }
- else
- {
- reprap.GetPlatform().Message(mtype, "no calibration data\n");
- }
+ reprap.GetPlatform().MessageF(mtype, "Extruder %u sensor: %s\n", extruder, statusText);
}
// End
diff --git a/src/FilamentMonitors/PulsedFilamentMonitor.h b/src/FilamentMonitors/PulsedFilamentMonitor.h
index bf102b77..1664b3d3 100644
--- a/src/FilamentMonitors/PulsedFilamentMonitor.h
+++ b/src/FilamentMonitors/PulsedFilamentMonitor.h
@@ -30,7 +30,6 @@ private:
void Init();
void Reset();
void Poll();
- float GetCurrentPosition() const;
FilamentSensorStatus CheckFilament(float amountCommanded, float amountMeasured, bool overdue);
// Configuration parameters
diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index 8c7e7215..187fd0b3 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -261,8 +261,7 @@ void GCodes::Reset()
currentZHop = 0.0; // clear this before calling ToolOffsetInverseTransform
lastPrintingMoveHeight = -1.0;
- moveBuffer.xAxes = DefaultXAxisMapping;
- moveBuffer.yAxes = DefaultYAxisMapping;
+ moveBuffer.tool = nullptr;
moveBuffer.virtualExtruderPosition = 0.0;
#if SUPPORT_LASER || SUPPORT_IOBITS
@@ -1578,8 +1577,7 @@ const char* GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated)
moveBuffer.checkEndstops = false;
moveBuffer.reduceAcceleration = false;
moveBuffer.moveType = 0;
- moveBuffer.xAxes = reprap.GetCurrentXAxes();
- moveBuffer.yAxes = reprap.GetCurrentYAxes();
+ moveBuffer.tool = reprap.GetCurrentTool();
moveBuffer.usePressureAdvance = false;
axesToSenseLength = 0;
@@ -1591,8 +1589,7 @@ const char* GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated)
if (ival >= 1 && ival <= 3)
{
moveBuffer.moveType = ival;
- moveBuffer.xAxes = DefaultXAxisMapping;
- moveBuffer.yAxes = DefaultYAxisMapping;
+ moveBuffer.tool = nullptr;
}
if (!gb.Seen('H'))
{
@@ -1664,7 +1661,7 @@ const char* GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated)
// This may be a raw motor move, in which case we need the current raw motor positions in moveBuffer.coords.
// If it isn't a raw motor move, it will still be applied without axis or bed transform applied,
// so make sure the initial coordinates don't have those either to avoid unwanted Z movement.
- reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, moveBuffer.moveType, reprap.GetCurrentXAxes(), reprap.GetCurrentYAxes());
+ reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, moveBuffer.moveType, reprap.GetCurrentTool());
}
// Set up the initial coordinates
@@ -2036,20 +2033,21 @@ const char* GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
moveBuffer.checkEndstops = false;
moveBuffer.reduceAcceleration = false;
moveBuffer.moveType = 0;
- moveBuffer.xAxes = reprap.GetCurrentXAxes();
- moveBuffer.yAxes = reprap.GetCurrentYAxes();
+ moveBuffer.tool = reprap.GetCurrentTool();
moveBuffer.isCoordinated = true;
// Set up the arc centre coordinates and record which axes behave like an X axis.
// The I and J parameters are always relative to present position.
// For X and Y we need to set up the arc centre for each axis that X or Y is mapped to.
+ const AxesBitmap xAxes = reprap.GetCurrentXAxes();
+ const AxesBitmap yAxes = reprap.GetCurrentYAxes();
for (size_t axis = 0; axis < numVisibleAxes; ++axis)
{
- if (IsBitSet(moveBuffer.xAxes, axis))
+ if (IsBitSet(xAxes, axis))
{
arcCentre[axis] = moveBuffer.initialCoords[axis] + iParam;
}
- else if (IsBitSet(moveBuffer.yAxes, axis))
+ else if (IsBitSet(yAxes, axis))
{
arcCentre[axis] = moveBuffer.initialCoords[axis] + jParam;
}
@@ -2210,12 +2208,12 @@ bool GCodes::ReadMove(RawMove& m)
for (size_t drive = 0; drive < numVisibleAxes; ++drive)
{
- if (doingArcMove && drive != Z_AXIS && IsBitSet(moveBuffer.yAxes, drive))
+ if (doingArcMove && drive != Z_AXIS && IsBitSet(Tool::GetYAxes(moveBuffer.tool), drive))
{
// Y axis or a substitute Y axis
moveBuffer.initialCoords[drive] = arcCentre[drive] + arcRadius * sinf(arcCurrentAngle);
}
- else if (doingArcMove && drive != Z_AXIS && IsBitSet(moveBuffer.xAxes, drive))
+ else if (doingArcMove && drive != Z_AXIS && IsBitSet(Tool::GetXAxes(moveBuffer.tool), drive))
{
// X axis or a substitute X axis
moveBuffer.initialCoords[drive] = arcCentre[drive] + arcRadius * cosf(arcCurrentAngle);
@@ -2667,7 +2665,7 @@ GCodeResult GCodes::SaveHeightMap(GCodeBuffer& gb, const StringRef& reply) const
void GCodes::ClearBedMapping()
{
reprap.GetMove().SetIdentityTransform();
- reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, reprap.GetCurrentXAxes(), reprap.GetCurrentYAxes());
+ reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, reprap.GetCurrentTool());
ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition); // update user coordinates to remove any height map offset there was at the current position
}
@@ -2688,7 +2686,7 @@ void GCodes::GetCurrentCoordinates(const StringRef& s) const
// Get the live machine coordinates, we'll need them later
float liveCoordinates[MaxAxesPlusExtruders];
- reprap.GetMove().LiveCoordinates(liveCoordinates, reprap.GetCurrentXAxes(), reprap.GetCurrentYAxes());
+ reprap.GetMove().LiveCoordinates(liveCoordinates, reprap.GetCurrentTool());
// Now the extruder coordinates
for (size_t i = 0; i < numExtruders; i++)
@@ -2715,7 +2713,7 @@ void GCodes::GetCurrentCoordinates(const StringRef& s) const
// Add the bed compensation
const float machineZ = machineCoordinates[Z_AXIS];
- reprap.GetMove().AxisAndBedTransform(machineCoordinates, reprap.GetCurrentXAxes(), reprap.GetCurrentYAxes(), true);
+ reprap.GetMove().AxisAndBedTransform(machineCoordinates, reprap.GetCurrentTool(), true);
s.catf(" Bed comp %.3f", (double)(machineCoordinates[Z_AXIS] - machineZ));
}
@@ -3021,10 +3019,10 @@ GCodeResult GCodes::ManageTool(GCodeBuffer& gb, const StringRef& reply)
FansBitmap fanMap;
if (gb.Seen('F'))
{
- uint32_t fanMapping[NumTotalFans];
- size_t fanCount = NumTotalFans;
+ uint32_t fanMapping[MaxFans];
+ size_t fanCount = MaxFans;
gb.GetUnsignedArray(fanMapping, fanCount, false);
- fanMap = UnsignedArrayToBitMap<FansBitmap>(fanMapping, fanCount) & LowestNBits<FansBitmap>(NumTotalFans);
+ fanMap = UnsignedArrayToBitMap<FansBitmap>(fanMapping, fanCount) & LowestNBits<FansBitmap>(MaxFans);
seen = true;
}
else
@@ -3032,11 +3030,18 @@ GCodeResult GCodes::ManageTool(GCodeBuffer& gb, const StringRef& reply)
fanMap = 1; // by default map fan 0 to fan 0
}
- // Check if filament support is being enforced
- const int filamentDrive = gb.Seen('L') ? gb.GetIValue()
- : ((dCount == 1) ? drives[0] : -1);
if (seen)
{
+ if (!LockMovementAndWaitForStandstill(gb))
+ {
+ return GCodeResult::notFinished;
+ }
+
+ // Check if filament support is being enforced
+ const int filamentDrive = (gb.Seen('L')) ? gb.GetIValue()
+ : ((dCount == 1) ? drives[0]
+ : -1);
+
// Add or delete tool, so start by deleting the old one with this number, if any
reprap.DeleteTool(reprap.GetTool(toolNumber));
@@ -3094,7 +3099,7 @@ void GCodes::SetMappedFanSpeed(float f)
else
{
const uint32_t fanMap = ct->GetFanMapping();
- for (size_t i = 0; i < NumTotalFans; ++i)
+ for (size_t i = 0; i < MaxFans; ++i)
{
if (IsBitSet(fanMap, i))
{
@@ -3115,7 +3120,7 @@ bool GCodes::IsMappedFan(unsigned int fanNumber)
// Save the speeds of all fans
void GCodes::SaveFanSpeeds()
{
- for (size_t i = 0; i < NumTotalFans; ++i)
+ for (size_t i = 0; i < MaxFans; ++i)
{
pausedFanSpeeds[i] = reprap.GetFansManager().GetFanValue(i);
}
@@ -3359,14 +3364,11 @@ GCodeResult GCodes::RetractFilament(GCodeBuffer& gb, bool retract)
// New code does the retraction and the Z hop as separate moves
// Get ready to generate a move
- const uint32_t xAxes = reprap.GetCurrentXAxes();
- const uint32_t yAxes = reprap.GetCurrentYAxes();
- reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, xAxes, yAxes);
+ moveBuffer.tool = reprap.GetCurrentTool();
+ reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, moveBuffer.tool);
SetMoveBufferDefaults();
moveBuffer.isFirmwareRetraction = true;
moveBuffer.filePos = (&gb == fileGCode) ? gb.GetFilePosition() : noFilePosition;
- moveBuffer.xAxes = xAxes;
- moveBuffer.yAxes = yAxes;
if (retract)
{
@@ -3686,7 +3688,7 @@ void GCodes::SetMachinePosition(const float positionNow[MaxAxesPlusExtruders], b
// Get the current position from the Move class
void GCodes::UpdateCurrentUserPosition()
{
- reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, reprap.GetCurrentXAxes(), reprap.GetCurrentYAxes());
+ reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, reprap.GetCurrentTool());
ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);
}
@@ -4216,7 +4218,7 @@ bool GCodes::LockHeater(const GCodeBuffer& gb, int heater)
bool GCodes::LockFan(const GCodeBuffer& gb, int fan)
{
- if (fan >= 0 && fan < (int)NumTotalFans)
+ if (fan >= 0 && fan < (int)MaxFans)
{
return LockResource(gb, FanResourceBase + fan);
}
@@ -4410,7 +4412,7 @@ void GCodes::ActivateHeightmap(bool activate)
if (activate)
{
// Update the current position to allow for any bed compensation at the current XY coordinates
- reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, reprap.GetCurrentXAxes(), reprap.GetCurrentYAxes());
+ reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, reprap.GetCurrentTool());
ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition); // update user coordinates to reflect any height map offset at the current position
}
}
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index da55bf75..d79b0523 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -213,7 +213,7 @@ private:
static const Resource FileSystemResource = 1; // Non-sharable parts of the file system
static const Resource HeaterResourceBase = 2;
static const Resource FanResourceBase = HeaterResourceBase + MaxHeaters;
- static const size_t NumResources = FanResourceBase + NumTotalFans;
+ static const size_t NumResources = FanResourceBase + MaxFans;
static_assert(NumResources <= 32, "Too many resources to keep a bitmap of them in class GCodeMachineState");
@@ -490,7 +490,7 @@ private:
AxesBitmap toBeHomed; // Bitmap of axes still to be homed
AxesBitmap axesHomed; // Bitmap of which axes have been homed
- float pausedFanSpeeds[NumTotalFans]; // Fan speeds when the print was paused or a tool change started
+ float pausedFanSpeeds[MaxFans]; // Fan speeds when the print was paused or a tool change started
float lastDefaultFanSpeed; // Last speed given in a M106 command with on fan number
float pausedDefaultFanSpeed; // The speed of the default print cooling fan when the print was paused or a tool change started
float speedFactor; // speed factor as a percentage (normally 100.0)
diff --git a/src/GCodes/GCodes4.cpp b/src/GCodes/GCodes4.cpp
index 5e267242..aa7e95b0 100644
--- a/src/GCodes/GCodes4.cpp
+++ b/src/GCodes/GCodes4.cpp
@@ -386,7 +386,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply)
case GCodeState::resuming3:
if (LockMovementAndWaitForStandstill(gb))
{
- for (size_t i = 0; i < NumTotalFans; ++i)
+ for (size_t i = 0; i < MaxFans; ++i)
{
reprap.GetFansManager().SetFanValue(i, pausedFanSpeeds[i]);
}
@@ -919,7 +919,15 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply)
// Reset the Z axis origin according to the height error so that we can move back up to the dive height
moveBuffer.coords[Z_AXIS] = zp.GetActualTriggerHeight();
reprap.GetMove().SetNewPosition(moveBuffer.coords, false);
- reprap.GetMove().SetZeroHeightError(moveBuffer.coords);
+
+ // Find the coordinates of the Z probe to pass to SetZeroHeightError
+ float tempCoords[MaxAxes];
+ memcpy(tempCoords, moveBuffer.coords, sizeof(tempCoords));
+ tempCoords[X_AXIS] += zp.GetXOffset();
+ tempCoords[Y_AXIS] += zp.GetYOffset();
+ reprap.GetMove().SetZeroHeightError(tempCoords);
+ ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);
+
g30zHeightErrorSum = g30zHeightError = 0; // there is no longer any height error from this probe
SetAxisIsHomed(Z_AXIS); // this is only correct if the Z axis is Cartesian-like, but other architectures must be homed before probing anyway
zDatumSetByProbing = true;
@@ -995,7 +1003,13 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply)
// Setting the Z height with G30
moveBuffer.coords[Z_AXIS] -= g30zHeightError;
reprap.GetMove().SetNewPosition(moveBuffer.coords, false);
- reprap.GetMove().SetZeroHeightError(moveBuffer.coords);
+
+ // Find the coordinates of the Z probe to pass to SetZeroHeightError
+ float tempCoords[MaxAxes];
+ memcpy(tempCoords, moveBuffer.coords, sizeof(tempCoords));
+ tempCoords[X_AXIS] += params.GetXOffset();
+ tempCoords[Y_AXIS] += params.GetYOffset();
+ reprap.GetMove().SetZeroHeightError(tempCoords);
ToolOffsetInverseTransform(moveBuffer.coords, currentUserPosition);
}
gb.AdvanceState();
@@ -1179,10 +1193,9 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply)
// We just did the retraction part of a firmware retraction, now we need to do the Z hop
if (segmentsLeft == 0)
{
- const AxesBitmap xAxes = reprap.GetCurrentXAxes();
- const AxesBitmap yAxes = reprap.GetCurrentYAxes();
- reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, xAxes, yAxes);
SetMoveBufferDefaults();
+ moveBuffer.tool = reprap.GetCurrentTool();
+ reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, moveBuffer.tool);
moveBuffer.coords[Z_AXIS] += retractHop;
moveBuffer.feedRate = platform.MaxFeedrate(Z_AXIS);
moveBuffer.filePos = (&gb == fileGCode) ? gb.GetFilePosition() : noFilePosition;
@@ -1200,15 +1213,14 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply)
const Tool * const tool = reprap.GetCurrentTool();
if (tool != nullptr)
{
- const uint32_t xAxes = reprap.GetCurrentXAxes();
- const uint32_t yAxes = reprap.GetCurrentYAxes();
- reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, xAxes, yAxes);
SetMoveBufferDefaults();
+ reprap.GetMove().GetCurrentUserPosition(moveBuffer.coords, 0, tool);
for (size_t i = 0; i < tool->DriveCount(); ++i)
{
moveBuffer.coords[ExtruderToLogicalDrive(tool->Drive(i))] = retractLength + retractExtra;
}
moveBuffer.feedRate = unRetractSpeed;
+ moveBuffer.tool = tool;
moveBuffer.isFirmwareRetraction = true;
moveBuffer.filePos = (&gb == fileGCode) ? gb.GetFilePosition() : noFilePosition;
moveBuffer.canPauseAfter = true;
diff --git a/src/Hardware/Cache.cpp b/src/Hardware/Cache.cpp
new file mode 100644
index 00000000..3c990099
--- /dev/null
+++ b/src/Hardware/Cache.cpp
@@ -0,0 +1,216 @@
+/*
+ * Cache.cpp
+ *
+ * Created on: 22 Nov 2019
+ * Author: David
+ */
+
+#include <Hardware/Cache.h>
+
+#if USE_CACHE
+
+#if SAME70
+# include <core_cm7.h>
+
+extern uint32_t _nocache_ram_start;
+extern uint32_t _nocache_ram_end;
+
+# if USE_MPU
+# include <mpu_armv7.h>
+
+// Macro ARM_MPU_RASR_EX is incorrectly defined in CMSIS 5.4.0, see https://github.com/ARM-software/CMSIS_5/releases. Redefine it here.
+
+# undef ARM_MPU_RASR_EX
+
+/**
+* MPU Region Attribute and Size Register Value
+*
+* \param DisableExec Instruction access disable bit, 1= disable instruction fetches.
+* \param AccessPermission Data access permissions, allows you to configure read/write access for User and Privileged mode.
+* \param AccessAttributes Memory access attribution, see \ref ARM_MPU_ACCESS_.
+* \param SubRegionDisable Sub-region disable field.
+* \param Size Region size of the region to be configured, for example 4K, 8K.
+*/
+# define ARM_MPU_RASR_EX(DisableExec, AccessPermission, AccessAttributes, SubRegionDisable, Size) \
+ ((((DisableExec) << MPU_RASR_XN_Pos) & MPU_RASR_XN_Msk) | \
+ (((AccessPermission) << MPU_RASR_AP_Pos) & MPU_RASR_AP_Msk) | \
+ (((AccessAttributes) & (MPU_RASR_TEX_Msk | MPU_RASR_S_Msk | MPU_RASR_C_Msk | MPU_RASR_B_Msk))) | \
+ (((SubRegionDisable) << MPU_RASR_SRD_Pos) & MPU_RASR_SRD_Msk) | \
+ (((Size) << MPU_RASR_SIZE_Pos) & MPU_RASR_SIZE_Msk) | \
+ (((MPU_RASR_ENABLE_Msk))))
+
+# endif
+
+#endif
+
+#if SAM4E
+# include <cmcc/cmcc.h>
+#endif
+
+static bool enabled = false;
+
+void Cache::Init()
+{
+#if SAME70
+
+# if USE_MPU
+ // Set up the MPU so that we can have a non-cacheable RAM region, and so that we can trap accesses to non-existent memory
+ // Where regions overlap, the region with the highest region number takes priority
+ constexpr ARM_MPU_Region_t regionTable[] =
+ {
+ // Flash memory: read-only, execute allowed, cacheable
+ {
+ ARM_MPU_RBAR(0, IFLASH_ADDR),
+ ARM_MPU_RASR_EX(0u, ARM_MPU_AP_RO, ARM_MPU_ACCESS_NORMAL(ARM_MPU_CACHEP_WB_WRA, ARM_MPU_CACHEP_WB_WRA, 1u), 0u, ARM_MPU_REGION_SIZE_1MB)
+ },
+ // First 256kb RAM, read-write, cacheable, execute disabled. Parts of this are overridden later.
+ {
+ ARM_MPU_RBAR(1, IRAM_ADDR),
+ ARM_MPU_RASR_EX(1u, ARM_MPU_AP_FULL, ARM_MPU_ACCESS_NORMAL(ARM_MPU_CACHEP_WB_WRA, ARM_MPU_CACHEP_WB_WRA, 1u), 0u, ARM_MPU_REGION_SIZE_256KB)
+ },
+ // Final 128kb RAM, read-write, cacheable, execute disabled
+ {
+ ARM_MPU_RBAR(2, IRAM_ADDR + 0x00040000),
+ ARM_MPU_RASR_EX(1u, ARM_MPU_AP_FULL, ARM_MPU_ACCESS_NORMAL(ARM_MPU_CACHEP_WB_WRA, ARM_MPU_CACHEP_WB_WRA, 1u), 0u, ARM_MPU_REGION_SIZE_128KB)
+ },
+ // Non-cachable RAM. This must be before normal RAM because it includes CAN buffers which must be within first 64kb.
+ // Read write, execute disabled, non-cacheable
+ {
+ ARM_MPU_RBAR(3, IRAM_ADDR),
+ ARM_MPU_RASR_EX(1u, ARM_MPU_AP_FULL, ARM_MPU_ACCESS_ORDERED, 0, ARM_MPU_REGION_SIZE_64KB)
+ },
+ // RAMFUNC memory. Read-only (the code has already been written to it), execution allowed. The initialised data memory follows, so it must be RW.
+ // 256 bytes is enough at present (check the linker memory map if adding more RAMFUNCs).
+ {
+ ARM_MPU_RBAR(4, IRAM_ADDR + 0x00010000),
+ ARM_MPU_RASR_EX(0u, ARM_MPU_AP_FULL, ARM_MPU_ACCESS_NORMAL(ARM_MPU_CACHEP_WB_WRA, ARM_MPU_CACHEP_WB_WRA, 1u), 0u, ARM_MPU_REGION_SIZE_256B)
+ },
+ // Peripherals
+ {
+ ARM_MPU_RBAR(5, 0x40000000),
+ ARM_MPU_RASR_EX(1u, ARM_MPU_AP_FULL, ARM_MPU_ACCESS_DEVICE(1u), 0u, ARM_MPU_REGION_SIZE_16MB)
+ },
+ // USBHS
+ {
+ ARM_MPU_RBAR(6, 0xA0100000),
+ ARM_MPU_RASR_EX(1u, ARM_MPU_AP_FULL, ARM_MPU_ACCESS_DEVICE(1u), 0u, ARM_MPU_REGION_SIZE_1MB)
+ },
+ // ROM
+ {
+ ARM_MPU_RBAR(7, IROM_ADDR),
+ ARM_MPU_RASR_EX(0u, ARM_MPU_AP_RO, ARM_MPU_ACCESS_NORMAL(ARM_MPU_CACHEP_WB_WRA, ARM_MPU_CACHEP_WB_WRA, 1u), 0u, ARM_MPU_REGION_SIZE_4MB)
+ },
+ // ARM Private Peripheral Bus
+ {
+ ARM_MPU_RBAR(8, 0xE0000000),
+ ARM_MPU_RASR_EX(1u, ARM_MPU_AP_FULL, ARM_MPU_ACCESS_ORDERED, 0u, ARM_MPU_REGION_SIZE_1MB)
+ }
+ };
+
+ // Ensure MPU is disabled
+ ARM_MPU_Disable();
+
+ // Clear all regions
+ const uint32_t numRegions = (MPU->TYPE & MPU_TYPE_DREGION_Msk) >> MPU_TYPE_DREGION_Pos;
+ for (unsigned int region = 0; region < numRegions; ++region)
+ {
+ ARM_MPU_ClrRegion(region);
+ }
+
+ // Load regions from our table
+ ARM_MPU_Load(regionTable, ARRAY_SIZE(regionTable));
+
+ // Enable the MPU, disabling the default map but allowing exception handlers to use it
+ ARM_MPU_Enable(0x01);
+# endif
+
+#elif SAM4E
+ cmcc_config g_cmcc_cfg;
+ cmcc_get_config_defaults(&g_cmcc_cfg);
+ cmcc_init(CMCC, &g_cmcc_cfg);
+#endif
+}
+
+void Cache::Enable()
+{
+ if (!enabled)
+ {
+ enabled = true;
+#if SAME70
+ SCB_EnableICache();
+ SCB_EnableDCache();
+
+#elif SAM4E
+ cmcc_invalidate_all(CMCC);
+ cmcc_enable(CMCC);
+#endif
+ }
+}
+
+void Cache::Disable()
+{
+ if (enabled)
+ {
+#if SAME70
+ SCB_DisableICache();
+ SCB_DisableDCache();
+#elif SAM4E
+ cmcc_disable(CMCC);
+#endif
+ enabled = false;
+ }
+}
+
+#if SAME70
+
+void Cache::Flush(const volatile void *start, size_t length)
+{
+ if (enabled)
+ {
+ // We assume that the DMA buffer is entirely inside or entirely outside the non-cached RAM area
+ if (start < (void*)&_nocache_ram_start || start >= (void*)&_nocache_ram_end)
+ {
+ const uint32_t startAddr = reinterpret_cast<uint32_t>(start);
+ SCB_CleanDCache_by_Addr(reinterpret_cast<uint32_t*>(startAddr & ~3), length + (startAddr & 3));
+ }
+ }
+}
+
+#endif
+
+void Cache::Invalidate(const volatile void *start, size_t length)
+{
+ if (enabled)
+ {
+#if SAME70
+ // We assume that the DMA buffer is entirely inside or entirely outside the non-cached RAM area
+ if (start < (void*)&_nocache_ram_start || start >= (void*)&_nocache_ram_end)
+ {
+ // Caution! if any part of the cache line is dirty, the written data will be lost!
+ const uint32_t startAddr = reinterpret_cast<uint32_t>(start);
+ SCB_InvalidateDCache_by_Addr(reinterpret_cast<uint32_t*>(startAddr & ~3), length + (startAddr & 3));
+ }
+#elif SAM4E
+ // The cache is only 2kb on the SAM4E so we just invalidate the whole cache
+ cmcc_invalidate_all(CMCC);
+#endif
+ }
+}
+
+#if SAM4E
+
+uint32_t Cache::GetHitCount()
+{
+ return cmcc_get_monitor_cnt(CMCC);
+}
+
+#endif
+
+#endif
+
+// Entry points that can be called from ASF C code
+void CacheFlushBeforeDMAReceive(const volatile void *start, size_t length) { Cache::FlushBeforeDMAReceive(start, length); }
+void CacheInvalidateAfterDMAReceive(const volatile void *start, size_t length) { Cache::InvalidateAfterDMAReceive(start, length); }
+void CacheFlushBeforeDMASend(const volatile void *start, size_t length) { Cache::FlushBeforeDMASend(start, length); }
+
+// End
diff --git a/src/Hardware/Cache.h b/src/Hardware/Cache.h
new file mode 100644
index 00000000..4211d189
--- /dev/null
+++ b/src/Hardware/Cache.h
@@ -0,0 +1,54 @@
+/*
+ * Cache.h
+ *
+ * Created on: 22 Nov 2019
+ * Author: David
+ */
+
+#ifndef SRC_HARDWARE_CACHE_H_
+#define SRC_HARDWARE_CACHE_H_
+
+#include <RepRapFirmware.h>
+
+namespace Cache
+{
+ void Init();
+ void Enable();
+ void Disable();
+ void Flush(const volatile void *start, size_t length);
+ void Invalidate(const volatile void *start, size_t length);
+
+ inline void FlushBeforeDMAReceive(const volatile void *start, size_t length) { Flush(start, length); }
+ inline void InvalidateAfterDMAReceive(const volatile void *start, size_t length) { Invalidate(start, length); }
+ inline void FlushBeforeDMASend(const volatile void *start, size_t length) { Flush(start, length); }
+
+#if SAM4E
+ uint32_t GetHitCount();
+#endif
+};
+
+// Entry points that can be called from ASF C code
+extern "C" void CacheFlushBeforeDMAReceive(const volatile void *start, size_t length);
+extern "C" void CacheInvalidateAfterDMAReceive(const volatile void *start, size_t length);
+extern "C" void CacheFlushBeforeDMASend(const volatile void *start, size_t length);
+
+#if USE_CACHE
+
+# if SAM4E
+
+// The SAM4E cache is write-through, so no need to flush it
+inline void Cache::Flush(const volatile void *start, size_t length) { }
+
+# endif
+
+#else
+
+inline void Cache::Init() {}
+inline void Cache::Enable() {}
+inline void Cache::Disable() {}
+inline void Cache::Flush(const void *start, size_t length) {}
+inline void Cache::Invalidate(const void *start, size_t length) {}
+
+#endif
+
+#endif /* SRC_HARDWARE_CACHE_H_ */
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index bb150ccc..b20fedd5 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -12,6 +12,7 @@
#include "StepTimer.h"
#include "Endstops/EndstopsManager.h"
#include "Kinematics/LinearDeltaKinematics.h"
+#include "Tools/Tool.h"
#if SUPPORT_CAN_EXPANSION
# include "CAN/CanMotion.h"
@@ -296,7 +297,7 @@ bool DDA::InitStandardMove(DDARing& ring, const RawMove &nextMove, bool doMotorM
endPoint[drive] = Move::MotorMovementToSteps(drive, nextMove.coords[drive]);
}
- int32_t delta = endPoint[drive] - positionNow[drive];
+ const int32_t delta = endPoint[drive] - positionNow[drive];
if (doMotorMapping)
{
if (drive >= numVisibleAxes)
@@ -307,7 +308,7 @@ bool DDA::InitStandardMove(DDARing& ring, const RawMove &nextMove, bool doMotorM
{
const float positionDelta = endCoordinates[drive] - prev->GetEndCoordinate(drive, false);
directionVector[drive] = positionDelta;
- if (positionDelta != 0.0 && (IsBitSet(nextMove.yAxes, drive) || IsBitSet(nextMove.xAxes, drive)))
+ if (positionDelta != 0.0 && (IsBitSet(Tool::GetXAxes(nextMove.tool), drive) || IsBitSet(Tool::GetYAxes(nextMove.tool), drive)))
{
flags.xyMoving = true;
}
@@ -370,8 +371,7 @@ bool DDA::InitStandardMove(DDARing& ring, const RawMove &nextMove, bool doMotorM
}
// 3. Store some values
- xAxes = nextMove.xAxes;
- yAxes = nextMove.yAxes;
+ tool = nextMove.tool;
flags.checkEndstops = nextMove.checkEndstops;
flags.reduceAcceleration = nextMove.reduceAcceleration;
filePos = nextMove.filePos;
@@ -548,8 +548,7 @@ bool DDA::InitLeadscrewMove(DDARing& ring, float feedrate, const float adjustmen
flags.goingSlow = false;
flags.continuousRotationShortcut = false;
virtualExtruderPosition = prev->virtualExtruderPosition;
- xAxes = prev->xAxes;
- yAxes = prev->yAxes;
+ tool = nullptr;
filePos = prev->filePos;
flags.endCoordinatesValid = prev->flags.endCoordinatesValid;
acceleration = deceleration = reprap.GetPlatform().Accelerations()[Z_AXIS];
@@ -631,8 +630,7 @@ bool DDA::InitAsyncMove(DDARing& ring, const AsyncMove& nextMove)
flags.goingSlow = false;
flags.continuousRotationShortcut = false;
virtualExtruderPosition = 0;
- xAxes = DefaultXAxisMapping;
- yAxes = DefaultYAxisMapping;
+ tool = nullptr;
filePos = noFilePosition;
flags.endCoordinatesValid = false;
@@ -1562,6 +1560,8 @@ float DDA::NormaliseXYZ()
// First calculate the magnitude of the vector. If there is more than one X or Y axis, take an average of their movements (they should be equal).
float xMagSquared = 0.0, yMagSquared = 0.0;
unsigned int numXaxes = 0, numYaxes = 0;
+ const AxesBitmap xAxes = Tool::GetXAxes(tool);
+ const AxesBitmap yAxes = Tool::GetYAxes(tool);
for (size_t d = 0; d < MaxAxes; ++d)
{
if (IsBitSet(xAxes, d))
diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h
index 19e79451..6bcf3a08 100644
--- a/src/Movement/DDA.h
+++ b/src/Movement/DDA.h
@@ -75,9 +75,8 @@ public:
float GetRequestedSpeed() const { return requestedSpeed; }
float GetTopSpeed() const { return topSpeed; }
float GetVirtualExtruderPosition() const { return virtualExtruderPosition; }
- float AdvanceBabyStepping(DDARing& ring, size_t axis, float amount); // Try to push babystepping earlier in the move queue
- uint32_t GetXAxes() const { return xAxes; }
- uint32_t GetYAxes() const { return yAxes; }
+ float AdvanceBabyStepping(DDARing& ring, size_t axis, float amount); // Try to push babystepping earlier in the move queue
+ const Tool *GetTool() const { return tool; }
float GetTotalDistance() const { return totalDistance; }
void LimitSpeedAndAcceleration(float maxSpeed, float maxAcceleration); // Limit the speed an acceleration of this move
@@ -215,29 +214,28 @@ private:
} flags;
#if SUPPORT_LASER || SUPPORT_IOBITS
- LaserPwmOrIoBits laserPwmOrIoBits; // laser PWM required or port state required during this move (here because it is currently 16 bits)
+ LaserPwmOrIoBits laserPwmOrIoBits; // laser PWM required or port state required during this move (here because it is currently 16 bits)
#endif
- AxesBitmap xAxes; // Which axes are behaving as X axes
- AxesBitmap yAxes; // Which axes are behaving as Y axes
+ const Tool *tool; // which tool (if any) is active
- FilePosition filePos; // The position in the SD card file after this move was read, or zero if not read from SD card
+ FilePosition filePos; // The position in the SD card file after this move was read, or zero if not read from SD card
int32_t endPoint[MaxAxesPlusExtruders]; // Machine coordinates of the endpoint
float endCoordinates[MaxAxesPlusExtruders]; // The Cartesian coordinates at the end of the move plus extrusion amounts
float directionVector[MaxAxesPlusExtruders]; // The normalised direction vector - first 3 are XYZ Cartesian coordinates even on a delta
- float totalDistance; // How long is the move in hypercuboid space
- float acceleration; // The acceleration to use
- float deceleration; // The deceleration to use
- float requestedSpeed; // The speed that the user asked for
- float virtualExtruderPosition; // the virtual extruder position at the end of this move, used for pause/resume
+ float totalDistance; // How long is the move in hypercuboid space
+ float acceleration; // The acceleration to use
+ float deceleration; // The deceleration to use
+ float requestedSpeed; // The speed that the user asked for
+ float virtualExtruderPosition; // the virtual extruder position at the end of this move, used for pause/resume
// These vary depending on how we connect the move with its predecessor and successor, but remain constant while the move is being executed
float startSpeed;
float endSpeed;
float topSpeed;
- float proportionLeft; // what proportion of the extrusion in the G1 or G0 move of which this is a part remains to be done after this segment is complete
+ float proportionLeft; // what proportion of the extrusion in the G1 or G0 move of which this is a part remains to be done after this segment is complete
uint32_t clocksNeeded;
union
@@ -247,8 +245,8 @@ private:
{
float accelDistance;
float decelDistance;
- float targetNextSpeed; // The speed that the next move would like to start at, used to keep track of the lookahead without making recursive calls
- float maxAcceleration; // the maximum allowed acceleration for this move according to the limits set by M201
+ float targetNextSpeed; // The speed that the next move would like to start at, used to keep track of the lookahead without making recursive calls
+ float maxAcceleration; // the maximum allowed acceleration for this move according to the limits set by M201
} beforePrepare;
// Values that are not set or accessed before Prepare is called
diff --git a/src/Movement/DDARing.cpp b/src/Movement/DDARing.cpp
index b23e9473..bfc811fe 100644
--- a/src/Movement/DDARing.cpp
+++ b/src/Movement/DDARing.cpp
@@ -607,7 +607,7 @@ bool DDARing::PauseMoves(RestorePoint& rp)
rp.moveCoords[axis] = prevDda->GetEndCoordinate(axis, false);
}
- reprap.GetMove().InverseAxisAndBedTransform(rp.moveCoords, prevDda->GetXAxes(), prevDda->GetYAxes()); // we assume that xAxes hasn't changed between the moves
+ reprap.GetMove().InverseAxisAndBedTransform(rp.moveCoords, prevDda->GetTool());
#if SUPPORT_LASER || SUPPORT_IOBITS
rp.laserPwmOrIoBits = dda->GetLaserPwmOrIoBits();
@@ -710,7 +710,7 @@ bool DDARing::LowPowerOrStallPause(RestorePoint& rp)
rp.moveCoords[axis] = prevDda->GetEndCoordinate(axis, false);
}
- reprap.GetMove().InverseAxisAndBedTransform(rp.moveCoords, prevDda->GetXAxes(), prevDda->GetYAxes()); // we assume that xAxes and yAxes have't changed between the moves
+ reprap.GetMove().InverseAxisAndBedTransform(rp.moveCoords, prevDda->GetTool());
// Free the DDAs for the moves we are going to skip
for (dda = addPointer; dda != savedDdaRingAddPointer; dda = dda->GetNext())
diff --git a/src/Movement/Kinematics/ScaraKinematics.cpp b/src/Movement/Kinematics/ScaraKinematics.cpp
index 9369e820..b590bb58 100644
--- a/src/Movement/Kinematics/ScaraKinematics.cpp
+++ b/src/Movement/Kinematics/ScaraKinematics.cpp
@@ -289,6 +289,8 @@ LimitPositionResult ScaraKinematics::LimitPosition(float finalCoords[], const fl
if (isCoordinated && initialCoords != nullptr)
{
// Calculate how far along the line the closest point of approach to the distal axis is
+ // From maxima, t = -(y0(y1-y0)+x0(x1-x0))/L^2, d^2=((x0y1-x1y0)^2)/L^2
+ // where t is how far from along the line from x0y0 to x1y1 the closest point of approach is (0..1), d is the closest approach distance, and L^2= (x1-x0)^2+(y1-y0)^2
const float xdiff = finalCoords[0] - initialCoords[0];
const float ydiff = finalCoords[1] - initialCoords[1];
const float sumOfSquares = fsquare(xdiff) + fsquare(ydiff);
@@ -477,7 +479,7 @@ void ScaraKinematics::Recalc()
twoPd = proximalArmLength * distalArmLength * 2;
minRadius = max<float>(sqrtf(proximalArmLengthSquared + distalArmLengthSquared
- - twoPd * max<float>(cosf(psiLimits[0] * DegreesToRadians), cosf(psiLimits[1] * DegreesToRadians))) * 1.005,
+ + twoPd * min<float>(cosf(psiLimits[0] * DegreesToRadians), cosf(psiLimits[1] * DegreesToRadians))) * 1.005,
requestedMinRadius);
minRadiusSquared = fsquare(minRadius);
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index 7dd8e642..705fb87a 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -184,7 +184,7 @@ void Move::Spin()
{
if (nextMove.moveType == 0)
{
- AxisAndBedTransform(nextMove.coords, nextMove.xAxes, nextMove.yAxes, true);
+ AxisAndBedTransform(nextMove.coords, nextMove.tool, true);
}
if (mainDDARing.AddStandardMove(nextMove, !IsRawMotorMove(nextMove.moveType)))
@@ -387,7 +387,7 @@ void Move::SetNewPosition(const float positionNow[MaxAxesPlusExtruders], bool do
{
float newPos[MaxAxesPlusExtruders];
memcpy(newPos, positionNow, sizeof(newPos)); // copy to local storage because Transform modifies it
- AxisAndBedTransform(newPos, reprap.GetCurrentXAxes(), reprap.GetCurrentYAxes(), doBedCompensation);
+ AxisAndBedTransform(newPos, reprap.GetCurrentTool(), doBedCompensation);
SetLiveCoordinates(newPos);
SetPositions(newPos);
}
@@ -458,100 +458,99 @@ bool Move::CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorP
return b;
}
-void Move::AxisAndBedTransform(float xyzPoint[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes, bool useBedCompensation) const
+void Move::AxisAndBedTransform(float xyzPoint[MaxAxes], const Tool *tool, bool useBedCompensation) const
{
- AxisTransform(xyzPoint, xAxes, yAxes);
+ AxisTransform(xyzPoint, tool);
if (useBedCompensation)
{
- BedTransform(xyzPoint, xAxes, yAxes);
+ BedTransform(xyzPoint, tool);
}
}
-void Move::InverseAxisAndBedTransform(float xyzPoint[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const
+void Move::InverseAxisAndBedTransform(float xyzPoint[MaxAxes], const Tool *tool) const
{
- InverseBedTransform(xyzPoint, xAxes, yAxes);
- InverseAxisTransform(xyzPoint, xAxes, yAxes);
+ InverseBedTransform(xyzPoint, tool);
+ InverseAxisTransform(xyzPoint, tool);
}
// Do the Axis transform BEFORE the bed transform
-void Move::AxisTransform(float xyzPoint[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const
+void Move::AxisTransform(float xyzPoint[MaxAxes], const Tool *tool) const
{
// Identify the lowest Y axis
- const size_t NumVisibleAxes = reprap.GetGCodes().GetVisibleAxes();
- for (size_t yAxis = Y_AXIS; yAxis < NumVisibleAxes; ++yAxis)
+ const size_t numVisibleAxes = reprap.GetGCodes().GetVisibleAxes();
+ const AxesBitmap xAxes = Tool::GetXAxes(tool);
+ const AxesBitmap yAxes = Tool::GetYAxes(tool);
+ const size_t lowestYAxis = LowestSetBit(yAxes);
+ if (lowestYAxis < numVisibleAxes)
{
- if (IsBitSet(yAxes, yAxis))
+ // Found a Y axis. Use this one when correcting the X coordinate.
+ for (size_t axis = 0; axis < numVisibleAxes; ++axis)
{
- // Found a Y axis. Use this one when correcting the X coordinate.
- for (size_t axis = 0; axis < NumVisibleAxes; ++axis)
+ if (IsBitSet(xAxes, axis))
{
- if (IsBitSet(xAxes, axis))
- {
- xyzPoint[axis] += tanXY*xyzPoint[yAxis] + tanXZ*xyzPoint[Z_AXIS];
- }
- if (IsBitSet(yAxes, axis))
- {
- xyzPoint[axis] += tanYZ*xyzPoint[Z_AXIS];
- }
+ xyzPoint[axis] += tanXY*xyzPoint[lowestYAxis] + tanXZ*xyzPoint[Z_AXIS];
+ }
+ if (IsBitSet(yAxes, axis))
+ {
+ xyzPoint[axis] += tanYZ*xyzPoint[Z_AXIS];
}
- break;
}
}
}
-// Get the height error at an XY position
+// Get the height error at a bed XY position
float Move::GetInterpolatedHeightError(float xCoord, float yCoord) const
{
return (usingMesh) ? heightMap.GetInterpolatedHeightError(xCoord, yCoord) : probePoints.GetInterpolatedHeightError(xCoord, yCoord);
}
// Invert the Axis transform AFTER the bed transform
-void Move::InverseAxisTransform(float xyzPoint[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const
+void Move::InverseAxisTransform(float xyzPoint[MaxAxes], const Tool *tool) const
{
// Identify the lowest Y axis
- const size_t NumVisibleAxes = reprap.GetGCodes().GetVisibleAxes();
- for (size_t yAxis = Y_AXIS; yAxis < NumVisibleAxes; ++yAxis)
+ const size_t numVisibleAxes = reprap.GetGCodes().GetVisibleAxes();
+ const AxesBitmap xAxes = Tool::GetXAxes(tool);
+ const AxesBitmap yAxes = Tool::GetYAxes(tool);
+ const size_t lowestYAxis = LowestSetBit(yAxes);
+ if (lowestYAxis < numVisibleAxes)
{
- if (IsBitSet(yAxes, yAxis))
+ // Found a Y axis. Use this one when correcting the X coordinate.
+ for (size_t axis = 0; axis < numVisibleAxes; ++axis)
{
- // Found a Y axis. Use this one when correcting the X coordinate.
- for (size_t axis = 0; axis < NumVisibleAxes; ++axis)
+ if (IsBitSet(yAxes, axis))
{
- if (IsBitSet(yAxes, axis))
- {
- xyzPoint[axis] -= tanYZ*xyzPoint[Z_AXIS];
- }
- if (IsBitSet(xAxes, axis))
- {
- xyzPoint[axis] -= (tanXY*xyzPoint[yAxis] + tanXZ*xyzPoint[Z_AXIS]);
- }
+ xyzPoint[axis] -= tanYZ*xyzPoint[Z_AXIS];
+ }
+ if (IsBitSet(xAxes, axis))
+ {
+ xyzPoint[axis] -= (tanXY*xyzPoint[lowestYAxis] + tanXZ*xyzPoint[Z_AXIS]);
}
- break;
}
}
}
// Do the bed transform AFTER the axis transform
-void Move::BedTransform(float xyzPoint[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const
+void Move::BedTransform(float xyzPoint[MaxAxes], const Tool *tool) const
{
if (!useTaper || xyzPoint[Z_AXIS] < taperHeight)
{
float zCorrection = 0.0;
const size_t numAxes = reprap.GetGCodes().GetVisibleAxes();
+ const AxesBitmap xAxes = Tool::GetXAxes(tool);
+ const AxesBitmap yAxes = Tool::GetYAxes(tool);
unsigned int numCorrections = 0;
- // Transform the Z coordinate based on the average correction for each axis used as an X axis.
- // We are assuming that the tool Y offsets are small enough to be ignored.
+ // Transform the Z coordinate based on the average correction for each axis used as an X or Y axis.
for (uint32_t xAxis = 0; xAxis < numAxes; ++xAxis)
{
if (IsBitSet(xAxes, xAxis))
{
- const float xCoord = xyzPoint[xAxis];
+ const float xCoord = xyzPoint[xAxis] + Tool::GetOffset(tool, xAxis);
for (uint32_t yAxis = 0; yAxis < numAxes; ++yAxis)
{
if (IsBitSet(yAxes, yAxis))
{
- const float yCoord = xyzPoint[yAxis];
+ const float yCoord = xyzPoint[yAxis] + Tool::GetOffset(tool, yAxis);
zCorrection += GetInterpolatedHeightError(xCoord, yCoord);
++numCorrections;
}
@@ -570,24 +569,25 @@ void Move::BedTransform(float xyzPoint[MaxAxes], AxesBitmap xAxes, AxesBitmap yA
}
// Invert the bed transform BEFORE the axis transform
-void Move::InverseBedTransform(float xyzPoint[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const
+void Move::InverseBedTransform(float xyzPoint[MaxAxes], const Tool *tool) const
{
float zCorrection = 0.0;
const size_t numAxes = reprap.GetGCodes().GetVisibleAxes();
+ const AxesBitmap xAxes = Tool::GetXAxes(tool);
+ const AxesBitmap yAxes = Tool::GetYAxes(tool);
unsigned int numCorrections = 0;
- // Transform the Z coordinate based on the average correction for each axis used as an X axis.
- // We are assuming that the tool Y offsets are small enough to be ignored.
+ // Transform the Z coordinate based on the average correction for each axis used as an X or Y axis.
for (uint32_t xAxis = 0; xAxis < numAxes; ++xAxis)
{
if (IsBitSet(xAxes, xAxis))
{
- const float xCoord = xyzPoint[xAxis];
+ const float xCoord = xyzPoint[xAxis] + Tool::GetOffset(tool, xAxis);
for (uint32_t yAxis = 0; yAxis < numAxes; ++yAxis)
{
if (IsBitSet(yAxes, yAxis))
{
- const float yCoord = xyzPoint[yAxis];
+ const float yCoord = xyzPoint[yAxis] + Tool::GetOffset(tool, yAxis);
zCorrection += GetInterpolatedHeightError(xCoord, yCoord);
++numCorrections;
}
@@ -616,12 +616,12 @@ void Move::InverseBedTransform(float xyzPoint[MaxAxes], AxesBitmap xAxes, AxesBi
}
}
-// Normalise the bed transform to have zero height error at these coordinates
+// Normalise the bed transform to have zero height error at these bed coordinates
void Move::SetZeroHeightError(const float coords[MaxAxes])
{
float tempCoords[MaxAxes];
memcpy(tempCoords, coords, sizeof(tempCoords));
- AxisTransform(tempCoords, DefaultXAxisMapping, DefaultYAxisMapping);
+ AxisTransform(tempCoords, nullptr);
zShift = -GetInterpolatedHeightError(tempCoords[X_AXIS], tempCoords[Y_AXIS]);
}
@@ -756,12 +756,12 @@ bool Move::FinishedBedProbing(int sParam, const StringRef& reply)
}
// Return the transformed machine coordinates
-void Move::GetCurrentUserPosition(float m[MaxAxes], uint8_t moveType, AxesBitmap xAxes, AxesBitmap yAxes) const
+void Move::GetCurrentUserPosition(float m[MaxAxes], uint8_t moveType, const Tool *tool) const
{
GetCurrentMachinePosition(m, IsRawMotorMove(moveType));
if (moveType == 0)
{
- InverseAxisAndBedTransform(m, xAxes, yAxes);
+ InverseAxisAndBedTransform(m, tool);
}
}
diff --git a/src/Movement/Move.h b/src/Movement/Move.h
index b855ea9c..9291d3c5 100644
--- a/src/Movement/Move.h
+++ b/src/Movement/Move.h
@@ -58,10 +58,10 @@ public:
void Exit(); // Shut down
void GetCurrentMachinePosition(float m[MaxAxes], bool disableMotorMapping) const; // Get the current position in untransformed coords
- void GetCurrentUserPosition(float m[MaxAxes], uint8_t moveType, AxesBitmap xAxes, AxesBitmap yAxes) const;
+ void GetCurrentUserPosition(float m[MaxAxes], uint8_t moveType, const Tool *tool) const;
// Return the position (after all queued moves have been executed) in transformed coords
int32_t GetEndPoint(size_t drive) const; // Get the current position of a motor
- void LiveCoordinates(float m[MaxAxesPlusExtruders], AxesBitmap xAxes, AxesBitmap yAxes); // Gives the last point at the end of the last complete DDA transformed to user coords
+ void LiveCoordinates(float m[MaxAxesPlusExtruders], const Tool *tool); // Gives the last point at the end of the last complete DDA transformed to user coords
bool AllMovesAreFinished(); // Is the look-ahead ring empty? Stops more moves being added as well.
void DoLookAhead() __attribute__ ((hot)); // Run the look-ahead procedure
void SetNewPosition(const float positionNow[MaxAxesPlusExtruders], bool doBedCompensation); // Set the current position to be this
@@ -74,11 +74,11 @@ public:
void SetAxisCompensation(unsigned int axis, float tangent); // Set an axis-pair compensation angle
float AxisCompensation(unsigned int axis) const; // The tangent value
void SetIdentityTransform(); // Cancel the bed equation; does not reset axis angle compensation
- void AxisAndBedTransform(float move[], AxesBitmap xAxes, AxesBitmap yAxes, bool useBedCompensation) const;
+ void AxisAndBedTransform(float move[], const Tool *tool, bool useBedCompensation) const;
// Take a position and apply the bed and the axis-angle compensations
- void InverseAxisAndBedTransform(float move[], AxesBitmap xAxes, AxesBitmap yAxes) const;
+ void InverseAxisAndBedTransform(float move[], const Tool *tool) const;
// Go from a transformed point back to user coordinates
- void SetZeroHeightError(const float coords[MaxAxes]); // Set zero height error at these coordinates
+ void SetZeroHeightError(const float coords[MaxAxes]); // Set zero height error at these bed coordinates
float GetTaperHeight() const { return (useTaper) ? taperHeight : 0.0; }
void SetTaperHeight(float h);
bool UseMesh(bool b); // Try to enable mesh bed compensation and report the final state
@@ -200,12 +200,12 @@ private:
timing // no moves being executed or in queue, motors are at full current
};
- void BedTransform(float move[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const; // Take a position and apply the bed compensations
- void InverseBedTransform(float move[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const; // Go from a bed-transformed point back to user coordinates
- void AxisTransform(float move[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const; // Take a position and apply the axis-angle compensations
- void InverseAxisTransform(float move[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const; // Go from an axis transformed point back to user coordinates
+ void BedTransform(float move[MaxAxes], const Tool *tool) const; // Take a position and apply the bed compensations
+ void InverseBedTransform(float move[MaxAxes],const Tool *tool) const; // Go from a bed-transformed point back to user coordinates
+ void AxisTransform(float move[MaxAxes], const Tool *tool) const; // Take a position and apply the axis-angle compensations
+ void InverseAxisTransform(float move[MaxAxes], const Tool *tool) const; // Go from an axis transformed point back to user coordinates
void SetPositions(const float move[MaxAxesPlusExtruders]) { return mainDDARing.SetPositions(move); } // Force the machine coordinates to be these;
- float GetInterpolatedHeightError(float xCoord, float yCoord) const; // Get the height error at an XY position
+ float GetInterpolatedHeightError(float xCoord, float yCoord) const; // Get the height error at an XY position on the bed
DDARing mainDDARing; // The DDA ring used for regular moves
@@ -283,10 +283,10 @@ inline void Move::AdjustMotorPositions(const float adjustment[], size_t numMotor
// Return the current live XYZ and extruder coordinates
// Interrupts are assumed enabled on entry
-inline void Move::LiveCoordinates(float m[MaxAxesPlusExtruders], AxesBitmap xAxes, AxesBitmap yAxes)
+inline void Move::LiveCoordinates(float m[MaxAxesPlusExtruders], const Tool *tool)
{
mainDDARing.LiveCoordinates(m);
- InverseAxisAndBedTransform(m, xAxes, yAxes);
+ InverseAxisAndBedTransform(m, tool);
}
// These are the actual numbers that we want to be the coordinates, so don't transform them.
diff --git a/src/Movement/RawMove.cpp b/src/Movement/RawMove.cpp
index 29dd1c9b..e4232172 100644
--- a/src/Movement/RawMove.cpp
+++ b/src/Movement/RawMove.cpp
@@ -18,8 +18,7 @@ void RawMove::SetDefaults(size_t firstDriveToZero)
reduceAcceleration = false;
hasExtrusion = false;
filePos = noFilePosition;
- xAxes = DefaultXAxisMapping;
- yAxes = DefaultYAxisMapping;
+ tool = nullptr;
for (size_t drive = firstDriveToZero; drive < MaxAxesPlusExtruders; ++drive)
{
coords[drive] = 0.0; // clear extrusion
diff --git a/src/Movement/RawMove.h b/src/Movement/RawMove.h
index b8905fd3..5cd29996 100644
--- a/src/Movement/RawMove.h
+++ b/src/Movement/RawMove.h
@@ -18,8 +18,7 @@ struct RawMove
float virtualExtruderPosition; // the virtual extruder position at the start of this move, for normal moves
FilePosition filePos; // offset in the file being printed at the start of reading this move
float proportionLeft; // what proportion of the entire move remains after this segment
- AxesBitmap xAxes; // axes that X is mapped to
- AxesBitmap yAxes; // axes that Y is mapped to
+ const Tool *tool; // which tool (if any) is being used
#if SUPPORT_LASER || SUPPORT_IOBITS
LaserPwmOrIoBits laserPwmOrIoBits; // the laser PWM or port bit settings required
#endif
diff --git a/src/Movement/StepperDrivers/TMC2660.cpp b/src/Movement/StepperDrivers/TMC2660.cpp
index d3df9553..27ac3045 100644
--- a/src/Movement/StepperDrivers/TMC2660.cpp
+++ b/src/Movement/StepperDrivers/TMC2660.cpp
@@ -14,6 +14,7 @@
#include "Movement/Move.h"
#include "Movement/StepTimer.h"
#include "Endstops/Endstop.h"
+#include "Hardware/Cache.h"
# if SAME70
# include "sam/drivers/xdmac/xdmac.h"
@@ -382,6 +383,8 @@ static TmcDriverState * volatile currentDriver = nullptr; // volatile because th
// SPI sends data MSB first, but the firmware uses little-endian mode, so we need to reverse the byte order
spiDataOut = cpu_to_be32(outVal << 8);
+ Cache::FlushBeforeDMASend(&spiDataOut, sizeof(spiDataOut));
+ Cache::FlushBeforeDMAReceive(&spiDataIn, sizeof(spiDataIn));
spiPdc->PERIPH_TPR = reinterpret_cast<uint32_t>(&spiDataOut);
spiPdc->PERIPH_TCR = 3;
@@ -721,6 +724,7 @@ inline void TmcDriverState::TransferDone()
fastDigitalWriteHigh(pin); // set the CS pin high for the driver we just polled
if (driversPowered) // if the power is still good, update the status
{
+ Cache::InvalidateAfterDMAReceive(&spiDataIn, sizeof(spiDataIn));
uint32_t status = be32_to_cpu(spiDataIn) >> 12; // get the status
const uint32_t interval = reprap.GetMove().GetStepInterval(axisNumber, microstepShiftFactor); // get the full step interval
if (interval == 0 || interval > maxStallStepInterval) // if the motor speed is too low to get reliable stall indication
diff --git a/src/Platform.cpp b/src/Platform.cpp
index d6e67576..b4bae02e 100644
--- a/src/Platform.cpp
+++ b/src/Platform.cpp
@@ -36,6 +36,7 @@
#include "Logger.h"
#include "Tasks.h"
#include "Hardware/DmacManager.h"
+#include "Hardware/Cache.h"
#include "Math/Isqrt.h"
#include "Hardware/I2C.h"
@@ -241,11 +242,11 @@ void Platform::Init()
#if SAM4E || SAM4S || SAME70
memset(uniqueId, 0, sizeof(uniqueId));
- DisableCache();
+ Cache::Disable();
cpu_irq_disable();
const uint32_t rc = flash_read_unique_id(uniqueId, 4);
cpu_irq_enable();
- EnableCache();
+ Cache::Enable();
if (rc == 0)
{
@@ -705,7 +706,7 @@ void Platform::UpdateFirmware()
reprap.EmergencyStop();
// Step 0 - disable the cache because it interferes with flash memory access
- DisableCache();
+ Cache::Disable();
#if USE_MPU
//TODO consider setting flash memory to strongly-ordered instead
@@ -1372,7 +1373,7 @@ void Platform::Spin()
ListDrivers(scratchString.GetRef(), stalledDriversToLog);
stalledDriversToLog = 0;
float liveCoordinates[MaxAxesPlusExtruders];
- reprap.GetMove().LiveCoordinates(liveCoordinates, reprap.GetCurrentXAxes(), reprap.GetCurrentYAxes());
+ reprap.GetMove().LiveCoordinates(liveCoordinates, reprap.GetCurrentTool());
MessageF(WarningMessage, "Driver(s)%s stalled at Z height %.2f", scratchString.c_str(), (double)liveCoordinates[Z_AXIS]);
reported = true;
}
@@ -1581,7 +1582,7 @@ void Platform::SoftwareReset(uint16_t reason, const uint32_t *stk)
rswdt_restart(RSWDT); // kick the secondary watchdog
#endif
- DisableCache(); // disable the cache, it seems to upset flash memory access
+ Cache::Disable();
#if USE_MPU
//TODO set the flash memory to strongly-ordered or device instead
@@ -1863,7 +1864,7 @@ void Platform::Diagnostics(MessageType mtype)
{
#if USE_CACHE && SAM4E
// Get the cache statistics before we start messing around with the cache
- const uint32_t cacheCount = cmcc_get_monitor_cnt(CMCC);
+ const uint32_t cacheCount = Cache::GetHitCount();
#endif
Message(mtype, "=== Platform ===\n");
@@ -1919,9 +1920,9 @@ void Platform::Diagnostics(MessageType mtype)
// Work around bug in ASF flash library: flash_read_user_signature calls a RAMFUNC without disabling interrupts first.
// This caused a crash (watchdog timeout) sometimes if we run M122 while a print is in progress
const irqflags_t flags = cpu_irq_save();
- DisableCache();
+ Cache::Disable();
const uint32_t rc = flash_read_user_signature(reinterpret_cast<uint32_t*>(srdBuf), sizeof(srdBuf)/sizeof(uint32_t));
- EnableCache();
+ Cache::Enable();
cpu_irq_restore(flags);
if (rc == FLASH_RC_OK)
diff --git a/src/RepRap.cpp b/src/RepRap.cpp
index 54dfcc72..9583d427 100644
--- a/src/RepRap.cpp
+++ b/src/RepRap.cpp
@@ -858,7 +858,7 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source)
else
#endif
{
- move->LiveCoordinates(liveCoordinates, GetCurrentXAxes(), GetCurrentYAxes());
+ move->LiveCoordinates(liveCoordinates, currentTool);
}
// Machine coordinates
@@ -1241,7 +1241,7 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source)
// Controllable Fans
FansBitmap controllableFans = 0;
- for (size_t fan = 0; fan < NumTotalFans; fan++)
+ for (size_t fan = 0; fan < MaxFans; fan++)
{
if (fansManager->IsFanControllable(fan))
{
@@ -1344,7 +1344,7 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source)
bool first = true;
for (size_t xi = 0; xi < MaxAxes; ++xi)
{
- if ((tool->GetXAxisMap() & (1u << xi)) != 0)
+ if (IsBitSet(tool->GetXAxisMap(), xi))
{
if (first)
{
@@ -1361,7 +1361,7 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source)
first = true;
for (size_t yi = 0; yi < MaxAxes; ++yi)
{
- if ((tool->GetYAxisMap() & (1u << yi)) != 0)
+ if (IsBitSet(tool->GetYAxisMap(), yi))
{
if (first)
{
@@ -1692,7 +1692,7 @@ OutputBuffer *RepRap::GetLegacyStatusResponse(uint8_t type, int seq)
// Now the machine coordinates
float liveCoordinates[MaxAxesPlusExtruders];
- move->LiveCoordinates(liveCoordinates, GetCurrentXAxes(), GetCurrentYAxes());
+ move->LiveCoordinates(liveCoordinates, currentTool);
response->catf("],\"machine\":"); // announce the machine position
ch = '[';
for (size_t drive = 0; drive < numVisibleAxes; drive++)
@@ -1736,7 +1736,7 @@ OutputBuffer *RepRap::GetLegacyStatusResponse(uint8_t type, int seq)
// Send the fan settings, for PanelDue firmware 1.13 and later
// Currently, PanelDue assumes that the first value is the print cooling fan speed and only uses that one, so send the mapped fan speed first
response->catf(",\"fanPercent\":[%.1f", (double)(gCodes->GetMappedFanSpeed() * 100.0));
- for (size_t i = 0; i < NumTotalFans; ++i)
+ for (size_t i = 0; i < MaxFans; ++i)
{
response->catf(",%.1f", (double)(fansManager->GetFanValue(i) * 100.0));
}
@@ -1745,7 +1745,7 @@ OutputBuffer *RepRap::GetLegacyStatusResponse(uint8_t type, int seq)
// Send fan RPM value(s)
response->cat(",\"fanRPM\":");
ch = '[';
- for (size_t i = 0; i < NumTotalFans; ++i)
+ for (size_t i = 0; i < MaxFans; ++i)
{
response->catf("%c%" PRIi32, ch, fansManager->GetFanRPM(i));
ch = ',';
@@ -2048,7 +2048,7 @@ bool RepRap::GetFileInfoResponse(const char *filename, OutputBuffer *&response,
}
response->catf("\"height\":%.2f,\"firstLayerHeight\":%.2f,\"layerHeight\":%.2f,",
- (double)info.objectHeight, (double)info.firstLayerHeight, (double)info.layerHeight);
+ HideNan(info.objectHeight), HideNan(info.firstLayerHeight), HideNan(info.layerHeight));
if (info.printTime != 0)
{
response->catf("\"printTime\":%" PRIu32 ",", info.printTime);
@@ -2068,7 +2068,7 @@ bool RepRap::GetFileInfoResponse(const char *filename, OutputBuffer *&response,
{
for (size_t i = 0; i < info.numFilaments; ++i)
{
- response->catf("%c%.1f", ch, (double)info.filamentNeeded[i]);
+ response->catf("%c%.1f", ch, HideNan(info.filamentNeeded[i]));
ch = ',';
}
}
@@ -2269,18 +2269,6 @@ GCodeResult RepRap::ClearTemperatureFault(int8_t wasDudHeater, const StringRef&
return rslt;
}
-// Get the current axes used as X axes
-AxesBitmap RepRap::GetCurrentXAxes() const
-{
- return (currentTool == nullptr) ? DefaultXAxisMapping : currentTool->GetXAxisMap();
-}
-
-// Get the current axes used as X axes
-AxesBitmap RepRap::GetCurrentYAxes() const
-{
- return (currentTool == nullptr) ? DefaultYAxisMapping : currentTool->GetYAxisMap();
-}
-
#if HAS_MASS_STORAGE
// Save some resume information, returning true if successful
diff --git a/src/RepRap.h b/src/RepRap.h
index aa9e0811..6fd8a4fa 100644
--- a/src/RepRap.h
+++ b/src/RepRap.h
@@ -27,6 +27,7 @@ Licence: GPL
#include "RTOSIface/RTOSIface.h"
#include "GCodes/GCodeResult.h"
#include "Fans/FansManager.h"
+#include "Tools/Tool.h"
enum class ResponseSource
{
@@ -81,9 +82,9 @@ public:
int GetCurrentToolNumber() const;
Tool* GetTool(int toolNumber) const;
Tool* GetCurrentOrDefaultTool() const;
- const Tool* GetFirstTool() const { return toolList; } // Return the lowest-numbered tool
- AxesBitmap GetCurrentXAxes() const; // Get the current axes used as X axes
- AxesBitmap GetCurrentYAxes() const; // Get the current axes used as Y axes
+ const Tool* GetFirstTool() const { return toolList; } // Return the lowest-numbered tool
+ AxesBitmap GetCurrentXAxes() const { return Tool::GetXAxes(currentTool); } // Get the current axes used as X axes
+ AxesBitmap GetCurrentYAxes() const { return Tool::GetYAxes(currentTool); } // Get the current axes used as Y axes
bool IsHeaterAssignedToTool(int8_t heater) const;
unsigned int GetNumberOfContiguousTools() const;
diff --git a/src/Tasks.cpp b/src/Tasks.cpp
index 5591e129..396fd6e9 100644
--- a/src/Tasks.cpp
+++ b/src/Tasks.cpp
@@ -9,63 +9,12 @@
#include "RepRap.h"
#include "Platform.h"
#include "Storage/CRC32.h"
-
-#include <malloc.h>
+#include "Hardware/Cache.h"
+#include <TaskPriorities.h>
#include "FreeRTOS.h"
#include "task.h"
-#include <TaskPriorities.h>
-
-#if USE_CACHE
-
-# if SAM4E
-# include "cmcc/cmcc.h"
-# endif
-
-# if SAME70
-# include <core_cm7.h>
-
-void EnableCache()
-{
- SCB_EnableICache();
- SCB_EnableDCache();
-}
-
-void DisableCache()
-{
- SCB_DisableICache();
- SCB_DisableDCache();
-}
-
-# endif
-
-#endif
-
-#if SAME70 && USE_MPU
-# include <mpu_armv7.h>
-
-// Macro ARM_MPU_RASR_EX is incorrectly defined in CMSIS 5.4.0, see https://github.com/ARM-software/CMSIS_5/releases. Redefine it here.
-
-# undef ARM_MPU_RASR_EX
-
-/**
-* MPU Region Attribute and Size Register Value
-*
-* \param DisableExec Instruction access disable bit, 1= disable instruction fetches.
-* \param AccessPermission Data access permissions, allows you to configure read/write access for User and Privileged mode.
-* \param AccessAttributes Memory access attribution, see \ref ARM_MPU_ACCESS_.
-* \param SubRegionDisable Sub-region disable field.
-* \param Size Region size of the region to be configured, for example 4K, 8K.
-*/
-# define ARM_MPU_RASR_EX(DisableExec, AccessPermission, AccessAttributes, SubRegionDisable, Size) \
- ((((DisableExec) << MPU_RASR_XN_Pos) & MPU_RASR_XN_Msk) | \
- (((AccessPermission) << MPU_RASR_AP_Pos) & MPU_RASR_AP_Msk) | \
- (((AccessAttributes) & (MPU_RASR_TEX_Msk | MPU_RASR_S_Msk | MPU_RASR_C_Msk | MPU_RASR_B_Msk))) | \
- (((SubRegionDisable) << MPU_RASR_SRD_Pos) & MPU_RASR_SRD_Msk) | \
- (((Size) << MPU_RASR_SIZE_Pos) & MPU_RASR_SIZE_Msk) | \
- (((MPU_RASR_ENABLE_Msk))))
-
-#endif
+#include <malloc.h>
const uint8_t memPattern = 0xA5;
@@ -155,9 +104,9 @@ extern "C" [[noreturn]] void AppMain()
for (unsigned int i = 0; ; ++i)
{
digitalWrite(DiagPin, (i & 1) == 0 && (i & 15) < 6); // turn LED on if count is 0, 2, 4 or 16, 18, 20 etc. otherwise turn it off
- for (unsigned int j = 0; j < 5000; ++j)
+ for (unsigned int j = 0; j < 500; ++j)
{
- delayMicroseconds(100); // delayMicroseconds only works with low values of delay so do 100us at a time
+ delayMicroseconds(1000); // delayMicroseconds only works with low values of delay so do 1ms at a time
}
}
}
@@ -176,74 +125,6 @@ extern "C" [[noreturn]] void AppMain()
SCB->CCR |= SCB_CCR_DIV_0_TRP_Msk;
#if SAME70 && USE_MPU
- // Set up the MPU so that we can have a non-cacheable RAM region, and so that we can trap accesses to non-existent memory
- // Where regions overlap, the region with the highest region number takes priority
- constexpr ARM_MPU_Region_t regionTable[] =
- {
- // Flash memory: read-only, execute allowed, cacheable
- {
- ARM_MPU_RBAR(0, IFLASH_ADDR),
- ARM_MPU_RASR_EX(0u, ARM_MPU_AP_RO, ARM_MPU_ACCESS_NORMAL(ARM_MPU_CACHEP_WB_WRA, ARM_MPU_CACHEP_WB_WRA, 1u), 0u, ARM_MPU_REGION_SIZE_1MB)
- },
- // First 256kb RAM, read-write, cacheable, execute disabled. Parts of this are overridden later.
- {
- ARM_MPU_RBAR(1, IRAM_ADDR),
- ARM_MPU_RASR_EX(1u, ARM_MPU_AP_FULL, ARM_MPU_ACCESS_NORMAL(ARM_MPU_CACHEP_WB_WRA, ARM_MPU_CACHEP_WB_WRA, 1u), 0u, ARM_MPU_REGION_SIZE_256KB)
- },
- // Final 128kb RAM, read-write, cacheable, execute disabled
- {
- ARM_MPU_RBAR(2, IRAM_ADDR + 0x00040000),
- ARM_MPU_RASR_EX(1u, ARM_MPU_AP_FULL, ARM_MPU_ACCESS_NORMAL(ARM_MPU_CACHEP_WB_WRA, ARM_MPU_CACHEP_WB_WRA, 1u), 0u, ARM_MPU_REGION_SIZE_128KB)
- },
- // Non-cachable RAM. This must be before normal RAM because it includes CAN buffers which must be within first 64kb.
- // Read write, execute disabled, non-cacheable
- {
- ARM_MPU_RBAR(3, IRAM_ADDR),
- ARM_MPU_RASR_EX(1u, ARM_MPU_AP_FULL, ARM_MPU_ACCESS_ORDERED, 0, ARM_MPU_REGION_SIZE_64KB)
- },
- // RAMFUNC memory. Read-only (the code has already been written to it), execution allowed. The initialised data memory follows, so it must be RW.
- // 256 bytes is enough at present (check the linker memory map if adding more RAMFUNCs).
- {
- ARM_MPU_RBAR(4, IRAM_ADDR + 0x00010000),
- ARM_MPU_RASR_EX(0u, ARM_MPU_AP_FULL, ARM_MPU_ACCESS_NORMAL(ARM_MPU_CACHEP_WB_WRA, ARM_MPU_CACHEP_WB_WRA, 1u), 0u, ARM_MPU_REGION_SIZE_256B)
- },
- // Peripherals
- {
- ARM_MPU_RBAR(5, 0x40000000),
- ARM_MPU_RASR_EX(1u, ARM_MPU_AP_FULL, ARM_MPU_ACCESS_DEVICE(1u), 0u, ARM_MPU_REGION_SIZE_16MB)
- },
- // USBHS
- {
- ARM_MPU_RBAR(6, 0xA0100000),
- ARM_MPU_RASR_EX(1u, ARM_MPU_AP_FULL, ARM_MPU_ACCESS_DEVICE(1u), 0u, ARM_MPU_REGION_SIZE_1MB)
- },
- // ROM
- {
- ARM_MPU_RBAR(7, IROM_ADDR),
- ARM_MPU_RASR_EX(0u, ARM_MPU_AP_RO, ARM_MPU_ACCESS_NORMAL(ARM_MPU_CACHEP_WB_WRA, ARM_MPU_CACHEP_WB_WRA, 1u), 0u, ARM_MPU_REGION_SIZE_4MB)
- },
- // ARM Private Peripheral Bus
- {
- ARM_MPU_RBAR(8, 0xE0000000),
- ARM_MPU_RASR_EX(1u, ARM_MPU_AP_FULL, ARM_MPU_ACCESS_ORDERED, 0u, ARM_MPU_REGION_SIZE_1MB)
- }
- };
-
- // Ensure MPU is disabled
- ARM_MPU_Disable();
-
- // Clear all regions
- const uint32_t numRegions = (MPU->TYPE & MPU_TYPE_DREGION_Msk) >> MPU_TYPE_DREGION_Pos;
- for (unsigned int region = 0; region < numRegions; ++region)
- {
- ARM_MPU_ClrRegion(region);
- }
-
- // Load regions from our table
- ARM_MPU_Load(regionTable, ARRAY_SIZE(regionTable));
-
- // Enable the MPU, disabling the default map but allowing exception handlers to use it
- ARM_MPU_Enable(0x01);
#endif
#ifdef __LPC17xx__
@@ -263,21 +144,12 @@ extern "C" [[noreturn]] void AppMain()
RSTC->RSTC_MR = RSTC_MR_KEY_PASSWD | RSTC_MR_URSTEN;
#endif
-#if USE_CACHE
-# if SAM4E
- // Initialise the cache controller
- cmcc_config g_cmcc_cfg;
- cmcc_get_config_defaults(&g_cmcc_cfg);
- cmcc_init(CMCC, &g_cmcc_cfg);
-#endif
- EnableCache();
-#endif
+ Cache::Init();
+ Cache::Enable();
#if SAM4S
efc_enable_cloe(EFC0); // enable code loop optimisation
-#endif
-
-#if SAM4E || SAME70
+#elif SAM4E || SAME70
efc_enable_cloe(EFC); // enable code loop optimisation
#endif
diff --git a/src/Tasks.h b/src/Tasks.h
index d369258f..43e06cba 100644
--- a/src/Tasks.h
+++ b/src/Tasks.h
@@ -12,37 +12,6 @@
#include "MessageType.h"
#include "RTOSIface/RTOSIface.h"
-#if USE_CACHE
-
-# if SAM4E
-# include "sam/drivers/cmcc/cmcc.h"
-
-inline void EnableCache()
-{
- cmcc_invalidate_all(CMCC);
- cmcc_enable(CMCC);
-}
-
-inline void DisableCache()
-{
- cmcc_disable(CMCC);
-}
-
-# elif SAME70
-
-void EnableCache();
-void DisableCache();
-
-# endif
-
-#else
-
-inline void EnableCache() {}
-inline void DisableCache() {}
-
-#endif
-
-
namespace Tasks
{
void Diagnostics(MessageType mtype);
diff --git a/src/Tools/Tool.cpp b/src/Tools/Tool.cpp
index 521178f1..324755fd 100644
--- a/src/Tools/Tool.cpp
+++ b/src/Tools/Tool.cpp
@@ -158,6 +158,21 @@ Tool * Tool::freelist = nullptr;
}
}
+/*static*/ AxesBitmap Tool::GetXAxes(const Tool *tool)
+{
+ return (tool == nullptr) ? DefaultXAxisMapping : tool->xMapping;
+}
+
+/*static*/ AxesBitmap Tool::GetYAxes(const Tool *tool)
+{
+ return (tool == nullptr) ? DefaultYAxisMapping : tool->yMapping;
+}
+
+/*static*/ float Tool::GetOffset(const Tool *tool, size_t axis)
+{
+ return (tool == nullptr) ? 0.0 : tool->offset[axis];
+}
+
void Tool::Print(const StringRef& reply) const
{
reply.printf("Tool %u - ", myNumber);
@@ -220,7 +235,7 @@ void Tool::Print(const StringRef& reply) const
reply.cat("; fans:");
sep = ' ';
- for (size_t fi = 0; fi < NumTotalFans; ++fi)
+ for (size_t fi = 0; fi < MaxFans; ++fi)
{
if ((fanMapping & (1u << fi)) != 0)
{
diff --git a/src/Tools/Tool.h b/src/Tools/Tool.h
index 21f92cfd..f8e37e75 100644
--- a/src/Tools/Tool.h
+++ b/src/Tools/Tool.h
@@ -49,6 +49,9 @@ public:
static Tool *Create(unsigned int toolNumber, const char *name, int32_t d[], size_t dCount, int32_t h[], size_t hCount, AxesBitmap xMap, AxesBitmap yMap, FansBitmap fanMap, int filamentDrive, const StringRef& reply);
static void Delete(Tool *t);
+ static AxesBitmap GetXAxes(const Tool *tool);
+ static AxesBitmap GetYAxes(const Tool *tool);
+ static float GetOffset(const Tool *tool, size_t axis) pre(axis < MaxAxes);
float GetOffset(size_t axis) const pre(axis < MaxAxes);
void SetOffset(size_t axis, float offs, bool byProbing) pre(axis < MaxAxes);
diff --git a/src/Version.h b/src/Version.h
index 9c8fb537..110b16f6 100644
--- a/src/Version.h
+++ b/src/Version.h
@@ -20,7 +20,7 @@
#endif
#ifndef DATE
-# define DATE "2019-11-15b1"
+# define DATE "2019-11-25b1"
#endif
#define AUTHORS "reprappro, dc42, chrishamm, t3p3, dnewman, printm3d"