public static ushort ReadUShort(this byte[] buffer, ref int offset, Endianity endianity)
{
ushort result = ReadUShort(buffer, offset, endianity);
offset += sizeof(ushort);
return(result);
}
public static int ReadInt16(this BinaryReader @this, Endianity endianity)
{
var b0 = @this.ReadByteAsByte();
var b1 = @this.ReadByteAsByte();
return(BinaryUtils.DeserializeInt16(b0, b1, endianity));
}
public static UInt48 ReadUInt48(this byte[] buffer, ref int offset, Endianity endianity)
{
UInt48 uint48 = ByteArrayExtensions.ReadUInt48(buffer, offset, endianity);
offset += 6;
return(uint48);
}
public static UInt24 ReadUInt24(this byte[] buffer, ref int offset, Endianity endianity)
{
UInt24 uint24 = ByteArrayExtensions.ReadUInt24(buffer, offset, endianity);
offset += 3;
return(uint24);
}
public static uint ReadUInt(this byte[] buffer, ref int offset, Endianity endianity)
{
uint num = ByteArrayExtensions.ReadUInt(buffer, offset, endianity);
offset += 4;
return(num);
}
public static ushort ReadUShort(this byte[] buffer, ref int offset, Endianity endianity)
{
ushort num = ByteArrayExtensions.ReadUShort(buffer, offset, endianity);
offset += 2;
return(num);
}
public static uint ReadUInt(this byte[] buffer, ref int offset, Endianity endianity)
{
uint result = ReadUInt(buffer, offset, endianity);
offset += sizeof(int);
return(result);
}
/// <summary>
/// Returns range in specified units
/// </summary>
/// <param name="rangeUnits">Ranging units</param>
/// <returns>Distance range</returns>
public int GetRange(SonarRangeUnits rangeUnits)
{
byte command;
// Set command by range units
switch (rangeUnits)
{
case SonarRangeUnits.Centimeters:
command = SRF02Sonar.RANGE_IN_CENTIMETERS;
break;
case SonarRangeUnits.Inches:
command = SRF02Sonar.RANGE_IN_INCHES;
break;
default:
command = SRF02Sonar.RANGE_IN_MICROSECONDS;
break;
}
// Write command
_slave.WriteRegister(0, command);
// Wait for a while
Thread.Sleep(40);
// Read range
_slave.ReadRegister(2, _dataBuffer);
this._autotuneMinimum = (int)Endianity.GetValue(_dataBuffer, 2, 2, ByteOrder.BigEndian);
return((int)Endianity.GetValue(_dataBuffer, 0, 2, ByteOrder.BigEndian));
}
public static uint ReadUint24(this BinaryReader @this, Endianity endianity)
{
var b0 = @this.ReadByteAsByte();
var b1 = @this.ReadByteAsByte();
var b2 = @this.ReadByteAsByte();
return(BinaryUtils.DeserializeUint24(b0, b1, b2, endianity));
}
/// <summary>
/// Writes the given value to the buffer using the given endianity.
/// </summary>
/// <param name="buffer">The buffer to write the value to.</param>
/// <param name="offset">The offset in the buffer to start writing.</param>
/// <param name="value">The value to write.</param>
/// <param name="endianity">The endianity to use when converting the value to bytes.</param>
public static void Write(this byte[] buffer, int offset, long value, Endianity endianity)
{
if (IsWrongEndianity(endianity))
{
value = IPAddress.HostToNetworkOrder(value);
}
Write(buffer, offset, value);
}
public static void Write(this byte[] buffer, int offset, UInt128 value, Endianity endianity)
{
if (ByteArrayExtensions.IsWrongEndianity(endianity))
{
value = ByteArrayExtensions.HostToNetworkOrder(value);
}
ByteArrayExtensions.Write(buffer, offset, value);
}
public static void Write(this byte[] buffer, int offset, short value, Endianity endianity)
{
if (ByteArrayExtensions.IsWrongEndianity(endianity))
{
value = ShortExtensions.ReverseEndianity(value);
}
ByteArrayExtensions.Write(buffer, offset, value);
}
/// <summary>
/// Writes SD20 mode into proper registry
/// </summary>
/// <param name="offset"></param>
/// <param name="variation"></param>
private void SetMode(int offset, byte variation)
{
Endianity.GetBytes(offset, _dataBuffer, ByteOrder.BigEndian);
_slave.WriteRegister(SD20ServoDriver.REG_EXTENDED_MODE_CTRL, variation);
_slave.WriteRegister(SD20ServoDriver.REG_EXTENDED_MODE_HIGH, _dataBuffer[0]);
_slave.WriteRegister(SD20ServoDriver.REG_EXTENDED_MODE_LOW, _dataBuffer[1]);
}
public static long ReadLong(this byte[] buffer, int offset, Endianity endianity)
{
long value = ReadLong(buffer, offset);
if (IsWrongEndianity(endianity))
{
value = IPAddress.HostToNetworkOrder(value);
}
return(value);
}
public static long ReadLong(this byte[] buffer, int offset, Endianity endianity)
{
long host = ByteArrayExtensions.ReadLong(buffer, offset);
if (ByteArrayExtensions.IsWrongEndianity(endianity))
{
host = IPAddress.HostToNetworkOrder(host);
}
return(host);
}
public static short ReadShort(this byte[] buffer, int offset, Endianity endianity)
{
short num = ByteArrayExtensions.ReadShort(buffer, offset);
if (ByteArrayExtensions.IsWrongEndianity(endianity))
{
num = ShortExtensions.ReverseEndianity(num);
}
return(num);
}
public static void WriteInt16(this BinaryWriter @this, int data, Endianity endianity)
{
byte b0;
byte b1;
data.SerializeInt16(out b0, out b1, endianity);
@this.WriteByte(b0);
@this.WriteByte(b1);
}
public static UInt128 ReadUInt128(this byte[] buffer, int offset, Endianity endianity)
{
UInt128 uint128 = ByteArrayExtensions.ReadUInt128(buffer, offset);
if (ByteArrayExtensions.IsWrongEndianity(endianity))
{
uint128 = ByteArrayExtensions.HostToNetworkOrder(uint128);
}
return(uint128);
}
public static void WriteUint24(this BinaryWriter @this, uint data, Endianity endianity)
{
byte b0;
byte b1;
byte b2;
data.SerializeUint24(out b0, out b1, out b2, endianity);
@this.WriteByte(b0);
@this.WriteByte(b1);
@this.WriteByte(b2);
}
public static void SerializeInt16(this int @this, out byte b0, out byte b1, Endianity endianity)
{
if (endianity == Endianity.Big)
{
b0 = (byte)((@this >> 8) & 0xFF);
b1 = (byte)(@this & 0xFF);
}
else
{
b1 = (byte)((@this >> 8) & 0xFF);
b0 = (byte)(@this & 0xFF);
}
}
public static void WriteInt32(this BinaryWriter @this, int data, Endianity endianity)
{
byte b0;
byte b1;
byte b2;
byte b3;
data.SerializeInt32(out b0, out b1, out b2, out b3, endianity);
@this.WriteByte(b0);
@this.WriteByte(b1);
@this.WriteByte(b2);
@this.WriteByte(b3);
}
public static float ReadFloat32(this BinaryReader @this, Endianity endianity)
{
if (endianity == Endianity.Big)
{
byte[] temp = new byte[4];
temp[3] = @this.ReadByteAsByte();
temp[2] = @this.ReadByteAsByte();
temp[1] = @this.ReadByteAsByte();
temp[0] = @this.ReadByteAsByte();
BinaryReader tempReader = new BinaryReader(new MemoryStream(temp));
return(tempReader.ReadSingle());
}
else
{
return(@this.ReadSingle());
}
}
public static int DeserializeInt(byte[] data, int offset, int length, Endianity endianity)
{
switch (length)
{
case 1:
return(DeserializeInt8(data[offset]));
case 2:
return(DeserializeInt16(data[offset], data[offset + 1], endianity));
case 3:
return(DeserializeInt24(data[offset], data[offset + 1], data[offset + 2], endianity));
case 4:
return(DeserializeInt32(data[offset], data[offset + 1], data[offset + 2], data[offset + 3], endianity));
default:
throw new ArgumentException("length must be between 1 and 4");
}
}
/// <summary>
/// Writes the given value to the buffer using the given endianity and increments the offset by the number of bytes written.
/// </summary>
/// <param name="buffer">The buffer to write the value to.</param>
/// <param name="offset">The offset in the buffer to start writing.</param>
/// <param name="value">The value to write.</param>
/// <param name="endianity">The endianity to use when converting the value to bytes.</param>
public static void Write(this byte[] buffer, ref int offset, UInt128 value, Endianity endianity)
{
buffer.Write(offset, value, endianity);
offset += UInt128.SizeOf;
}
/// <summary>
/// Writes the given value to the buffer using the given endianity.
/// </summary>
/// <param name="buffer">The buffer to write the value to.</param>
/// <param name="offset">The offset in the buffer to start writing.</param>
/// <param name="value">The value to write.</param>
/// <param name="endianity">The endianity to use when converting the value to bytes.</param>
public static void Write(this byte[] buffer, int offset, MacAddress value, Endianity endianity)
{
buffer.Write(offset, value.ToValue(), endianity);
}
/// <summary>
/// Writes the given value to the buffer using the given endianity.
/// </summary>
/// <param name="buffer">The buffer to write the value to.</param>
/// <param name="offset">The offset in the buffer to start writing.</param>
/// <param name="value">The value to write.</param>
/// <param name="endianity">The endianity to use when converting the value to bytes.</param>
public static void Write(this byte[] buffer, int offset, uint value, Endianity endianity)
{
Write(buffer, offset, (int)value, endianity);
}
/// <summary>
/// Writes the given value to the buffer using the given endianity.
/// </summary>
/// <param name="buffer">The buffer to write the value to.</param>
/// <param name="offset">The offset in the buffer to start writing.</param>
/// <param name="value">The value to write.</param>
/// <param name="endianity">The endianity to use when converting the value to bytes.</param>
public static void Write(this byte[] buffer, int offset, ulong value, Endianity endianity)
{
buffer.Write(offset, (long)value, endianity);
}
/// <summary>
/// Reads 4 bytes from a specific offset as an IPv4 time of day with a given endianity and increments the offset by the number of bytes read.
/// </summary>
/// <param name="buffer">The buffer to read the bytes from.</param>
/// <param name="offset">The offset in the buffer to start reading.</param>
/// <param name="endianity">The endianity to use to translate the bytes to the value.</param>
/// <returns>The value converted from the read bytes according to the endianity.</returns>
public static IpV4TimeOfDay ReadIpV4TimeOfDay(this byte[] buffer, ref int offset, Endianity endianity)
{
return new IpV4TimeOfDay(buffer.ReadUInt(ref offset, endianity));
}
/// <summary>
/// Writes the given value to the buffer using the given endianity.
/// </summary>
/// <param name="buffer">The buffer to write the value to.</param>
/// <param name="offset">The offset in the buffer to start writing.</param>
/// <param name="value">The value to write.</param>
/// <param name="endianity">The endianity to use when converting the value to bytes.</param>
public static void Write(this byte[] buffer, int offset, short value, Endianity endianity)
{
if (IsWrongEndianity(endianity))
value = value.ReverseEndianity();
Write(buffer, offset, value);
}
/// <summary>
/// Reads 16 bytes from a specific offset as a UInt128 with a given endianity.
/// </summary>
/// <param name="buffer">The buffer to read the bytes from.</param>
/// <param name="offset">The offset in the buffer to start reading.</param>
/// <param name="endianity">The endianity to use to translate the bytes to the value.</param>
/// <returns>The value converted from the read bytes according to the endianity.</returns>
public static UInt128 ReadUInt128(this byte[] buffer, int offset, Endianity endianity)
{
UInt128 value = ReadUInt128(buffer, offset);
if (IsWrongEndianity(endianity))
value = HostToNetworkOrder(value);
return value;
}
/// <summary>
/// Reads 6 bytes from a specific offset as a MacAddress with a given endianity and increments the offset by the number of bytes read.
/// </summary>
/// <param name="buffer">The buffer to read the bytes from.</param>
/// <param name="offset">The offset in the buffer to start reading.</param>
/// <param name="endianity">The endianity to use to translate the bytes to the value.</param>
/// <returns>The value converted from the read bytes according to the endianity.</returns>
public static MacAddress ReadMacAddress(this byte[] buffer, ref int offset, Endianity endianity)
{
return new MacAddress(buffer.ReadUInt48(ref offset, endianity));
}
private static bool IsWrongEndianity(Endianity endianity)
{
return(BitConverter.IsLittleEndian == (endianity == Endianity.Big));
}
/// <summary>
/// Writes the given value to the buffer using the given endianity and increments the offset by the number of bytes written.
/// </summary>
/// <param name="buffer">The buffer to write the value to.</param>
/// <param name="offset">The offset in the buffer to start writing.</param>
/// <param name="value">The value to write.</param>
/// <param name="endianity">The endianity to use when converting the value to bytes.</param>
public static void Write(this byte[] buffer, ref int offset, IpV6Address value, Endianity endianity)
{
buffer.Write(ref offset, value.ToValue(), endianity);
}
/// <summary>
/// Writes the given value to the buffer using the given endianity.
/// </summary>
/// <param name="buffer">The buffer to write the value to.</param>
/// <param name="offset">The offset in the buffer to start writing.</param>
/// <param name="value">The value to write.</param>
/// <param name="endianity">The endianity to use when converting the value to bytes.</param>
public static void Write(this byte[] buffer, int offset, ulong value, Endianity endianity)
{
buffer.Write(offset, (long)value, endianity);
}
/// <summary>
/// Writes the given value to the buffer using the given endianity and increments the offset by the number of bytes written.
/// </summary>
/// <param name="buffer">The buffer to write the value to.</param>
/// <param name="offset">The offset in the buffer to start writing.</param>
/// <param name="value">The value to write.</param>
/// <param name="endianity">The endianity to use when converting the value to bytes.</param>
public static void Write(this byte[] buffer, ref int offset, ulong value, Endianity endianity)
{
buffer.Write(offset, value, endianity);
offset += sizeof(ulong);
}
/// <summary>
/// Writes the given value to the buffer using the given endianity and increments the offset by the number of bytes written.
/// </summary>
/// <param name="buffer">The buffer to write the value to.</param>
/// <param name="offset">The offset in the buffer to start writing.</param>
/// <param name="value">The value to write.</param>
/// <param name="endianity">The endianity to use when converting the value to bytes.</param>
public static void Write(this byte[] buffer, ref int offset, uint value, Endianity endianity)
{
Write(buffer, offset, value, endianity);
offset += sizeof(uint);
}
/// <summary>
/// Writes the given value to the buffer using the given endianity.
/// </summary>
/// <param name="buffer">The buffer to write the value to.</param>
/// <param name="offset">The offset in the buffer to start writing.</param>
/// <param name="value">The value to write.</param>
/// <param name="endianity">The endianity to use when converting the value to bytes.</param>
public static void Write(this byte[] buffer, int offset, uint value, Endianity endianity)
{
Write(buffer, offset, (int)value, endianity);
}
public static ulong ReadULong(this byte[] buffer, int offset, Endianity endianity)
{
return((ulong)buffer.ReadLong(offset, endianity));
}
/// <summary>
/// Returns actual azimuth
/// </summary>
/// <returns>Azimuth</returns>
public float GetAzimuth()
{
_slave.ReadRegister(CMPS03Compass.REG_AZIMUTH_HIGH, _dataBuffer);
return(Endianity.GetValue(_dataBuffer, ByteOrder.BigEndian) / 10f);
}
public static short ReadShort(this byte[] buffer, int offset, Endianity endianity)
{
short value = ReadShort(buffer, offset);
if (IsWrongEndianity(endianity))
value = value.ReverseEndianity();
return value;
}
/// <summary>
/// Reads a given amount of bytes from a specific offset as an unsigned BigInteger with a given endianity.
/// </summary>
/// <param name="buffer">The buffer to read the bytes from.</param>
/// <param name="offset">The offset in the buffer to start reading.</param>
/// <param name="length">The number of bytes to read.</param>
/// <param name="endianity">The endianity to use to translate the bytes to the value.</param>
/// <returns>The value converted from the read bytes according to the endianity.</returns>
public static BigInteger ReadUnsignedBigInteger(this byte[] buffer, int offset, int length, Endianity endianity)
{
if (buffer == null)
throw new ArgumentNullException("buffer");
BigInteger value = BigInteger.Zero;
for (int i = 0; i != length; ++i)
{
value <<= 8;
if (endianity == Endianity.Big)
value += buffer[offset + i];
else
value += buffer[offset + length - i - 1];
}
return value;
}
public static ushort ReadUShort(this byte[] buffer, int offset, Endianity endianity)
{
return (ushort)ReadShort(buffer, offset, endianity);
}
/// <summary>
/// Reads 4 bytes from a specific offset as an IPv4 address with a given endianity and increments the offset by the number of bytes read.
/// </summary>
/// <param name="buffer">The buffer to read the bytes from.</param>
/// <param name="offset">The offset in the buffer to start reading.</param>
/// <param name="endianity">The endianity to use to translate the bytes to the value.</param>
/// <returns>The value converted from the read bytes according to the endianity.</returns>
public static IpV4Address ReadIpV4Address(this byte[] buffer, ref int offset, Endianity endianity)
{
return new IpV4Address(buffer.ReadUInt(ref offset, endianity));
}
public static ushort ReadUShort(this byte[] buffer, ref int offset, Endianity endianity)
{
ushort result = ReadUShort(buffer, offset, endianity);
offset += sizeof(ushort);
return result;
}
/// <summary>
/// Reads 16 bytes from a specific offset as an IPv6 address with a given endianity.
/// </summary>
/// <param name="buffer">The buffer to read the bytes from.</param>
/// <param name="offset">The offset in the buffer to start reading.</param>
/// <param name="endianity">The endianity to use to translate the bytes to the value.</param>
/// <returns>The value converted from the read bytes according to the endianity.</returns>
public static IpV6Address ReadIpV6Address(this byte[] buffer, int offset, Endianity endianity)
{
return new IpV6Address(buffer.ReadUInt128(offset, endianity));
}
/// <summary>
/// Reads 3 bytes from a specific offset as a UInt24 with a given endianity and increments the offset by the number of bytes read.
/// </summary>
/// <param name="buffer">The buffer to read the bytes from.</param>
/// <param name="offset">The offset in the buffer to start reading.</param>
/// <param name="endianity">The endianity to use to translate the bytes to the value.</param>
/// <returns>The value converted from the read bytes according to the endianity.</returns>
public static UInt24 ReadUInt24(this byte[] buffer, ref int offset, Endianity endianity)
{
UInt24 result = ReadUInt24(buffer, offset, endianity);
offset += UInt24.SizeOf;
return result;
}
/// <summary>
/// Writes the given value to the buffer using the given endianity and increments the offset by the number of bytes written.
/// </summary>
/// <param name="buffer">The buffer to write the value to.</param>
/// <param name="offset">The offset in the buffer to start writing.</param>
/// <param name="value">The value to write.</param>
/// <param name="endianity">The endianity to use when converting the value to bytes.</param>
public static void Write(this byte[] buffer, ref int offset, UInt24 value, Endianity endianity)
{
Write(buffer, offset, value, endianity);
offset += UInt24.SizeOf;
}
public static uint ReadUInt(this byte[] buffer, int offset, Endianity endianity)
{
return (uint)ReadInt(buffer, offset, endianity);
}
/// <summary>
/// Writes the given value to the buffer using the given endianity and increments the offset by the number of bytes written.
/// </summary>
/// <param name="buffer">The buffer to write the value to.</param>
/// <param name="offset">The offset in the buffer to start writing.</param>
/// <param name="value">The value to write.</param>
/// <param name="endianity">The endianity to use when converting the value to bytes.</param>
public static void Write(this byte[] buffer, ref int offset, uint value, Endianity endianity)
{
Write(buffer, offset, value, endianity);
offset += sizeof(uint);
}
public static uint ReadUInt(this byte[] buffer, ref int offset, Endianity endianity)
{
uint result = ReadUInt(buffer, offset, endianity);
offset += sizeof(int);
return result;
}
/// <summary>
/// Writes the given value to the buffer using the given endianity.
/// </summary>
/// <param name="buffer">The buffer to write the value to.</param>
/// <param name="offset">The offset in the buffer to start writing.</param>
/// <param name="value">The value to write.</param>
/// <param name="endianity">The endianity to use when converting the value to bytes.</param>
public static void Write(this byte[] buffer, int offset, UInt128 value, Endianity endianity)
{
if (IsWrongEndianity(endianity))
value = HostToNetworkOrder(value);
Write(buffer, offset, value);
}
/// <summary>
/// Reads 6 bytes from a specific offset as a UInt48 with a given endianity and increments the offset by the number of bytes read.
/// </summary>
/// <param name="buffer">The buffer to read the bytes from.</param>
/// <param name="offset">The offset in the buffer to start reading.</param>
/// <param name="endianity">The endianity to use to translate the bytes to the value.</param>
/// <returns>The value converted from the read bytes according to the endianity.</returns>
public static UInt48 ReadUInt48(this byte[] buffer, ref int offset, Endianity endianity)
{
UInt48 result = buffer.ReadUInt48(offset, endianity);
offset += UInt48.SizeOf;
return result;
}
/// <summary>
/// Writes the given amount of least significant bytes of the value to the buffer using the given endianity.
/// Doesn't write leading zero bytes.
/// </summary>
/// <param name="buffer">The buffer to write the value to.</param>
/// <param name="offset">The offset in the buffer to start writing.</param>
/// <param name="value">The value to write.</param>
/// <param name="length">The maximum amount of bytes to write.</param>
/// <param name="endianity">The endianity to use when converting the value to bytes.</param>
public static void WriteUnsigned(this byte[] buffer, int offset, BigInteger value, int length, Endianity endianity)
{
if (buffer == null)
throw new ArgumentNullException("buffer");
if (value.Sign < 0)
throw new ArgumentOutOfRangeException("value", value, "Must be non-negative.");
for (int i = 0; i != length && value != BigInteger.Zero; ++i, value >>= 8)
{
byte byteValue = (byte)(value & 0xFF);
if (endianity == Endianity.Small)
buffer[offset + i] = byteValue;
else
buffer[offset + length - i - 1] = byteValue;
}
}
public static long ReadLong(this byte[] buffer, int offset, Endianity endianity)
{
long value = ReadLong(buffer, offset);
if (IsWrongEndianity(endianity))
value = IPAddress.HostToNetworkOrder(value);
return value;
}
/// <summary>
/// Writes the given value to the buffer using the given endianity and increments the offset by the number of bytes written.
/// </summary>
/// <param name="buffer">The buffer to write the value to.</param>
/// <param name="offset">The offset in the buffer to start writing.</param>
/// <param name="value">The value to write.</param>
/// <param name="endianity">The endianity to use when converting the value to bytes.</param>
public static void Write(this byte[] buffer, ref int offset, IpV4TimeOfDay value, Endianity endianity)
{
buffer.Write(ref offset, value.MillisecondsSinceMidnightUniversalTime, endianity);
}
public static ulong ReadULong(this byte[] buffer, int offset, Endianity endianity)
{
return (ulong)buffer.ReadLong(offset, endianity);
}
private static bool IsWrongEndianity(Endianity endianity)
{
return (BitConverter.IsLittleEndian == (endianity == Endianity.Big));
}
public static uint ReadUInt(this byte[] buffer, int offset, Endianity endianity)
{
return((uint)ReadInt(buffer, offset, endianity));
}