/// <summary>
/// Calculates the CRC of a packet.
/// </summary>
public static byte CalculateCrc(Px4ioPacket packet)
{
// Validate
if (packet == null)
{
throw new ArgumentNullException(nameof(packet));
}
// Calculate header CRC
byte crc = 0;
crc = _crcTable[crc ^ packet.CountCode];
crc = _crcTable[crc ^ 0]; // Cannot CRC self
crc = _crcTable[crc ^ packet.Page];
crc = _crcTable[crc ^ packet.Offset];
// Add register CRCs
for (var index = 0; index < packet.Count; index++)
{
var value = packet.Registers[index];
var bytes = BitConverter.GetBytes(value);
crc = _crcTable[crc ^ bytes[0]];
crc = _crcTable[crc ^ bytes[1]];
}
// Return result
return(crc);
}
/// <summary>
/// Sends a command to write a register.
/// </summary>
/// <param name="page"><see cref="Px4ioPage"/> index.</param>
/// <param name="offset"><see cref="Px4ioPage"/> register offset.</param>
/// <param name="values">Data values to write.</param>
/// <returns>Response packet with data read and validated after request.</returns>
public Px4ioPacket WriteRegister(byte page, byte offset, ushort[] values)
{
// Create write request packet
var request = new Px4ioPacket((byte)Px4ioRequestCode.Write, page, offset, values);
request.CalculateCrc();
// Transfer and return result
return(Transfer(request));
}
/// <summary>
/// Sends a command to read a single register.
/// </summary>
/// <param name="page"><see cref="Px4ioPage"/> index.</param>
/// <param name="offset"><see cref="Px4ioPage"/> register offset.</param>
public ushort ReadRegister(byte page, byte offset)
{
// Create read request packet
var request = new Px4ioPacket((byte)Px4ioRequestCode.Read, page, offset, 1);
request.CalculateCrc();
// Transfer and return result
return(Transfer(request).Registers[0]);
}
/// <summary>
/// Compares this object with another of the same type by value.
/// </summary>
/// <param name="value">Object with which to compare by value.</param>
public bool Equals(Px4ioPacket value)
{
// Check null
if (ReferenceEquals(value, null))
{
return(false);
}
// Compare values
return
(value.CountCode == CountCode &&
value.Crc == Crc &&
value.Offset == Offset &&
value.Page == Page &&
ArrayExtensions.AreEqual(value.Registers, Registers));
}
/// <summary>
/// Sends a command to read multiple registers.
/// </summary>
/// <param name="page"><see cref="Px4ioPage"/> index.</param>
/// <param name="offset"><see cref="Px4ioPage"/> register offset.</param>
/// <param name="count">
/// Amount of data to read. When greater than <see cref="Px4ioPacket.CountMask"/>
/// multiple transfers will occur.
/// </param>
/// <param name="requireCount">
/// Indicates the exact <paramref name="count"/> must be returned, otherwise
/// an error is thrown. Set true by default. Set false for requests which
/// may return a variable count.
/// </param>
/// <remarks>
/// Automatically splits the request into multiple transfers and joins
/// resulting data when <paramref name="count"/> exceeds the value possible
/// in <see cref="Px4ioPacket.CountMask"/>.
/// </remarks>
public ushort[] ReadRegisters(byte page, byte offset, ushort count, bool requireCount = true)
{
// Send requests
var result = new List <ushort>();
var resultOffset = (ushort)0;
var remaining = count;
do
{
// Create read request packet
var requestOffset = (byte)(offset + resultOffset > byte.MaxValue ? byte.MaxValue : offset + resultOffset);
var requestCount = (byte)(remaining > Px4ioPacket.RegistersMaximum ? Px4ioPacket.RegistersMaximum : remaining);
var request = new Px4ioPacket((byte)Px4ioRequestCode.Read, page, requestOffset, requestCount);
request.CalculateCrc();
// Transfer and copy result
var response = Transfer(request);
result.AddRange(response.Registers);
// Stop transfer when less data was received than requested
if (response.Count != request.Count)
{
if (requireCount)
{
// Error when exact size required
throw new ArgumentOutOfRangeException(nameof(count));
}
// Okay to exit with less data
break;
}
// Next...
resultOffset += requestCount;
remaining -= requestCount;
}while (remaining > 0);
// Return result
return(result.ToArray());
}
/// <summary>
/// Sends a request packet, checks the returns the response.
/// </summary>
/// <param name="request">Request packet with <see cref="Px4ioPacket.Crc"/> calculated.</param>
/// <returns>Response packet with data read and validated after request.</returns>
/// <exception cref="FormatException">Thrown when corruption occurred during send or receive.</exception>
/// <exception cref="InvalidOperationException">Thrown when the request was rejected in error.</exception>
/// <exception cref="ArgumentOutOfRangeException">Thrown when the returned register count doesn't match the requested count.</exception>
public Px4ioPacket Transfer(Px4ioPacket request)
{
// Validate
if (request == null)
{
throw new ArgumentNullException(nameof(request));
}
// Wait a bit
// TODO: Wait for data ready interrupt from STM32 before reading.
Task.Delay(TimeSpanExtensions.FromMicroseconds(150)).Wait();
// Write request to SPI bus
var writeBuffer = request.ToByteArray();
_hardware.Write(writeBuffer);
// Wait a bit
// TODO: Wait for data ready interrupt from STM32 before reading.
Task.Delay(TimeSpanExtensions.FromMicroseconds(150)).Wait();
// Read response from SPI bus
var readBuffer = new byte[Px4ioPacket.Size];
_hardware.Read(readBuffer);
// Check and return response when valid
var response = new Px4ioPacket(readBuffer);
switch ((Px4ioResponseCode)response.Code)
{
case Px4ioResponseCode.Corrupt:
{
// Rejected by RCIO as corrupt (CRC mismatch)
throw new FormatException(Resources.Strings.Px4ioPacketCorruptTransmit);
}
case Px4ioResponseCode.Error:
{
// Rejected by RCIO as error (request specific)
throw new InvalidOperationException(Resources.Strings.Px4ioPacketError);
}
case Px4ioResponseCode.Success:
{
// Successful response
// Check CRC
if (!response.ValidateCrc())
{
throw new FormatException(Resources.Strings.Px4ioPacketCorruptReceive);
}
// Return valid data
return(response);
}
default:
{
// Corrupt or unsupported response code
throw new FormatException(Resources.Strings.Px4ioPacketCorruptReceive);
}
}
}
