/// <summary>
/// Create a new command sequence
/// </summary>
/// <param name="command">The first byte of the command</param>
internal FirmataCommandSequence(FirmataCommand command = FirmataCommand.START_SYSEX)
{
_sequence = new List <byte>()
{
(byte)command
};
}
private void ProcessInput()
{
if (_dataQueue.Count == 0)
{
FillQueue();
}
if (_dataQueue.Count == 0)
{
// Still no data? (End of stream or stream closed)
return;
}
int data = _dataQueue.Dequeue();
// OnError?.Invoke($"0x{data:X}", null);
byte b = (byte)(data & 0x00FF);
byte upper_nibble = (byte)(data & 0xF0);
byte lower_nibble = (byte)(data & 0x0F);
/*
* the relevant bits in the command depends on the value of the data byte. If it is less than 0xF0 (start sysex), only the upper nibble identifies the command
* while the lower nibble contains additional data
*/
FirmataCommand command = (FirmataCommand)((data < ((ushort)FirmataCommand.START_SYSEX) ? upper_nibble : b));
// determine the number of bytes remaining in the message
int bytes_remaining = 0;
bool isMessageSysex = false;
switch (command)
{
default: // command not understood
case FirmataCommand.END_SYSEX: // should never happen
return;
// commands that require 2 additional bytes
case FirmataCommand.DIGITAL_MESSAGE:
case FirmataCommand.ANALOG_MESSAGE:
case FirmataCommand.SET_PIN_MODE:
case FirmataCommand.PROTOCOL_VERSION:
bytes_remaining = 2;
break;
// commands that require 1 additional byte
case FirmataCommand.REPORT_ANALOG_PIN:
case FirmataCommand.REPORT_DIGITAL_PIN:
bytes_remaining = 1;
break;
// commands that do not require additional bytes
case FirmataCommand.SYSTEM_RESET:
// do nothing, as there is nothing to reset
return;
case FirmataCommand.START_SYSEX:
// this is a special case with no set number of bytes remaining
isMessageSysex = true;
break;
}
// read the remaining message while keeping track of elapsed time to timeout in case of incomplete message
List <byte> message = new List <byte>();
int bytes_read = 0;
Stopwatch timeout_start = Stopwatch.StartNew();
while (bytes_remaining > 0 || isMessageSysex)
{
if (_dataQueue.Count == 0)
{
int timeout = 10;
while (!FillQueue() && timeout-- > 0)
{
Thread.Sleep(5);
}
if (timeout == 0)
{
// Synchronisation problem: The remainder of the expected message is missing
return;
}
}
data = _dataQueue.Dequeue();
// OnError?.Invoke($"0x{data:X}", null);
// if no data was available, check for timeout
if (data == 0xFFFF)
{
// get elapsed seconds, given as a double with resolution in nanoseconds
var elapsed = timeout_start.Elapsed;
if (elapsed > DefaultReplyTimeout)
{
return;
}
continue;
}
timeout_start.Restart();
// if we're parsing sysex and we've just read the END_SYSEX command, we're done.
if (isMessageSysex && (data == (short)FirmataCommand.END_SYSEX))
{
break;
}
message.Add((byte)(data & 0xFF));
++bytes_read;
--bytes_remaining;
}
// process the message
switch (command)
{
// ignore these message types (they should not be in a reply)
default:
case FirmataCommand.REPORT_ANALOG_PIN:
case FirmataCommand.REPORT_DIGITAL_PIN:
case FirmataCommand.SET_PIN_MODE:
case FirmataCommand.END_SYSEX:
case FirmataCommand.SYSTEM_RESET:
return;
case FirmataCommand.PROTOCOL_VERSION:
if (_actualFirmataProtocolMajorVersion != 0)
{
// Firmata sends this message automatically after a device reset (if you press the reset button on the arduino)
// If we know the version already, this is unexpected.
OnError?.Invoke("The device was unexpectedly reset. Please restart the communication.", null);
}
_actualFirmataProtocolMajorVersion = message[0];
_actualFirmataProtocolMinorVersion = message[1];
_dataReceived.Set();
return;
case FirmataCommand.ANALOG_MESSAGE:
// report analog commands store the pin number in the lower nibble of the command byte, the value is split over two 7-bit bytes
// AnalogValueUpdated(this,
// new CallbackEventArgs(lower_nibble, (ushort)(message[0] | (message[1] << 7))));
{
int pin = lower_nibble;
uint value = (uint)(message[0] | (message[1] << 7));
lock (_lastAnalogValueLock)
{
_lastAnalogValues[pin] = value;
}
AnalogPinValueUpdated?.Invoke(pin, value);
}
break;
case FirmataCommand.DIGITAL_MESSAGE:
// digital messages store the port number in the lower nibble of the command byte, the port value is split over two 7-bit bytes
// Each port corresponds to 8 pins
{
int offset = lower_nibble * 8;
ushort pinValues = (ushort)(message[0] | (message[1] << 7));
lock (_lastPinValueLock)
{
for (int i = 0; i < 8; i++)
{
PinValue oldValue = _lastPinValues[i + offset];
int mask = 1 << i;
PinValue newValue = (pinValues & mask) == 0 ? PinValue.Low : PinValue.High;
if (newValue != oldValue)
{
PinEventTypes eventTypes = newValue == PinValue.High ? PinEventTypes.Rising : PinEventTypes.Falling;
_lastPinValues[i + offset] = newValue;
// TODO: The callback should not be within the lock
DigitalPortValueUpdated?.Invoke(this, new PinValueChangedEventArgs(eventTypes, i + offset));
}
}
}
}
break;
case FirmataCommand.START_SYSEX:
// a sysex message must include at least one extended-command byte
if (bytes_read < 1)
{
return;
}
// retrieve the raw data array & extract the extended-command byte
var raw_data = message.ToArray();
FirmataSysexCommand sysCommand = (FirmataSysexCommand)(raw_data[0]);
int index = 0;
++index;
--bytes_read;
switch (sysCommand)
{
case FirmataSysexCommand.REPORT_FIRMWARE:
// See https://github.com/firmata/protocol/blob/master/protocol.md
// Byte 0 is the command (0x79) and can be skipped here, as we've already interpreted it
{
_firmwareVersionMajor = raw_data[1];
_firmwareVersionMinor = raw_data[2];
int stringLength = (raw_data.Length - 3) / 2;
Span <byte> bytesReceived = stackalloc byte[stringLength];
ReassembleByteString(raw_data, 3, stringLength * 2, bytesReceived);
_firmwareName = Encoding.ASCII.GetString(bytesReceived);
_dataReceived.Set();
}
return;
case FirmataSysexCommand.STRING_DATA:
{
// condense back into 1-byte data
int stringLength = (raw_data.Length - 1) / 2;
Span <byte> bytesReceived = stackalloc byte[stringLength];
ReassembleByteString(raw_data, 1, stringLength * 2, bytesReceived);
string message1 = Encoding.ASCII.GetString(bytesReceived);
int idxNull = message1.IndexOf('\0');
if (message1.Contains("%") && idxNull > 0) // C style printf formatters
{
message1 = message1.Substring(0, idxNull);
string message2 = PrintfFromByteStream(message1, bytesReceived, idxNull + 1);
OnError?.Invoke(message2, null);
}
else
{
OnError?.Invoke(message1, null);
}
}
break;
case FirmataSysexCommand.CAPABILITY_RESPONSE:
{
_supportedPinConfigurations.Clear();
int idx = 1;
var currentPin = new SupportedPinConfiguration(0);
int pin = 0;
while (idx < raw_data.Length)
{
int mode = raw_data[idx++];
if (mode == 0x7F)
{
_supportedPinConfigurations.Add(currentPin);
currentPin = new SupportedPinConfiguration(++pin);
continue;
}
int resolution = raw_data[idx++];
switch ((SupportedMode)mode)
{
default:
currentPin.PinModes.Add((SupportedMode)mode);
break;
case SupportedMode.AnalogInput:
currentPin.PinModes.Add(SupportedMode.AnalogInput);
currentPin.AnalogInputResolutionBits = resolution;
break;
case SupportedMode.Pwm:
currentPin.PinModes.Add(SupportedMode.Pwm);
currentPin.PwmResolutionBits = resolution;
break;
}
}
// Add 8 entries, so that later we do not need to check whether a port (bank) is complete
_lastPinValues = new PinValue[_supportedPinConfigurations.Count + 8].ToList();
_dataReceived.Set();
// Do not add the last instance, should also be terminated by 0xF7
}
break;
case FirmataSysexCommand.ANALOG_MAPPING_RESPONSE:
{
// This needs to have been set up previously
if (_supportedPinConfigurations.Count == 0)
{
return;
}
int idx = 1;
int pin = 0;
while (idx < raw_data.Length)
{
if (raw_data[idx] != 127)
{
_supportedPinConfigurations[pin].AnalogPinNumber = raw_data[idx];
}
idx++;
pin++;
}
_dataReceived.Set();
}
break;
case FirmataSysexCommand.I2C_REPLY:
_lastResponse = raw_data;
_dataReceived.Set();
break;
case FirmataSysexCommand.SPI_DATA:
_lastResponse = raw_data;
_dataReceived.Set();
break;
case FirmataSysexCommand.DHT_SENSOR_DATA_REQUEST:
case FirmataSysexCommand.PIN_STATE_RESPONSE:
_lastResponse = raw_data; // the instance is constant, so we can just remember the pointer
_dataReceived.Set();
break;
default:
// we pass the data forward as-is for any other type of sysex command
break;
}
break;
}
}