diff --git a/src/Accelerometers/Accelerometers.cpp b/src/Accelerometers/Accelerometers.cpp index 7b1835a8b5..ea91b66ccd 100644 --- a/src/Accelerometers/Accelerometers.cpp +++ b/src/Accelerometers/Accelerometers.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #if SUPPORT_CAN_EXPANSION # include @@ -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 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,7 +602,7 @@ 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) { @@ -580,7 +610,7 @@ 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 { @@ -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); } } } diff --git a/src/Accelerometers/Accelerometers.h b/src/Accelerometers/Accelerometers.h index bc45a42af3..9c3c6d98e0 100644 --- a/src/Accelerometers/Accelerometers.h +++ b/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); diff --git a/src/CAN/ExpansionManager.cpp b/src/CAN/ExpansionManager.cpp index 8e82cb0427..c0d6a848db 100644 --- a/src/CAN/ExpansionManager.cpp +++ b/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(); } diff --git a/src/CAN/ExpansionManager.h b/src/CAN/ExpansionManager.h index a204627477..667ac196c8 100644 --- a/src/CAN/ExpansionManager.h +++ b/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; diff --git a/src/Platform/Platform.cpp b/src/Platform/Platform.cpp index 72ebaa4f05..cd3b309be5 100644 --- a/src/Platform/Platform.cpp +++ b/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