More changes to support ATE

2 files changed, 396 insertions, 0 deletions

diff --git a/src/InputMonitors/InputMonitor.cpp b/src/InputMonitors/InputMonitor.cpp
new file mode 100644
index 00000000..25f1ff74
--- /dev/null
+++ b/src/InputMonitors/InputMonitor.cpp
@@ -0,0 +1,328 @@
+/*
+ * InputMonitor.cpp
+ *
+ *  Created on: 17 Sep 2019
+ *      Author: David
+ */
+
+#include "InputMonitor.h"
+
+#if SUPPORT_CAN_EXPANSION
+
+#include <CanMessageFormats.h>
+#include <Hardware/IoPorts.h>
+#include <CAN/CanInterface.h>
+#include <CanMessageBuffer.h>
+
+InputMonitor * volatile InputMonitor::monitorsList = nullptr;
+InputMonitor * volatile InputMonitor::freeList = nullptr;
+ReadWriteLock InputMonitor::listLock;
+
+bool InputMonitor::Activate() noexcept
+{
+	bool ok = true;
+	if (!active)
+	{
+		if (threshold == 0)
+		{
+			// Digital input
+			const irqflags_t flags = cpu_irq_save();
+			ok = port.AttachInterrupt(CommonDigitalPortInterrupt, InterruptMode::change, CallbackParameter(this));
+			state = port.ReadDigital();
+			cpu_irq_restore(flags);
+		}
+		else
+		{
+			// Analog port
+			state = port.ReadAnalog() >= threshold;
+			ok = port.SetAnalogCallback(CommonAnalogPortInterrupt, CallbackParameter(this), 1);
+		}
+		active = true;
+		whenLastSent = millis();
+	}
+
+	return ok;
+}
+
+void InputMonitor::Deactivate() noexcept
+{
+	//TODO
+	active = false;
+}
+
+// Return the analog value of this input
+uint16_t InputMonitor::GetAnalogValue() const noexcept
+{
+	return (threshold != 0) ? port.ReadAnalog()
+			: port.ReadDigital() ? 0xFFFF
+				: 0;
+}
+
+void InputMonitor::DigitalInterrupt() noexcept
+{
+	const bool newState = port.ReadDigital();
+	if (newState != state)
+	{
+		state = newState;
+		if (active)
+		{
+			sendDue = true;
+			CanInterface::WakeAsyncSenderFromIsr();
+		}
+	}
+}
+
+void InputMonitor::AnalogInterrupt(uint16_t reading) noexcept
+{
+	const bool newState = reading >= threshold;
+	if (newState != state)
+	{
+		state = newState;
+		if (active)
+		{
+			sendDue = true;
+			CanInterface::WakeAsyncSenderFromIsr();
+		}
+	}
+}
+
+/*static*/ void InputMonitor::Init() noexcept
+{
+	// Nothing needed here yet
+}
+
+/*static*/ void InputMonitor::CommonDigitalPortInterrupt(CallbackParameter cbp) noexcept
+{
+	static_cast<InputMonitor*>(cbp.vp)->DigitalInterrupt();
+}
+
+/*static*/ void InputMonitor::CommonAnalogPortInterrupt(CallbackParameter cbp, uint16_t reading) noexcept
+{
+	static_cast<InputMonitor*>(cbp.vp)->AnalogInterrupt(reading);
+}
+
+/*static*/ ReadLockedPointer<InputMonitor> InputMonitor::Find(uint16_t hndl) noexcept
+{
+	ReadLocker lock(listLock);
+	InputMonitor *current = monitorsList;
+	while (current != nullptr && current->handle != hndl)
+	{
+		current = current->next;
+	}
+	return ReadLockedPointer<InputMonitor>(lock, current);
+}
+
+// Delete a monitor. Must own the write lock before calling this.
+/*static*/ bool InputMonitor::Delete(uint16_t hndl) noexcept
+{
+	InputMonitor *prev = nullptr, *current = monitorsList;
+	while (current != nullptr)
+	{
+		if (current->handle == hndl)
+		{
+			current->Deactivate();
+			if (prev == nullptr)
+			{
+				monitorsList = current->next;
+			}
+			else
+			{
+				prev->next = current->next;
+			}
+			current->next = freeList;
+			freeList = current;
+			return true;
+		}
+		prev = current;
+		current = current->next;
+	}
+
+	return false;
+}
+
+/*static*/ GCodeResult InputMonitor::Create(const CanMessageCreateInputMonitor& msg, size_t dataLength, const StringRef& reply, uint8_t& extra) noexcept
+{
+	WriteLocker lock(listLock);
+
+	Delete(msg.handle.u.all);						// delete any existing monitor with the same handle
+
+	// Allocate a new one
+	InputMonitor *newMonitor;
+	if (freeList == nullptr)
+	{
+		newMonitor = new InputMonitor;
+	}
+	else
+	{
+		newMonitor = freeList;
+		freeList = newMonitor->next;
+	}
+
+	newMonitor->handle = msg.handle.u.all;
+	newMonitor->active = false;
+	newMonitor->state = false;
+	newMonitor->minInterval = msg.minInterval;
+	newMonitor->threshold = msg.threshold;
+	newMonitor->sendDue = false;
+	String<StringLength50> pinName;
+	pinName.copy(msg.pinName, msg.GetMaxPinNameLength(dataLength));
+	if (newMonitor->port.AssignPort(pinName.c_str(), reply, PinUsedBy::endstop, (msg.threshold == 0) ? PinAccess::read : PinAccess::readAnalog))
+	{
+		newMonitor->next = monitorsList;
+		monitorsList = newMonitor;
+		const bool ok = newMonitor->Activate();
+		extra = (newMonitor->state) ? 1 : 0;
+		if (!ok)
+		{
+			reply.copy("Failed to set pin change interrupt");
+			return GCodeResult::error;
+		}
+		return GCodeResult::ok;
+	}
+
+	newMonitor->next = freeList;
+	freeList = newMonitor;
+	return GCodeResult::error;
+}
+
+/*static*/ GCodeResult InputMonitor::Change(const CanMessageChangeInputMonitor& msg, const StringRef& reply, uint8_t& extra) noexcept
+{
+	if (msg.action == CanMessageChangeInputMonitor::actionDelete)
+	{
+		WriteLocker lock(listLock);
+
+		if (Delete(msg.handle.u.all))
+		{
+			return GCodeResult::ok;
+		}
+
+		reply.printf("Board %u does not have input handle %04x", CanInterface::GetCanAddress(), msg.handle.u.all);
+		return GCodeResult::warning;					// only a warning when deleting a non-existent handle
+	}
+
+	auto m = Find(msg.handle.u.all);
+	if (m.IsNull())
+	{
+		reply.printf("Board %u does not have input handle %04x", CanInterface::GetCanAddress(), msg.handle.u.all);
+		return GCodeResult::error;
+	}
+
+	GCodeResult rslt;
+	switch (msg.action)
+	{
+	case CanMessageChangeInputMonitor::actionDoMonitor:
+		rslt = (m->Activate()) ? GCodeResult::ok : GCodeResult::error;
+		break;
+
+	case CanMessageChangeInputMonitor::actionDontMonitor:
+		m->Deactivate();
+		rslt = GCodeResult::ok;
+		break;
+
+	case CanMessageChangeInputMonitor::actionReturnPinName:
+		m->port.AppendPinName(reply);
+		reply.catf(", min interval %ums", m->minInterval);
+		rslt = GCodeResult::ok;
+		break;
+
+	case CanMessageChangeInputMonitor::actionChangeThreshold:
+		m->threshold = msg.param;
+		rslt = GCodeResult::ok;
+		break;
+
+	case CanMessageChangeInputMonitor::actionChangeMinInterval:
+		m->minInterval = msg.param;
+		rslt = GCodeResult::ok;
+		break;
+
+	default:
+		reply.printf("ChangeInputMonitor action #%u not implemented", msg.action);
+		rslt = GCodeResult::error;
+		break;
+	}
+
+	extra = (m->state) ? 1 : 0;
+	return rslt;
+}
+
+// Check the input monitors and add any pending ones to the message
+// Return the number of ticks before we should be woken again, or TaskBase::TimeoutUnlimited if we shouldn't be work until an input changes state
+/*static*/ uint32_t InputMonitor::AddStateChanges(CanMessageInputChanged *msg) noexcept
+{
+	uint32_t timeToWait = TaskBase::TimeoutUnlimited;
+	ReadLocker lock(listLock);
+
+	const uint32_t now = millis();
+	for (InputMonitor *p = monitorsList; p != nullptr; p = p->next)
+	{
+		if (p->sendDue)
+		{
+			const uint32_t age = now - p->whenLastSent;
+			if (age >= p->minInterval)
+			{
+				bool monitorState;
+				{
+					InterruptCriticalSectionLocker ilock;
+					p->sendDue = false;
+					monitorState = p->state;
+				}
+
+				if (msg->AddEntry(p->handle, monitorState))
+				{
+					p->whenLastSent = now;
+				}
+				else
+				{
+					p->sendDue = true;
+					return 1;
+				}
+			}
+			else
+			{
+				// The state has changed but we've recently sent a state change for this input
+				const uint32_t timeLeft = p->minInterval - age;
+				if (timeLeft < timeToWait)
+				{
+					timeToWait = timeLeft;
+				}
+			}
+		}
+	}
+	return timeToWait;
+}
+
+// Read the specified inputs. The incoming message is a CanMessageReadInputsRequest. We return a CanMessageReadInputsReply in the same buffer.
+/*static*/ void InputMonitor::ReadInputs(CanMessageBuffer *buf) noexcept
+{
+	// Extract data before we overwrite the message
+	const CanMessageReadInputsRequest& req = buf->msg.readInputsRequest;
+	const CanAddress srcAddress = buf->id.Src();
+	const uint16_t rid = req.requestId;
+	const uint16_t mask = req.mask.u.all;
+	const uint16_t pattern = req.pattern.u.all & mask;
+
+	// Construct the new message in the same buffer
+	auto reply = buf->SetupResponseMessage<CanMessageReadInputsReply>(rid, CanInterface::GetCanAddress(), srcAddress);
+
+	unsigned int count = 0;
+	ReadLocker lock(listLock);
+	InputMonitor *h = monitorsList;
+	while (h != nullptr && count < ARRAY_SIZE(reply->results))
+	{
+		if ((h->handle & mask) == pattern)
+		{
+			reply->results[count].handle.Set(h->handle);
+			reply->results[count].value = h->GetAnalogValue();
+			++count;
+		}
+		h = h->next;
+	}
+
+	reply->numReported = count;
+	reply->resultCode = (uint32_t)GCodeResult::ok;
+	buf->dataLength = reply->GetActualDataLength();
+}
+
+#endif	// SUPPORT_CAN_EXPANSION
+
+// End
diff --git a/src/InputMonitors/InputMonitor.h b/src/InputMonitors/InputMonitor.h
new file mode 100644
index 00000000..ac43f2ea
--- /dev/null
+++ b/src/InputMonitors/InputMonitor.h
@@ -0,0 +1,68 @@
+/*
+ * InputMonitor.h
+ *
+ *  Created on: 17 Sep 2019
+ *      Author: David
+ */
+
+#ifndef SRC_ENDSTOPS_INPUTMONITOR_H_
+#define SRC_ENDSTOPS_INPUTMONITOR_H_
+
+#include <RepRapFirmware.h>
+
+#if SUPPORT_CAN_EXPANSION
+
+#include <Hardware/IoPorts.h>
+#include <GCodes/GCodeResult.h>
+#include <RTOSIface/RTOSIface.h>
+
+struct CanMessageCreateInputMonitor;
+struct CanMessageChangeInputMonitor;
+struct CanMessageInputChanged;
+class CanMessageBuffer;
+
+class InputMonitor
+{
+public:
+	InputMonitor() noexcept { }
+
+	static void Init() noexcept;
+
+	static GCodeResult Create(const CanMessageCreateInputMonitor& msg, size_t dataLength, const StringRef& reply, uint8_t& extra) noexcept;
+	static GCodeResult Change(const CanMessageChangeInputMonitor& msg, const StringRef& reply, uint8_t& extra) noexcept;
+
+	static uint32_t AddStateChanges(CanMessageInputChanged *msg) noexcept;
+	static void ReadInputs(CanMessageBuffer *buf) noexcept;
+
+	static void CommonDigitalPortInterrupt(CallbackParameter cbp) noexcept;
+	static void CommonAnalogPortInterrupt(CallbackParameter cbp, uint16_t reading) noexcept;
+
+private:
+	bool Activate() noexcept;
+	void Deactivate() noexcept;
+	void DigitalInterrupt() noexcept;
+	void AnalogInterrupt(uint16_t reading) noexcept;
+	uint16_t GetAnalogValue() const noexcept;
+
+	static bool Delete(uint16_t hndl) noexcept;
+	static ReadLockedPointer<InputMonitor> Find(uint16_t hndl) noexcept;
+
+	InputMonitor *next;
+	IoPort port;
+	uint32_t whenLastSent;
+	uint16_t handle;
+	uint16_t minInterval;
+	uint16_t threshold;
+	bool active;
+	volatile bool state;
+	volatile bool sendDue;
+
+	static InputMonitor * volatile monitorsList;
+	static InputMonitor * volatile freeList;
+
+	static ReadWriteLock listLock;
+};
+
+#endif	// SUPPORT_CAN_EXPANSION
+
+#endif /* SRC_ENDSTOPS_INPUTMONITOR_H_ */