From cb91c8b1512a00be9e2c636c95ee8f914d98fd1d Mon Sep 17 00:00:00 2001 From: Louis Irwin Date: Mon, 12 Jul 2021 13:56:07 +0100 Subject: [PATCH 01/10] Added basic support for closed loop data collection --- .cproject | 76 +---- src/CAN/CanInterface.cpp | 16 + src/CAN/CanInterface.h | 1 + src/CAN/CommandProcessor.cpp | 5 + src/ClosedLoop/ClosedLoop.cpp | 560 ++++++++++++++++++++++++++++++++++ src/ClosedLoop/ClosedLoop.h | 25 ++ src/GCodes/GCodes.h | 3 + src/GCodes/GCodes3.cpp | 6 + 8 files changed, 618 insertions(+), 74 deletions(-) create mode 100644 src/ClosedLoop/ClosedLoop.cpp create mode 100644 src/ClosedLoop/ClosedLoop.h diff --git a/.cproject b/.cproject index 6a390cdc0b..56e0d19314 100644 --- a/.cproject +++ b/.cproject @@ -1569,9 +1569,6 @@ - - - @@ -1581,99 +1578,30 @@ - - - - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - - - - - - - - - - - diff --git a/src/CAN/CanInterface.cpp b/src/CAN/CanInterface.cpp index 502fad4f24..a532693df2 100644 --- a/src/CAN/CanInterface.cpp +++ b/src/CAN/CanInterface.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #if HAS_LINUX_INTERFACE # include "Linux/LinuxInterface.h" @@ -905,6 +906,9 @@ pre(driver.IsRemote()) return cons.SendAndGetResponse(CanMessageType::m569p1, driver.boardAddress, reply); } + case 5: + return ClosedLoop::StartDataCollection(driver, gb, reply); + default: return GCodeResult::errorNotSupported; } @@ -1343,6 +1347,18 @@ GCodeResult CanInterface::StartAccelerometer(DriverId device, uint8_t axes, uint # endif +GCodeResult CanInterface::StartClosedLoopDataCollection(DriverId device, uint16_t numSamples, uint8_t mode, const GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException) +{ + CanMessageBuffer* const buf = AllocateBuffer(&gb); + const CanRequestId rid = CanInterface::AllocateRequestId(device.boardAddress); + auto msg = buf->SetupRequestMessage(rid, GetCanAddress(), device.boardAddress); + msg->deviceNumber = device.localDriver; + msg->numSamples = numSamples; + msg->delayedStart = 0; + msg->startTime = false; + return SendRequestAndGetStandardReply(buf, rid, reply); +} + #endif // End diff --git a/src/CAN/CanInterface.h b/src/CAN/CanInterface.h index f397b72949..607cd0a4e1 100644 --- a/src/CAN/CanInterface.h +++ b/src/CAN/CanInterface.h @@ -137,6 +137,7 @@ namespace CanInterface #if SUPPORT_ACCELEROMETERS GCodeResult StartAccelerometer(DriverId device, uint8_t axes, uint16_t numSamples, uint8_t mode, const GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); #endif + GCodeResult StartClosedLoopDataCollection(DriverId device, uint16_t numSamples, uint8_t mode, const GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); } // Members of template class CanDriversData diff --git a/src/CAN/CommandProcessor.cpp b/src/CAN/CommandProcessor.cpp index 460a1ca09c..8ebf044cd2 100644 --- a/src/CAN/CommandProcessor.cpp +++ b/src/CAN/CommandProcessor.cpp @@ -15,6 +15,7 @@ #include #include #include "ExpansionManager.h" +# include #ifndef DUET3_ATE # include @@ -539,6 +540,10 @@ void CommandProcessor::ProcessReceivedMessage(CanMessageBuffer *buf) noexcept FilamentMonitor::UpdateRemoteFilamentStatus(buf->id.Src(), buf->msg.filamentMonitorsStatus); break; + case CanMessageType::closedLoopData: + ClosedLoop::ProcessReceivedData(buf->id.Src(), buf->msg.closedLoopData, buf->dataLength); + break; + #if SUPPORT_ACCELEROMETERS case CanMessageType::accelerometerData: Accelerometers::ProcessReceivedData(buf->id.Src(), buf->msg.accelerometerData, buf->dataLength); diff --git a/src/ClosedLoop/ClosedLoop.cpp b/src/ClosedLoop/ClosedLoop.cpp new file mode 100644 index 0000000000..19f38d610d --- /dev/null +++ b/src/ClosedLoop/ClosedLoop.cpp @@ -0,0 +1,560 @@ +/* + * ClosedLoop.cpp + * + * Created on: 19 Mar 2021 + * Author: Louis + */ + +#include "ClosedLoop.h" + +#if SUPPORT_CAN_EXPANSION + +#include +#include +#include +#include +//#include +//#include +//#include + +# include +# include +# include +# include + +//constexpr uint8_t DefaultResolution = 10; +constexpr uint16_t DefaultSamplingRate = 1000; + +static uint8_t modeRequested; // The sampling mode +static DriverId deviceRequested; // The driver being sampled +static volatile uint16_t numSamplesRequested; // The number of samples to collect +//static uint8_t resolution = DefaultResolution; // The resolution at which to collect the samples +static FileStore* volatile closedLoopFile = nullptr; // This is non-null when the data collection is running, null otherwise + +static unsigned int numRemoteOverflows; +static unsigned int expectedRemoteSampleNumber = 0; +static CanAddress expectedRemoteBoardAddress = CanId::NoAddress; + +// Handle M569.5 - Collect closed loop data +GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException) +{ + deviceRequested = driverId; + + // Parse the GCODE params + gb.MustSee('S'); + numSamplesRequested = min(gb.GetUIValue(), 65535); + gb.MustSee('A'); + modeRequested = gb.GetUIValue(); + + // Check the chosen drive is a closed loop drive + // TODO: Think of how we can do this. For now, just check it is remote + if (!deviceRequested.IsRemote()) + { + reply.copy("Drive is not in closed loop mode"); + return GCodeResult::error; + } + + // Check if we are already collecting data + if (closedLoopFile != nullptr) + { + reply.copy("Closed loop data is already being collected"); + return GCodeResult::error; + } + + // Estimate how large the file will be + const uint32_t preallocSize = 1000; //TODO! numSamplesRequested * ((numAxes * (3 + GetDecimalPlaces(resolution))) + 4); + + // Create the filename + String closedLoopFileName; + if (gb.Seen('F')) + { + String temp; + gb.GetQuotedString(temp.GetRef(), false); + MassStorage::CombineName(closedLoopFileName.GetRef(), "0:/sys/closed-loop/", temp.c_str()); + } + else + { + const time_t time = reprap.GetPlatform().GetDateTime(); + tm timeInfo; + gmtime_r(&time, &timeInfo); + closedLoopFileName.printf("0:/sys/closed-loop/%u_%04u-%02u-%02u_%02u.%02u.%02u.csv", + (unsigned int) deviceRequested.boardAddress, + timeInfo.tm_year + 1900, timeInfo.tm_mon + 1, timeInfo.tm_mday, timeInfo.tm_hour, timeInfo.tm_min, timeInfo.tm_sec); + } + + // Create the file + FileStore * const f = MassStorage::OpenFile(closedLoopFileName.c_str(), OpenMode::write, preallocSize); + if (f == nullptr) + { + reply.copy("Failed to create closed loop data file"); + return GCodeResult::error; + } + + // Write the header line + { + // TODO! + String temp; + temp.printf("Sample,Encoder,Target,Control\n"); +// 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()); + } + closedLoopFile = f; + + // If no samples have been requested, return with an info message + if (numSamplesRequested == 0) + { + closedLoopFile->Truncate(); + closedLoopFile->Close(); + closedLoopFile = nullptr; + reply.copy("no samples recorded"); + return GCodeResult::warning; + } + + // Set up & start the CAN data transfer + expectedRemoteSampleNumber = 0; + expectedRemoteBoardAddress = deviceRequested.boardAddress; + numRemoteOverflows = 0; + const GCodeResult rslt = CanInterface::StartClosedLoopDataCollection(deviceRequested, numSamplesRequested, modeRequested, gb, reply); + if (rslt > GCodeResult::warning) + { + closedLoopFile->Close(); + MassStorage::Delete(closedLoopFileName.c_str(), false); + } + return rslt; +} + +// Process closed loop data received over CAN +void ClosedLoop::ProcessReceivedData(CanAddress src, const CanMessageClosedLoopData& msg, size_t msgLen) noexcept +{ + + FileStore * const f = closedLoopFile; + if (f != nullptr) + { +// if (msgLen < msg.GetActualDataLength()) +// { +// f->Write("Received bad data\n"); +// f->Truncate(); // truncate the file in case we didn't write all the preallocated space +// f->Close(); +// closedLoopFile = nullptr; +// } else if + if (msg.firstSampleNumber != expectedRemoteSampleNumber || src != expectedRemoteBoardAddress) + { + f->Write("Received mismatched data\n"); + String temp; + temp.printf("%d instead of %d\n", msg.firstSampleNumber, expectedRemoteSampleNumber); + f->Write(temp.c_str()); + f->Truncate(); // truncate the file in case we didn't write all the preallocated space + f->Close(); + closedLoopFile = nullptr; + } + else + { + unsigned int numSamples = msg.numSamples; +// const unsigned int numAxes = (expectedRemoteAxes & 1u) + ((expectedRemoteAxes >> 1) & 1u) + ((expectedRemoteAxes >> 2) & 1u); + size_t dataIndex = 0; + uint16_t currentBits = 0; + unsigned int bitsLeft = 0; +// const unsigned int receivedResolution = msg.bitsPerSampleMinusOne + 1; +// const uint16_t mask = (1u << receivedResolution) - 1; +// const int decimalPlaces = GetDecimalPlaces(receivedResolution); +// if (msg.overflowed)/ +// { +// ++numRemoteOverflows; +// } + + while (numSamples != 0) + { + String temp; + temp.printf("%u", expectedRemoteSampleNumber); + ++expectedRemoteSampleNumber; + + // TODO: Parse data! +// for (unsigned int axis = 0; axis < numAxes; ++axis) +// { +// // Extract one value from the message. A value spans at most two words in the buffer. +// uint16_t val = currentBits; +// if (bitsLeft >= receivedResolution) +// { +// bitsLeft -= receivedResolution; +// currentBits >>= receivedResolution; +// } +// else +// { +// currentBits = msg.data[dataIndex++]; +// val |= currentBits << bitsLeft; +// currentBits >>= receivedResolution - bitsLeft; +// bitsLeft += 16 - receivedResolution; +// } +// val &= mask; +// +// // Sign-extend it +// if (val & (1u << (receivedResolution - 1))) +// { +// val |= ~mask; +// } +// +// // Convert it to a float number of g +// const float fVal = (float)(int16_t)val/(float)(1u << GetBitsAfterPoint(receivedResolution)); +// +// // Append it to the buffer +// temp.catf(",%.*f", decimalPlaces, (double)fVal); +// } + + temp.catf(",%d", msg.encoderData[0]); + temp.catf(",%d", msg.targetData[0]); + temp.catf(",%d\n", msg.controlData[0]); + + f->Write(temp.c_str()); + --numSamples; + } + + if (msg.lastPacket) + { +// String temp; +// temp.printf("Rate %u, overflows %u\n", msg.actualSampleRate, numRemoteOverflows); +// f->Write(temp.c_str()); + f->Truncate(); // truncate the file in case we didn't write all the preallocated space + f->Close(); + closedLoopFile = nullptr; + } + } + } +} + +#endif + + + + + + + + + + + + + + + + + + + +//static unsigned int expectedRemoteSampleNumber = 0; +//static CanAddress expectedRemoteBoardAddress = CanId::NoAddress; +//static uint8_t expectedRemoteAxes; +//static unsigned int numRemoteOverflows; +// +// +//// Get the number of binary digits after the decimal point +//static inline unsigned int GetBitsAfterPoint(uint8_t dataResolution) noexcept +//{ +// return dataResolution - 2; // assumes the range is +/- 2g +//} +// +//// Get the number of decimal places that we should use when we print each acceleration value +//static unsigned int GetDecimalPlaces(uint8_t dataResolution) noexcept +//{ +// return (GetBitsAfterPoint(dataResolution) >= 11) ? 4 : (GetBitsAfterPoint(dataResolution) >= 8) ? 3 : 2; +//} +// +//// Local accelerometer handling +// +//#include "LIS3DH.h" +// +//constexpr uint16_t DefaultSamplingRate = 1000; +//constexpr uint8_t DefaultResolution = 10; +// +//constexpr size_t AccelerometerTaskStackWords = 400; // big enough to handle printf and file writes +//static Task *accelerometerTask; +// +//static LIS3DH *accelerometer = nullptr; +// +//static uint16_t samplingRate = DefaultSamplingRate; +//static volatile uint16_t numSamplesRequested; +//static uint8_t resolution = DefaultResolution; +//static uint8_t orientation = 20; // +Z -> +Z, +X -> +X +//static volatile uint8_t axesRequested; +//static FileStore* volatile accelerometerFile = nullptr; // this is non-null when the accelerometer is running, null otherwise +//static uint8_t axisLookup[3]; +//static bool axisInverted[3]; +// +//static IoPort spiCsPort; +//static IoPort irqPort; +// +//[[noreturn]] void AccelerometerTaskCode(void*) noexcept +//{ +// for (;;) +// { +// TaskBase::Take(); +// FileStore * const f = accelerometerFile; // capture volatile variable +// if (f != nullptr) +// { +// // Collect and write the samples +// unsigned int samplesWritten = 0; +// unsigned int samplesWanted = numSamplesRequested; +// unsigned int numOverflows = 0; +// const uint16_t mask = (1u << resolution) - 1; +// const int decimalPlaces = GetDecimalPlaces(resolution); +// +// if (accelerometer->StartCollecting(axesRequested)) +// { +// uint16_t dataRate = 0; +// do +// { +// const uint16_t *data; +// bool overflowed; +// unsigned int samplesRead = accelerometer->CollectData(&data, dataRate, overflowed); +// if (samplesRead == 0) +// { +// // samplesRead == 0 indicates an error, e.g. no interrupt +// samplesWanted = 0; +// if (f != nullptr) +// { +// f->Write("Failed to collect data from accelerometer\n"); +// f->Truncate(); // truncate the file in case we didn't write all the preallocated space +// f->Close(); +// accelerometerFile = nullptr; +// } +// break; +// } +// else +// { +// if (overflowed) +// { +// ++numOverflows; +// } +// if (samplesWritten == 0) +// { +// // The first sample taken after waking up is inaccurate, so discard it +// --samplesRead; +// data += 3; +// } +// if (samplesRead >= samplesWanted) +// { +// samplesRead = samplesWanted; +// } +// +// while (samplesRead != 0) +// { +// // Write a row of data +// String temp; +// temp.printf("%u", samplesWritten); +// +// for (unsigned int axis = 0; axis < 3; ++axis) +// { +// if (axesRequested & (1u << axis)) +// { +// uint16_t dataVal = data[axisLookup[axis]]; +// if (axisInverted[axis]) +// { +// dataVal = (dataVal == 0x8000) ? ~dataVal : ~dataVal + 1; +// } +// dataVal >>= (16u - resolution); // data from LIS3DH is left justified +// +// // Sign-extend it +// if (dataVal & (1u << (resolution - 1))) +// { +// dataVal |= ~mask; +// } +// +// // Convert it to a float number of g +// const float fVal = (float)(int16_t)dataVal/(float)(1u << GetBitsAfterPoint(resolution)); +// +// // Append it to the buffer +// temp.catf(",%.*f", decimalPlaces, (double)fVal); +// } +// } +// +// data += 3; +// +// temp.cat('\n'); +// f->Write(temp.c_str()); +// +// --samplesRead; +// --samplesWanted; +// ++samplesWritten; +// } +// } +// } while (samplesWanted != 0); +// +// if (f != nullptr) +// { +// String temp; +// temp.printf("Rate %u, overflows %u\n", dataRate, numOverflows); +// f->Write(temp.c_str()); +// } +// } +// else if (f != nullptr) +// { +// f->Write("Failed to start accelerometer\n"); +// } +// +// if (f != nullptr) +// { +// f->Truncate(); // truncate the file in case we didn't write all the preallocated space +// f->Close(); +// accelerometerFile = nullptr; +// } +// +// accelerometer->StopCollecting(); +// +// // Wait for another command +// accelerometerFile = nullptr; +// } +// } +//} +// +//// Translate the orientation returning true if successful +//static bool TranslateOrientation(uint32_t input) noexcept +//{ +// if (input >= 70u) { return false; } +// const uint8_t xDigit = input % 10u; +// if (xDigit >= 7u) { return false; } +// const uint8_t zDigit = input / 10u; +// const uint8_t xOrientation = xDigit & 0x03; +// const uint8_t zOrientation = zDigit & 0x03; +// if (xOrientation == 3u || zOrientation == 3u || xOrientation == zOrientation) { return false; } +// const uint8_t xInverted = xDigit & 0x04; +// const uint8_t zInverted = zDigit & 0x04; +// uint8_t yInverted = xInverted ^ zInverted; +// +// // Calculate the Y orientation. We must have axes 0, 1 and 2 so they must add up to 3. +// const uint8_t yOrientation = 3u - xOrientation - zOrientation; +// +// // The total number of inversions must be even if the cyclic order of the axes is 012, odd if it is 210 (can we prove this?) +// if ((xOrientation + 1) % 3 != yOrientation) +// { +// yInverted ^= 0x04; // we need an odd number of axis inversions +// } +// +// // Now fill in the axis table +// axisLookup[xOrientation] = 0; +// axisInverted[xOrientation] = xInverted; +// axisLookup[yOrientation] = 1; +// axisInverted[yOrientation] = yInverted; +// axisLookup[zOrientation] = 2; +// axisInverted[zOrientation] = zInverted; +// return true; +//} +// +//// Deal with M955 +//GCodeResult Accelerometers::ConfigureAccelerometer(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException) +//{ +// gb.MustSee('P'); +// DriverId device = gb.GetDriverId(); +// +//# if SUPPORT_CAN_EXPANSION +// if (device.IsRemote()) +// { +// CanMessageGenericConstructor cons(M955Params); +// cons.PopulateFromCommand(gb); +// return cons.SendAndGetResponse(CanMessageType::accelerometerConfig, device.boardAddress, reply); +// } +//# endif +// +// if (device.localDriver != 0) +// { +// reply.copy("Only one local accelerometer is supported"); +// return GCodeResult::error; +// } +// +// // No need for task lock here because this function and the M956 function are called only by the MAIN task +// if (accelerometerFile != nullptr) +// { +// reply.copy("Cannot reconfigure accelerometer while it is collecting data"); +// return GCodeResult::error; +// } +// +// bool seen = false; +// if (gb.Seen('C')) +// { +// seen = true; +// +// // Creating a new accelerometer. First delete any existing accelerometer. +// DeleteObject(accelerometer); +// spiCsPort.Release(); +// irqPort.Release(); +// +// IoPort * const ports[2] = { &spiCsPort, &irqPort }; +// const PinAccess access[2] = { PinAccess::write1, PinAccess::read }; +// if (IoPort::AssignPorts(gb, reply, PinUsedBy::sensor, 2, ports, access) != 2) +// { +// spiCsPort.Release(); // in case it was allocated +// return GCodeResult::error; +// } +// +// const uint32_t spiFrequency = (gb.Seen('Q')) ? gb.GetLimitedUIValue('Q', 500000, 10000001) : DefaultAccelerometerSpiFrequency; +// auto temp = new LIS3DH(SharedSpiDevice::GetMainSharedSpiDevice(), spiFrequency, spiCsPort.GetPin(), irqPort.GetPin()); +// if (temp->CheckPresent()) +// { +// accelerometer = temp; +// if (accelerometerTask == nullptr) +// { +// accelerometerTask = new Task; +// accelerometerTask->Create(AccelerometerTaskCode, "ACCEL", nullptr, TaskPriority::Accelerometer); +// } +// } +// else +// { +// delete temp; +// reply.copy("Accelerometer not found on specified port"); +// return GCodeResult::error; +// } +// } +// else if (accelerometer == nullptr) +// { +// reply.copy("Accelerometer not found"); +// return GCodeResult::error; +// } +// +// uint32_t temp32; +// if (gb.TryGetLimitedUIValue('R', temp32, seen, 17)) +// { +// resolution = temp32; +// } +// +// if (gb.TryGetLimitedUIValue('S', temp32, seen, 10000)) +// { +// samplingRate = temp32; +// } +// +// if (seen) +// { +// if (!accelerometer->Configure(samplingRate, resolution)) +// { +// reply.copy("Failed to configure accelerometer"); +// return GCodeResult::error; +// } +// (void)TranslateOrientation(orientation); +// } +// +// if (gb.Seen('I')) +// { +// seen = true; +// if (!TranslateOrientation(gb.GetUIValue())) +// { +// reply.copy("Bad orientation parameter"); +// return GCodeResult::error; +// } +// } +// +//# if SUPPORT_CAN_EXPANSION +// reply.printf("Accelerometer %u:%u with orientation %u samples at %uHz with %u-bit resolution, SPI frequency %" PRIu32, +// CanInterface::GetCanAddress(), 0, orientation, samplingRate, resolution, accelerometer->GetFrequency()); +//# else +// reply.printf("Accelerometer %u with orientation %u samples at %uHz with %u-bit resolution, SPI frequency %" PRIu32, +// 0, orientation, samplingRate, resolution, accelerometer->GetFrequency()); +//# endif +// return GCodeResult::ok; +//} +// +// +// +// + +// End diff --git a/src/ClosedLoop/ClosedLoop.h b/src/ClosedLoop/ClosedLoop.h new file mode 100644 index 0000000000..c506533ed8 --- /dev/null +++ b/src/ClosedLoop/ClosedLoop.h @@ -0,0 +1,25 @@ +/* + * ClosedLoop.h + * + * Created on: 19 Mar 2021 + * Author: Louis + */ + +#include +#include + +//#if SUPPORT_CAN_EXPANSION +//# include +//#endif + +class CanMessageClosedLoopData; + +namespace ClosedLoop +{ +// GCodeResult ConfigureAccelerometer(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); +// GCodeResult StartAccelerometer(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); +# if SUPPORT_CAN_EXPANSION + GCodeResult StartDataCollection(DriverId, GCodeBuffer&, const StringRef&) THROWS(GCodeException); + void ProcessReceivedData(CanAddress, const CanMessageClosedLoopData&, size_t) noexcept; +# endif +} diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h index 40d0ca691a..15c916e926 100644 --- a/src/GCodes/GCodes.h +++ b/src/GCodes/GCodes.h @@ -381,6 +381,9 @@ class GCodes GCodeResult ConfigureAccelerometer(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); // Deal with M955 GCodeResult StartAccelerometer(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); // Deal with M956 #endif +#if SUPPORT_CAN_EXPANSION + GCodeResult StartClosedLoopDataCollection(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); // Deal with M569.5 +#endif bool SetupM675ProbingMove(GCodeBuffer& gb, bool towardsMin) noexcept; void SetupM675BackoffMove(GCodeBuffer& gb, float position) noexcept; diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp index b48ae06b22..88733042bc 100644 --- a/src/GCodes/GCodes3.cpp +++ b/src/GCodes/GCodes3.cpp @@ -35,6 +35,7 @@ #if SUPPORT_CAN_EXPANSION # include # include +# include #endif #ifdef I2C_IFACE @@ -1375,6 +1376,11 @@ GCodeResult GCodes::ConfigureDriver(GCodeBuffer& gb, const StringRef& reply) THR } #endif +#if SUPPORT_CAN_EXPANSION + case 5: + return ClosedLoop::StartDataCollection(id, gb, reply); +#endif + default: return GCodeResult::warningNotSupported; } From 53ee918831a8b0588336828442672c72049f7150 Mon Sep 17 00:00:00 2001 From: Louis Irwin Date: Fri, 16 Jul 2021 13:29:52 +0100 Subject: [PATCH 02/10] Tidied up closed loop control code --- src/CAN/CanInterface.cpp | 8 +- src/CAN/CanInterface.h | 2 +- src/CAN/CommandProcessor.cpp | 2 +- src/ClosedLoop/ClosedLoop.cpp | 127 +++++-- src/ClosedLoop/ClosedLoop.h | 2 +- src/GCodes/GCodeBuffer/GCodeBuffer.h | 477 ++++++++++++++++----------- 6 files changed, 387 insertions(+), 231 deletions(-) diff --git a/src/CAN/CanInterface.cpp b/src/CAN/CanInterface.cpp index a532693df2..cc8c4da87e 100644 --- a/src/CAN/CanInterface.cpp +++ b/src/CAN/CanInterface.cpp @@ -1347,15 +1347,15 @@ GCodeResult CanInterface::StartAccelerometer(DriverId device, uint8_t axes, uint # endif -GCodeResult CanInterface::StartClosedLoopDataCollection(DriverId device, uint16_t numSamples, uint8_t mode, const GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException) +GCodeResult CanInterface::StartClosedLoopDataCollection(DriverId device, uint16_t filter, uint16_t numSamples, uint8_t mode, const GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException) { CanMessageBuffer* const buf = AllocateBuffer(&gb); const CanRequestId rid = CanInterface::AllocateRequestId(device.boardAddress); auto msg = buf->SetupRequestMessage(rid, GetCanAddress(), device.boardAddress); - msg->deviceNumber = device.localDriver; + msg->mode = mode; + msg->filter = filter; msg->numSamples = numSamples; - msg->delayedStart = 0; - msg->startTime = false; + msg->deviceNumber = device.localDriver; return SendRequestAndGetStandardReply(buf, rid, reply); } diff --git a/src/CAN/CanInterface.h b/src/CAN/CanInterface.h index 607cd0a4e1..25f44ab515 100644 --- a/src/CAN/CanInterface.h +++ b/src/CAN/CanInterface.h @@ -137,7 +137,7 @@ namespace CanInterface #if SUPPORT_ACCELEROMETERS GCodeResult StartAccelerometer(DriverId device, uint8_t axes, uint16_t numSamples, uint8_t mode, const GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); #endif - GCodeResult StartClosedLoopDataCollection(DriverId device, uint16_t numSamples, uint8_t mode, const GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); + GCodeResult StartClosedLoopDataCollection(DriverId device, uint16_t filter, uint16_t numSamples, uint8_t mode, const GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); } // Members of template class CanDriversData diff --git a/src/CAN/CommandProcessor.cpp b/src/CAN/CommandProcessor.cpp index 8ebf044cd2..6fa3336a7b 100644 --- a/src/CAN/CommandProcessor.cpp +++ b/src/CAN/CommandProcessor.cpp @@ -541,7 +541,7 @@ void CommandProcessor::ProcessReceivedMessage(CanMessageBuffer *buf) noexcept break; case CanMessageType::closedLoopData: - ClosedLoop::ProcessReceivedData(buf->id.Src(), buf->msg.closedLoopData, buf->dataLength); + ClosedLoop::ProcessReceivedData(buf->id.Src(), buf->msg.closedLoopData, buf->dataLength, buf->msg.raw); break; #if SUPPORT_ACCELEROMETERS diff --git a/src/ClosedLoop/ClosedLoop.cpp b/src/ClosedLoop/ClosedLoop.cpp index 19f38d610d..23da4060f8 100644 --- a/src/ClosedLoop/ClosedLoop.cpp +++ b/src/ClosedLoop/ClosedLoop.cpp @@ -26,8 +26,9 @@ constexpr uint16_t DefaultSamplingRate = 1000; static uint8_t modeRequested; // The sampling mode +static uint32_t filterRequested; // A filter for what data is collected static DriverId deviceRequested; // The driver being sampled -static volatile uint16_t numSamplesRequested; // The number of samples to collect +static volatile uint32_t numSamplesRequested; // The number of samples to collect //static uint8_t resolution = DefaultResolution; // The resolution at which to collect the samples static FileStore* volatile closedLoopFile = nullptr; // This is non-null when the data collection is running, null otherwise @@ -35,32 +36,76 @@ static unsigned int numRemoteOverflows; static unsigned int expectedRemoteSampleNumber = 0; static CanAddress expectedRemoteBoardAddress = CanId::NoAddress; +// DEBUG - Remove +static bool received; +static uint8_t rawMsg[64]; + // Handle M569.5 - Collect closed loop data GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException) { - deviceRequested = driverId; + // Check what the user wants - record or report + // Record - A number of samples (Snn) is given to record + // Report - Samples (Snn) is not given, and we are already recording + bool recording = false; + uint32_t parsedS; + gb.TryGetUIValue('S', parsedS, recording); + + // If the user is requesting a recording, check if one is already happening + if (recording && closedLoopFile != nullptr) + { + reply.copy("Closed loop data is already being collected"); + return GCodeResult::error; + } - // Parse the GCODE params - gb.MustSee('S'); - numSamplesRequested = min(gb.GetUIValue(), 65535); - gb.MustSee('A'); - modeRequested = gb.GetUIValue(); + // If the user is requesting a report, check if a recording is happening + if (!recording && closedLoopFile == nullptr) + { + reply.copy("Closed loop data is not being collected"); + return GCodeResult::warning; + } - // Check the chosen drive is a closed loop drive - // TODO: Think of how we can do this. For now, just check it is remote - if (!deviceRequested.IsRemote()) + // If we are reporting, deal with that + if (!recording) { - reply.copy("Drive is not in closed loop mode"); - return GCodeResult::error; + reply.printf("Collecting sample: %d/%d", (int) expectedRemoteSampleNumber, (int) numSamplesRequested); + return GCodeResult::ok; + // TODO: Check if the user supplied additional params & send them back a warning if so? } - // Check if we are already collecting data - if (closedLoopFile != nullptr) + // We are recording, parse the additional parameters + bool seen = false; + uint32_t parsedA, parsedD; + + gb.TryGetUIValue('A', parsedA, seen); + if (!seen) {parsedA = 0;} + + seen = false; + gb.TryGetUIValue('D', parsedD, seen); + if (!seen) {parsedD = 0;} + + // Validate the parameters + if (!driverId.IsRemote())// Check the chosen drive is a closed loop drive - TODO: Think of how we can do this. For now, just check it is remote { - reply.copy("Closed loop data is already being collected"); + reply.printf("Drive is not in closed loop mode (driverId.IsRemote() = %d)", driverId.IsRemote()); + return GCodeResult::error; + } + if (parsedS > 65535) // TODO: What is a good maximum value here? + { + reply.copy("Maximum number of samples that can be collected is 65535"); + return GCodeResult::error; + } + if (parsedA > 1) + { + reply.printf("Mode (A) can be either 0 or 1 - A%d is invalid", parsedA); return GCodeResult::error; } + // Validation passed - store the values + modeRequested = parsedA; + filterRequested = parsedD; + deviceRequested = driverId; + numSamplesRequested = parsedS; + // Estimate how large the file will be const uint32_t preallocSize = 1000; //TODO! numSamplesRequested * ((numAxes * (3 + GetDecimalPlaces(resolution))) + 4); @@ -92,13 +137,22 @@ GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, // Write the header line { - // TODO! - String temp; - temp.printf("Sample,Encoder,Target,Control\n"); -// if (axes & 1u) { temp.cat(",X"); } -// if (axes & 2u) { temp.cat(",Y"); } -// if (axes & 4u) { temp.cat(",Z"); } -// temp.cat('\n'); + String temp; + temp.printf("Sample"); + if (filterRequested & 1) {temp.cat(",Raw Encoder Reading");} + if (filterRequested & 2) {temp.cat(",Current Motor Steps");} + if (filterRequested & 4) {temp.cat(",TargetMotorSteps");} + if (filterRequested & 8) {temp.cat(",Step Phase");} + if (filterRequested & 16) {temp.cat(",PID Control Signal");} + if (filterRequested & 32) {temp.cat(",PID P Term");} + if (filterRequested & 64) {temp.cat(",PID I Term");} + if (filterRequested & 128) {temp.cat(",PID D Term");} + if (filterRequested & 256) {temp.cat(",Phase Shift");} + if (filterRequested & 512) {temp.cat(",Desired Step Phase");} + if (filterRequested & 1024) {temp.cat(",Coil A Current");} + if (filterRequested & 2048) {temp.cat(",Coil B Current");} + temp.Erase(temp.strlen(), 1); + temp.cat("\n"); f->Write(temp.c_str()); } closedLoopFile = f; @@ -117,19 +171,23 @@ GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, expectedRemoteSampleNumber = 0; expectedRemoteBoardAddress = deviceRequested.boardAddress; numRemoteOverflows = 0; - const GCodeResult rslt = CanInterface::StartClosedLoopDataCollection(deviceRequested, numSamplesRequested, modeRequested, gb, reply); + const GCodeResult rslt = CanInterface::StartClosedLoopDataCollection(deviceRequested, filterRequested, numSamplesRequested, modeRequested, gb, reply); if (rslt > GCodeResult::warning) { closedLoopFile->Close(); + closedLoopFile = nullptr; MassStorage::Delete(closedLoopFileName.c_str(), false); } return rslt; } // Process closed loop data received over CAN -void ClosedLoop::ProcessReceivedData(CanAddress src, const CanMessageClosedLoopData& msg, size_t msgLen) noexcept +void ClosedLoop::ProcessReceivedData(CanAddress src, const CanMessageClosedLoopData& msg, size_t msgLen, uint8_t raw[]) noexcept { +// memcpy(rawMsg, &raw, 64); +// received = true; + FileStore * const f = closedLoopFile; if (f != nullptr) { @@ -167,7 +225,7 @@ void ClosedLoop::ProcessReceivedData(CanAddress src, const CanMessageClosedLoopD while (numSamples != 0) { - String temp; + String temp; temp.printf("%u", expectedRemoteSampleNumber); ++expectedRemoteSampleNumber; @@ -203,9 +261,22 @@ void ClosedLoop::ProcessReceivedData(CanAddress src, const CanMessageClosedLoopD // temp.catf(",%.*f", decimalPlaces, (double)fVal); // } - temp.catf(",%d", msg.encoderData[0]); - temp.catf(",%d", msg.targetData[0]); - temp.catf(",%d\n", msg.controlData[0]); + // Count how many bits are set in 'filter' + // TODO: Look into a more efficient way of doing this + int variableCount = 0; + int tmpFilter = filterRequested; + while (tmpFilter != 0) { + variableCount += tmpFilter & 0x1; + tmpFilter >>= 1; + } + + // Pull all the variables from the data packet + for (int i=0;i < variableCount; i++) + { + int16_t val = msg.data[i]; + temp.catf(",%d", val); + } + temp.cat("\n"); f->Write(temp.c_str()); --numSamples; diff --git a/src/ClosedLoop/ClosedLoop.h b/src/ClosedLoop/ClosedLoop.h index c506533ed8..b9ed2469be 100644 --- a/src/ClosedLoop/ClosedLoop.h +++ b/src/ClosedLoop/ClosedLoop.h @@ -20,6 +20,6 @@ namespace ClosedLoop // GCodeResult StartAccelerometer(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); # if SUPPORT_CAN_EXPANSION GCodeResult StartDataCollection(DriverId, GCodeBuffer&, const StringRef&) THROWS(GCodeException); - void ProcessReceivedData(CanAddress, const CanMessageClosedLoopData&, size_t) noexcept; + void ProcessReceivedData(CanAddress, const CanMessageClosedLoopData&, size_t, uint8_t[64]) noexcept; # endif } diff --git a/src/GCodes/GCodeBuffer/GCodeBuffer.h b/src/GCodes/GCodeBuffer/GCodeBuffer.h index fb1f7a3ff1..68ddaa9244 100644 --- a/src/GCodes/GCodeBuffer/GCodeBuffer.h +++ b/src/GCodes/GCodeBuffer/GCodeBuffer.h @@ -20,162 +20,236 @@ class FileGCodeInput; // The processing state of each buffer -enum class GCodeBufferState : uint8_t -{ - parseNotStarted, // we haven't started parsing yet - parsingLineNumber, // we saw N at the start and we are parsing the line number - parsingWhitespace, // parsing whitespace after the line number - parsingComment, // parsing a whole-line comment that we may be interested in +enum class GCodeBufferState : uint8_t { + parseNotStarted, // we haven't started parsing yet + parsingLineNumber,// we saw N at the start and we are parsing the line number + parsingWhitespace, // parsing whitespace after the line number + parsingComment, // parsing a whole-line comment that we may be interested in parsingGCode, // parsing GCode words parsingBracketedComment, // inside a (...) comment - parsingQuotedString, // inside a double-quoted string - parsingChecksum, // parsing the checksum after '*' - discarding, // discarding characters after the checksum or an end-of-line comment - ready, // we have a complete gcode but haven't started executing it - executing // we have a complete gcode and have started executing it + parsingQuotedString, // inside a double-quoted string + parsingChecksum, // parsing the checksum after '*' + discarding, // discarding characters after the checksum or an end-of-line comment + ready, // we have a complete gcode but haven't started executing it + executing // we have a complete gcode and have started executing it }; // Class to hold an individual GCode and provide functions to allow it to be parsed -class GCodeBuffer INHERIT_OBJECT_MODEL -{ +class GCodeBuffer INHERIT_OBJECT_MODEL { public: friend class BinaryParser; friend class StringParser; - GCodeBuffer(GCodeChannel::RawType channel, GCodeInput *normalIn, FileGCodeInput *fileIn, MessageType mt, Compatibility::RawType c = Compatibility::RepRapFirmware) noexcept; - void Reset() noexcept; // Reset it to its state after start-up - void Init() noexcept; // Set it up to parse another G-code - void Diagnostics(MessageType mtype) noexcept; // Write some debug info + GCodeBuffer(GCodeChannel::RawType channel, GCodeInput *normalIn, + FileGCodeInput *fileIn, MessageType mt, Compatibility::RawType c = + Compatibility::RepRapFirmware) noexcept; + void Reset() noexcept; // Reset it to its state after start-up + void Init() noexcept; // Set it up to parse another G-code + void Diagnostics(MessageType mtype) noexcept; // Write some debug info - bool Put(char c) noexcept SPEED_CRITICAL; // Add a character to the end + bool Put(char c) noexcept SPEED_CRITICAL; // Add a character to the end #if HAS_LINUX_INTERFACE - void PutBinary(const uint32_t *data, size_t len) noexcept; // Add an entire binary G-Code, overwriting any existing content + void PutBinary(const uint32_t *data, size_t len) noexcept;// Add an entire binary G-Code, overwriting any existing content #endif - void PutAndDecode(const char *data, size_t len) noexcept; // Add an entire G-Code, overwriting any existing content - void PutAndDecode(const char *str) noexcept; // Add a null-terminated string, overwriting any existing content - void StartNewFile() noexcept; // Called when we start a new file - bool FileEnded() noexcept; // Called when we reach the end of the file we are reading from - void DecodeCommand() noexcept; // Decode the command in the buffer when it is complete - bool CheckMetaCommand(const StringRef& reply) THROWS(GCodeException); // Check whether the current command is a meta command, or we are skipping a block + void PutAndDecode(const char *data, size_t len) noexcept;// Add an entire G-Code, overwriting any existing content + void PutAndDecode(const char *str) noexcept;// Add a null-terminated string, overwriting any existing content + void StartNewFile() noexcept; // Called when we start a new file + bool FileEnded() noexcept;// Called when we reach the end of the file we are reading from + void DecodeCommand() noexcept;// Decode the command in the buffer when it is complete + bool CheckMetaCommand(const StringRef &reply) THROWS(GCodeException);// Check whether the current command is a meta command, or we are skipping a block char GetCommandLetter() const noexcept; bool HasCommandNumber() const noexcept; int GetCommandNumber() const noexcept; - int8_t GetCommandFraction() const noexcept; // Return the command fraction, or -1 if none given + int8_t GetCommandFraction() const noexcept; // Return the command fraction, or -1 if none given bool ContainsExpression() const noexcept; - void GetCompleteParameters(const StringRef& str) THROWS(GCodeException); // Get all of the line following the command. Currently called only for the Q0 command. - int32_t GetLineNumber() const noexcept { return CurrentFileMachineState().lineNumber; } + void GetCompleteParameters(const StringRef &str) THROWS(GCodeException);// Get all of the line following the command. Currently called only for the Q0 command. + int32_t GetLineNumber() const noexcept { + return CurrentFileMachineState().lineNumber; + } bool IsLastCommand() const noexcept; - GCodeResult GetLastResult() const noexcept { return lastResult; } - void SetLastResult(GCodeResult r) noexcept { lastResult = r; } - - bool Seen(char c) noexcept SPEED_CRITICAL; // Is a character present? - void MustSee(char c) THROWS(GCodeException); // Test for character present, throw error if not - char MustSee(char c1, char c2) THROWS(GCodeException); // Test for one of two characters present, throw error if not - - float GetFValue() THROWS(GCodeException) SPEED_CRITICAL; // Get a float after a key letter - float GetDistance() THROWS(GCodeException); // Get a distance or coordinate and convert it from inches to mm if necessary - int32_t GetIValue() THROWS(GCodeException) SPEED_CRITICAL; // Get an integer after a key letter - int32_t GetLimitedIValue(char c, int32_t minValue, int32_t maxValue) THROWS(GCodeException) - pre(minvalue <= maxValue) - post(minValue <= result; result <= maxValue); // Get an integer after a key letter - uint32_t GetUIValue() THROWS(GCodeException); // Get an unsigned integer value - uint32_t GetLimitedUIValue(char c, uint32_t minValue, uint32_t maxValuePlusOne) THROWS(GCodeException) // Get an unsigned integer value, throw if outside limits - pre(maxValuePlusOne > minValue) // Get an unsigned integer value, throw if outside limits - post(result >= minValue; result < maxValuePlusOne); - uint32_t GetLimitedUIValue(char c, uint32_t maxValuePlusOne) THROWS(GCodeException) - post(result < maxValuePlusOne) { return GetLimitedUIValue(c, 0, maxValuePlusOne); } - float GetLimitedFValue(char c, float minValue, float maxValue) THROWS(GCodeException) - pre(minvalue <= maxValue) - post(minValue <= result; result <= maxValue); // Get a float after a key letter - void GetIPAddress(IPAddress& returnedIp) THROWS(GCodeException); // Get an IP address quad after a key letter - void GetMacAddress(MacAddress& mac) THROWS(GCodeException); // Get a MAC address sextet after a key letter - PwmFrequency GetPwmFrequency() THROWS(GCodeException); // Get a PWM frequency - float GetPwmValue() THROWS(GCodeException); // Get a PWM value - DriverId GetDriverId() THROWS(GCodeException); // Get a driver ID - void GetUnprecedentedString(const StringRef& str, bool allowEmpty = false) THROWS(GCodeException); // Get a string with no preceding key letter - void GetQuotedString(const StringRef& str, bool allowEmpty = false) THROWS(GCodeException); // Get and copy a quoted string - void GetPossiblyQuotedString(const StringRef& str, bool allowEmpty = false) THROWS(GCodeException); // Get and copy a string which may or may not be quoted - void GetReducedString(const StringRef& str) THROWS(GCodeException); // Get and copy a quoted string, removing certain characters - void GetFloatArray(float arr[], size_t& length, bool doPad) THROWS(GCodeException) SPEED_CRITICAL; // Get a colon-separated list of floats after a key letter - void GetIntArray(int32_t arr[], size_t& length, bool doPad) THROWS(GCodeException); // Get a :-separated list of ints after a key letter - void GetUnsignedArray(uint32_t arr[], size_t& length, bool doPad) THROWS(GCodeException); // Get a :-separated list of unsigned ints after a key letter - void GetDriverIdArray(DriverId arr[], size_t& length) THROWS(GCodeException); // Get a :-separated list of drivers after a key letter - - bool TryGetFValue(char c, float& val, bool& seen) THROWS(GCodeException); - bool TryGetIValue(char c, int32_t& val, bool& seen) THROWS(GCodeException); - bool TryGetLimitedIValue(char c, int32_t& val, bool& seen, int32_t minValue, int32_t maxValue) THROWS(GCodeException); - bool TryGetUIValue(char c, uint32_t& val, bool& seen) THROWS(GCodeException); - bool TryGetLimitedUIValue(char c, uint32_t& val, bool& seen, uint32_t maxValuePlusOne) THROWS(GCodeException); - bool TryGetBValue(char c, bool& val, bool& seen) THROWS(GCodeException); - bool TryGetFloatArray(char c, size_t numVals, float vals[], const StringRef& reply, bool& seen, bool doPad = false) THROWS(GCodeException); - bool TryGetUIArray(char c, size_t numVals, uint32_t vals[], const StringRef& reply, bool& seen, bool doPad = false) THROWS(GCodeException); - bool TryGetQuotedString(char c, const StringRef& str, bool& seen, bool allowEmpty = false) THROWS(GCodeException); - bool TryGetPossiblyQuotedString(char c, const StringRef& str, bool& seen) THROWS(GCodeException); + GCodeResult GetLastResult() const noexcept { + return lastResult; + } + void SetLastResult(GCodeResult r) noexcept { + lastResult = r; + } + + bool Seen(char c) noexcept SPEED_CRITICAL; // Is a character present? + void MustSee(char c) THROWS(GCodeException);// Test for character present, throw error if not + char MustSee(char c1, char c2) THROWS(GCodeException);// Test for one of two characters present, throw error if not + + float GetFValue() THROWS(GCodeException) SPEED_CRITICAL;// Get a float after a key letter + float GetDistance() THROWS(GCodeException); // Get a distance or coordinate and convert it from inches to mm if necessary + int32_t GetIValue() THROWS(GCodeException) SPEED_CRITICAL;// Get an integer after a key letter + int32_t GetLimitedIValue(char c, int32_t minValue, int32_t maxValue) + THROWS(GCodeException) + pre(minvalue <= maxValue) + post(minValue <= result; result <= maxValue);// Get an integer after a key letter + uint32_t GetUIValue() THROWS(GCodeException);// Get an unsigned integer value + uint32_t GetLimitedUIValue(char c, uint32_t minValue, + uint32_t maxValuePlusOne) THROWS(GCodeException)// Get an unsigned integer value, throw if outside limits + pre(maxValuePlusOne > minValue)// Get an unsigned integer value, throw if outside limits + post(result >= minValue; result < maxValuePlusOne); + uint32_t GetLimitedUIValue(char c, uint32_t maxValuePlusOne) + THROWS(GCodeException) + post(result < maxValuePlusOne) { + return GetLimitedUIValue(c, 0, maxValuePlusOne); + } + float GetLimitedFValue(char c, float minValue, float maxValue) + THROWS(GCodeException) + pre(minvalue <= maxValue) + post(minValue <= result; result <= maxValue);// Get a float after a key letter + void GetIPAddress(IPAddress &returnedIp) THROWS(GCodeException);// Get an IP address quad after a key letter + void GetMacAddress(MacAddress &mac) THROWS(GCodeException); // Get a MAC address sextet after a key letter + PwmFrequency GetPwmFrequency() THROWS(GCodeException);// Get a PWM frequency + float GetPwmValue() THROWS(GCodeException); // Get a PWM value + DriverId GetDriverId() THROWS(GCodeException); // Get a driver ID + void GetUnprecedentedString(const StringRef &str, bool allowEmpty = false) + THROWS(GCodeException);// Get a string with no preceding key letter + void GetQuotedString(const StringRef &str, bool allowEmpty = false) + THROWS(GCodeException); // Get and copy a quoted string + void GetPossiblyQuotedString(const StringRef &str, bool allowEmpty = false) + THROWS(GCodeException);// Get and copy a string which may or may not be quoted + void GetReducedString(const StringRef &str) THROWS(GCodeException); // Get and copy a quoted string, removing certain characters + void GetFloatArray(float arr[], size_t &length, bool doPad) + THROWS(GCodeException) SPEED_CRITICAL; // Get a colon-separated list of floats after a key letter + void GetIntArray(int32_t arr[], size_t &length, bool doPad) + THROWS(GCodeException);// Get a :-separated list of ints after a key letter + void GetUnsignedArray(uint32_t arr[], size_t &length, bool doPad) + THROWS(GCodeException);// Get a :-separated list of unsigned ints after a key letter + void GetDriverIdArray(DriverId arr[], size_t &length) + THROWS(GCodeException);// Get a :-separated list of drivers after a key letter + + bool TryGetFValue(char c, float &val, bool &seen) THROWS(GCodeException); + bool TryGetIValue(char c, int32_t &val, bool &seen) THROWS(GCodeException); + bool TryGetLimitedIValue(char c, int32_t &val, bool &seen, int32_t minValue, + int32_t maxValue) THROWS(GCodeException); + bool TryGetUIValue(char c, uint32_t &val, bool &seen) + THROWS(GCodeException); + bool TryGetLimitedUIValue(char c, uint32_t &val, bool &seen, + uint32_t maxValuePlusOne) THROWS(GCodeException); + bool TryGetBValue(char c, bool &val, bool &seen) THROWS(GCodeException); + bool TryGetFloatArray(char c, size_t numVals, float vals[], + const StringRef &reply, bool &seen, bool doPad = false) + THROWS(GCodeException); + bool TryGetUIArray(char c, size_t numVals, uint32_t vals[], + const StringRef &reply, bool &seen, bool doPad = false) + THROWS(GCodeException); + bool TryGetQuotedString(char c, const StringRef &str, bool &seen, + bool allowEmpty = false) THROWS(GCodeException); + bool TryGetPossiblyQuotedString(char c, const StringRef &str, bool &seen) + THROWS(GCodeException); bool IsIdle() const noexcept; bool IsCompletelyIdle() const noexcept; - bool IsReady() const noexcept; // Return true if a gcode is ready but hasn't been started yet - bool IsExecuting() const noexcept; // Return true if a gcode has been started and is not paused - void SetFinished(bool f) noexcept; // Set the G Code executed (or not) + bool IsReady() const noexcept;// Return true if a gcode is ready but hasn't been started yet + bool IsExecuting() const noexcept;// Return true if a gcode has been started and is not paused + void SetFinished(bool f) noexcept; // Set the G Code executed (or not) void SetCommsProperties(uint32_t arg) noexcept; - GCodeMachineState& LatestMachineState() const noexcept { return *machineState; } + GCodeMachineState& LatestMachineState() const noexcept { + return *machineState; + } GCodeMachineState& CurrentFileMachineState() const noexcept; GCodeMachineState& OriginalMachineState() const noexcept; - GCodeMachineState::BlockState& GetBlockState() const noexcept { return CurrentFileMachineState().CurrentBlockState(); } - uint16_t GetBlockIndent() const noexcept { return GetBlockState().GetIndent(); } + GCodeMachineState::BlockState& GetBlockState() const noexcept { + return CurrentFileMachineState().CurrentBlockState(); + } + uint16_t GetBlockIndent() const noexcept { + return GetBlockState().GetIndent(); + } float ConvertDistance(float distance) const noexcept; float InverseConvertDistance(float distance) const noexcept; unsigned int GetStackDepth() const noexcept; - bool PushState(bool withinSameFile) noexcept; // Push state returning true if successful (i.e. stack not overflowed) - bool PopState(bool withinSameFile) noexcept; // Pop state returning true if successful (i.e. no stack underrun) + bool PushState(bool withinSameFile) noexcept;// Push state returning true if successful (i.e. stack not overflowed) + bool PopState(bool withinSameFile) noexcept;// Pop state returning true if successful (i.e. no stack underrun) void AbortFile(bool abortAll, bool requestAbort = true) noexcept; - bool IsDoingFile() const noexcept; // Return true if this source is executing a file - bool IsDoingLocalFile() const noexcept; // Return true if this source is executing a file from the local SD card - bool IsDoingFileMacro() const noexcept; // Return true if this source is executing a file macro - FilePosition GetFilePosition() const noexcept; // Get the file position at the start of the current command + bool IsDoingFile() const noexcept;// Return true if this source is executing a file + bool IsDoingLocalFile() const noexcept; // Return true if this source is executing a file from the local SD card + bool IsDoingFileMacro() const noexcept; // Return true if this source is executing a file macro + FilePosition GetFilePosition() const noexcept;// Get the file position at the start of the current command - void WaitForAcknowledgement() noexcept; // Flag that we are waiting for acknowledgement + void WaitForAcknowledgement() noexcept; // Flag that we are waiting for acknowledgement #if HAS_LINUX_INTERFACE - bool IsBinary() const noexcept { return isBinaryBuffer; } // Return true if the code is in binary format - - bool IsFileFinished() const noexcept; // Return true if this source has finished execution of a file - void SetFileFinished() noexcept; // Mark the current file as finished - void SetPrintFinished() noexcept; // Mark the current print file as finished - - bool RequestMacroFile(const char *filename, bool fromCode) noexcept; // Request execution of a file macro - volatile bool IsWaitingForMacro() const noexcept { return isWaitingForMacro; } // Indicates if the GB is waiting for a macro to be opened - bool HasJustStartedMacro() const noexcept { return macroJustStarted; } // Has this GB just started a new macro file? - bool IsMacroRequestPending() const noexcept { return !requestedMacroFile.IsEmpty(); } // Indicates if a macro file is being requested - const char *GetRequestedMacroFile() const noexcept { return requestedMacroFile.c_str(); } // Return requested macro file or nullptr if none - bool IsMacroStartedByCode() const noexcept; // Indicates if the last macro was requested from a code - void MacroRequestSent() noexcept { requestedMacroFile.Clear(); } // Called when a macro file request has been sent - void ResolveMacroRequest(bool hadError, bool isEmpty) noexcept; // Resolve the call waiting for a macro to be executed - bool IsMacroEmpty() const noexcept { return macroFileEmpty; } // Return true if the opened macro file is actually empty - - void MacroFileClosed() noexcept; // Called to notify the SBC about the file being internally closed on success - volatile bool IsMacroFileClosed() const noexcept { return macroFileClosed; } // Indicates if a file has been closed internally in RRF - void MacroFileClosedSent() noexcept { macroFileClosed = false; } // Called when the SBC has been notified about the internally closed file - - bool IsAbortRequested() const noexcept { return abortFile; } // Is the cancellation of the current file requested? - bool IsAbortAllRequested() const noexcept { return abortAllFiles; } // Is the cancellation of all files being executed on this channel requested? - void FileAbortSent() noexcept { abortFile = abortAllFiles = false; } // Called when the SBC has been notified about a file (or all files) being closed - - bool IsMessagePromptPending() const noexcept { return messagePromptPending; } // Is the SBC supposed to be notified about a message waiting for acknowledgement? - void MessagePromptSent() noexcept { messagePromptPending = false; } // Called when the SBC has been notified about a message waiting for acknowledgement - bool IsMessageAcknowledged() const noexcept { return messageAcknowledged; } // Indicates if a message has been acknowledged - void MessageAcknowledgementSent() noexcept { messageAcknowledged = false; } // Called when the SBC has been notified about the message acknowledgement - - bool IsInvalidated() const noexcept { return invalidated; } // Indicates if the channel is invalidated - void Invalidate(bool i = true) noexcept { invalidated = i; } // Invalidate this channel (or not) - - bool IsSendRequested() const noexcept { return sendToSbc; } // Is this code supposed to be sent to the SBC - void SendToSbc() noexcept { sendToSbc = true; } // Send this code to the attached SBC + bool IsBinary() const noexcept { + return isBinaryBuffer; + } // Return true if the code is in binary format + + bool IsFileFinished() const noexcept;// Return true if this source has finished execution of a file + void SetFileFinished() noexcept; // Mark the current file as finished + void SetPrintFinished() noexcept;// Mark the current print file as finished + + bool RequestMacroFile(const char *filename, bool fromCode) noexcept;// Request execution of a file macro + volatile bool IsWaitingForMacro() const noexcept { + return isWaitingForMacro; + } // Indicates if the GB is waiting for a macro to be opened + bool HasJustStartedMacro() const noexcept { + return macroJustStarted; + } // Has this GB just started a new macro file? + bool IsMacroRequestPending() const noexcept { + return !requestedMacroFile.IsEmpty(); + } // Indicates if a macro file is being requested + const char* GetRequestedMacroFile() const noexcept { + return requestedMacroFile.c_str(); + } // Return requested macro file or nullptr if none + bool IsMacroStartedByCode() const noexcept; // Indicates if the last macro was requested from a code + void MacroRequestSent() noexcept { + requestedMacroFile.Clear(); + } // Called when a macro file request has been sent + void ResolveMacroRequest(bool hadError, bool isEmpty) noexcept; // Resolve the call waiting for a macro to be executed + bool IsMacroEmpty() const noexcept { + return macroFileEmpty; + } // Return true if the opened macro file is actually empty + + void MacroFileClosed() noexcept;// Called to notify the SBC about the file being internally closed on success + volatile bool IsMacroFileClosed() const noexcept { + return macroFileClosed; + } // Indicates if a file has been closed internally in RRF + void MacroFileClosedSent() noexcept { + macroFileClosed = false; + }// Called when the SBC has been notified about the internally closed file + + bool IsAbortRequested() const noexcept { + return abortFile; + } // Is the cancellation of the current file requested? + bool IsAbortAllRequested() const noexcept { + return abortAllFiles; + }// Is the cancellation of all files being executed on this channel requested? + void FileAbortSent() noexcept { + abortFile = abortAllFiles = false; + }// Called when the SBC has been notified about a file (or all files) being closed + + bool IsMessagePromptPending() const noexcept { + return messagePromptPending; + }// Is the SBC supposed to be notified about a message waiting for acknowledgement? + void MessagePromptSent() noexcept { + messagePromptPending = false; + }// Called when the SBC has been notified about a message waiting for acknowledgement + bool IsMessageAcknowledged() const noexcept { + return messageAcknowledged; + } // Indicates if a message has been acknowledged + void MessageAcknowledgementSent() noexcept { + messageAcknowledged = false; + }// Called when the SBC has been notified about the message acknowledgement + + bool IsInvalidated() const noexcept { + return invalidated; + } // Indicates if the channel is invalidated + void Invalidate(bool i = true) noexcept { + invalidated = i; + } // Invalidate this channel (or not) + + bool IsSendRequested() const noexcept { + return sendToSbc; + } // Is this code supposed to be sent to the SBC + void SendToSbc() noexcept { + sendToSbc = true; + } // Send this code to the attached SBC #endif GCodeState GetState() const noexcept; @@ -184,70 +258,94 @@ class GCodeBuffer INHERIT_OBJECT_MODEL void AdvanceState() noexcept; void MessageAcknowledged(bool cancelled) noexcept; - GCodeChannel GetChannel() const noexcept { return codeChannel; } - const char *GetIdentity() const noexcept { return codeChannel.ToString(); } + GCodeChannel GetChannel() const noexcept { + return codeChannel; + } + const char* GetIdentity() const noexcept { + return codeChannel.ToString(); + } bool CanQueueCodes() const noexcept; MessageType GetResponseMessageType() const noexcept; #if HAS_MASS_STORAGE - bool OpenFileToWrite(const char* directory, const char* fileName, const FilePosition size, const bool binaryWrite, const uint32_t fileCRC32) noexcept; - // open a file to write to - bool IsWritingFile() const noexcept; // Returns true if writing a file - void WriteToFile() noexcept; // Write the current GCode to file - - bool IsWritingBinary() const noexcept; // Returns true if writing binary - bool WriteBinaryToFile(char b) noexcept; // Write a byte to the file, returning true if the upload is now complete + bool OpenFileToWrite(const char *directory, const char *fileName, + const FilePosition size, const bool binaryWrite, + const uint32_t fileCRC32) noexcept; + // open a file to write to + bool IsWritingFile() const noexcept; // Returns true if writing a file + void WriteToFile() noexcept; // Write the current GCode to file + + bool IsWritingBinary() const noexcept; // Returns true if writing binary + bool WriteBinaryToFile(char b) noexcept;// Write a byte to the file, returning true if the upload is now complete void FinishWritingBinary() noexcept; #endif - const char* DataStart() const noexcept; // Get the start of the current command - size_t DataLength() const noexcept; // Get the length of the current command + const char* DataStart() const noexcept; // Get the start of the current command + size_t DataLength() const noexcept; // Get the length of the current command - void PrintCommand(const StringRef& s) const noexcept; + void PrintCommand(const StringRef &s) const noexcept; void AppendFullCommand(const StringRef &s) const noexcept; - bool IsTimerRunning() const noexcept { return timerRunning; } - uint32_t WhenTimerStarted() const noexcept { return whenTimerStarted; } + bool IsTimerRunning() const noexcept { + return timerRunning; + } + uint32_t WhenTimerStarted() const noexcept { + return whenTimerStarted; + } void StartTimer() noexcept; - void StopTimer() noexcept { timerRunning = false; } - bool DoDwellTime(uint32_t dwellMillis) noexcept; // Execute a dwell returning true if it has finished - - void ResetReportDueTimer() noexcept { whenReportDueTimerStarted = millis(); }; + void StopTimer() noexcept { + timerRunning = false; + } + bool DoDwellTime(uint32_t dwellMillis) noexcept;// Execute a dwell returning true if it has finished + + void ResetReportDueTimer() noexcept { + whenReportDueTimerStarted = millis(); + } + ; bool IsReportDue() noexcept; void RestartFrom(FilePosition pos) noexcept; #if HAS_MASS_STORAGE - FileGCodeInput *GetFileInput() const noexcept { return fileInput; } + FileGCodeInput* GetFileInput() const noexcept { + return fileInput; + } #endif - GCodeInput *GetNormalInput() const noexcept { return normalInput; } - - void MotionCommanded() noexcept { motionCommanded = true; } - void MotionStopped() noexcept { motionCommanded = false; } - bool WasMotionCommanded() const noexcept { return motionCommanded; } - - void AddParameters(VariableSet& vars, int codeRunning) noexcept; + GCodeInput* GetNormalInput() const noexcept { + return normalInput; + } + + void MotionCommanded() noexcept { + motionCommanded = true; + } + void MotionStopped() noexcept { + motionCommanded = false; + } + bool WasMotionCommanded() const noexcept { + return motionCommanded; + } + + void AddParameters(VariableSet &vars, int codeRunning) noexcept; VariableSet& GetVariables() const noexcept; Mutex mutex; -protected: - DECLARE_OBJECT_MODEL +protected:DECLARE_OBJECT_MODEL private: #if SUPPORT_OBJECT_MODEL - const char *GetStateText() const noexcept; + const char* GetStateText() const noexcept; #endif - const GCodeChannel codeChannel; // Channel number of this instance - GCodeInput *normalInput; // Our normal input stream, or nullptr if there isn't one + const GCodeChannel codeChannel; // Channel number of this instance + GCodeInput *normalInput;// Our normal input stream, or nullptr if there isn't one #if HAS_MASS_STORAGE - FileGCodeInput *fileInput; // Our file input stream for when we are reading from a print file or a macro file, may be shared with other GCodeBuffers + FileGCodeInput *fileInput;// Our file input stream for when we are reading from a print file or a macro file, may be shared with other GCodeBuffers #endif - const MessageType responseMessageType; // The message type we use for responses to string codes coming from this channel + const MessageType responseMessageType;// The message type we use for responses to string codes coming from this channel GCodeResult lastResult; @@ -257,21 +355,21 @@ class GCodeBuffer INHERIT_OBJECT_MODEL StringParser stringParser; - GCodeBufferState bufferState; // Idle, executing or paused - GCodeMachineState *machineState; // Machine state for this gcode source + GCodeBufferState bufferState; // Idle, executing or paused + GCodeMachineState *machineState; // Machine state for this gcode source - uint32_t whenTimerStarted; // When we started waiting - uint32_t whenReportDueTimerStarted; // When the report-due-timer has been started + uint32_t whenTimerStarted; // When we started waiting + uint32_t whenReportDueTimerStarted; // When the report-due-timer has been started static constexpr uint32_t reportDueInterval = 1000; // Interval in which we send in ms #if HAS_LINUX_INTERFACE bool isBinaryBuffer; #endif - bool timerRunning; // True if we are waiting - bool motionCommanded; // true if this GCode stream has commanded motion since it last waited for motion to stop + bool timerRunning; // True if we are waiting + bool motionCommanded;// true if this GCode stream has commanded motion since it last waited for motion to stop #if HAS_LINUX_INTERFACE - alignas(4) char buffer[MaxCodeBufferSize]; // must be aligned because we do dword fetches from it + alignas(4) char buffer[MaxCodeBufferSize];// must be aligned because we do dword fetches from it #else char buffer[GCODE_LENGTH]; #endif @@ -280,27 +378,25 @@ class GCodeBuffer INHERIT_OBJECT_MODEL // Accessed by both the Main and Linux tasks BinarySemaphore macroSemaphore; volatile bool isWaitingForMacro; // Is this GB waiting in DoFileMacro? - volatile bool macroFileClosed; // Last macro file has been closed in RRF, tell the SBC + volatile bool macroFileClosed;// Last macro file has been closed in RRF, tell the SBC // Accessed only when the GB mutex is acquired String requestedMacroFile; - uint8_t - macroJustStarted : 1, // Whether the GB has just started a macro file - macroFileError : 1, // Whether the macro file could be opened or if an error occurred - macroFileEmpty : 1, // Whether the macro file is actually empty - abortFile : 1, // Whether to abort the last file on the stack - abortAllFiles : 1, // Whether to abort all opened files - sendToSbc : 1, // Indicates if the GB string content is supposed to be sent to the SBC - messagePromptPending : 1, // Has the SBC been notified about a message waiting for acknowledgement? - messageAcknowledged : 1; // Last message has been acknowledged + uint8_t macroJustStarted :1,// Whether the GB has just started a macro file + macroFileError :1,// Whether the macro file could be opened or if an error occurred + macroFileEmpty :1, // Whether the macro file is actually empty + abortFile :1, // Whether to abort the last file on the stack + abortAllFiles :1, // Whether to abort all opened files + sendToSbc :1,// Indicates if the GB string content is supposed to be sent to the SBC + messagePromptPending :1,// Has the SBC been notified about a message waiting for acknowledgement? + messageAcknowledged :1; // Last message has been acknowledged // Accessed only by the Linux task - bool invalidated; // Set to true if the GB content is not valid and about to be cleared + bool invalidated;// Set to true if the GB content is not valid and about to be cleared #endif }; -inline bool GCodeBuffer::IsDoingFileMacro() const noexcept -{ +inline bool GCodeBuffer::IsDoingFileMacro() const noexcept { #if HAS_LINUX_INTERFACE return machineState->doingFileMacro || IsMacroRequestPending(); #else @@ -310,47 +406,39 @@ inline bool GCodeBuffer::IsDoingFileMacro() const noexcept #if HAS_LINUX_INTERFACE -inline bool GCodeBuffer::IsFileFinished() const noexcept -{ +inline bool GCodeBuffer::IsFileFinished() const noexcept { return machineState->fileFinished; } -inline bool GCodeBuffer::IsMacroStartedByCode() const noexcept -{ +inline bool GCodeBuffer::IsMacroStartedByCode() const noexcept { return machineState->macroStartedByCode; } #endif -inline GCodeState GCodeBuffer::GetState() const noexcept -{ +inline GCodeState GCodeBuffer::GetState() const noexcept { return machineState->GetState(); } -inline void GCodeBuffer::SetState(GCodeState newState) noexcept -{ +inline void GCodeBuffer::SetState(GCodeState newState) noexcept { machineState->SetState(newState); } -inline void GCodeBuffer::SetState(GCodeState newState, uint16_t param) noexcept -{ +inline void GCodeBuffer::SetState(GCodeState newState, uint16_t param) noexcept { machineState->stateParameter = param; machineState->SetState(newState); } -inline void GCodeBuffer::AdvanceState() noexcept -{ +inline void GCodeBuffer::AdvanceState() noexcept { machineState->AdvanceState(); } // Return true if we can queue gcodes from this source. This is the case if a file is being executed -inline bool GCodeBuffer::CanQueueCodes() const noexcept -{ +inline bool GCodeBuffer::CanQueueCodes() const noexcept { return machineState->DoingFile(); } -inline bool GCodeBuffer::IsDoingFile() const noexcept -{ +inline bool GCodeBuffer::IsDoingFile() const noexcept { #if HAS_LINUX_INTERFACE return machineState->DoingFile() || IsMacroRequestPending(); #else @@ -358,19 +446,16 @@ inline bool GCodeBuffer::IsDoingFile() const noexcept #endif } -inline bool GCodeBuffer::IsReady() const noexcept -{ +inline bool GCodeBuffer::IsReady() const noexcept { return bufferState == GCodeBufferState::ready; } -inline bool GCodeBuffer::IsExecuting() const noexcept -{ +inline bool GCodeBuffer::IsExecuting() const noexcept { return bufferState == GCodeBufferState::executing; } // Return true if this source is executing a file from the local SD card -inline bool GCodeBuffer::IsDoingLocalFile() const noexcept -{ +inline bool GCodeBuffer::IsDoingLocalFile() const noexcept { #if HAS_LINUX_INTERFACE return !IsBinary() && IsDoingFile(); #else From b246b45864d8ff7b853f6a8ca2f5d04d60252632 Mon Sep 17 00:00:00 2001 From: Louis Irwin Date: Wed, 4 Aug 2021 14:37:12 +0100 Subject: [PATCH 03/10] Various stability improvements for closed loop control --- src/CAN/CanInterface.cpp | 11 ++- src/CAN/CanInterface.h | 2 +- src/CAN/CommandProcessor.cpp | 4 + src/ClosedLoop/ClosedLoop.cpp | 142 ++++++++++++++++++++++------------ src/GCodes/GCodes3.cpp | 1 + src/Platform/Logger.h | 2 +- src/Platform/Platform.cpp | 31 ++++++++ src/Platform/Platform.h | 1 + 8 files changed, 143 insertions(+), 51 deletions(-) diff --git a/src/CAN/CanInterface.cpp b/src/CAN/CanInterface.cpp index cc8c4da87e..64420085c2 100644 --- a/src/CAN/CanInterface.cpp +++ b/src/CAN/CanInterface.cpp @@ -909,6 +909,13 @@ pre(driver.IsRemote()) case 5: return ClosedLoop::StartDataCollection(driver, gb, reply); + case 6: + { + CanMessageGenericConstructor cons(M569Point6Params); + cons.PopulateFromCommand(gb); + return cons.SendAndGetResponse(CanMessageType::m569p6, driver.boardAddress, reply); + } + default: return GCodeResult::errorNotSupported; } @@ -1347,14 +1354,16 @@ GCodeResult CanInterface::StartAccelerometer(DriverId device, uint8_t axes, uint # endif -GCodeResult CanInterface::StartClosedLoopDataCollection(DriverId device, uint16_t filter, uint16_t numSamples, uint8_t mode, const GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException) +GCodeResult CanInterface::StartClosedLoopDataCollection(DriverId device, uint16_t filter, uint16_t numSamples, uint16_t rateRequested, uint8_t movementRequested, uint8_t mode, const GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException) { CanMessageBuffer* const buf = AllocateBuffer(&gb); const CanRequestId rid = CanInterface::AllocateRequestId(device.boardAddress); auto msg = buf->SetupRequestMessage(rid, GetCanAddress(), device.boardAddress); msg->mode = mode; msg->filter = filter; + msg->rate = rateRequested; msg->numSamples = numSamples; + msg->movement = movementRequested; msg->deviceNumber = device.localDriver; return SendRequestAndGetStandardReply(buf, rid, reply); } diff --git a/src/CAN/CanInterface.h b/src/CAN/CanInterface.h index 25f44ab515..d7adc0c937 100644 --- a/src/CAN/CanInterface.h +++ b/src/CAN/CanInterface.h @@ -137,7 +137,7 @@ namespace CanInterface #if SUPPORT_ACCELEROMETERS GCodeResult StartAccelerometer(DriverId device, uint8_t axes, uint16_t numSamples, uint8_t mode, const GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); #endif - GCodeResult StartClosedLoopDataCollection(DriverId device, uint16_t filter, uint16_t numSamples, uint8_t mode, const GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); + GCodeResult StartClosedLoopDataCollection(DriverId device, uint16_t filter, uint16_t numSamples, uint16_t rateRequested, uint8_t movementRequested, uint8_t mode, const GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); } // Members of template class CanDriversData diff --git a/src/CAN/CommandProcessor.cpp b/src/CAN/CommandProcessor.cpp index 6fa3336a7b..1fe0158f82 100644 --- a/src/CAN/CommandProcessor.cpp +++ b/src/CAN/CommandProcessor.cpp @@ -544,6 +544,10 @@ void CommandProcessor::ProcessReceivedMessage(CanMessageBuffer *buf) noexcept ClosedLoop::ProcessReceivedData(buf->id.Src(), buf->msg.closedLoopData, buf->dataLength, buf->msg.raw); break; + case CanMessageType::logMessage: + reprap.GetPlatform().LogRemoteMessage(buf->id.Src(), buf->msg.canMessageLogMessage, buf->dataLength); + break; + #if SUPPORT_ACCELEROMETERS case CanMessageType::accelerometerData: Accelerometers::ProcessReceivedData(buf->id.Src(), buf->msg.accelerometerData, buf->dataLength); diff --git a/src/ClosedLoop/ClosedLoop.cpp b/src/ClosedLoop/ClosedLoop.cpp index 23da4060f8..0910104e8e 100644 --- a/src/ClosedLoop/ClosedLoop.cpp +++ b/src/ClosedLoop/ClosedLoop.cpp @@ -25,9 +25,11 @@ //constexpr uint8_t DefaultResolution = 10; constexpr uint16_t DefaultSamplingRate = 1000; +static uint8_t rateRequested; // The sampling rate static uint8_t modeRequested; // The sampling mode static uint32_t filterRequested; // A filter for what data is collected static DriverId deviceRequested; // The driver being sampled +static uint8_t movementRequested; // The movement to be made whilst recording static volatile uint32_t numSamplesRequested; // The number of samples to collect //static uint8_t resolution = DefaultResolution; // The resolution at which to collect the samples static FileStore* volatile closedLoopFile = nullptr; // This is non-null when the data collection is running, null otherwise @@ -36,6 +38,8 @@ static unsigned int numRemoteOverflows; static unsigned int expectedRemoteSampleNumber = 0; static CanAddress expectedRemoteBoardAddress = CanId::NoAddress; +uint16_t typeFilter; + // DEBUG - Remove static bool received; static uint8_t rawMsg[64]; @@ -53,6 +57,10 @@ GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, // If the user is requesting a recording, check if one is already happening if (recording && closedLoopFile != nullptr) { + closedLoopFile->Truncate(); // truncate the file in case we didn't write all the preallocated space + closedLoopFile->Close(); + closedLoopFile = nullptr; + reply.copy("Closed loop data is already being collected"); return GCodeResult::error; } @@ -74,7 +82,7 @@ GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, // We are recording, parse the additional parameters bool seen = false; - uint32_t parsedA, parsedD; + uint32_t parsedA, parsedD, parsedR, parsedV; gb.TryGetUIValue('A', parsedA, seen); if (!seen) {parsedA = 0;} @@ -83,7 +91,16 @@ GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, gb.TryGetUIValue('D', parsedD, seen); if (!seen) {parsedD = 0;} + seen = false; + gb.TryGetUIValue('R', parsedR, seen); + if (!seen) {parsedR = 100;} + + seen = false; + gb.TryGetUIValue('V', parsedV, seen); + if (!seen) {parsedV = 0;} + // Validate the parameters + // TODO: Move a lot of this to the expansion board! if (!driverId.IsRemote())// Check the chosen drive is a closed loop drive - TODO: Think of how we can do this. For now, just check it is remote { reply.printf("Drive is not in closed loop mode (driverId.IsRemote() = %d)", driverId.IsRemote()); @@ -102,10 +119,30 @@ GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, // Validation passed - store the values modeRequested = parsedA; + rateRequested = parsedR; filterRequested = parsedD; deviceRequested = driverId; + movementRequested = parsedV; numSamplesRequested = parsedS; + // Calculate the type filter (i.e. the types of the data in a given position in the packet) + // 0 indicates int16_t, 1 indicates float + typeFilter = 0; + for (int i = 15; i>=0; i--) + { + if ((filterRequested >> 1) & 0x1) { + typeFilter <<= 1; + switch(i) { + case 1: + case 2: + case 5: + case 6: + case 7: + typeFilter |= 0x1; + } + } + } + // Estimate how large the file will be const uint32_t preallocSize = 1000; //TODO! numSamplesRequested * ((numAxes * (3 + GetDecimalPlaces(resolution))) + 4); @@ -171,7 +208,7 @@ GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, expectedRemoteSampleNumber = 0; expectedRemoteBoardAddress = deviceRequested.boardAddress; numRemoteOverflows = 0; - const GCodeResult rslt = CanInterface::StartClosedLoopDataCollection(deviceRequested, filterRequested, numSamplesRequested, modeRequested, gb, reply); + const GCodeResult rslt = CanInterface::StartClosedLoopDataCollection(deviceRequested, filterRequested, numSamplesRequested, rateRequested, movementRequested, modeRequested, gb, reply); if (rslt > GCodeResult::warning) { closedLoopFile->Close(); @@ -182,6 +219,7 @@ GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, } // Process closed loop data received over CAN +// TODO: Remove raw! void ClosedLoop::ProcessReceivedData(CanAddress src, const CanMessageClosedLoopData& msg, size_t msgLen, uint8_t raw[]) noexcept { @@ -198,23 +236,23 @@ void ClosedLoop::ProcessReceivedData(CanAddress src, const CanMessageClosedLoopD // f->Close(); // closedLoopFile = nullptr; // } else if - if (msg.firstSampleNumber != expectedRemoteSampleNumber || src != expectedRemoteBoardAddress) - { - f->Write("Received mismatched data\n"); - String temp; - temp.printf("%d instead of %d\n", msg.firstSampleNumber, expectedRemoteSampleNumber); - f->Write(temp.c_str()); - f->Truncate(); // truncate the file in case we didn't write all the preallocated space - f->Close(); - closedLoopFile = nullptr; - } - else - { - unsigned int numSamples = msg.numSamples; +// if (msg.firstSampleNumber != expectedRemoteSampleNumber || src != expectedRemoteBoardAddress) +// { +// f->Write("Received mismatched data\n"); +// String temp; +// temp.printf("%d instead of %d\n", msg.firstSampleNumber, expectedRemoteSampleNumber); +// f->Write(temp.c_str()); +// f->Truncate(); // truncate the file in case we didn't write all the preallocated space +// f->Close(); +// closedLoopFile = nullptr; +// } +// else +// { + unsigned int numSamples = msg.numSamples; // const unsigned int numAxes = (expectedRemoteAxes & 1u) + ((expectedRemoteAxes >> 1) & 1u) + ((expectedRemoteAxes >> 2) & 1u); - size_t dataIndex = 0; - uint16_t currentBits = 0; - unsigned int bitsLeft = 0; + size_t dataIndex = 0; + uint16_t currentBits = 0; + unsigned int bitsLeft = 0; // const unsigned int receivedResolution = msg.bitsPerSampleMinusOne + 1; // const uint16_t mask = (1u << receivedResolution) - 1; // const int decimalPlaces = GetDecimalPlaces(receivedResolution); @@ -223,13 +261,14 @@ void ClosedLoop::ProcessReceivedData(CanAddress src, const CanMessageClosedLoopD // ++numRemoteOverflows; // } - while (numSamples != 0) - { - String temp; - temp.printf("%u", expectedRemoteSampleNumber); - ++expectedRemoteSampleNumber; + int sampleNum = 0; + while (numSamples != 0) + { + String temp; + temp.printf("%u", msg.firstSampleNumber + sampleNum); + ++expectedRemoteSampleNumber; - // TODO: Parse data! + // TODO: Parse data! // for (unsigned int axis = 0; axis < numAxes; ++axis) // { // // Extract one value from the message. A value spans at most two words in the buffer. @@ -261,37 +300,44 @@ void ClosedLoop::ProcessReceivedData(CanAddress src, const CanMessageClosedLoopD // temp.catf(",%.*f", decimalPlaces, (double)fVal); // } - // Count how many bits are set in 'filter' - // TODO: Look into a more efficient way of doing this - int variableCount = 0; - int tmpFilter = filterRequested; - while (tmpFilter != 0) { - variableCount += tmpFilter & 0x1; - tmpFilter >>= 1; - } - - // Pull all the variables from the data packet - for (int i=0;i < variableCount; i++) - { - int16_t val = msg.data[i]; - temp.catf(",%d", val); - } - temp.cat("\n"); - - f->Write(temp.c_str()); - --numSamples; + // Count how many bits are set in 'filter' + // TODO: Look into a more efficient way of doing this + int variableCount = 0; + int tmpFilter = filterRequested; + while (tmpFilter != 0) { + variableCount += tmpFilter & 0x1; + tmpFilter >>= 1; } - if (msg.lastPacket) + // Pull all the variables from the data packet + for (int i=0;i < variableCount; i++) { + float val = msg.data[sampleNum*variableCount + i]; +// if ((typeFilter >> i) & 0x1) +// { +// temp.catf(",%f", (float) val); +// } else { + temp.catf(",%f", val); +// } + + } + temp.cat("\n"); + + f->Write(temp.c_str()); + --numSamples; + sampleNum++; + } + + if (msg.lastPacket) + { // String temp; // temp.printf("Rate %u, overflows %u\n", msg.actualSampleRate, numRemoteOverflows); // f->Write(temp.c_str()); - f->Truncate(); // truncate the file in case we didn't write all the preallocated space - f->Close(); - closedLoopFile = nullptr; - } + f->Truncate(); // truncate the file in case we didn't write all the preallocated space + f->Close(); + closedLoopFile = nullptr; } +// } } } diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp index 88733042bc..f5dd5a648d 100644 --- a/src/GCodes/GCodes3.cpp +++ b/src/GCodes/GCodes3.cpp @@ -1378,6 +1378,7 @@ GCodeResult GCodes::ConfigureDriver(GCodeBuffer& gb, const StringRef& reply) THR #if SUPPORT_CAN_EXPANSION case 5: + // TODO: Move this into `if (id.boardAddress != CanInterface::GetCanAddress())` at the top? return ClosedLoop::StartDataCollection(id, gb, reply); #endif diff --git a/src/Platform/Logger.h b/src/Platform/Logger.h index b4c24f2534..739fc4f5d5 100644 --- a/src/Platform/Logger.h +++ b/src/Platform/Logger.h @@ -11,7 +11,7 @@ #include #include -NamedEnum(LogLevel, uint8_t, off, warn, info, debug); +#include #if HAS_MASS_STORAGE diff --git a/src/Platform/Platform.cpp b/src/Platform/Platform.cpp index 2a54ca6eed..470dd0214e 100644 --- a/src/Platform/Platform.cpp +++ b/src/Platform/Platform.cpp @@ -3677,6 +3677,37 @@ const char *Platform::GetLogFileName() const noexcept return (logger == nullptr) ? nullptr : logger->GetFileName(); } +// Log a message from a remote board +String remoteMessageBuffer; +// TODO: Does it make sense to extend this to receive any message types from a remote board? +void Platform::LogRemoteMessage(CanAddress src, const CanMessageLogMessage& msg, size_t msgLen) noexcept +{ + // TODO: There will be a better way of doing this + MessageType type; + if (msg.type == LogLevel::warn) { + type = MessageType::LogWarn; + } else if (msg.type == LogLevel::info) { + type = MessageType::LogInfo; + } else if (msg.type == LogLevel::debug) { + type = (MessageType)(~(LogMessageLowBit | LogMessageHighBit)); + } else { + type = MessageType::LogOff; + } + + // TODO: What do we do for a message > 500 chars? + + // Cat to the buffer + // TODO: There needs to be some check that all these packets are all part of the same message + // They could even be from different boards atm! + remoteMessageBuffer.cat(msg.message); + + // If this is the last packet, log out to the file + if (msg.lastPacket) { + Message(type, remoteMessageBuffer.c_str()); + remoteMessageBuffer.Clear(); + } +} + #endif const char *Platform::GetLogLevel() const noexcept diff --git a/src/Platform/Platform.h b/src/Platform/Platform.h index ac73580e6e..b101a3b1ba 100644 --- a/src/Platform/Platform.h +++ b/src/Platform/Platform.h @@ -592,6 +592,7 @@ class Platform INHERIT_OBJECT_MODEL #if HAS_MASS_STORAGE GCodeResult ConfigureLogging(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); const char *GetLogFileName() const noexcept; + void LogRemoteMessage(CanAddress src, const CanMessageLogMessage& msg, size_t msgLen) noexcept; #endif // Ancillary PWM From 45d768ce8072ebcb6a64cd0c0bdc8839cd8620db Mon Sep 17 00:00:00 2001 From: Louis Irwin Date: Thu, 12 Aug 2021 15:52:43 +0100 Subject: [PATCH 04/10] Added current error to closed loop data collection --- src/ClosedLoop/ClosedLoop.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/ClosedLoop/ClosedLoop.cpp b/src/ClosedLoop/ClosedLoop.cpp index 0910104e8e..b1857f3eaf 100644 --- a/src/ClosedLoop/ClosedLoop.cpp +++ b/src/ClosedLoop/ClosedLoop.cpp @@ -178,7 +178,7 @@ GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, temp.printf("Sample"); if (filterRequested & 1) {temp.cat(",Raw Encoder Reading");} if (filterRequested & 2) {temp.cat(",Current Motor Steps");} - if (filterRequested & 4) {temp.cat(",TargetMotorSteps");} + if (filterRequested & 4) {temp.cat(",Target Motor Steps");} if (filterRequested & 8) {temp.cat(",Step Phase");} if (filterRequested & 16) {temp.cat(",PID Control Signal");} if (filterRequested & 32) {temp.cat(",PID P Term");} @@ -188,6 +188,7 @@ GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, if (filterRequested & 512) {temp.cat(",Desired Step Phase");} if (filterRequested & 1024) {temp.cat(",Coil A Current");} if (filterRequested & 2048) {temp.cat(",Coil B Current");} + if (filterRequested & 4096) {temp.cat(",Current Error");} temp.Erase(temp.strlen(), 1); temp.cat("\n"); f->Write(temp.c_str()); From 941718add495c19bf59bdc0a6cba1a3ee1121232 Mon Sep 17 00:00:00 2001 From: Louis Irwin Date: Mon, 23 Aug 2021 11:02:27 +0100 Subject: [PATCH 05/10] Tidied closed loop control files --- src/CAN/CommandProcessor.cpp | 6 +- src/ClosedLoop/ClosedLoop.cpp | 286 ++++++++++------------------------ src/ClosedLoop/ClosedLoop.h | 12 +- src/GCodes/GCodes3.cpp | 3 +- src/Platform/Logger.h | 2 +- src/Platform/Platform.cpp | 31 ---- src/Platform/Platform.h | 1 - 7 files changed, 91 insertions(+), 250 deletions(-) diff --git a/src/CAN/CommandProcessor.cpp b/src/CAN/CommandProcessor.cpp index f774eedac9..3dd962d7aa 100644 --- a/src/CAN/CommandProcessor.cpp +++ b/src/CAN/CommandProcessor.cpp @@ -547,11 +547,7 @@ void CommandProcessor::ProcessReceivedMessage(CanMessageBuffer *buf) noexcept break; case CanMessageType::closedLoopData: - ClosedLoop::ProcessReceivedData(buf->id.Src(), buf->msg.closedLoopData, buf->dataLength, buf->msg.raw); - break; - - case CanMessageType::logMessage: - reprap.GetPlatform().LogRemoteMessage(buf->id.Src(), buf->msg.canMessageLogMessage, buf->dataLength); + ClosedLoop::ProcessReceivedData(buf->id.Src(), buf->msg.closedLoopData, buf->dataLength); break; #if SUPPORT_ACCELEROMETERS diff --git a/src/ClosedLoop/ClosedLoop.cpp b/src/ClosedLoop/ClosedLoop.cpp index b1857f3eaf..753d04a3b3 100644 --- a/src/ClosedLoop/ClosedLoop.cpp +++ b/src/ClosedLoop/ClosedLoop.cpp @@ -9,40 +9,78 @@ #if SUPPORT_CAN_EXPANSION -#include -#include -#include -#include -//#include -//#include -//#include - -# include +# include # include +# include +# include +# include # include +# include # include -//constexpr uint8_t DefaultResolution = 10; -constexpr uint16_t DefaultSamplingRate = 1000; +constexpr unsigned int MaxSamples = 65535; // This comes from the fact CanMessageClosedLoopData->firstSampleNumber has a max value of 65535 static uint8_t rateRequested; // The sampling rate -static uint8_t modeRequested; // The sampling mode +static uint8_t modeRequested; // The sampling mode(immediate or on next move) static uint32_t filterRequested; // A filter for what data is collected static DriverId deviceRequested; // The driver being sampled static uint8_t movementRequested; // The movement to be made whilst recording static volatile uint32_t numSamplesRequested; // The number of samples to collect -//static uint8_t resolution = DefaultResolution; // The resolution at which to collect the samples static FileStore* volatile closedLoopFile = nullptr; // This is non-null when the data collection is running, null otherwise -static unsigned int numRemoteOverflows; static unsigned int expectedRemoteSampleNumber = 0; static CanAddress expectedRemoteBoardAddress = CanId::NoAddress; -uint16_t typeFilter; +bool OpenDataCollectionFile(String filename, unsigned int size) +{ + // Create default filename if one is not provided + if (filename.IsEmpty()) + { + const time_t time = reprap.GetPlatform().GetDateTime(); + tm timeInfo; + gmtime_r(&time, &timeInfo); + filename.printf("0:/sys/closed-loop/%u_%04u-%02u-%02u_%02u.%02u.%02u.csv", + (unsigned int) deviceRequested.boardAddress, + timeInfo.tm_year + 1900, timeInfo.tm_mon + 1, timeInfo.tm_mday, timeInfo.tm_hour, timeInfo.tm_min, timeInfo.tm_sec); + } -// DEBUG - Remove -static bool received; -static uint8_t rawMsg[64]; + // Create the file + FileStore * const f = MassStorage::OpenFile(filename.c_str(), OpenMode::write, size); + if (f == nullptr) {return false;} + + // Write the header line + { + String temp; + temp.printf("Sample"); + if (filterRequested & 1) {temp.cat(",Raw Encoder Reading");} + if (filterRequested & 2) {temp.cat(",Current Motor Steps");} + if (filterRequested & 4) {temp.cat(",Target Motor Steps");} + if (filterRequested & 8) {temp.cat(",Current Error");} + if (filterRequested & 16) {temp.cat(",PID Control Signal");} + if (filterRequested & 32) {temp.cat(",PID P Term");} + if (filterRequested & 64) {temp.cat(",PID I Term");} + if (filterRequested & 128) {temp.cat(",PID D Term");} + if (filterRequested & 256) {temp.cat(",Step Phase");} + if (filterRequested & 512) {temp.cat(",Desired Step Phase");} + if (filterRequested & 1024) {temp.cat(",Phase Shift");} + if (filterRequested & 2048) {temp.cat(",Coil A Current");} + if (filterRequested & 4096) {temp.cat(",Coil B Current");} + + temp.Erase(temp.strlen(), 1); + temp.cat("\n"); + f->Write(temp.c_str()); + } + + closedLoopFile = f; + return true; +} + +void CloseDataCollectionFile() +{ + closedLoopFile->Truncate(); // truncate the file in case we didn't write all the preallocated space + closedLoopFile->Close(); + closedLoopFile = nullptr; +} // Handle M569.5 - Collect closed loop data GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException) @@ -57,10 +95,7 @@ GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, // If the user is requesting a recording, check if one is already happening if (recording && closedLoopFile != nullptr) { - closedLoopFile->Truncate(); // truncate the file in case we didn't write all the preallocated space - closedLoopFile->Close(); - closedLoopFile = nullptr; - + CloseDataCollectionFile(); reply.copy("Closed loop data is already being collected"); return GCodeResult::error; } @@ -77,7 +112,6 @@ GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, { reply.printf("Collecting sample: %d/%d", (int) expectedRemoteSampleNumber, (int) numSamplesRequested); return GCodeResult::ok; - // TODO: Check if the user supplied additional params & send them back a warning if so? } // We are recording, parse the additional parameters @@ -100,20 +134,19 @@ GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, if (!seen) {parsedV = 0;} // Validate the parameters - // TODO: Move a lot of this to the expansion board! - if (!driverId.IsRemote())// Check the chosen drive is a closed loop drive - TODO: Think of how we can do this. For now, just check it is remote + if (!driverId.IsRemote()) // Check the chosen drive is a closed loop drive - The expansion board does this also, so we just need to check that the driver is remote. { reply.printf("Drive is not in closed loop mode (driverId.IsRemote() = %d)", driverId.IsRemote()); return GCodeResult::error; } - if (parsedS > 65535) // TODO: What is a good maximum value here? + if (parsedS > MaxSamples) { - reply.copy("Maximum number of samples that can be collected is 65535"); + reply.copy("Maximum number of samples that can be collected is %d", MaxSamples); return GCodeResult::error; } if (parsedA > 1) { - reply.printf("Mode (A) can be either 0 or 1 - A%d is invalid", parsedA); + reply.printf("Mode (A) can be either 0 or 1 - A%d is invalid", (int) parsedA); return GCodeResult::error; } @@ -125,82 +158,23 @@ GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, movementRequested = parsedV; numSamplesRequested = parsedS; - // Calculate the type filter (i.e. the types of the data in a given position in the packet) - // 0 indicates int16_t, 1 indicates float - typeFilter = 0; - for (int i = 15; i>=0; i--) - { - if ((filterRequested >> 1) & 0x1) { - typeFilter <<= 1; - switch(i) { - case 1: - case 2: - case 5: - case 6: - case 7: - typeFilter |= 0x1; - } - } - } - // Estimate how large the file will be const uint32_t preallocSize = 1000; //TODO! numSamplesRequested * ((numAxes * (3 + GetDecimalPlaces(resolution))) + 4); - // Create the filename + // Create the file String closedLoopFileName; if (gb.Seen('F')) { - String temp; - gb.GetQuotedString(temp.GetRef(), false); - MassStorage::CombineName(closedLoopFileName.GetRef(), "0:/sys/closed-loop/", temp.c_str()); - } - else - { - const time_t time = reprap.GetPlatform().GetDateTime(); - tm timeInfo; - gmtime_r(&time, &timeInfo); - closedLoopFileName.printf("0:/sys/closed-loop/%u_%04u-%02u-%02u_%02u.%02u.%02u.csv", - (unsigned int) deviceRequested.boardAddress, - timeInfo.tm_year + 1900, timeInfo.tm_mon + 1, timeInfo.tm_mday, timeInfo.tm_hour, timeInfo.tm_min, timeInfo.tm_sec); - } - - // Create the file - FileStore * const f = MassStorage::OpenFile(closedLoopFileName.c_str(), OpenMode::write, preallocSize); - if (f == nullptr) - { - reply.copy("Failed to create closed loop data file"); - return GCodeResult::error; - } - - // Write the header line - { - String temp; - temp.printf("Sample"); - if (filterRequested & 1) {temp.cat(",Raw Encoder Reading");} - if (filterRequested & 2) {temp.cat(",Current Motor Steps");} - if (filterRequested & 4) {temp.cat(",Target Motor Steps");} - if (filterRequested & 8) {temp.cat(",Step Phase");} - if (filterRequested & 16) {temp.cat(",PID Control Signal");} - if (filterRequested & 32) {temp.cat(",PID P Term");} - if (filterRequested & 64) {temp.cat(",PID I Term");} - if (filterRequested & 128) {temp.cat(",PID D Term");} - if (filterRequested & 256) {temp.cat(",Phase Shift");} - if (filterRequested & 512) {temp.cat(",Desired Step Phase");} - if (filterRequested & 1024) {temp.cat(",Coil A Current");} - if (filterRequested & 2048) {temp.cat(",Coil B Current");} - if (filterRequested & 4096) {temp.cat(",Current Error");} - temp.Erase(temp.strlen(), 1); - temp.cat("\n"); - f->Write(temp.c_str()); + String tempFilename; + gb.GetQuotedString(tempFilename.GetRef(), false); + MassStorage::CombineName(closedLoopFileName.GetRef(), "0:/sys/closed-loop/", tempFilename.c_str()); } - closedLoopFile = f; + OpenDataCollectionFile(closedLoopFileName, preallocSize); // If no samples have been requested, return with an info message if (numSamplesRequested == 0) { - closedLoopFile->Truncate(); - closedLoopFile->Close(); - closedLoopFile = nullptr; + CloseDataCollectionFile(); reply.copy("no samples recorded"); return GCodeResult::warning; } @@ -208,137 +182,45 @@ GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, // Set up & start the CAN data transfer expectedRemoteSampleNumber = 0; expectedRemoteBoardAddress = deviceRequested.boardAddress; - numRemoteOverflows = 0; const GCodeResult rslt = CanInterface::StartClosedLoopDataCollection(deviceRequested, filterRequested, numSamplesRequested, rateRequested, movementRequested, modeRequested, gb, reply); if (rslt > GCodeResult::warning) { - closedLoopFile->Close(); - closedLoopFile = nullptr; + CloseDataCollectionFile(); MassStorage::Delete(closedLoopFileName.c_str(), false); } return rslt; } // Process closed loop data received over CAN -// TODO: Remove raw! -void ClosedLoop::ProcessReceivedData(CanAddress src, const CanMessageClosedLoopData& msg, size_t msgLen, uint8_t raw[]) noexcept +void ClosedLoop::ProcessReceivedData(CanAddress src, const CanMessageClosedLoopData& msg, size_t msgLen) noexcept { - -// memcpy(rawMsg, &raw, 64); -// received = true; - FileStore * const f = closedLoopFile; if (f != nullptr) { -// if (msgLen < msg.GetActualDataLength()) -// { -// f->Write("Received bad data\n"); -// f->Truncate(); // truncate the file in case we didn't write all the preallocated space -// f->Close(); -// closedLoopFile = nullptr; -// } else if -// if (msg.firstSampleNumber != expectedRemoteSampleNumber || src != expectedRemoteBoardAddress) -// { -// f->Write("Received mismatched data\n"); -// String temp; -// temp.printf("%d instead of %d\n", msg.firstSampleNumber, expectedRemoteSampleNumber); -// f->Write(temp.c_str()); -// f->Truncate(); // truncate the file in case we didn't write all the preallocated space -// f->Close(); -// closedLoopFile = nullptr; -// } -// else -// { unsigned int numSamples = msg.numSamples; -// const unsigned int numAxes = (expectedRemoteAxes & 1u) + ((expectedRemoteAxes >> 1) & 1u) + ((expectedRemoteAxes >> 2) & 1u); - size_t dataIndex = 0; - uint16_t currentBits = 0; - unsigned int bitsLeft = 0; -// const unsigned int receivedResolution = msg.bitsPerSampleMinusOne + 1; -// const uint16_t mask = (1u << receivedResolution) - 1; -// const int decimalPlaces = GetDecimalPlaces(receivedResolution); -// if (msg.overflowed)/ -// { -// ++numRemoteOverflows; -// } + const size_t variableCount = msg.GetFilterSetBits(); - int sampleNum = 0; while (numSamples != 0) { - String temp; - temp.printf("%u", msg.firstSampleNumber + sampleNum); - ++expectedRemoteSampleNumber; - - // TODO: Parse data! -// for (unsigned int axis = 0; axis < numAxes; ++axis) -// { -// // Extract one value from the message. A value spans at most two words in the buffer. -// uint16_t val = currentBits; -// if (bitsLeft >= receivedResolution) -// { -// bitsLeft -= receivedResolution; -// currentBits >>= receivedResolution; -// } -// else -// { -// currentBits = msg.data[dataIndex++]; -// val |= currentBits << bitsLeft; -// currentBits >>= receivedResolution - bitsLeft; -// bitsLeft += 16 - receivedResolution; -// } -// val &= mask; -// -// // Sign-extend it -// if (val & (1u << (receivedResolution - 1))) -// { -// val |= ~mask; -// } -// -// // Convert it to a float number of g -// const float fVal = (float)(int16_t)val/(float)(1u << GetBitsAfterPoint(receivedResolution)); -// -// // Append it to the buffer -// temp.catf(",%.*f", decimalPlaces, (double)fVal); -// } - - // Count how many bits are set in 'filter' - // TODO: Look into a more efficient way of doing this - int variableCount = 0; - int tmpFilter = filterRequested; - while (tmpFilter != 0) { - variableCount += tmpFilter & 0x1; - tmpFilter >>= 1; - } - - // Pull all the variables from the data packet - for (int i=0;i < variableCount; i++) + // Compile the data + String currentLine; + size_t sampleIndex = msg.numSamples - numSamples; + currentLine.printf("%u", msg.firstSampleNumber + sampleIndex); + for (size_t i=0; i < variableCount; i++) { - float val = msg.data[sampleNum*variableCount + i]; -// if ((typeFilter >> i) & 0x1) -// { -// temp.catf(",%f", (float) val); -// } else { - temp.catf(",%f", val); -// } - + currentLine.catf(",%f", (double) msg.data[sampleIndex*variableCount + i]); } - temp.cat("\n"); + currentLine.cat("\n"); - f->Write(temp.c_str()); - --numSamples; - sampleNum++; - } + // Write the data + f->Write(currentLine.c_str()); - if (msg.lastPacket) - { -// String temp; -// temp.printf("Rate %u, overflows %u\n", msg.actualSampleRate, numRemoteOverflows); -// f->Write(temp.c_str()); - f->Truncate(); // truncate the file in case we didn't write all the preallocated space - f->Close(); - closedLoopFile = nullptr; + // Increment the working variables + expectedRemoteSampleNumber++; + numSamples--; } -// } + + if (msg.lastPacket) {CloseDataCollectionFile();} } } diff --git a/src/ClosedLoop/ClosedLoop.h b/src/ClosedLoop/ClosedLoop.h index b9ed2469be..250dfec2d8 100644 --- a/src/ClosedLoop/ClosedLoop.h +++ b/src/ClosedLoop/ClosedLoop.h @@ -8,18 +8,12 @@ #include #include -//#if SUPPORT_CAN_EXPANSION -//# include -//#endif - class CanMessageClosedLoopData; namespace ClosedLoop { -// GCodeResult ConfigureAccelerometer(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); -// GCodeResult StartAccelerometer(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); -# if SUPPORT_CAN_EXPANSION +#if SUPPORT_CAN_EXPANSION GCodeResult StartDataCollection(DriverId, GCodeBuffer&, const StringRef&) THROWS(GCodeException); - void ProcessReceivedData(CanAddress, const CanMessageClosedLoopData&, size_t, uint8_t[64]) noexcept; -# endif + void ProcessReceivedData(CanAddress, const CanMessageClosedLoopData&, size_t) noexcept; +#endif } diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp index 8ac689d914..42441c6bef 100644 --- a/src/GCodes/GCodes3.cpp +++ b/src/GCodes/GCodes3.cpp @@ -1375,7 +1375,8 @@ GCodeResult GCodes::ConfigureDriver(GCodeBuffer& gb, const StringRef& reply) THR #if SUPPORT_CAN_EXPANSION case 5: - // TODO: Move this into `if (id.boardAddress != CanInterface::GetCanAddress())` at the top? + // In all valid cases, this will be handled by CanInterface::ConfigureRemoteDriver at the top of this function + // However, this is required here to reject the erroneous case of the requested driver not being remote. return ClosedLoop::StartDataCollection(id, gb, reply); #endif diff --git a/src/Platform/Logger.h b/src/Platform/Logger.h index 739fc4f5d5..b4c24f2534 100644 --- a/src/Platform/Logger.h +++ b/src/Platform/Logger.h @@ -11,7 +11,7 @@ #include #include -#include +NamedEnum(LogLevel, uint8_t, off, warn, info, debug); #if HAS_MASS_STORAGE diff --git a/src/Platform/Platform.cpp b/src/Platform/Platform.cpp index e53e0e4edb..73c030792e 100644 --- a/src/Platform/Platform.cpp +++ b/src/Platform/Platform.cpp @@ -3679,37 +3679,6 @@ const char *Platform::GetLogFileName() const noexcept return (logger == nullptr) ? nullptr : logger->GetFileName(); } -// Log a message from a remote board -String remoteMessageBuffer; -// TODO: Does it make sense to extend this to receive any message types from a remote board? -void Platform::LogRemoteMessage(CanAddress src, const CanMessageLogMessage& msg, size_t msgLen) noexcept -{ - // TODO: There will be a better way of doing this - MessageType type; - if (msg.type == LogLevel::warn) { - type = MessageType::LogWarn; - } else if (msg.type == LogLevel::info) { - type = MessageType::LogInfo; - } else if (msg.type == LogLevel::debug) { - type = (MessageType)(~(LogMessageLowBit | LogMessageHighBit)); - } else { - type = MessageType::LogOff; - } - - // TODO: What do we do for a message > 500 chars? - - // Cat to the buffer - // TODO: There needs to be some check that all these packets are all part of the same message - // They could even be from different boards atm! - remoteMessageBuffer.cat(msg.message); - - // If this is the last packet, log out to the file - if (msg.lastPacket) { - Message(type, remoteMessageBuffer.c_str()); - remoteMessageBuffer.Clear(); - } -} - #endif const char *Platform::GetLogLevel() const noexcept diff --git a/src/Platform/Platform.h b/src/Platform/Platform.h index 6afad5d4c8..041869ff49 100644 --- a/src/Platform/Platform.h +++ b/src/Platform/Platform.h @@ -587,7 +587,6 @@ class Platform INHERIT_OBJECT_MODEL #if HAS_MASS_STORAGE GCodeResult ConfigureLogging(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); const char *GetLogFileName() const noexcept; - void LogRemoteMessage(CanAddress src, const CanMessageLogMessage& msg, size_t msgLen) noexcept; #endif // Ancillary PWM From 3b5028008d88431d5be2bb45e75abb689a8cc0e1 Mon Sep 17 00:00:00 2001 From: Louis Irwin Date: Mon, 23 Aug 2021 11:06:11 +0100 Subject: [PATCH 06/10] Reverted changes to cproject --- .cproject | 76 +++++++++++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 74 insertions(+), 2 deletions(-) diff --git a/.cproject b/.cproject index 19c18b8eab..6a31679011 100644 --- a/.cproject +++ b/.cproject @@ -1569,6 +1569,9 @@ + + + @@ -1578,30 +1581,99 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + From 7678e35e665e361ad5876c676eee26802e8fc93d Mon Sep 17 00:00:00 2001 From: Louis Irwin Date: Mon, 23 Aug 2021 11:21:03 +0100 Subject: [PATCH 07/10] Moved closed-loop recording consts to shared CANlib file --- src/ClosedLoop/ClosedLoop.cpp | 357 ++-------------------------------- 1 file changed, 13 insertions(+), 344 deletions(-) diff --git a/src/ClosedLoop/ClosedLoop.cpp b/src/ClosedLoop/ClosedLoop.cpp index 753d04a3b3..7f4a3d33cc 100644 --- a/src/ClosedLoop/ClosedLoop.cpp +++ b/src/ClosedLoop/ClosedLoop.cpp @@ -52,19 +52,19 @@ bool OpenDataCollectionFile(String filename, unsigned int siz { String temp; temp.printf("Sample"); - if (filterRequested & 1) {temp.cat(",Raw Encoder Reading");} - if (filterRequested & 2) {temp.cat(",Current Motor Steps");} - if (filterRequested & 4) {temp.cat(",Target Motor Steps");} - if (filterRequested & 8) {temp.cat(",Current Error");} - if (filterRequested & 16) {temp.cat(",PID Control Signal");} - if (filterRequested & 32) {temp.cat(",PID P Term");} - if (filterRequested & 64) {temp.cat(",PID I Term");} - if (filterRequested & 128) {temp.cat(",PID D Term");} - if (filterRequested & 256) {temp.cat(",Step Phase");} - if (filterRequested & 512) {temp.cat(",Desired Step Phase");} - if (filterRequested & 1024) {temp.cat(",Phase Shift");} - if (filterRequested & 2048) {temp.cat(",Coil A Current");} - if (filterRequested & 4096) {temp.cat(",Coil B Current");} + if (filterRequested & CL_RECORD_RAW_ENCODER_READING) {temp.cat(",Raw Encoder Reading");} + if (filterRequested & CL_RECORD_CURRENT_MOTOR_STEPS) {temp.cat(",Current Motor Steps");} + if (filterRequested & CL_RECORD_TARGET_MOTOR_STEPS) {temp.cat(",Target Motor Steps");} + if (filterRequested & CL_RECORD_CURRENT_ERROR) {temp.cat(",Current Error");} + if (filterRequested & CL_RECORD_PID_CONTROL_SIGNAL) {temp.cat(",PID Control Signal");} + if (filterRequested & CL_RECORD_PID_P_TERM) {temp.cat(",PID P Term");} + if (filterRequested & CL_RECORD_PID_I_TERM) {temp.cat(",PID I Term");} + if (filterRequested & CL_RECORD_PID_D_TERM) {temp.cat(",PID D Term");} + if (filterRequested & CL_RECORD_STEP_PHASE) {temp.cat(",Step Phase");} + if (filterRequested & CL_RECORD_DESIRED_STEP_PHASE) {temp.cat(",Desired Step Phase");} + if (filterRequested & CL_RECORD_PHASE_SHIFT) {temp.cat(",Phase Shift");} + if (filterRequested & CL_RECORD_COIL_A_CURRENT) {temp.cat(",Coil A Current");} + if (filterRequested & CL_RECORD_COIL_B_CURRENT) {temp.cat(",Coil B Current");} temp.Erase(temp.strlen(), 1); temp.cat("\n"); @@ -226,335 +226,4 @@ void ClosedLoop::ProcessReceivedData(CanAddress src, const CanMessageClosedLoopD #endif - - - - - - - - - - - - - - - - - - -//static unsigned int expectedRemoteSampleNumber = 0; -//static CanAddress expectedRemoteBoardAddress = CanId::NoAddress; -//static uint8_t expectedRemoteAxes; -//static unsigned int numRemoteOverflows; -// -// -//// Get the number of binary digits after the decimal point -//static inline unsigned int GetBitsAfterPoint(uint8_t dataResolution) noexcept -//{ -// return dataResolution - 2; // assumes the range is +/- 2g -//} -// -//// Get the number of decimal places that we should use when we print each acceleration value -//static unsigned int GetDecimalPlaces(uint8_t dataResolution) noexcept -//{ -// return (GetBitsAfterPoint(dataResolution) >= 11) ? 4 : (GetBitsAfterPoint(dataResolution) >= 8) ? 3 : 2; -//} -// -//// Local accelerometer handling -// -//#include "LIS3DH.h" -// -//constexpr uint16_t DefaultSamplingRate = 1000; -//constexpr uint8_t DefaultResolution = 10; -// -//constexpr size_t AccelerometerTaskStackWords = 400; // big enough to handle printf and file writes -//static Task *accelerometerTask; -// -//static LIS3DH *accelerometer = nullptr; -// -//static uint16_t samplingRate = DefaultSamplingRate; -//static volatile uint16_t numSamplesRequested; -//static uint8_t resolution = DefaultResolution; -//static uint8_t orientation = 20; // +Z -> +Z, +X -> +X -//static volatile uint8_t axesRequested; -//static FileStore* volatile accelerometerFile = nullptr; // this is non-null when the accelerometer is running, null otherwise -//static uint8_t axisLookup[3]; -//static bool axisInverted[3]; -// -//static IoPort spiCsPort; -//static IoPort irqPort; -// -//[[noreturn]] void AccelerometerTaskCode(void*) noexcept -//{ -// for (;;) -// { -// TaskBase::Take(); -// FileStore * const f = accelerometerFile; // capture volatile variable -// if (f != nullptr) -// { -// // Collect and write the samples -// unsigned int samplesWritten = 0; -// unsigned int samplesWanted = numSamplesRequested; -// unsigned int numOverflows = 0; -// const uint16_t mask = (1u << resolution) - 1; -// const int decimalPlaces = GetDecimalPlaces(resolution); -// -// if (accelerometer->StartCollecting(axesRequested)) -// { -// uint16_t dataRate = 0; -// do -// { -// const uint16_t *data; -// bool overflowed; -// unsigned int samplesRead = accelerometer->CollectData(&data, dataRate, overflowed); -// if (samplesRead == 0) -// { -// // samplesRead == 0 indicates an error, e.g. no interrupt -// samplesWanted = 0; -// if (f != nullptr) -// { -// f->Write("Failed to collect data from accelerometer\n"); -// f->Truncate(); // truncate the file in case we didn't write all the preallocated space -// f->Close(); -// accelerometerFile = nullptr; -// } -// break; -// } -// else -// { -// if (overflowed) -// { -// ++numOverflows; -// } -// if (samplesWritten == 0) -// { -// // The first sample taken after waking up is inaccurate, so discard it -// --samplesRead; -// data += 3; -// } -// if (samplesRead >= samplesWanted) -// { -// samplesRead = samplesWanted; -// } -// -// while (samplesRead != 0) -// { -// // Write a row of data -// String temp; -// temp.printf("%u", samplesWritten); -// -// for (unsigned int axis = 0; axis < 3; ++axis) -// { -// if (axesRequested & (1u << axis)) -// { -// uint16_t dataVal = data[axisLookup[axis]]; -// if (axisInverted[axis]) -// { -// dataVal = (dataVal == 0x8000) ? ~dataVal : ~dataVal + 1; -// } -// dataVal >>= (16u - resolution); // data from LIS3DH is left justified -// -// // Sign-extend it -// if (dataVal & (1u << (resolution - 1))) -// { -// dataVal |= ~mask; -// } -// -// // Convert it to a float number of g -// const float fVal = (float)(int16_t)dataVal/(float)(1u << GetBitsAfterPoint(resolution)); -// -// // Append it to the buffer -// temp.catf(",%.*f", decimalPlaces, (double)fVal); -// } -// } -// -// data += 3; -// -// temp.cat('\n'); -// f->Write(temp.c_str()); -// -// --samplesRead; -// --samplesWanted; -// ++samplesWritten; -// } -// } -// } while (samplesWanted != 0); -// -// if (f != nullptr) -// { -// String temp; -// temp.printf("Rate %u, overflows %u\n", dataRate, numOverflows); -// f->Write(temp.c_str()); -// } -// } -// else if (f != nullptr) -// { -// f->Write("Failed to start accelerometer\n"); -// } -// -// if (f != nullptr) -// { -// f->Truncate(); // truncate the file in case we didn't write all the preallocated space -// f->Close(); -// accelerometerFile = nullptr; -// } -// -// accelerometer->StopCollecting(); -// -// // Wait for another command -// accelerometerFile = nullptr; -// } -// } -//} -// -//// Translate the orientation returning true if successful -//static bool TranslateOrientation(uint32_t input) noexcept -//{ -// if (input >= 70u) { return false; } -// const uint8_t xDigit = input % 10u; -// if (xDigit >= 7u) { return false; } -// const uint8_t zDigit = input / 10u; -// const uint8_t xOrientation = xDigit & 0x03; -// const uint8_t zOrientation = zDigit & 0x03; -// if (xOrientation == 3u || zOrientation == 3u || xOrientation == zOrientation) { return false; } -// const uint8_t xInverted = xDigit & 0x04; -// const uint8_t zInverted = zDigit & 0x04; -// uint8_t yInverted = xInverted ^ zInverted; -// -// // Calculate the Y orientation. We must have axes 0, 1 and 2 so they must add up to 3. -// const uint8_t yOrientation = 3u - xOrientation - zOrientation; -// -// // The total number of inversions must be even if the cyclic order of the axes is 012, odd if it is 210 (can we prove this?) -// if ((xOrientation + 1) % 3 != yOrientation) -// { -// yInverted ^= 0x04; // we need an odd number of axis inversions -// } -// -// // Now fill in the axis table -// axisLookup[xOrientation] = 0; -// axisInverted[xOrientation] = xInverted; -// axisLookup[yOrientation] = 1; -// axisInverted[yOrientation] = yInverted; -// axisLookup[zOrientation] = 2; -// axisInverted[zOrientation] = zInverted; -// return true; -//} -// -//// Deal with M955 -//GCodeResult Accelerometers::ConfigureAccelerometer(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException) -//{ -// gb.MustSee('P'); -// DriverId device = gb.GetDriverId(); -// -//# if SUPPORT_CAN_EXPANSION -// if (device.IsRemote()) -// { -// CanMessageGenericConstructor cons(M955Params); -// cons.PopulateFromCommand(gb); -// return cons.SendAndGetResponse(CanMessageType::accelerometerConfig, device.boardAddress, reply); -// } -//# endif -// -// if (device.localDriver != 0) -// { -// reply.copy("Only one local accelerometer is supported"); -// return GCodeResult::error; -// } -// -// // No need for task lock here because this function and the M956 function are called only by the MAIN task -// if (accelerometerFile != nullptr) -// { -// reply.copy("Cannot reconfigure accelerometer while it is collecting data"); -// return GCodeResult::error; -// } -// -// bool seen = false; -// if (gb.Seen('C')) -// { -// seen = true; -// -// // Creating a new accelerometer. First delete any existing accelerometer. -// DeleteObject(accelerometer); -// spiCsPort.Release(); -// irqPort.Release(); -// -// IoPort * const ports[2] = { &spiCsPort, &irqPort }; -// const PinAccess access[2] = { PinAccess::write1, PinAccess::read }; -// if (IoPort::AssignPorts(gb, reply, PinUsedBy::sensor, 2, ports, access) != 2) -// { -// spiCsPort.Release(); // in case it was allocated -// return GCodeResult::error; -// } -// -// const uint32_t spiFrequency = (gb.Seen('Q')) ? gb.GetLimitedUIValue('Q', 500000, 10000001) : DefaultAccelerometerSpiFrequency; -// auto temp = new LIS3DH(SharedSpiDevice::GetMainSharedSpiDevice(), spiFrequency, spiCsPort.GetPin(), irqPort.GetPin()); -// if (temp->CheckPresent()) -// { -// accelerometer = temp; -// if (accelerometerTask == nullptr) -// { -// accelerometerTask = new Task; -// accelerometerTask->Create(AccelerometerTaskCode, "ACCEL", nullptr, TaskPriority::Accelerometer); -// } -// } -// else -// { -// delete temp; -// reply.copy("Accelerometer not found on specified port"); -// return GCodeResult::error; -// } -// } -// else if (accelerometer == nullptr) -// { -// reply.copy("Accelerometer not found"); -// return GCodeResult::error; -// } -// -// uint32_t temp32; -// if (gb.TryGetLimitedUIValue('R', temp32, seen, 17)) -// { -// resolution = temp32; -// } -// -// if (gb.TryGetLimitedUIValue('S', temp32, seen, 10000)) -// { -// samplingRate = temp32; -// } -// -// if (seen) -// { -// if (!accelerometer->Configure(samplingRate, resolution)) -// { -// reply.copy("Failed to configure accelerometer"); -// return GCodeResult::error; -// } -// (void)TranslateOrientation(orientation); -// } -// -// if (gb.Seen('I')) -// { -// seen = true; -// if (!TranslateOrientation(gb.GetUIValue())) -// { -// reply.copy("Bad orientation parameter"); -// return GCodeResult::error; -// } -// } -// -//# if SUPPORT_CAN_EXPANSION -// reply.printf("Accelerometer %u:%u with orientation %u samples at %uHz with %u-bit resolution, SPI frequency %" PRIu32, -// CanInterface::GetCanAddress(), 0, orientation, samplingRate, resolution, accelerometer->GetFrequency()); -//# else -// reply.printf("Accelerometer %u with orientation %u samples at %uHz with %u-bit resolution, SPI frequency %" PRIu32, -// 0, orientation, samplingRate, resolution, accelerometer->GetFrequency()); -//# endif -// return GCodeResult::ok; -//} -// -// -// -// - // End From c2868801b7a028a77bd144813eaf8e2399a2a349 Mon Sep 17 00:00:00 2001 From: Louis Irwin Date: Mon, 23 Aug 2021 16:30:16 +0100 Subject: [PATCH 08/10] Addressed PR feedback for closed loop control --- src/ClosedLoop/ClosedLoop.cpp | 6 +- src/GCodes/GCodeBuffer/GCodeBuffer.h | 347 +++++++++++---------------- 2 files changed, 148 insertions(+), 205 deletions(-) diff --git a/src/ClosedLoop/ClosedLoop.cpp b/src/ClosedLoop/ClosedLoop.cpp index 7f4a3d33cc..0850959d3e 100644 --- a/src/ClosedLoop/ClosedLoop.cpp +++ b/src/ClosedLoop/ClosedLoop.cpp @@ -31,7 +31,7 @@ static FileStore* volatile closedLoopFile = nullptr; // This is non-null when th static unsigned int expectedRemoteSampleNumber = 0; static CanAddress expectedRemoteBoardAddress = CanId::NoAddress; -bool OpenDataCollectionFile(String filename, unsigned int size) +static bool OpenDataCollectionFile(String filename, unsigned int size) noexcept { // Create default filename if one is not provided if (filename.IsEmpty()) @@ -75,7 +75,7 @@ bool OpenDataCollectionFile(String filename, unsigned int siz return true; } -void CloseDataCollectionFile() +static void CloseDataCollectionFile() noexcept { closedLoopFile->Truncate(); // truncate the file in case we didn't write all the preallocated space closedLoopFile->Close(); @@ -141,7 +141,7 @@ GCodeResult ClosedLoop::StartDataCollection(DriverId driverId, GCodeBuffer& gb, } if (parsedS > MaxSamples) { - reply.copy("Maximum number of samples that can be collected is %d", MaxSamples); + reply.printf("Maximum number of samples that can be collected is %d", MaxSamples); return GCodeResult::error; } if (parsedA > 1) diff --git a/src/GCodes/GCodeBuffer/GCodeBuffer.h b/src/GCodes/GCodeBuffer/GCodeBuffer.h index b3a332589d..d2106f5fbe 100644 --- a/src/GCodes/GCodeBuffer/GCodeBuffer.h +++ b/src/GCodes/GCodeBuffer/GCodeBuffer.h @@ -20,53 +20,51 @@ class FileGCodeInput; // The processing state of each buffer -enum class GCodeBufferState : uint8_t { - parseNotStarted, // we haven't started parsing yet - parsingLineNumber,// we saw N at the start and we are parsing the line number - parsingWhitespace, // parsing whitespace after the line number - parsingComment, // parsing a whole-line comment that we may be interested in +enum class GCodeBufferState : uint8_t +{ + parseNotStarted, // we haven't started parsing yet + parsingLineNumber, // we saw N at the start and we are parsing the line number + parsingWhitespace, // parsing whitespace after the line number + parsingComment, // parsing a whole-line comment that we may be interested in parsingGCode, // parsing GCode words parsingBracketedComment, // inside a (...) comment - parsingQuotedString, // inside a double-quoted string - parsingChecksum, // parsing the checksum after '*' - discarding, // discarding characters after the checksum or an end-of-line comment - ready, // we have a complete gcode but haven't started executing it - executing // we have a complete gcode and have started executing it + parsingQuotedString, // inside a double-quoted string + parsingChecksum, // parsing the checksum after '*' + discarding, // discarding characters after the checksum or an end-of-line comment + ready, // we have a complete gcode but haven't started executing it + executing // we have a complete gcode and have started executing it }; // Class to hold an individual GCode and provide functions to allow it to be parsed -class GCodeBuffer INHERIT_OBJECT_MODEL { +class GCodeBuffer INHERIT_OBJECT_MODEL +{ public: friend class BinaryParser; friend class StringParser; - GCodeBuffer(GCodeChannel::RawType channel, GCodeInput *normalIn, - FileGCodeInput *fileIn, MessageType mt, Compatibility::RawType c = - Compatibility::RepRapFirmware) noexcept; - void Reset() noexcept; // Reset it to its state after start-up - void Init() noexcept; // Set it up to parse another G-code - void Diagnostics(MessageType mtype) noexcept; // Write some debug info + GCodeBuffer(GCodeChannel::RawType channel, GCodeInput *normalIn, FileGCodeInput *fileIn, MessageType mt, Compatibility::RawType c = Compatibility::RepRapFirmware) noexcept; + void Reset() noexcept; // Reset it to its state after start-up + void Init() noexcept; // Set it up to parse another G-code + void Diagnostics(MessageType mtype) noexcept; // Write some debug info - bool Put(char c) noexcept SPEED_CRITICAL; // Add a character to the end + bool Put(char c) noexcept SPEED_CRITICAL; // Add a character to the end #if HAS_LINUX_INTERFACE - void PutBinary(const uint32_t *data, size_t len) noexcept;// Add an entire binary G-Code, overwriting any existing content + void PutBinary(const uint32_t *data, size_t len) noexcept; // Add an entire binary G-Code, overwriting any existing content #endif - void PutAndDecode(const char *data, size_t len) noexcept;// Add an entire G-Code, overwriting any existing content - void PutAndDecode(const char *str) noexcept;// Add a null-terminated string, overwriting any existing content - void StartNewFile() noexcept; // Called when we start a new file - bool FileEnded() noexcept;// Called when we reach the end of the file we are reading from - void DecodeCommand() noexcept;// Decode the command in the buffer when it is complete - bool CheckMetaCommand(const StringRef &reply) THROWS(GCodeException);// Check whether the current command is a meta command, or we are skipping a block + void PutAndDecode(const char *data, size_t len) noexcept; // Add an entire G-Code, overwriting any existing content + void PutAndDecode(const char *str) noexcept; // Add a null-terminated string, overwriting any existing content + void StartNewFile() noexcept; // Called when we start a new file + bool FileEnded() noexcept; // Called when we reach the end of the file we are reading from + void DecodeCommand() noexcept; // Decode the command in the buffer when it is complete + bool CheckMetaCommand(const StringRef& reply) THROWS(GCodeException); // Check whether the current command is a meta command, or we are skipping a block char GetCommandLetter() const noexcept; bool HasCommandNumber() const noexcept; int GetCommandNumber() const noexcept; - int8_t GetCommandFraction() const noexcept; // Return the command fraction, or -1 if none given + int8_t GetCommandFraction() const noexcept; // Return the command fraction, or -1 if none given bool ContainsExpression() const noexcept; - void GetCompleteParameters(const StringRef &str) THROWS(GCodeException);// Get all of the line following the command. Currently called only for the Q0 command. - int32_t GetLineNumber() const noexcept { - return CurrentFileMachineState().lineNumber; - } + void GetCompleteParameters(const StringRef& str) THROWS(GCodeException); // Get all of the line following the command. Currently called only for the Q0 command. + int32_t GetLineNumber() const noexcept { return CurrentFileMachineState().lineNumber; } bool IsLastCommand() const noexcept; GCodeResult GetLastResult() const noexcept { return lastResult; } void SetLastResult(GCodeResult r) noexcept { lastResult = r; } @@ -121,23 +119,17 @@ class GCodeBuffer INHERIT_OBJECT_MODEL { bool IsIdle() const noexcept; bool IsCompletelyIdle() const noexcept; - bool IsReady() const noexcept;// Return true if a gcode is ready but hasn't been started yet - bool IsExecuting() const noexcept;// Return true if a gcode has been started and is not paused - void SetFinished(bool f) noexcept; // Set the G Code executed (or not) + bool IsReady() const noexcept; // Return true if a gcode is ready but hasn't been started yet + bool IsExecuting() const noexcept; // Return true if a gcode has been started and is not paused + void SetFinished(bool f) noexcept; // Set the G Code executed (or not) void SetCommsProperties(uint32_t arg) noexcept; - GCodeMachineState& LatestMachineState() const noexcept { - return *machineState; - } + GCodeMachineState& LatestMachineState() const noexcept { return *machineState; } GCodeMachineState& CurrentFileMachineState() const noexcept; GCodeMachineState& OriginalMachineState() const noexcept; - GCodeMachineState::BlockState& GetBlockState() const noexcept { - return CurrentFileMachineState().CurrentBlockState(); - } - uint16_t GetBlockIndent() const noexcept { - return GetBlockState().GetIndent(); - } + GCodeMachineState::BlockState& GetBlockState() const noexcept { return CurrentFileMachineState().CurrentBlockState(); } + uint16_t GetBlockIndent() const noexcept { return GetBlockState().GetIndent(); } void UseInches(bool inchesNotMm) noexcept { machineState->usingInches = inchesNotMm; } bool UsingInches() const noexcept { return machineState->usingInches; } @@ -147,92 +139,52 @@ class GCodeBuffer INHERIT_OBJECT_MODEL { float InverseConvertSpeed(float speed) const noexcept; const char *GetDistanceUnits() const noexcept; unsigned int GetStackDepth() const noexcept; - bool PushState(bool withinSameFile) noexcept;// Push state returning true if successful (i.e. stack not overflowed) - bool PopState(bool withinSameFile) noexcept;// Pop state returning true if successful (i.e. no stack underrun) + bool PushState(bool withinSameFile) noexcept; // Push state returning true if successful (i.e. stack not overflowed) + bool PopState(bool withinSameFile) noexcept; // Pop state returning true if successful (i.e. no stack underrun) void AbortFile(bool abortAll, bool requestAbort = true) noexcept; - bool IsDoingFile() const noexcept;// Return true if this source is executing a file - bool IsDoingLocalFile() const noexcept; // Return true if this source is executing a file from the local SD card - bool IsDoingFileMacro() const noexcept; // Return true if this source is executing a file macro - FilePosition GetFilePosition() const noexcept;// Get the file position at the start of the current command + bool IsDoingFile() const noexcept; // Return true if this source is executing a file + bool IsDoingLocalFile() const noexcept; // Return true if this source is executing a file from the local SD card + bool IsDoingFileMacro() const noexcept; // Return true if this source is executing a file macro + FilePosition GetFilePosition() const noexcept; // Get the file position at the start of the current command - void WaitForAcknowledgement() noexcept; // Flag that we are waiting for acknowledgement + void WaitForAcknowledgement() noexcept; // Flag that we are waiting for acknowledgement #if HAS_LINUX_INTERFACE - bool IsBinary() const noexcept { - return isBinaryBuffer; - } // Return true if the code is in binary format - - bool IsFileFinished() const noexcept;// Return true if this source has finished execution of a file - void SetFileFinished() noexcept; // Mark the current file as finished - void SetPrintFinished() noexcept;// Mark the current print file as finished - - bool RequestMacroFile(const char *filename, bool fromCode) noexcept;// Request execution of a file macro - volatile bool IsWaitingForMacro() const noexcept { - return isWaitingForMacro; - } // Indicates if the GB is waiting for a macro to be opened - bool HasJustStartedMacro() const noexcept { - return macroJustStarted; - } // Has this GB just started a new macro file? - bool IsMacroRequestPending() const noexcept { - return !requestedMacroFile.IsEmpty(); - } // Indicates if a macro file is being requested - const char* GetRequestedMacroFile() const noexcept { - return requestedMacroFile.c_str(); - } // Return requested macro file or nullptr if none - bool IsMacroStartedByCode() const noexcept; // Indicates if the last macro was requested from a code - void MacroRequestSent() noexcept { - requestedMacroFile.Clear(); - } // Called when a macro file request has been sent - void ResolveMacroRequest(bool hadError, bool isEmpty) noexcept; // Resolve the call waiting for a macro to be executed - bool IsMacroEmpty() const noexcept { - return macroFileEmpty; - } // Return true if the opened macro file is actually empty - - void MacroFileClosed() noexcept;// Called to notify the SBC about the file being internally closed on success - volatile bool IsMacroFileClosed() const noexcept { - return macroFileClosed; - } // Indicates if a file has been closed internally in RRF - void MacroFileClosedSent() noexcept { - macroFileClosed = false; - }// Called when the SBC has been notified about the internally closed file - - bool IsAbortRequested() const noexcept { - return abortFile; - } // Is the cancellation of the current file requested? - bool IsAbortAllRequested() const noexcept { - return abortAllFiles; - }// Is the cancellation of all files being executed on this channel requested? - void FileAbortSent() noexcept { - abortFile = abortAllFiles = false; - }// Called when the SBC has been notified about a file (or all files) being closed - - bool IsMessagePromptPending() const noexcept { - return messagePromptPending; - }// Is the SBC supposed to be notified about a message waiting for acknowledgement? - void MessagePromptSent() noexcept { - messagePromptPending = false; - }// Called when the SBC has been notified about a message waiting for acknowledgement - bool IsMessageAcknowledged() const noexcept { - return messageAcknowledged; - } // Indicates if a message has been acknowledged - void MessageAcknowledgementSent() noexcept { - messageAcknowledged = false; - }// Called when the SBC has been notified about the message acknowledgement - - bool IsInvalidated() const noexcept { - return invalidated; - } // Indicates if the channel is invalidated - void Invalidate(bool i = true) noexcept { - invalidated = i; - } // Invalidate this channel (or not) - - bool IsSendRequested() const noexcept { - return sendToSbc; - } // Is this code supposed to be sent to the SBC - void SendToSbc() noexcept { - sendToSbc = true; - } // Send this code to the attached SBC + bool IsBinary() const noexcept { return isBinaryBuffer; } // Return true if the code is in binary format + + bool IsFileFinished() const noexcept; // Return true if this source has finished execution of a file + void SetFileFinished() noexcept; // Mark the current file as finished + void SetPrintFinished() noexcept; // Mark the current print file as finished + + bool RequestMacroFile(const char *filename, bool fromCode) noexcept; // Request execution of a file macro + volatile bool IsWaitingForMacro() const noexcept { return isWaitingForMacro; } // Indicates if the GB is waiting for a macro to be opened + bool HasJustStartedMacro() const noexcept { return macroJustStarted; } // Has this GB just started a new macro file? + bool IsMacroRequestPending() const noexcept { return !requestedMacroFile.IsEmpty(); } // Indicates if a macro file is being requested + const char *GetRequestedMacroFile() const noexcept { return requestedMacroFile.c_str(); } // Return requested macro file or nullptr if none + bool IsMacroStartedByCode() const noexcept; // Indicates if the last macro was requested from a code + void MacroRequestSent() noexcept { requestedMacroFile.Clear(); } // Called when a macro file request has been sent + void ResolveMacroRequest(bool hadError, bool isEmpty) noexcept; // Resolve the call waiting for a macro to be executed + bool IsMacroEmpty() const noexcept { return macroFileEmpty; } // Return true if the opened macro file is actually empty + + void MacroFileClosed() noexcept; // Called to notify the SBC about the file being internally closed on success + volatile bool IsMacroFileClosed() const noexcept { return macroFileClosed; } // Indicates if a file has been closed internally in RRF + void MacroFileClosedSent() noexcept { macroFileClosed = false; } // Called when the SBC has been notified about the internally closed file + + bool IsAbortRequested() const noexcept { return abortFile; } // Is the cancellation of the current file requested? + bool IsAbortAllRequested() const noexcept { return abortAllFiles; } // Is the cancellation of all files being executed on this channel requested? + void FileAbortSent() noexcept { abortFile = abortAllFiles = false; } // Called when the SBC has been notified about a file (or all files) being closed + + bool IsMessagePromptPending() const noexcept { return messagePromptPending; } // Is the SBC supposed to be notified about a message waiting for acknowledgement? + void MessagePromptSent() noexcept { messagePromptPending = false; } // Called when the SBC has been notified about a message waiting for acknowledgement + bool IsMessageAcknowledged() const noexcept { return messageAcknowledged; } // Indicates if a message has been acknowledged + void MessageAcknowledgementSent() noexcept { messageAcknowledged = false; } // Called when the SBC has been notified about the message acknowledgement + + bool IsInvalidated() const noexcept { return invalidated; } // Indicates if the channel is invalidated + void Invalidate(bool i = true) noexcept { invalidated = i; } // Invalidate this channel (or not) + + bool IsSendRequested() const noexcept { return sendToSbc; } // Is this code supposed to be sent to the SBC + void SendToSbc() noexcept { sendToSbc = true; } // Send this code to the attached SBC #endif GCodeState GetState() const noexcept; @@ -241,50 +193,35 @@ class GCodeBuffer INHERIT_OBJECT_MODEL { void AdvanceState() noexcept; void MessageAcknowledged(bool cancelled) noexcept; - GCodeChannel GetChannel() const noexcept { - return codeChannel; - } - const char* GetIdentity() const noexcept { - return codeChannel.ToString(); - } + GCodeChannel GetChannel() const noexcept { return codeChannel; } + const char *GetIdentity() const noexcept { return codeChannel.ToString(); } bool CanQueueCodes() const noexcept; MessageType GetResponseMessageType() const noexcept; #if HAS_MASS_STORAGE - bool OpenFileToWrite(const char *directory, const char *fileName, - const FilePosition size, const bool binaryWrite, - const uint32_t fileCRC32) noexcept; - // open a file to write to - bool IsWritingFile() const noexcept; // Returns true if writing a file - void WriteToFile() noexcept; // Write the current GCode to file - - bool IsWritingBinary() const noexcept; // Returns true if writing binary - bool WriteBinaryToFile(char b) noexcept;// Write a byte to the file, returning true if the upload is now complete + bool OpenFileToWrite(const char* directory, const char* fileName, const FilePosition size, const bool binaryWrite, const uint32_t fileCRC32) noexcept; + // open a file to write to + bool IsWritingFile() const noexcept; // Returns true if writing a file + void WriteToFile() noexcept; // Write the current GCode to file + + bool IsWritingBinary() const noexcept; // Returns true if writing binary + bool WriteBinaryToFile(char b) noexcept; // Write a byte to the file, returning true if the upload is now complete void FinishWritingBinary() noexcept; #endif - const char* DataStart() const noexcept; // Get the start of the current command - size_t DataLength() const noexcept; // Get the length of the current command + const char* DataStart() const noexcept; // Get the start of the current command + size_t DataLength() const noexcept; // Get the length of the current command - void PrintCommand(const StringRef &s) const noexcept; + void PrintCommand(const StringRef& s) const noexcept; void AppendFullCommand(const StringRef &s) const noexcept; - bool IsTimerRunning() const noexcept { - return timerRunning; - } - uint32_t WhenTimerStarted() const noexcept { - return whenTimerStarted; - } + bool IsTimerRunning() const noexcept { return timerRunning; } + uint32_t WhenTimerStarted() const noexcept { return whenTimerStarted; } void StartTimer() noexcept; - void StopTimer() noexcept { - timerRunning = false; - } - bool DoDwellTime(uint32_t dwellMillis) noexcept;// Execute a dwell returning true if it has finished + void StopTimer() noexcept { timerRunning = false; } + bool DoDwellTime(uint32_t dwellMillis) noexcept; // Execute a dwell returning true if it has finished - void ResetReportDueTimer() noexcept { - whenReportDueTimerStarted = millis(); - } - ; + void ResetReportDueTimer() noexcept { whenReportDueTimerStarted = millis(); }; bool IsReportDue() noexcept; void RestartFrom(FilePosition pos) noexcept; @@ -292,26 +229,19 @@ class GCodeBuffer INHERIT_OBJECT_MODEL { #if HAS_MASS_STORAGE || HAS_EMBEDDED_FILES FileGCodeInput *GetFileInput() const noexcept { return fileInput; } #endif - GCodeInput* GetNormalInput() const noexcept { - return normalInput; - } + GCodeInput *GetNormalInput() const noexcept { return normalInput; } - void MotionCommanded() noexcept { - motionCommanded = true; - } - void MotionStopped() noexcept { - motionCommanded = false; - } - bool WasMotionCommanded() const noexcept { - return motionCommanded; - } + void MotionCommanded() noexcept { motionCommanded = true; } + void MotionStopped() noexcept { motionCommanded = false; } + bool WasMotionCommanded() const noexcept { return motionCommanded; } - void AddParameters(VariableSet &vars, int codeRunning) noexcept; + void AddParameters(VariableSet& vars, int codeRunning) noexcept; VariableSet& GetVariables() const noexcept; Mutex mutex; -protected:DECLARE_OBJECT_MODEL +protected: + DECLARE_OBJECT_MODEL private: bool SeenAny(Bitmap bm) const noexcept; // Return true if any of the parameter letters in the bitmap were seen @@ -325,17 +255,17 @@ protected:DECLARE_OBJECT_MODEL } #if SUPPORT_OBJECT_MODEL - const char* GetStateText() const noexcept; + const char *GetStateText() const noexcept; #endif - const GCodeChannel codeChannel; // Channel number of this instance - GCodeInput *normalInput;// Our normal input stream, or nullptr if there isn't one + const GCodeChannel codeChannel; // Channel number of this instance + GCodeInput *normalInput; // Our normal input stream, or nullptr if there isn't one #if HAS_MASS_STORAGE || HAS_EMBEDDED_FILES FileGCodeInput *fileInput; // Our file input stream for when we are reading from a print file or a macro file, may be shared with other GCodeBuffers #endif - const MessageType responseMessageType;// The message type we use for responses to string codes coming from this channel + const MessageType responseMessageType; // The message type we use for responses to string codes coming from this channel GCodeResult lastResult; @@ -345,21 +275,21 @@ protected:DECLARE_OBJECT_MODEL StringParser stringParser; - GCodeBufferState bufferState; // Idle, executing or paused - GCodeMachineState *machineState; // Machine state for this gcode source + GCodeBufferState bufferState; // Idle, executing or paused + GCodeMachineState *machineState; // Machine state for this gcode source - uint32_t whenTimerStarted; // When we started waiting - uint32_t whenReportDueTimerStarted; // When the report-due-timer has been started + uint32_t whenTimerStarted; // When we started waiting + uint32_t whenReportDueTimerStarted; // When the report-due-timer has been started static constexpr uint32_t reportDueInterval = 1000; // Interval in which we send in ms #if HAS_LINUX_INTERFACE bool isBinaryBuffer; #endif - bool timerRunning; // True if we are waiting - bool motionCommanded;// true if this GCode stream has commanded motion since it last waited for motion to stop + bool timerRunning; // True if we are waiting + bool motionCommanded; // true if this GCode stream has commanded motion since it last waited for motion to stop #if HAS_LINUX_INTERFACE - alignas(4) char buffer[MaxCodeBufferSize];// must be aligned because we do dword fetches from it + alignas(4) char buffer[MaxCodeBufferSize]; // must be aligned because we do dword fetches from it #else char buffer[GCODE_LENGTH]; #endif @@ -368,25 +298,27 @@ protected:DECLARE_OBJECT_MODEL // Accessed by both the Main and Linux tasks BinarySemaphore macroSemaphore; volatile bool isWaitingForMacro; // Is this GB waiting in DoFileMacro? - volatile bool macroFileClosed;// Last macro file has been closed in RRF, tell the SBC + volatile bool macroFileClosed; // Last macro file has been closed in RRF, tell the SBC // Accessed only when the GB mutex is acquired String requestedMacroFile; - uint8_t macroJustStarted :1,// Whether the GB has just started a macro file - macroFileError :1,// Whether the macro file could be opened or if an error occurred - macroFileEmpty :1, // Whether the macro file is actually empty - abortFile :1, // Whether to abort the last file on the stack - abortAllFiles :1, // Whether to abort all opened files - sendToSbc :1,// Indicates if the GB string content is supposed to be sent to the SBC - messagePromptPending :1,// Has the SBC been notified about a message waiting for acknowledgement? - messageAcknowledged :1; // Last message has been acknowledged + uint8_t + macroJustStarted : 1, // Whether the GB has just started a macro file + macroFileError : 1, // Whether the macro file could be opened or if an error occurred + macroFileEmpty : 1, // Whether the macro file is actually empty + abortFile : 1, // Whether to abort the last file on the stack + abortAllFiles : 1, // Whether to abort all opened files + sendToSbc : 1, // Indicates if the GB string content is supposed to be sent to the SBC + messagePromptPending : 1, // Has the SBC been notified about a message waiting for acknowledgement? + messageAcknowledged : 1; // Last message has been acknowledged // Accessed only by the Linux task - bool invalidated;// Set to true if the GB content is not valid and about to be cleared + bool invalidated; // Set to true if the GB content is not valid and about to be cleared #endif }; -inline bool GCodeBuffer::IsDoingFileMacro() const noexcept { +inline bool GCodeBuffer::IsDoingFileMacro() const noexcept +{ #if HAS_LINUX_INTERFACE return machineState->doingFileMacro || IsMacroRequestPending(); #else @@ -396,39 +328,47 @@ inline bool GCodeBuffer::IsDoingFileMacro() const noexcept { #if HAS_LINUX_INTERFACE -inline bool GCodeBuffer::IsFileFinished() const noexcept { +inline bool GCodeBuffer::IsFileFinished() const noexcept +{ return machineState->fileFinished; } -inline bool GCodeBuffer::IsMacroStartedByCode() const noexcept { +inline bool GCodeBuffer::IsMacroStartedByCode() const noexcept +{ return machineState->macroStartedByCode; } #endif -inline GCodeState GCodeBuffer::GetState() const noexcept { +inline GCodeState GCodeBuffer::GetState() const noexcept +{ return machineState->GetState(); } -inline void GCodeBuffer::SetState(GCodeState newState) noexcept { +inline void GCodeBuffer::SetState(GCodeState newState) noexcept +{ machineState->SetState(newState); } -inline void GCodeBuffer::SetState(GCodeState newState, uint16_t param) noexcept { +inline void GCodeBuffer::SetState(GCodeState newState, uint16_t param) noexcept +{ machineState->stateParameter = param; machineState->SetState(newState); } -inline void GCodeBuffer::AdvanceState() noexcept { +inline void GCodeBuffer::AdvanceState() noexcept +{ machineState->AdvanceState(); } // Return true if we can queue gcodes from this source. This is the case if a file is being executed -inline bool GCodeBuffer::CanQueueCodes() const noexcept { +inline bool GCodeBuffer::CanQueueCodes() const noexcept +{ return machineState->DoingFile(); } -inline bool GCodeBuffer::IsDoingFile() const noexcept { +inline bool GCodeBuffer::IsDoingFile() const noexcept +{ #if HAS_LINUX_INTERFACE return machineState->DoingFile() || IsMacroRequestPending(); #else @@ -436,16 +376,19 @@ inline bool GCodeBuffer::IsDoingFile() const noexcept { #endif } -inline bool GCodeBuffer::IsReady() const noexcept { +inline bool GCodeBuffer::IsReady() const noexcept +{ return bufferState == GCodeBufferState::ready; } -inline bool GCodeBuffer::IsExecuting() const noexcept { +inline bool GCodeBuffer::IsExecuting() const noexcept +{ return bufferState == GCodeBufferState::executing; } // Return true if this source is executing a file from the local SD card -inline bool GCodeBuffer::IsDoingLocalFile() const noexcept { +inline bool GCodeBuffer::IsDoingLocalFile() const noexcept +{ #if HAS_LINUX_INTERFACE return !IsBinary() && IsDoingFile(); #else From 577da7b0c8e836f80a413119e2941fc2e86efbce Mon Sep 17 00:00:00 2001 From: Louis Irwin Date: Tue, 24 Aug 2021 13:42:43 +0100 Subject: [PATCH 09/10] Added timestamp to closed loop data packets --- src/ClosedLoop/ClosedLoop.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ClosedLoop/ClosedLoop.cpp b/src/ClosedLoop/ClosedLoop.cpp index 0850959d3e..ea28230062 100644 --- a/src/ClosedLoop/ClosedLoop.cpp +++ b/src/ClosedLoop/ClosedLoop.cpp @@ -51,7 +51,7 @@ static bool OpenDataCollectionFile(String filename, unsigned // Write the header line { String temp; - temp.printf("Sample"); + temp.printf("Sample,Timestamp"); if (filterRequested & CL_RECORD_RAW_ENCODER_READING) {temp.cat(",Raw Encoder Reading");} if (filterRequested & CL_RECORD_CURRENT_MOTOR_STEPS) {temp.cat(",Current Motor Steps");} if (filterRequested & CL_RECORD_TARGET_MOTOR_STEPS) {temp.cat(",Target Motor Steps");} @@ -198,7 +198,7 @@ void ClosedLoop::ProcessReceivedData(CanAddress src, const CanMessageClosedLoopD if (f != nullptr) { unsigned int numSamples = msg.numSamples; - const size_t variableCount = msg.GetFilterSetBits(); + const size_t variableCount = msg.GetVariableCount(); while (numSamples != 0) { From 3487b46a833bdc2173283f9220eb786a360606bf Mon Sep 17 00:00:00 2001 From: Louis Irwin Date: Tue, 24 Aug 2021 13:50:45 +0100 Subject: [PATCH 10/10] Minor bugfix on closed loop data collection --- src/ClosedLoop/ClosedLoop.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ClosedLoop/ClosedLoop.cpp b/src/ClosedLoop/ClosedLoop.cpp index ea28230062..3a7ce018fa 100644 --- a/src/ClosedLoop/ClosedLoop.cpp +++ b/src/ClosedLoop/ClosedLoop.cpp @@ -51,7 +51,7 @@ static bool OpenDataCollectionFile(String filename, unsigned // Write the header line { String temp; - temp.printf("Sample,Timestamp"); + temp.copy("Sample,Timestamp"); if (filterRequested & CL_RECORD_RAW_ENCODER_READING) {temp.cat(",Raw Encoder Reading");} if (filterRequested & CL_RECORD_CURRENT_MOTOR_STEPS) {temp.cat(",Current Motor Steps");} if (filterRequested & CL_RECORD_TARGET_MOTOR_STEPS) {temp.cat(",Target Motor Steps");}