Store average accelerometer readings in Object Model

src/Accelerometers/Accelerometers.cpp

@@ -16,6 +16,7 @@
 #include <RTOSIface/RTOSIface.h>
 #include <Platform/TaskPriorities.h>
 #include <Hardware/Spi/SharedSpiDevice.h>
+#include <RRF3Common.h>
 
 #if SUPPORT_CAN_EXPANSION
 # include <CanMessageFormats.h>
@@ -68,6 +69,7 @@ static volatile uint32_t numSamplesRequested;
 static uint8_t resolution = DefaultResolution;
 static uint8_t orientation = 20;							// +Z -> +Z, +X -> +X
 static volatile uint8_t axesRequested;
+static float lastRunAverages[NumAccelerometerAxes] = {0.0f, 0.0f, 0.0f};
 static FileStore* volatile accelerometerFile = nullptr;		// this is non-null when the accelerometer is running, null otherwise
 static unsigned int numLocalRunsCompleted = 0;
 static unsigned int lastRunNumSamplesReceived = 0;
@@ -80,17 +82,32 @@ static IoPort spiCsPort;
 static IoPort irqPort;
 
 // Add a local accelerometer run
-static void AddLocalAccelerometerRun(unsigned int numDataPoints) noexcept
+static void AddLocalAccelerometerRun(unsigned int numDataPoints, float averages[]) noexcept
 {
 	lastRunNumSamplesReceived = numDataPoints;
 	++numLocalRunsCompleted;
+
+	memcpyf(lastRunAverages, averages, NumAccelerometerAxes);
+
+	reprap.BoardsUpdated();
+}
+
+// Add a local failed accelerometer run
+static void AddLocalFailedAccelerometerRun() noexcept
+{
+	lastRunNumSamplesReceived = 0;
+	++numLocalRunsCompleted;
+
+	// reset the last run averages
+	for (float& f : lastRunAverages) { f = 0.0f; }
+
 	reprap.BoardsUpdated();
 }
 
 static uint8_t TranslateAxes(uint8_t axes) noexcept
 {
 	uint8_t rslt = 0;
-	for (unsigned int i = 0; i < 3; ++i)
+	for (unsigned int i = 0; i < NumAccelerometerAxes; ++i)
 	{
 		if (axes & (1u << i))
 		{
@@ -115,6 +132,7 @@ static uint8_t TranslateAxes(uint8_t axes) noexcept
 			const uint16_t mask = (1u << resolution) - 1;
 			const int decimalPlaces = GetDecimalPlaces(resolution);
 			bool recordFailedStart = false;
+			float accumulatedSamples[NumAccelerometerAxes] = {0.0f, 0.0f, 0.0f};
 
 			if (accelerometer->StartCollecting(TranslateAxes(axesRequested)))
 			{
@@ -133,7 +151,7 @@ static uint8_t TranslateAxes(uint8_t axes) noexcept
 						f->Truncate();				// truncate the file in case we didn't write all the preallocated space
 						f->Close();
 						f = nullptr;
-						AddLocalAccelerometerRun(0);
+						AddLocalFailedAccelerometerRun();
 					}
 					else
 					{
@@ -158,7 +176,7 @@ static uint8_t TranslateAxes(uint8_t axes) noexcept
 							String<StringLength50> temp;
 							temp.printf("%u", samplesWritten);
 
-							for (unsigned int axis = 0; axis < 3; ++axis)
+							for (unsigned int axis = 0; axis < NumAccelerometerAxes; ++axis)
 							{
 								if (axesRequested & (1u << axis))
 								{
@@ -180,6 +198,9 @@ static uint8_t TranslateAxes(uint8_t axes) noexcept
 
 									// Append it to the buffer
 									temp.catf(",%.*f", decimalPlaces, (double)fVal);
+
+									// accumulate the values so they can be averaged later
+									accumulatedSamples[axis] += fVal;
 								}
 							}
 
@@ -215,7 +236,11 @@ static uint8_t TranslateAxes(uint8_t axes) noexcept
 			{
 				f->Truncate();				// truncate the file in case we didn't write all the preallocated space
 				f->Close();
-				AddLocalAccelerometerRun(samplesWritten);
+
+				// find the average value for each axis
+				for (float& sample : accumulatedSamples) { sample /= float(samplesWritten); }
+
+				AddLocalAccelerometerRun(samplesWritten, accumulatedSamples);
 			}
 
 			accelerometer->StopCollecting();
@@ -457,12 +482,12 @@ GCodeResult Accelerometers::StartAccelerometer(GCodeBuffer& gb, const StringRef&
 # if SUPPORT_CAN_EXPANSION
 		if (device.IsRemote())
 		{
-			reprap.GetExpansion().AddAccelerometerRun(device.boardAddress, 0);
+			reprap.GetExpansion().AddFailedAccelerometerRun(device.boardAddress);
 		}
 		else
 # endif
 		{
-			AddLocalAccelerometerRun(0);
+			AddLocalFailedAccelerometerRun();
 		}
 		return GCodeResult::error;
 	}
@@ -493,7 +518,7 @@ GCodeResult Accelerometers::StartAccelerometer(GCodeBuffer& gb, const StringRef&
 			accelerometerFile->Close();
 			accelerometerFile = nullptr;
 			MassStorage::Delete(accelerometerFileName.c_str(), false);
-			reprap.GetExpansion().AddAccelerometerRun(device.boardAddress, 0);
+			reprap.GetExpansion().AddFailedAccelerometerRun(device.boardAddress);
 		}
 		return rslt;
 	}
@@ -532,6 +557,11 @@ bool Accelerometers::HasLocalAccelerometer() noexcept
 	return accelerometer != nullptr;
 }
 
+float Accelerometers::GetLocalAccelerometerLastRunAverage(const int &axis) noexcept
+{
+	return lastRunAverages[axis];
+}
+
 unsigned int Accelerometers::GetLocalAccelerometerDataPoints() noexcept
 {
 	return lastRunNumSamplesReceived;
@@ -572,15 +602,15 @@ void Accelerometers::ProcessReceivedData(CanAddress src, const CanMessageAcceler
 			f->Truncate();				// truncate the file in case we didn't write all the preallocated space
 			f->Close();
 			accelerometerFile = nullptr;
-			reprap.GetExpansion().AddAccelerometerRun(src, 0);
+			reprap.GetExpansion().AddFailedAccelerometerRun(src);
 		}
 		else if (msg.axes != expectedRemoteAxes || msg.firstSampleNumber != expectedRemoteSampleNumber || src != expectedRemoteBoardAddress)
 		{
 			f->Write("Received mismatched data\n");
 			f->Truncate();				// truncate the file in case we didn't write all the preallocated space
 			f->Close();
 			accelerometerFile = nullptr;
-			reprap.GetExpansion().AddAccelerometerRun(src, 0);
+			reprap.GetExpansion().AddFailedAccelerometerRun(src);
 		}
 		else
 		{
@@ -592,6 +622,8 @@ void Accelerometers::ProcessReceivedData(CanAddress src, const CanMessageAcceler
 			const unsigned int receivedResolution = msg.bitsPerSampleMinusOne + 1;
 			const uint16_t mask = (1u << receivedResolution) - 1;
 			const int decimalPlaces = GetDecimalPlaces(receivedResolution);
+			float accumulatedSamples[NumAccelerometerAxes] = {0.0f, 0.0f, 0.0f};
+
 			if (msg.overflowed)
 			{
 				++numRemoteOverflows;
@@ -632,6 +664,9 @@ void Accelerometers::ProcessReceivedData(CanAddress src, const CanMessageAcceler
 
 					// Append it to the buffer
 					temp.catf(",%.*f", decimalPlaces, (double)fVal);
+
+					// accumulate the values so they can be averaged later
+					accumulatedSamples[axis] += fVal;
 				}
 
 				temp.cat('\n');
@@ -647,7 +682,11 @@ void Accelerometers::ProcessReceivedData(CanAddress src, const CanMessageAcceler
 				f->Truncate();				// truncate the file in case we didn't write all the preallocated space
 				f->Close();
 				accelerometerFile = nullptr;
-				reprap.GetExpansion().AddAccelerometerRun(src, expectedRemoteSampleNumber);
+
+				// find the average value for each axis
+				for (float& sample : accumulatedSamples) { sample /= float(msg.numSamples); }
+
+				reprap.GetExpansion().AddAccelerometerRun(src, expectedRemoteSampleNumber, accumulatedSamples);
 			}
 		}
 	}

src/Accelerometers/Accelerometers.h

@@ -23,6 +23,7 @@ class CanMessageAccelerometerData;
 namespace Accelerometers
 {
 	bool HasLocalAccelerometer() noexcept;
+	float GetLocalAccelerometerLastRunAverage(const int& axis) noexcept;
 	unsigned int GetLocalAccelerometerRuns() noexcept;
 	unsigned int GetLocalAccelerometerDataPoints() noexcept;
 	GCodeResult ConfigureAccelerometer(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);

src/CAN/ExpansionManager.cpp

@@ -24,6 +24,18 @@
 #define OBJECT_MODEL_FUNC(...) OBJECT_MODEL_FUNC_BODY(ExpansionManager, __VA_ARGS__)
 #define OBJECT_MODEL_FUNC_IF(...) OBJECT_MODEL_FUNC_IF_BODY(ExpansionManager, __VA_ARGS__)
 
+constexpr ObjectModelArrayTableEntry ExpansionManager::objectModelArrayTable[] =
+{
+	{
+		nullptr,					// no lock needed
+		[] (const ObjectModel *self, const ObjectExplorationContext& context) noexcept -> size_t { return NumAccelerometerAxes; },
+		[] (const ObjectModel *self, ObjectExplorationContext& context) noexcept -> ExpressionValue
+				{ return ExpressionValue(((const ExpansionManager*)self)->FindIndexedBoard(context.GetIndex(1)).accelerometerLastRunAverages[context.GetLastIndex()]); }
+	}
+};
+
+DEFINE_GET_OBJECT_MODEL_ARRAY_TABLE(ExpansionManager)
+
 constexpr ObjectModelTableEntry ExpansionManager::objectModelTable[] =
 {
 	// 0. boards[] members
@@ -59,6 +71,7 @@ constexpr ObjectModelTableEntry ExpansionManager::objectModelTable[] =
 	{ "min",				OBJECT_MODEL_FUNC(self->FindIndexedBoard(context.GetLastIndex()).v12.minimum, 1),								ObjectModelEntryFlags::none },
 
 	// 4. accelerometer members
+	{ "lastRunAverages",	OBJECT_MODEL_FUNC_ARRAY(0),																						ObjectModelEntryFlags::none },
 	{ "points",				OBJECT_MODEL_FUNC((int32_t)self->FindIndexedBoard(context.GetLastIndex()).accelerometerLastRunDataPoints),		ObjectModelEntryFlags::none },
 	{ "runs",				OBJECT_MODEL_FUNC((int32_t)self->FindIndexedBoard(context.GetLastIndex()).accelerometerRuns),					ObjectModelEntryFlags::none },
 
@@ -74,7 +87,7 @@ constexpr uint8_t ExpansionManager::objectModelTableDescriptor[] =
 	3,				// section 1: mcuTemp
 	3,				// section 2: vIn
 	3,				// section 3: v12
-	2,				// section 4: accelerometer
+	3,				// section 4: accelerometer
 	2				// section 5: closed loop
 };
 
@@ -294,10 +307,24 @@ void ExpansionManager::UpdateFailed(CanAddress address) noexcept
 	UpdateBoardState(address, BoardState::flashFailed);
 }
 
-void ExpansionManager::AddAccelerometerRun(CanAddress address, unsigned int numDataPoints) noexcept
+void ExpansionManager::AddAccelerometerRun(CanAddress address, unsigned int numDataPoints, float averages[]) noexcept
 {
 	boards[address].accelerometerLastRunDataPoints = numDataPoints;
 	++boards[address].accelerometerRuns;
+
+	memcpyf(boards[address].accelerometerLastRunAverages, averages, NumAccelerometerAxes);
+
+	reprap.BoardsUpdated();
+}
+
+void ExpansionManager::AddFailedAccelerometerRun(CanAddress address) noexcept
+{
+	boards[address].accelerometerLastRunDataPoints = 0;
+	++boards[address].accelerometerRuns;
+
+	// reset the averages to 0.0f
+	for (float& f : boards[address].accelerometerLastRunAverages) { f = 0.0f; }
+
 	reprap.BoardsUpdated();
 }
 

src/CAN/ExpansionManager.h

@@ -26,6 +26,7 @@ struct ExpansionBoardData
 
 	const char *_ecv_array typeName;
 	MinCurMax mcuTemp, vin, v12;
+	float accelerometerLastRunAverages[NumAccelerometerAxes];
 	uint32_t accelerometerLastRunDataPoints;
 	uint32_t closedLoopLastRunDataPoints;
 	UniqueId uniqueId;
@@ -58,14 +59,15 @@ class ExpansionManager INHERIT_OBJECT_MODEL
 
 	void UpdateFinished(CanAddress address) noexcept;
 	void UpdateFailed(CanAddress address) noexcept;
-	void AddAccelerometerRun(CanAddress address, unsigned int numDataPoints) noexcept;
+	void AddAccelerometerRun(CanAddress address, unsigned int numDataPoints, float averages[]) noexcept;
+	void AddFailedAccelerometerRun(CanAddress address) noexcept;
 	void AddClosedLoopRun(CanAddress address, unsigned int numDataPoints) noexcept;
 	bool IsFlashing() const noexcept { return numBoardsFlashing != 0; }
 
 	void EmergencyStop() noexcept;
 
 protected:
-	DECLARE_OBJECT_MODEL
+	DECLARE_OBJECT_MODEL_WITH_ARRAYS
 
 private:
 	const ExpansionBoardData& FindIndexedBoard(unsigned int index) const noexcept;

src/Platform/Platform.cpp

@@ -219,6 +219,13 @@ constexpr ObjectModelArrayTableEntry Platform::objectModelArrayTable[] =
 		[] (const ObjectModel *self, const ObjectExplorationContext& context) noexcept -> size_t { return NumCoordinateSystems; },
 		[] (const ObjectModel *self, ObjectExplorationContext& context) noexcept -> ExpressionValue
 				{ return ExpressionValue(reprap.GetGCodes().GetWorkplaceOffset(context.GetIndex(1), context.GetLastIndex()), 3); }
+	},
+	// 2. Average readings from last accelerometer run
+	{
+		nullptr,					// no lock needed
+		[] (const ObjectModel *self, const ObjectExplorationContext& context) noexcept -> size_t { return NumAccelerometerAxes; },
+		[] (const ObjectModel *self, ObjectExplorationContext& context) noexcept -> ExpressionValue
+				{ return ExpressionValue((float)Accelerometers::GetLocalAccelerometerLastRunAverage(context.GetLastIndex())); }
 	}
 };
 
@@ -357,6 +364,7 @@ constexpr ObjectModelTableEntry Platform::objectModelTable[] =
 
 #if SUPPORT_ACCELEROMETERS
 	// 9. boards[0].accelerometer members
+	{ "lastRunAverages",	OBJECT_MODEL_FUNC_ARRAY(2),																					ObjectModelEntryFlags::none },
 	{ "points",				OBJECT_MODEL_FUNC_NOSELF((int32_t)Accelerometers::GetLocalAccelerometerDataPoints()),						ObjectModelEntryFlags::none },
 	{ "runs",				OBJECT_MODEL_FUNC_NOSELF((int32_t)Accelerometers::GetLocalAccelerometerRuns()),								ObjectModelEntryFlags::none },
 #endif
@@ -393,7 +401,7 @@ constexpr uint8_t Platform::objectModelTableDescriptor[] =
 	2,																		// section 7: move.axes[].microstepping
 	2,																		// section 8: move.extruders[].microstepping
 #if SUPPORT_ACCELEROMETERS
-	2,																		// section 9: boards[0].accelerometer
+	3,																		// section 9: boards[0].accelerometer
 #else
 	0,
 #endif