Added Q parameter to M955 command

3 files changed, 7 insertions, 5 deletions

diff --git a/src/Accelerometers/Accelerometers.cpp b/src/Accelerometers/Accelerometers.cpp
index 5465e61e..4a006b7f 100644
--- a/src/Accelerometers/Accelerometers.cpp
+++ b/src/Accelerometers/Accelerometers.cpp
@@ -28,6 +28,8 @@
 # include <Duet3Ate.h>
 #endif
 
+constexpr uint32_t DefaultAccelerometerSpiFrequency = 2000000;
+
 // Get the number of binary digits after the decimal point
 static inline unsigned int GetBitsAfterPoint(uint8_t dataResolution) noexcept
 {
@@ -415,7 +417,8 @@ GCodeResult Accelerometers::ConfigureAccelerometer(GCodeBuffer& gb, const String
 			return GCodeResult::error;
 		}
 
-		temp = new LIS3DH(SharedSpiDevice::GetMainSharedSpiDevice(), spiCsPort.GetPin(), irqPort.GetPin());
+		const uint32_t spiFrequency = (gb.Seen('Q')) ? gb.GetUIValue() : DefaultAccelerometerSpiFrequency;
+		temp = new LIS3DH(SharedSpiDevice::GetMainSharedSpiDevice(), spiFrequency, spiCsPort.GetPin(), irqPort.GetPin());
 		if (temp->CheckPresent())
 		{
 			accelerometer = temp;
diff --git a/src/Accelerometers/LIS3DH.cpp b/src/Accelerometers/LIS3DH.cpp
index 6a4275e5..d4e22502 100644
--- a/src/Accelerometers/LIS3DH.cpp
+++ b/src/Accelerometers/LIS3DH.cpp
@@ -15,13 +15,12 @@
 constexpr uint32_t Lis3dSpiTimeout = 25;							// timeout while waiting for the SPI bus
 constexpr uint32_t DataCollectionTimeout = (1000 * 32)/400 + 2;		// timeout whole collecting data, enough to fill the FIFO at 400Hz
 constexpr uint8_t FifoInterruptLevel = 20;							// how full the FIFO must get before we want an interrupt
-constexpr uint32_t SpiFrequency = 4000000;
 const SpiMode lisMode = SpiMode::mode3;
 
 static constexpr uint8_t WhoAmIValue = 0x33;
 
-LIS3DH::LIS3DH(SharedSpiDevice& dev, Pin p_csPin, Pin p_int1Pin) noexcept
-	: SharedSpiClient(dev, SpiFrequency, lisMode, p_csPin, false), taskWaiting(nullptr), int1Pin(p_int1Pin)
+LIS3DH::LIS3DH(SharedSpiDevice& dev, uint32_t freq, Pin p_csPin, Pin p_int1Pin) noexcept
+	: SharedSpiClient(dev, freq, lisMode, p_csPin, false), taskWaiting(nullptr), int1Pin(p_int1Pin)
 {
 }
 
diff --git a/src/Accelerometers/LIS3DH.h b/src/Accelerometers/LIS3DH.h
index d3cd24f5..af4a122c 100644
--- a/src/Accelerometers/LIS3DH.h
+++ b/src/Accelerometers/LIS3DH.h
@@ -17,7 +17,7 @@
 class LIS3DH : public SharedSpiClient
 {
 public:
-	LIS3DH(SharedSpiDevice& dev, Pin p_csPin, Pin p_int1Pin) noexcept;
+	LIS3DH(SharedSpiDevice& dev, uint32_t freq, Pin p_csPin, Pin p_int1Pin) noexcept;
 
 	// Do a quick test to check whether the accelerometer is present, returning true if it is
 	bool CheckPresent() noexcept;