Added more remote command functionality

3 files changed, 218 insertions, 7 deletions

diff --git a/src/GPIO/GpOutPort.cpp b/src/GPIO/GpOutPort.cpp
index 51f32c78..e1de8dbf 100644
--- a/src/GPIO/GpOutPort.cpp
+++ b/src/GPIO/GpOutPort.cpp
@@ -15,6 +15,10 @@
 # include <CAN/CanMessageGenericConstructor.h>
 #endif
 
+#if SUPPORT_REMOTE_COMMANDS
+# include <CanMessageGenericParser.h>
+#endif
+
 #if SUPPORT_OBJECT_MODEL
 
 // Object model table and functions
@@ -42,7 +46,7 @@ bool GpOutputPort::IsUnused() const noexcept
 {
 	return
 #if SUPPORT_CAN_EXPANSION
-		boardAddress == CanId::MasterAddress &&
+		boardAddress == CanInterface::GetCanAddress() &&
 #endif
 		!port.IsValid();
 }
@@ -63,7 +67,7 @@ GCodeResult GpOutputPort::Configure(uint32_t gpioNumber, bool isServo, GCodeBuff
 
 		// Remove any existing assignment
 #if SUPPORT_CAN_EXPANSION
-		if (boardAddress != CanId::MasterAddress)
+		if (boardAddress != CanInterface::GetCanAddress())
 		{
 			CanMessageGenericConstructor cons(M950GpioParams);
 			cons.AddUParam('P', gpioNumber);
@@ -73,7 +77,7 @@ GCodeResult GpOutputPort::Configure(uint32_t gpioNumber, bool isServo, GCodeBuff
 				reprap.GetPlatform().Message(WarningMessage, reply.c_str());
 				reply.Clear();
 			}
-			boardAddress = CanId::MasterAddress;
+			boardAddress = CanInterface::GetCanAddress();
 		}
 #endif
 		port.Release();
@@ -87,7 +91,7 @@ GCodeResult GpOutputPort::Configure(uint32_t gpioNumber, bool isServo, GCodeBuff
 
 #if SUPPORT_CAN_EXPANSION
 		boardAddress = IoPort::RemoveBoardAddress(pinName.GetRef());
-		if (boardAddress != CanId::MasterAddress)
+		if (boardAddress != CanInterface::GetCanAddress())
 		{
 			CanMessageGenericConstructor cons(M950GpioParams);
 			cons.AddUParam('P', gpioNumber);
@@ -116,7 +120,7 @@ GCodeResult GpOutputPort::Configure(uint32_t gpioNumber, bool isServo, GCodeBuff
 	else if (seenFreq)
 	{
 #if SUPPORT_CAN_EXPANSION
-		if (boardAddress != CanId::MasterAddress)
+		if (boardAddress != CanInterface::GetCanAddress())
 		{
 			CanMessageGenericConstructor cons(M950GpioParams);
 			cons.AddUParam('P', gpioNumber);
@@ -131,7 +135,7 @@ GCodeResult GpOutputPort::Configure(uint32_t gpioNumber, bool isServo, GCodeBuff
 	else
 	{
 #if SUPPORT_CAN_EXPANSION
-		if (boardAddress != CanId::MasterAddress)
+		if (boardAddress != CanInterface::GetCanAddress())
 		{
 			CanMessageGenericConstructor cons(M950GpioParams);
 			cons.AddUParam('P', gpioNumber);
@@ -148,7 +152,7 @@ GCodeResult GpOutputPort::WriteAnalog(uint32_t gpioPortNumber, bool isServo, flo
 {
 	lastPwm = pwm;
 #if SUPPORT_CAN_EXPANSION
-	if (boardAddress != CanId::MasterAddress)
+	if (boardAddress != CanInterface::GetCanAddress())
 	{
 		return CanInterface::WriteGpio(boardAddress, gpioPortNumber, pwm, isServo, gb, reply);
 	}
@@ -157,6 +161,67 @@ GCodeResult GpOutputPort::WriteAnalog(uint32_t gpioPortNumber, bool isServo, flo
 	return GCodeResult::ok;
 }
 
+#if SUPPORT_REMOTE_COMMANDS
+
+GCodeResult GpOutputPort::AssignFromRemote(uint32_t gpioPortNumber, const CanMessageGenericParser& parser, const StringRef& reply) noexcept
+{
+	bool isServo;
+	if (!parser.GetBoolParam('S', isServo))
+	{
+		isServo = false;
+	}
+
+	PwmFrequency freq;
+	const bool seenFreq = parser.GetUintParam('Q', freq);
+	if (!seenFreq)
+	{
+		freq = (isServo) ? ServoRefreshFrequency : DefaultPinWritePwmFreq;
+	}
+
+	String<StringLength50> pinName;
+	if (parser.GetStringParam('C', pinName.GetRef()))
+	{
+		// Creating or destroying a port
+		const bool ok = port.AssignPort(pinName.c_str(), reply, PinUsedBy::gpout, (isServo) ? PinAccess::servo : PinAccess::pwm);
+		if (ok && port.IsValid())
+		{
+			port.SetFrequency(freq);
+		}
+		return (ok) ? GCodeResult::ok : GCodeResult::error;
+	}
+	else
+	{
+		// Changing frequency, or reporting on a port
+		if (!port.IsValid())
+		{
+			reply.printf("Board %u does not have GPIO/servo port %" PRIu32, CanInterface::GetCanAddress(), gpioPortNumber);
+			return GCodeResult::error;
+		}
+
+		if (seenFreq)
+		{
+			port.SetFrequency(freq);
+		}
+		else
+		{
+			reply.printf("GPIO/servo port %" PRIu32, gpioPortNumber);
+			port.AppendDetails(reply);
+		}
+		return GCodeResult::ok;
+	}
+}
+
+void GpOutputPort::WriteAnalog(float pwm) noexcept
+{
+	if (boardAddress == CanInterface::GetCanAddress())
+	{
+		lastPwm = pwm;
+		port.WriteAnalog(pwm);
+	}
+}
+
+#endif
+
 #ifdef PCCB
 
 // Function used to assign default GPOUT devices
diff --git a/src/GPIO/GpOutPort.h b/src/GPIO/GpOutPort.h
index fbae879a..cf90a9ea 100644
--- a/src/GPIO/GpOutPort.h
+++ b/src/GPIO/GpOutPort.h
@@ -13,6 +13,10 @@
 #include <GCodes/GCodeResult.h>
 #include <ObjectModel/ObjectModel.h>
 
+#if SUPPORT_REMOTE_COMMANDS
+	class CanMessageGenericParser;
+#endif
+
 class GpOutputPort INHERIT_OBJECT_MODEL
 {
 public:
@@ -29,6 +33,11 @@ public:
 	GCodeResult WriteAnalog(uint32_t gpioPortNumber, bool isServo, float pwm, const GCodeBuffer& gb, const StringRef& reply) noexcept;
 	GCodeResult Configure(uint32_t gpioNumber, bool isServo, GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);
 
+#if SUPPORT_REMOTE_COMMANDS
+	GCodeResult AssignFromRemote(uint32_t gpioPortNumber, const CanMessageGenericParser& parser, const StringRef& reply) noexcept;
+	void WriteAnalog(float pwm) noexcept;
+#endif
+
 #ifdef PCCB
 	void Assign(const char *pinName) noexcept;
 #endif
diff --git a/src/Platform.cpp b/src/Platform.cpp
index 4b7eae2b..e3b80cc2 100644
--- a/src/Platform.cpp
+++ b/src/Platform.cpp
@@ -100,6 +100,10 @@ using AnalogIn::AdcBits;
 # include "CAN/CanInterface.h"
 #endif
 
+#if SUPPORT_REMOTE_COMMANDS
+# include <CanMessageGenericParser.h>
+#endif
+
 #include <climits>
 #include <utility>					// for std::swap
 
@@ -4605,6 +4609,139 @@ void Platform::TemperatureCalibrationInit() noexcept
 
 #endif
 
+#if SUPPORT_REMOTE_COMMANDS
+
+GCodeResult Platform::EutHandleM950Gpio(const CanMessageGeneric& msg, const StringRef& reply) noexcept
+{
+	// Get and validate the port number
+	CanMessageGenericParser parser(msg, M950GpioParams);
+	uint16_t gpioNumber;
+	if (!parser.GetUintParam('P', gpioNumber))
+	{
+		reply.copy("Missing port number parameter in M950Gpio message");
+		return GCodeResult::error;
+	}
+	if (gpioNumber >= MaxGpOutPorts)
+	{
+		reply.printf("GPIO port number %u is too high for board %u", gpioNumber, CanInterface::GetCanAddress());
+		return GCodeResult::error;
+	}
+
+	return gpoutPorts[gpioNumber].AssignFromRemote(gpioNumber, parser, reply);
+}
+
+GCodeResult Platform::EutHandleGpioWrite(const CanMessageWriteGpio& msg, const StringRef& reply) noexcept
+{
+	if (msg.portNumber >= MaxGpOutPorts)
+	{
+		reply.printf("GPIO port# %u is too high for this expansion board", msg.portNumber);
+		return GCodeResult::error;
+	}
+
+	gpoutPorts[msg.portNumber].WriteAnalog(msg.pwm);
+	return GCodeResult::ok;
+}
+
+GCodeResult Platform::EutSetMotorCurrents(const CanMessageMultipleDrivesRequest<float>& msg, size_t dataLength, const StringRef& reply) noexcept
+{
+# if HAS_SMART_DRIVERS
+	const auto drivers = Bitmap<uint16_t>::MakeFromRaw(msg.driversToUpdate);
+	if (dataLength < msg.GetActualDataLength(drivers.CountSetBits()))
+	{
+		reply.copy("bad data length");
+		return GCodeResult::error;
+	}
+
+	GCodeResult rslt = GCodeResult::ok;
+	drivers.Iterate([this, msg, reply, &rslt](unsigned int driver, unsigned int count) -> void
+						{
+							if (driver >= NumDirectDrivers)
+							{
+								reply.lcatf("No such driver %u.%u", CanInterface::GetCanAddress(), driver);
+								rslt = GCodeResult::error;
+							}
+							else
+							{
+								SetMotorCurrent(driver, msg.values[count], 906, reply);
+							}
+						}
+				   );
+	return rslt;
+# else
+	reply.copy("Setting not available for external drivers");
+	return GCodeResult::error;
+# endif
+}
+
+GCodeResult Platform::EutSetStepsPerMmAndMicrostepping(const CanMessageMultipleDrivesRequest<StepsPerUnitAndMicrostepping>& msg, size_t dataLength, const StringRef& reply) noexcept
+{
+	const auto drivers = Bitmap<uint16_t>::MakeFromRaw(msg.driversToUpdate);
+	if (dataLength < msg.GetActualDataLength(drivers.CountSetBits()))
+	{
+		reply.copy("bad data length");
+		return GCodeResult::error;
+	}
+
+	GCodeResult rslt = GCodeResult::ok;
+	drivers.Iterate([this, msg, reply, &rslt](unsigned int driver, unsigned int count) -> void
+						{
+							if (driver >= NumDirectDrivers)
+							{
+								reply.lcatf("No such driver %u.%u", CanInterface::GetCanAddress(), driver);
+								rslt = GCodeResult::error;
+							}
+							else
+							{
+								SetDriveStepsPerUnit(driver, msg.values[count].GetStepsPerUnit(), 0);
+#if HAS_SMART_DRIVERS
+								const uint16_t microstepping = msg.values[count].GetMicrostepping() & 0x03FF;
+								const bool interpolate = (msg.values[count].GetMicrostepping() & 0x8000) != 0;
+								if (!SmartDrivers::SetMicrostepping(driver, microstepping, interpolate))
+								{
+									reply.lcatf("Driver %u.%u does not support x%u microstepping", CanInterface::GetCanAddress(), driver, microstepping);
+									if (interpolate)
+									{
+										reply.cat(" with interpolation");
+									}
+									rslt = GCodeResult::error;
+								}
+#endif
+							}
+						}
+					);
+	return rslt;
+}
+
+GCodeResult Platform::EutHandleSetDriverStates(const CanMessageMultipleDrivesRequest<DriverStateControl>& msg, const StringRef& reply) noexcept
+{
+	//TODO check message is long enough for the number of drivers specified
+	const auto drivers = Bitmap<uint16_t>::MakeFromRaw(msg.driversToUpdate);
+	drivers.Iterate([this, msg](unsigned int driver, unsigned int count) -> void
+		{
+			switch (msg.values[count].mode)
+			{
+			case DriverStateControl::driverActive:
+				EnableOneLocalDriver(driver, motorCurrents[driver]);
+				driverState[driver] = DriverStatus::enabled;
+				break;
+
+			case DriverStateControl::driverIdle:
+				UpdateMotorCurrent(driver, motorCurrents[driver] * idleCurrentFactor);
+				driverState[driver] = DriverStatus::idle;
+				break;
+
+			case DriverStateControl::driverDisabled:
+			default:
+				DisableOneLocalDriver(driver);
+				driverState[driver] = DriverStatus::disabled;
+				break;
+			}
+		});
+	return GCodeResult::ok;
+}
+
+#endif
+
 // Process a 1ms tick interrupt
 // This function must be kept fast so as not to disturb the stepper timing, so don't do any floating point maths in here.
 // This is what we need to do: