Accelerometer fixes

2 files changed, 31 insertions, 19 deletions

diff --git a/src/Accelerometers/Accelerometers.cpp b/src/Accelerometers/Accelerometers.cpp
index b44b08d5..1f44fd52 100644
--- a/src/Accelerometers/Accelerometers.cpp
+++ b/src/Accelerometers/Accelerometers.cpp
@@ -19,7 +19,7 @@ static unsigned int expectedSampleNumber;
 static CanAddress currentBoard = CanId::NoAddress;
 static uint8_t axes;
 
-void Accelerometers::ProcessReceivedData(CanAddress src, const CanMessageAccelerometerData& msg) noexcept
+void Accelerometers::ProcessReceivedData(CanAddress src, const CanMessageAccelerometerData& msg, size_t msgLen) noexcept
 {
 	if (msg.firstSampleNumber == 0)
 	{
@@ -28,6 +28,7 @@ void Accelerometers::ProcessReceivedData(CanAddress src, const CanMessageAcceler
 		{
 			f->Write("Data incomplete\n");
 			f->Close();
+			f = nullptr;
 		}
 
 		Platform& p = reprap.GetPlatform();
@@ -35,50 +36,57 @@ void Accelerometers::ProcessReceivedData(CanAddress src, const CanMessageAcceler
 		tm timeInfo;
 		gmtime_r(&time, &timeInfo);
 		String<StringLength50> temp;
-		temp.printf("accelerometer/%u_%04u-%02u-%02u_%02u.%02u.%02u.csv",
+		temp.printf("0:/sys/accelerometer/%u_%04u-%02u-%02u_%02u.%02u.%02u.csv",
 						(unsigned int)src, timeInfo.tm_year + 1900, timeInfo.tm_mon + 1, timeInfo.tm_mday, timeInfo.tm_hour, timeInfo.tm_min, timeInfo.tm_sec);
-		f = p.OpenSysFile(temp.c_str(), OpenMode::write);
-
+		f = MassStorage::OpenFile(temp.c_str(), OpenMode::write, 0);
 		if (f != nullptr)
 		{
 			currentBoard = src;
 			axes = msg.axes;
 			expectedSampleNumber = 0;
-			temp.printf("Sample,");
-			if (axes & 1u) { temp.cat("X,"); }
-			if (axes & 2u) { temp.cat("Y,"); }
-			if (axes & 4u) { temp.cat("Z,"); }
-			temp.cat("Overflowed\n");
+			temp.printf("Sample,Rate,Overflowed");
+			if (axes & 1u) { temp.cat(",X"); }
+			if (axes & 2u) { temp.cat(",Y"); }
+			if (axes & 4u) { temp.cat(",Z"); }
+			temp.cat('\n');
 			f->Write(temp.c_str());
 		}
 	}
 
 	if (f != nullptr)
 	{
-		if (msg.axes != axes || msg.firstSampleNumber != expectedSampleNumber || src != currentBoard)
+		if (msgLen < msg.GetActualDataLength())
+		{
+			f->Write("Received bad data\n");
+			f->Close();
+			f = nullptr;
+		}
+		else if (msg.axes != axes || msg.firstSampleNumber != expectedSampleNumber || src != currentBoard)
 		{
 			f->Write("Received mismatched data\n");
 			f->Close();
+			f = nullptr;
 		}
 		else
 		{
 			unsigned int numSamples = msg.numSamples;
-			//TODO check that the message is long enough to hold this number of samples
-			unsigned int numAxes = (axes & 1u) + ((axes & 2u) >> 1) + ((axes & 4u) >> 2);
+			const unsigned int numAxes = (axes & 1u) + ((axes >> 1) & 1u) + ((axes >> 2) & 1u);
 			size_t dataIndex = 0;
 			uint16_t currentBits = 0;
 			unsigned int bitsLeft = 0;
-			unsigned int resolution = msg.bitsPerSampleMinusOne + 1;
-			uint16_t mask = (1u << resolution) - 1;
+			const unsigned int resolution = msg.bitsPerSampleMinusOne + 1;
+			const uint16_t mask = (1u << resolution) - 1;
+			const unsigned int bitsAfterPoint = resolution - 2;					// assumes the range is +/- 2g
+			const int decimalPlaces = (bitsAfterPoint >= 11) ? 4 : (bitsAfterPoint >= 8) ? 3 : 2;
 			while (numSamples != 0)
 			{
 				String<StringLength50> temp;
-				temp.printf("%u,", expectedSampleNumber);
+				temp.printf("%u,%u,%u", expectedSampleNumber, msg.actualSampleRate, msg.overflowed);
 				++expectedSampleNumber;
 
 				for (unsigned int axis = 0; axis < numAxes; ++axis)
 				{
-					// Extract one value from the message
+					// Extract one value from the message. A value spans at most two words in the buffer.
 					uint16_t val = currentBits;
 					if (bitsLeft >= resolution)
 					{
@@ -100,11 +108,14 @@ void Accelerometers::ProcessReceivedData(CanAddress src, const CanMessageAcceler
 						val |= ~mask;
 					}
 
+					// Convert it to a float number of g
+					const float fVal = (float)(int16_t)val/(float)(1u << bitsAfterPoint);
+
 					// Append it to the buffer
-					temp.catf("%d,", (int16_t)val);
+					temp.catf(",%.*f", decimalPlaces, (double)fVal);
 				}
 
-				temp.cat((msg.overflowed) ? "1\n" : "0\n");
+				temp.cat('\n');
 				f->Write(temp.c_str());
 				--numSamples;
 			}
@@ -112,6 +123,7 @@ void Accelerometers::ProcessReceivedData(CanAddress src, const CanMessageAcceler
 		if (msg.lastPacket)
 		{
 			f->Close();
+			f = nullptr;
 		}
 	}
 }
diff --git a/src/Accelerometers/Accelerometers.h b/src/Accelerometers/Accelerometers.h
index 96090952..665a7075 100644
--- a/src/Accelerometers/Accelerometers.h
+++ b/src/Accelerometers/Accelerometers.h
@@ -18,7 +18,7 @@ class CanMessageAccelerometerData;
 
 namespace Accelerometers
 {
-	void ProcessReceivedData(CanAddress src, const CanMessageAccelerometerData& msg) noexcept;
+	void ProcessReceivedData(CanAddress src, const CanMessageAccelerometerData& msg, size_t msgLen) noexcept;
 }
 
 #endif