diff --git a/BetaCameras/Sick.VisionaryT/AccessModes.cs b/BetaCameras/Sick.VisionaryT/AccessModes.cs
new file mode 100644
index 00000000..97f7a514
--- /dev/null
+++ b/BetaCameras/Sick.VisionaryT/AccessModes.cs
@@ -0,0 +1,14 @@
+namespace MetriCam2.Cameras.Internal.Sick
+{
+ ///
+ /// Access modes on the camera.
+ ///
+ internal enum AccessModes
+ {
+ Always_Run = 0,
+ Operator = 1,
+ Maintenance = 2,
+ AuthorizedClient = 3,
+ Service = 4,
+ }
+}
diff --git a/BetaCameras/Sick.VisionaryT/Control.cs b/BetaCameras/Sick.VisionaryT/Control.cs
new file mode 100644
index 00000000..2c62aa58
--- /dev/null
+++ b/BetaCameras/Sick.VisionaryT/Control.cs
@@ -0,0 +1,403 @@
+using System;
+using System.Collections.Generic;
+using System.Net.Sockets;
+using System.Text;
+using Metrilus.Logging;
+
+namespace MetriCam2.Cameras.Internal.Sick
+{
+ internal class Control
+ {
+ private const int TCP_PORT_SOPAS = 2112;
+ private readonly byte[] START_STX = { 0x02, 0x02, 0x02, 0x02 };
+
+ private readonly MetriLog log;
+ private readonly NetworkStream streamControl;
+ private readonly TcpClient sockControl;
+
+ internal AccessModes _accessMode { get; private set; }
+
+ public Control(MetriLog log, string ipAddress)
+ {
+ this.log = log;
+
+ try
+ {
+ sockControl = new TcpClient(ipAddress, TCP_PORT_SOPAS);
+ streamControl = sockControl.GetStream();
+ }
+ catch (Exception ex)
+ {
+ string msg = string.Format("Failed to connect to IP={0}, reasons={1}", ipAddress, ex.Message);
+ log.Error(msg);
+ throw new Exceptions.ConnectionFailedException(msg, ex);
+ }
+
+ _accessMode = GetAccessMode();
+ InitStream();
+ }
+
+ internal void Close()
+ {
+ StopStream();
+ streamControl.Close();
+ sockControl.Close();
+ }
+
+ ///
+ /// Tells the device that there is a streaming channel.
+ ///
+ private void InitStream()
+ {
+ log.Debug("InitStream");
+ SendCommand("sMN GetBlobClientConfig");
+
+ bool success = ReceiveResponse(out byte[] payload, out byte checkSum);
+ if (!success)
+ {
+ throw new InvalidOperationException("Failed to init control stream.");
+ }
+ }
+
+ ///
+ /// Starts streaming the data by calling "PLAYSTART" method on the device.
+ ///
+ internal void StartStream()
+ {
+ log.Debug("Starting data stream");
+ SendCommand("sMN PLAYSTART");
+
+ bool success = ReceiveResponse(out byte[] payload, out byte checkSum);
+ if (!success)
+ {
+ throw new InvalidOperationException("Failed to start stream.");
+ }
+ }
+
+ ///
+ /// Stops the data stream on the device.
+ ///
+ internal void StopStream()
+ {
+ log.Debug("Stopping data stream");
+ SendCommand("sMN PLAYSTOP");
+
+ bool success = ReceiveResponse(out byte[] payload, out byte checkSum);
+ if (!success)
+ {
+ throw new InvalidOperationException("Failed to stop stream.");
+ }
+ }
+
+
+ private AccessModes GetAccessMode()
+ {
+ SendCommand("sMN GetAccessMode");
+
+ bool success = ReceiveResponse(out byte[] payload, out byte checkSum);
+ if (!success)
+ {
+ throw new InvalidOperationException("Failed to get access mode.");
+ }
+
+ byte value = payload[payload.Length - 1];
+ log.DebugFormat("Got access mode: {0}", (AccessModes)value);
+ return (AccessModes)value;
+ }
+
+ ///
+ /// Sets the access mode on the device.
+ ///
+ internal void SetAccessMode(AccessModes newMode)
+ {
+ if (newMode == _accessMode)
+ {
+ log.DebugFormat("Skipping SetAccessMode: New access mode ({0}) is the same as the current one.", newMode);
+ return;
+ }
+
+ if ((int)newMode < (int)_accessMode)
+ {
+ log.DebugFormat("Skipping SetAccessMode: New access mode ({0}) lower than the current one.", newMode);
+ return;
+ }
+
+ byte[] dig;
+ switch (newMode)
+ {
+ case AccessModes.Operator:
+ dig = new byte[] { 59, 117, 101, 94 };
+ break;
+ case AccessModes.Maintenance:
+ dig = new byte[] { 85, 119, 0, 230 };
+ break;
+ case AccessModes.AuthorizedClient:
+ dig = new byte[] { 251, 53, 108, 222 };
+ break;
+ case AccessModes.Service:
+ dig = new byte[] { 237, 120, 75, 170 };
+ break;
+ default:
+ throw new NotImplementedException($"Changing to access level of {newMode} is not supported by MetriCam 2.");
+ }
+
+ byte[] commandAsBytes = Encoding.ASCII.GetBytes("sMN SetAccessMode ");
+ Array.Resize(ref commandAsBytes, commandAsBytes.Length + 1 + dig.Length);
+ int offsetOfMode = commandAsBytes.Length - dig.Length - 1;
+ commandAsBytes[offsetOfMode] = (byte)newMode;
+ for (int i = commandAsBytes.Length - dig.Length, j = 0; i < commandAsBytes.Length; i++, j++)
+ {
+ commandAsBytes[i] = dig[j];
+ }
+ SendCommand(commandAsBytes);
+
+ bool success = ReceiveResponse(out byte[] payload, out byte checkSum);
+ if (!success)
+ {
+ throw new InvalidOperationException($"Failed to set access mode to {newMode} (no response).");
+ }
+
+ byte cmdSuccess = payload[payload.Length - 1];
+ if (0 == cmdSuccess)
+ {
+ throw new InvalidOperationException($"Failed to set access mode to {newMode} (not successful).");
+ }
+
+ _accessMode = newMode;
+ log.DebugFormat("Access mode set to {0}", newMode);
+ }
+
+ internal void SetIntegrationTime(VisionaryTIntegrationTime value)
+ {
+ SetAccessMode(AccessModes.Service);
+ log.Debug("Setting integration time");
+ WriteVariable("integrationTime", (byte)value);
+ }
+ internal VisionaryTIntegrationTime GetIntegrationTime()
+ {
+ log.Debug("Getting integration time");
+ byte value = ReadVariableByte("integrationTime");
+ return (VisionaryTIntegrationTime)value;
+ }
+
+ internal void SetCoexistenceMode(VisionaryTCoexistenceMode value)
+ {
+ SetAccessMode(AccessModes.AuthorizedClient);
+ log.Debug("Setting coexistence mode / modulation frequency");
+ WriteVariable("modFreq", (byte)value);
+ }
+ internal VisionaryTCoexistenceMode GetCoexistenceMode()
+ {
+ log.Debug("Getting coexistence mode / modulation frequency");
+ byte value = ReadVariableByte("modFreq");
+ return (VisionaryTCoexistenceMode)value;
+ }
+
+ private void WriteVariable(string name, byte value)
+ {
+ SendCommand("sWN " + name, value);
+
+ bool success = ReceiveResponse(out byte[] payload, out byte checkSum);
+ if (!success)
+ {
+ throw new InvalidOperationException($"Failed to write variable {name} (no response).");
+ }
+ success = CheckWriteAcknowledge(payload);
+ if (!success)
+ {
+ throw new InvalidOperationException($"Failed to write variable {name} (not successful).");
+ }
+ }
+
+ private void WriteVariable(string name, int value)
+ {
+ SendCommand("sWN " + name, value);
+
+ bool success = ReceiveResponse(out byte[] payload, out byte checkSum);
+ if (!success)
+ {
+ throw new InvalidOperationException($"Failed to write variable {name} (no response).");
+ }
+ success = CheckWriteAcknowledge(payload);
+ if (!success)
+ {
+ throw new InvalidOperationException($"Failed to write variable {name} (not successful).");
+ }
+ }
+
+ private bool CheckWriteAcknowledge(byte[] payload)
+ {
+ byte[] writeAck = Encoding.ASCII.GetBytes("sWA");
+ for (int i = 0; i < writeAck.Length; i++)
+ {
+ if (writeAck[i] != payload[i])
+ {
+ return false;
+ }
+ }
+
+ return true;
+ }
+
+ private int ReadVariableInt32(string name)
+ {
+ SendCommand("sRN " + name);
+
+ bool success = ReceiveResponse(out byte[] payload, out byte checkSum);
+ if (!success)
+ {
+ throw new InvalidOperationException($"Failed to read variable {name}.");
+ }
+
+ int l = payload.Length;
+ byte[] responseValueBytes = new byte[] { payload[l - 4], payload[l - 3], payload[l - 2], payload[l - 1] };
+ int value = BitConverter.ToInt32(responseValueBytes, 0);
+ log.DebugFormat("Got value: {0}", value);
+ return value;
+ }
+
+ private byte ReadVariableByte(string name)
+ {
+ SendCommand("sRN " + name);
+
+ bool success = ReceiveResponse(out byte[] payload, out byte checkSum);
+ if (!success)
+ {
+ throw new InvalidOperationException($"Failed to read variable {name}.");
+ }
+
+ byte value = payload[payload.Length - 1];
+ log.DebugFormat("Got value: {0}", value);
+ return value;
+ }
+
+ #region Frame Wrapping
+
+ ///
+ /// Binary framing used to serialize commands.
+ /// Adds START_STX, length and checksum.
+ ///
+ /// actual command
+ /// framed message
+ private byte[] AddFraming(byte[] bytes)
+ {
+ // calculate sizes and prepare message
+ int msgSize = bytes.Length + START_STX.Length + 1 + 4; // +1 for checksum, +4 for size of payload
+ uint payloadSize = (uint)bytes.Length;
+ byte[] payloadSizeBytes = BitConverter.GetBytes(payloadSize);
+ if (BitConverter.IsLittleEndian)
+ {
+ Array.Reverse(payloadSizeBytes);
+ }
+ byte[] message = new byte[msgSize];
+ byte checksum = ChkSumCola(bytes);
+
+ // build message
+ int i;
+ for (i = 0; i < START_STX.Length; ++i)
+ {
+ message[i] = START_STX[i];
+ }
+ for (int j = 0; j < 4; ++j)
+ {
+ message[i++] = payloadSizeBytes[j];
+ }
+ for (int j = 0; j < bytes.Length; ++j)
+ {
+ message[i++] = bytes[j];
+ }
+ message[i] = checksum;
+
+ return message;
+ }
+
+ ///
+ /// CheckSum: XOR over all bytes.
+ ///
+ /// byte array to compute checksum for
+ /// checksum byte
+ private static byte ChkSumCola(byte[] value)
+ {
+ if (value.Length == 0)
+ return 0x00;
+ if (value.Length == 1)
+ return value[0];
+
+ byte x = value[0];
+ for (int i = 1; i < value.Length; ++i)
+ x ^= value[i];
+
+ return x;
+ }
+
+ #endregion Frame Wrapping
+
+ private void SendCommand(byte[] command)
+ {
+ byte[] telegram = AddFraming(command);
+ streamControl.Write(telegram, 0, telegram.Length);
+ }
+
+ private void SendCommand(string command)
+ {
+ SendCommand(Encoding.ASCII.GetBytes(command));
+ }
+
+ private void SendCommand(string command, byte value)
+ {
+ List bytes = new List(Encoding.ASCII.GetBytes(command + " "));
+ bytes.Add(value);
+ SendCommand(bytes.ToArray());
+ }
+
+ private void SendCommand(string command, int value)
+ {
+ List bytes = new List(Encoding.ASCII.GetBytes(command + " "));
+ byte[] valueBytes = BitConverter.GetBytes(value);
+ if (BitConverter.IsLittleEndian)
+ {
+ Array.Reverse(valueBytes);
+ }
+ bytes.AddRange(valueBytes);
+ SendCommand(bytes.ToArray());
+ }
+
+ private bool ReceiveResponse(out byte[] payload, out byte checkSum)
+ {
+ payload = new byte[0];
+ checkSum = 0;
+
+ byte[] receiveHeader = new byte[8];
+ if (streamControl.Read(receiveHeader, 0, receiveHeader.Length) == 0)
+ {
+ log.Error("Got no response from camera");
+ return false;
+ }
+
+ // first 4 bytes must be STX
+ for (int i = 0; i < START_STX.Length; i++)
+ {
+ if (START_STX[i] != receiveHeader[i])
+ {
+ log.Error($"Response did not start with {nameof(START_STX)}.");
+ return false;
+ }
+ }
+
+ // Remark: On big endian machines this won't work:
+ byte[] payloadLengthBytes = new byte[] { receiveHeader[7], receiveHeader[6], receiveHeader[5], receiveHeader[4] };
+ int payloadLength = BitConverter.ToInt32(payloadLengthBytes, 0);
+
+ byte[] receivePayload = new byte[payloadLength + 1]; // 1 Byte for checksum
+ streamControl.Read(receivePayload, 0, receivePayload.Length);
+
+ payload = new byte[payloadLength];
+ Array.Copy(receivePayload, payload, payloadLength);
+
+ checkSum = receivePayload[receivePayload.Length - 1];
+
+ log.DebugFormat("Received paylod: {0}", Encoding.ASCII.GetString(payload));
+ return true;
+ }
+ }
+}
diff --git a/BetaCameras/Sick.VisionaryT/Device.cs b/BetaCameras/Sick.VisionaryT/Device.cs
index d124d672..2a457fa8 100644
--- a/BetaCameras/Sick.VisionaryT/Device.cs
+++ b/BetaCameras/Sick.VisionaryT/Device.cs
@@ -17,22 +17,15 @@ internal class Device
{
#region Private Constants
private const int TCP_PORT_BLOBSERVER = 2113;
- private const int TCP_PORT_SOPAS = 2112;
- private readonly byte[] START_STX = { 0x02, 0x02, 0x02, 0x02 };
- // The following command was generated with the python sample
- private readonly byte[] commandServiceLevel = { 0x02, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x17, 0x73, 0x4d,
- 0x4e, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, 0x73, 0x73, 0x4d, 0x6f, 0x64, 0x65, 0x20, 0x04, 0xed, 0x78, 0x4b, 0xaa, 0x45 };
private const int FRAGMENT_SIZE = 1024;
private const string HEARTBEAT_MSG = "BlbReq";
#endregion
#region Private Variables
- private string ipAddress;
- private Camera cam;
+ private readonly string ipAddress;
+ private readonly Camera cam;
private MetriLog log;
- private TcpClient sockControl;
private TcpClient sockData;
- private NetworkStream streamControl;
private NetworkStream streamData;
#endregion
@@ -44,202 +37,37 @@ internal class Device
/// IP address of client
/// MetriCam2 camera object used for exceptions
/// MetriLog
- public Device(string ip, Camera cam, MetriLog log)
+ internal Device(string ip, Camera cam, MetriLog log)
{
ipAddress = ip;
this.cam = cam;
this.log = log;
- sockControl = null;
- sockData = null;
- streamControl = null;
- streamData = null;
- }
- #endregion
-
- #region Public Methods
- ///
- /// Tells the device that there is a streaming channel.
- ///
- public void Control_InitStream()
- {
- log.Debug("Initializing streaming");
- byte[] toSend = AddFraming("sMN GetBlobClientConfig");
- byte[] receive = new byte[50];
-
- // send ctrl message
- streamControl.Write(toSend, 0, toSend.Length);
-
- // get response
- if (streamControl.Read(receive, 0, receive.Length) == 0)
- {
- log.Error("Got no answer from camera");
- ExceptionBuilder.Throw(typeof(InvalidOperationException), cam, "error_setParameter", "Failed to init stream.");
- }
- else
- {
- string response = Encoding.ASCII.GetString(receive);
- log.DebugFormat("Got response: {0}", response);
- }
- log.Debug("Done: Initializing streaming");
- }
-
- ///
- /// Starts streaming the data by calling "PLAYSTART" method on the device.
- ///
- public void Control_StartStream()
- {
- log.Debug("Starting Stream");
- byte[] toSend = AddFraming("sMN PLAYSTART");
- byte[] receive = new byte[14];
-
- // send ctrl message
- streamControl.Write(toSend, 0, toSend.Length);
-
- // get response
- if (streamControl.Read(receive, 0, receive.Length) == 0)
- {
- log.Error("Got no answer from camera");
- ExceptionBuilder.Throw(typeof(InvalidOperationException), cam, "error_setParameter", "Failed to start stream.");
- }
- else
- {
- string response = Encoding.ASCII.GetString(receive);
- log.DebugFormat("Got response: {0}", response);
- }
-
- log.Debug("Done: Starting Stream");
- }
-
- ///
- /// Stops the data stream on the device.
- ///
- public void Control_StopStream()
- {
- log.Debug("Stopping Stream");
- byte[] toSend = AddFraming("sMN PLAYSTOP");
- byte[] receive = new byte[14];
-
- // send ctrl message
- streamControl.Write(toSend, 0, toSend.Length);
-
- // get response
- if (streamControl.Read(receive, 0, receive.Length) == 0)
- {
- log.Error("Got no answer from camera");
- ExceptionBuilder.Throw(typeof(InvalidOperationException), cam, "error_setParameter", "Failed to stop stream.");
- }
- else
- {
- string response = Encoding.ASCII.GetString(receive);
- log.DebugFormat("Got response: {0}", response);
- }
-
- log.Debug("Done: Stopping Stream");
- }
-
- ///
- /// Sets the access mode on the device.
- ///
- public void Control_SetServiceAccessMode()
- {
- byte[] receive = new byte[28];
- // TODO: Instead of using hard-coded command, implement propper command builder.
- streamControl.Write(commandServiceLevel, 0, commandServiceLevel.Length);
-
- // get response
- if (streamControl.Read(receive, 0, receive.Length) == 0)
- {
- log.Error("Got no answer from camera");
- ExceptionBuilder.Throw(typeof(InvalidOperationException), cam, "error_setParameter", "Failed to stop stream.");
- }
- else
+ try
{
- string response = Encoding.ASCII.GetString(receive);
- log.DebugFormat("Got response: {0}", response);
+ sockData = new TcpClient(ipAddress, TCP_PORT_BLOBSERVER);
}
-
- log.Debug("Done: Setting access mode");
- }
- public void Control_SetIntegrationTime(int value)
- {
- log.Debug("Setting integration time");
- Control_WriteVariable("integrationTimeUs", value);
- log.Debug("Done: Setting integration time");
- }
- public int Control_GetIntegrationTime()
- {
- log.Debug("Getting integration time");
- int value = Control_ReadVariable("integrationTimeUs");
- log.Debug("Done: Getting integration time");
-
- return value;
- }
- private void Control_WriteVariable(string name, int value)
- {
- List bytes = new List(Encoding.ASCII.GetBytes("sWN " + name + " "));
- byte[] valueBytes = BitConverter.GetBytes(value);
- if (BitConverter.IsLittleEndian)
+ catch (Exception ex)
{
- Array.Reverse(valueBytes);
+ log.ErrorFormat("Failed to connect to IP={0}, reasons={1}", ipAddress, ex.Message);
+ ExceptionBuilder.Throw(ex.GetType(), cam, "error_connectionFailed", "Unable to connect to camera.");
}
- bytes.AddRange(valueBytes);
- byte[] toSend = AddFraming(bytes.ToArray());
- byte[] receive = new byte[14];
- // send ctrl message
- streamControl.Write(toSend, 0, toSend.Length);
+ streamData = sockData.GetStream();
- // get response
- if (streamControl.Read(receive, 0, receive.Length) == 0)
- {
- log.Error("Got no answer from camera");
- ExceptionBuilder.Throw(typeof(InvalidOperationException), cam, "error_setParameter", "Failed to start stream.");
- }
- else
- {
- string response = Encoding.ASCII.GetString(receive);
- log.DebugFormat("Got response: {0}", response);
- }
+ // say "hello" to camera
+ byte[] hbBytes = Encoding.ASCII.GetBytes(HEARTBEAT_MSG);
+ streamData.Write(hbBytes, 0, hbBytes.Length);
}
- private int Control_ReadVariable(string name)
- {
- List bytes = new List(Encoding.ASCII.GetBytes("sRN " + name));
- byte[] toSend = AddFraming(bytes.ToArray());
- byte[] receiveHeader = new byte[8];
-
- // send ctrl message
- streamControl.Write(toSend, 0, toSend.Length);
+ #endregion
- // get response
- if (streamControl.Read(receiveHeader, 0, receiveHeader.Length) == 0)
- {
- log.Error("Got no answer from camera");
- ExceptionBuilder.Throw(typeof(InvalidOperationException), cam, "error_setParameter", "Failed to start stream.");
- return 0;
- }
- else
- {
- // TODO: Check if header matches expected.
- // Remark: On big endian machines this won't work:
- byte[] payloadLengthBytes = new byte[] { receiveHeader[7], receiveHeader[6], receiveHeader[5], receiveHeader[4] };
- int payloadLength = BitConverter.ToInt32(payloadLengthBytes, 0);
- byte[] receivePayload = new byte[payloadLength + 1];
- streamControl.Read(receivePayload, 0, receivePayload.Length);
- // last byte is checksum, the 4 bytes before are the response value in big endian.
- int l = receivePayload.Length;
- byte[] responseValueBytes = new byte[] { receivePayload[l - 5], receivePayload[l - 4], receivePayload[l - 3], receivePayload[l - 2] };
- int value = BitConverter.ToInt32(responseValueBytes, 0);
- log.DebugFormat("Got value: {0}", value);
- return value;
- }
- }
+ #region Internal Methods
///
/// Gets the raw frame data from camera.
///
/// Data is checked for correct protocol version and packet type.
/// Raw frame
- public byte[] Stream_GetFrame()
+ internal byte[] Stream_GetFrame()
{
log.Debug("Start getting frame");
List data = new List();
@@ -302,40 +130,14 @@ public byte[] Stream_GetFrame()
return data.ToArray();
}
- ///
- /// This methods establishes TCP connections to control and data port.
- ///
- public void Connect()
- {
- try
- {
- sockControl = new TcpClient(ipAddress, TCP_PORT_SOPAS);
- sockData = new TcpClient(ipAddress, TCP_PORT_BLOBSERVER);
- }
- catch (Exception ex)
- {
- log.ErrorFormat("Failed to connect to IP={0}, reasons={1}", ipAddress, ex.Message);
- ExceptionBuilder.Throw(ex.GetType(), cam, "error_connectionFailed", "Unable to connect to camera.");
- }
-
- streamControl = sockControl.GetStream();
- streamData = sockData.GetStream();
-
- // say "hello" to camera
- byte[] hbBytes = Encoding.ASCII.GetBytes(HEARTBEAT_MSG);
- streamData.Write(hbBytes, 0, hbBytes.Length);
- }
-
///
/// Close connections to both ports.
///
- public void Disconnect()
+ internal void Disconnect()
{
try
{
- streamControl.Close();
streamData.Close();
- sockControl.Close();
sockData.Close();
}
catch (Exception ex)
@@ -344,92 +146,5 @@ public void Disconnect()
}
}
#endregion
-
- #region Internal Methods
- ///
- /// Binary framing used to serialize commands.
- /// Adds START_STX, size and checksum.
- ///
- /// actual command
- /// framed message
- private byte[] AddFraming(string payload)
- {
- // transform to ASCII (1 byte per character)
- byte[] bytes = Encoding.ASCII.GetBytes(payload);
-
- return AddFraming(bytes);
- }
-
- private byte[] AddFraming(byte[] bytes)
- {
- // calculate sizes and prepare message
- int msgSize = bytes.Length + START_STX.Length + 1 + 4; // +1 for checksum, +4 for size of payload
- uint payloadSize = (uint)bytes.Length;
- byte[] payloadSizeBytes = BitConverter.GetBytes(payloadSize);
- if (BitConverter.IsLittleEndian)
- {
- Array.Reverse(payloadSizeBytes);
- }
- byte[] message = new byte[msgSize];
- byte checksum = ChkSumCola(bytes);
-
- // build message
- int i;
- for (i = 0; i < START_STX.Length; ++i)
- {
- message[i] = START_STX[i];
- }
- for (int j = 0; j < 4; ++j)
- {
- message[i++] = payloadSizeBytes[j];
- }
- for (int j = 0; j < bytes.Length; ++j)
- {
- message[i++] = bytes[j];
- }
- message[i] = checksum;
-
- return message;
- }
-
- ///
- /// CheckSum: XOR over all bytes.
- ///
- /// byte array to compute checksum for
- /// checksum byte
- private byte ChkSumCola(byte[] value)
- {
- if (value.Length == 0)
- return 0x00;
- if (value.Length == 1)
- return value[0];
-
- byte x = value[0];
- for (int i = 1; i < value.Length; ++i)
- x ^= value[i];
-
- return x;
- }
-
- ///
- /// CheckSum: XOR over all bytes.
- ///
- /// string to compute checksum for
- /// checksum byte
- private byte ChkSumCola(string value)
- {
- return ChkSumCola(Encoding.ASCII.GetBytes(value));
- }
-
- ///
- /// Gets the byte array as hex string. Used for debugging purpose only.
- ///
- /// bytes
- /// hex string
- private string GetHex(byte[] bytes)
- {
- return BitConverter.ToString(bytes);
- }
- #endregion
}
}
diff --git a/BetaCameras/Sick.VisionaryT/FrameData.cs b/BetaCameras/Sick.VisionaryT/FrameData.cs
index e8eed573..0450c3fc 100644
--- a/BetaCameras/Sick.VisionaryT/FrameData.cs
+++ b/BetaCameras/Sick.VisionaryT/FrameData.cs
@@ -18,8 +18,8 @@ internal class FrameData
{
#region Private Variables
// camera instance and log
- private Camera cam;
- private MetriLog log;
+ private readonly Camera cam;
+ private readonly MetriLog log;
// image data
private byte[] imageData;
// internal definitions
@@ -32,181 +32,74 @@ internal class FrameData
private uint[] offsets;
private uint[] changedCounters;
// camera parameters
- private int width;
- private int height;
private float[] cam2WorldMatrix;
- private float fx;
- private float fy;
- private float cx;
- private float cy;
- private float k1;
- private float k2;
- private float f2rc;
- // image properties
- private int numBytesIntensity;
- private int numBytesDistance;
- private int numBytesConfidence;
- private int numBytesPerIntensityValue;
- private int numBytesPerDistanceValue;
- private int numBytesPerConfidenceValue;
- private int intensityStartOffset;
- private int distanceStartOffset;
- private int confidenceStartOffset;
- private ulong timeStamp;
#endregion
#region Properties
///
/// Width of image.
///
- public int Width
- {
- get { return width; }
- }
+ internal int Width { get; private set; }
///
/// Height of image.
///
- public int Height
- {
- get { return height; }
- }
+ internal int Height { get; private set; }
///
/// FX.
///
- public float FX
- {
- get { return fx; }
- }
+ internal float FX { get; private set; }
///
/// FY.
///
- public float FY
- {
- get { return fy; }
- }
+ internal float FY { get; private set; }
///
/// CX.
///
- public float CX
- {
- get { return cx; }
- }
+ internal float CX { get; private set; }
///
/// CY.
///
- public float CY
- {
- get { return cy; }
- }
+ internal float CY { get; private set; }
///
/// K1.
///
- public float K1
- {
- get { return k1; }
- }
+ internal float K1 { get; private set; }
///
/// K2.
///
- public float K2
- {
- get { return k2; }
- }
+ internal float K2 { get; private set; }
///
/// Focal to ray cross.
///
- public float F2RC
- {
- get { return f2rc; }
- }
-
- ///
- /// Total number of bytes for intensity image.
- ///
- public int NumBytesIntensity
- {
- get { return numBytesIntensity; }
- }
-
- ///
- /// How many bytes are used to store one intensity value.
- ///
- public int NumBytesPerIntensityValue
- {
- get { return numBytesPerIntensityValue; }
- }
+ internal float F2RC { get; private set; }
///
/// Where does the intensity data start (relative to start of imageData).
///
- public int IntensityStartOffset
- {
- get { return intensityStartOffset; }
- }
-
- ///
- /// Total number of bytes for distance image.
- ///
- public int NumBytesDistance
- {
- get { return numBytesDistance; }
- }
-
- ///
- /// How many bytes are used to store one distance value.
- ///
- public int NumBytesPerDistanceValue
- {
- get { return numBytesPerIntensityValue; }
- }
+ internal int IntensityStartOffset { get; private set; }
///
/// Where does the intensity data start (relative to start of imageData).
///
- public int DistanceStartOffset
- {
- get { return distanceStartOffset; }
- }
-
- ///
- /// Total number of bytes for confidence image.
- ///
- public int NumBytesConfidence
- {
- get { return numBytesConfidence; }
- }
-
- ///
- /// How many bytes are used to store one confidence value.
- ///
- public int NumBytesPerConfidenceValue
- {
- get { return numBytesPerConfidenceValue; }
- }
+ internal int DistanceStartOffset { get; private set; }
///
/// Where does the confidence data start (relative to start of imageData).
///
- public int ConfidenceStartOffset
- {
- get { return confidenceStartOffset; }
- }
+ internal int ConfidenceStartOffset { get; private set; }
///
/// Time stamp of image.
///
- public ulong TimeStamp
- {
- get { return timeStamp; }
- }
+ internal ulong TimeStamp { get; private set; }
#endregion
#region Constructor
@@ -216,20 +109,20 @@ public ulong TimeStamp
/// image data
/// camera instance
/// metri log from camera instance
- public FrameData(byte[] data, Camera cam, MetriLog log)
+ internal FrameData(byte[] data, Camera cam, MetriLog log)
{
imageData = data;
this.cam = cam;
this.log = log;
- DefaultValues();
+ SetDefaultValues();
}
#endregion
- #region Public Methods
+ #region Internal Methods
///
/// This methods parses the data provided by constructor and sets the properties accordingly.
///
- public void Read()
+ internal void Read()
{
// first 11 bytes: internal definitions consisting of:
// 4 bytes STx
@@ -298,25 +191,30 @@ public void Read()
// now: XML segment
string xml = Encoding.ASCII.GetString(imageData, (int)offsets[0], (int)offsets[1] - (int)offsets[0]);
- ReadXML(xml);
+ ReadXML(xml, out int numBytesPerIntensityValue, out int numBytesPerDistanceValue, out int numBytesPerConfidenceValue);
// calc sizes
- numBytesIntensity = width * height * numBytesPerIntensityValue;
- numBytesDistance = width * height * numBytesPerDistanceValue;
- numBytesConfidence = width * height * numBytesPerConfidenceValue;
+ int numBytesIntensity = Width * Height * numBytesPerIntensityValue;
+ int numBytesDistance = Width * Height * numBytesPerDistanceValue;
+ int numBytesConfidence = Width * Height * numBytesPerConfidenceValue;
// now: save image data offsets
- ReadBinary();
+ ReadBinary(numBytesIntensity, numBytesDistance, numBytesConfidence);
}
#endregion
- #region Internal Methods
+ #region Private Methods
///
/// Reads the XML part and saves the image properties (e.g. width/height).
///
/// XML string
- private void ReadXML(string xml)
+ private void ReadXML(string xml, out int numBytesPerIntensityValue, out int numBytesPerDistanceValue, out int numBytesPerConfidenceValue)
{
+ // set default values to make compiler happy
+ numBytesPerIntensityValue = 2;
+ numBytesPerDistanceValue = 2;
+ numBytesPerConfidenceValue = 2;
+
XmlDocument doc = new XmlDocument();
doc.Load(new StringReader(xml));
@@ -326,8 +224,8 @@ private void ReadXML(string xml)
XmlNode dataStream = formatDescriptionDepthMap["DataStream"];
// get camera/image parameters
- width = Convert.ToInt32(dataStream["Width"].InnerText);
- height = Convert.ToInt32(dataStream["Height"].InnerText);
+ Width = Convert.ToInt32(dataStream["Width"].InnerText);
+ Height = Convert.ToInt32(dataStream["Height"].InnerText);
XmlNode cameraToWorldTransform = dataStream["CameraToWorldTransform"];
List cam2WorldList = new List();
@@ -339,16 +237,16 @@ private void ReadXML(string xml)
cam2WorldList = null;
XmlNode cameraMatrix = dataStream["CameraMatrix"];
- fx = float.Parse(cameraMatrix.ChildNodes[0].InnerText, CultureInfo.InvariantCulture.NumberFormat);
- fy = float.Parse(cameraMatrix.ChildNodes[1].InnerText, CultureInfo.InvariantCulture.NumberFormat);
- cx = float.Parse(cameraMatrix.ChildNodes[2].InnerText, CultureInfo.InvariantCulture.NumberFormat);
- cy = float.Parse(cameraMatrix.ChildNodes[3].InnerText, CultureInfo.InvariantCulture.NumberFormat);
+ FX = float.Parse(cameraMatrix.ChildNodes[0].InnerText, CultureInfo.InvariantCulture.NumberFormat);
+ FY = float.Parse(cameraMatrix.ChildNodes[1].InnerText, CultureInfo.InvariantCulture.NumberFormat);
+ CX = float.Parse(cameraMatrix.ChildNodes[2].InnerText, CultureInfo.InvariantCulture.NumberFormat);
+ CY = float.Parse(cameraMatrix.ChildNodes[3].InnerText, CultureInfo.InvariantCulture.NumberFormat);
XmlNode distortionParams = dataStream["CameraDistortionParams"];
- k1 = float.Parse(distortionParams["K1"].InnerText, CultureInfo.InvariantCulture.NumberFormat);
- k2 = float.Parse(distortionParams["K2"].InnerText, CultureInfo.InvariantCulture.NumberFormat);
+ K1 = float.Parse(distortionParams["K1"].InnerText, CultureInfo.InvariantCulture.NumberFormat);
+ K2 = float.Parse(distortionParams["K2"].InnerText, CultureInfo.InvariantCulture.NumberFormat);
- f2rc = Convert.ToSingle(dataStream["FocalToRayCross"].InnerText, CultureInfo.InvariantCulture.NumberFormat);
+ F2RC = Convert.ToSingle(dataStream["FocalToRayCross"].InnerText, CultureInfo.InvariantCulture.NumberFormat);
// data types, should always be uint16_t
if (dataStream["Distance"].InnerText.ToLower() == "uint16")
@@ -383,7 +281,7 @@ private void ReadXML(string xml)
///
/// Calculates the offsets where to find the image data for channels.
///
- private void ReadBinary()
+ private void ReadBinary(int numBytesIntensity, int numBytesDistance, int numBytesConfidence)
{
int offset = (int)offsets[1];
@@ -393,8 +291,8 @@ private void ReadBinary()
offset += 4;
// 8 bytes timestamp
- timeStamp = BitConverter.ToUInt64(imageData, offset);
- timeStamp = Utils.ConvertEndiannessUInt64(timeStamp);
+ TimeStamp = BitConverter.ToUInt64(imageData, offset);
+ TimeStamp = Utils.ConvertEndiannessUInt64(TimeStamp);
offset += 8;
// 2 bytes version
@@ -416,13 +314,13 @@ private void ReadBinary()
offset += 1;
// 176 * 144 * 2 bytes distance data
- distanceStartOffset = offset;
+ DistanceStartOffset = offset;
offset += numBytesDistance;
// 176 * 144 * 2 bytes intensity data
- intensityStartOffset = offset;
+ IntensityStartOffset = offset;
offset += numBytesIntensity;
// 176 * 144 * 2 bytes confidence data
- confidenceStartOffset = offset;
+ ConfidenceStartOffset = offset;
offset += numBytesConfidence;
// 4 bytes CRC of data
@@ -445,37 +343,34 @@ private void ReadBinary()
///
/// Setup default values for camera parameters.
///
- private void DefaultValues()
+ private void SetDefaultValues()
{
- width = 176;
- height = 144;
- fx = 146.5f;
- fy = 146.5f;
- cx = 84.4f;
- cy = 71.2f;
- k1 = 0.326442f;
- k2 = 0.219623f;
- f2rc = 0.0f;
+ Width = 176;
+ Height = 144;
+ FX = 146.5f;
+ FY = 146.5f;
+ CX = 84.4f;
+ CY = 71.2f;
+ K1 = 0.326442f;
+ K2 = 0.219623f;
+ F2RC = 0.0f;
cam2WorldMatrix = new float[16];
- cam2WorldMatrix[0 ] = 1.0f;
- cam2WorldMatrix[1 ] = 0.0f;
- cam2WorldMatrix[2 ] = 0.0f;
- cam2WorldMatrix[3 ] = 0.0f;
- cam2WorldMatrix[4 ] = 0.0f;
- cam2WorldMatrix[5 ] = 1.0f;
- cam2WorldMatrix[6 ] = 0.0f;
- cam2WorldMatrix[7 ] = 0.0f;
- cam2WorldMatrix[8 ] = 0.0f;
- cam2WorldMatrix[9 ] = 0.0f;
+ cam2WorldMatrix[0] = 1.0f;
+ cam2WorldMatrix[1] = 0.0f;
+ cam2WorldMatrix[2] = 0.0f;
+ cam2WorldMatrix[3] = 0.0f;
+ cam2WorldMatrix[4] = 0.0f;
+ cam2WorldMatrix[5] = 1.0f;
+ cam2WorldMatrix[6] = 0.0f;
+ cam2WorldMatrix[7] = 0.0f;
+ cam2WorldMatrix[8] = 0.0f;
+ cam2WorldMatrix[9] = 0.0f;
cam2WorldMatrix[10] = 1.0f;
cam2WorldMatrix[11] = 0.0f;
cam2WorldMatrix[12] = 0.0f;
cam2WorldMatrix[13] = 0.0f;
cam2WorldMatrix[14] = 0.0f;
cam2WorldMatrix[15] = 1.0f;
- numBytesPerDistanceValue = 2;
- numBytesPerIntensityValue = 2;
- numBytesPerConfidenceValue = 2;
}
#endregion
}
diff --git a/BetaCameras/Sick.VisionaryT/Sick.VisionaryT.csproj b/BetaCameras/Sick.VisionaryT/Sick.VisionaryT.csproj
index 3a218865..0e9386bd 100644
--- a/BetaCameras/Sick.VisionaryT/Sick.VisionaryT.csproj
+++ b/BetaCameras/Sick.VisionaryT/Sick.VisionaryT.csproj
@@ -79,6 +79,8 @@
SolutionAssemblyInfo.cs
+
+
@@ -89,6 +91,8 @@
+
+
diff --git a/BetaCameras/Sick.VisionaryT/Utils.cs b/BetaCameras/Sick.VisionaryT/Utils.cs
index 959794f1..f15b415c 100644
--- a/BetaCameras/Sick.VisionaryT/Utils.cs
+++ b/BetaCameras/Sick.VisionaryT/Utils.cs
@@ -19,7 +19,7 @@ internal class Utils
///
/// big endian uint32_t
/// corresponding little endian uint32_t
- public static UInt32 ConvertEndiannessUInt32(UInt32 value)
+ internal static UInt32 ConvertEndiannessUInt32(UInt32 value)
{
if (!BitConverter.IsLittleEndian)
{
@@ -37,7 +37,7 @@ public static UInt32 ConvertEndiannessUInt32(UInt32 value)
///
/// big endian uint16_t
/// corresponding little endian uint16_t
- public static UInt16 ConvertEndiannessUInt16(UInt16 value)
+ internal static UInt16 ConvertEndiannessUInt16(UInt16 value)
{
if (!BitConverter.IsLittleEndian)
{
@@ -55,7 +55,7 @@ public static UInt16 ConvertEndiannessUInt16(UInt16 value)
///
/// big endian uint64_t
/// corresponding little endian uint64_t
- public static UInt64 ConvertEndiannessUInt64(UInt64 value)
+ internal static UInt64 ConvertEndiannessUInt64(UInt64 value)
{
if (!BitConverter.IsLittleEndian)
{
@@ -67,5 +67,38 @@ public static UInt64 ConvertEndiannessUInt64(UInt64 value)
return BitConverter.ToUInt64(bytes, 0);
}
+
+ ///
+ /// Gets the byte array as hex string. Used for debugging purpose only.
+ ///
+ /// bytes
+ /// hex string
+ internal static string GetHex(byte[] bytes)
+ {
+ return BitConverter.ToString(bytes);
+ }
+
+ internal static byte[] CalculatePasswordHash(byte[] password)
+ {
+ var md5 = System.Security.Cryptography.MD5.Create();
+ var hash = md5.ComputeHash(password);
+ var hash2 = md5.Hash;
+
+ //dig = m.digest()
+ for (int i = 0; i < hash.Length; i++)
+ {
+ hash[i] = (byte)hash[i];
+ }
+ //var dig = [ord(x) for x in dig]; // convert bytes to int
+ var dig = hash;
+ // 128 bit to 32 bit by XOR
+ var byte0 = (byte)(dig[0] ^ dig[4] ^ dig[8] ^ dig[12]);
+ var byte1 = (byte)(dig[1] ^ dig[5] ^ dig[9] ^ dig[13]);
+ var byte2 = (byte)(dig[2] ^ dig[6] ^ dig[10] ^ dig[14]);
+ var byte3 = (byte)(dig[3] ^ dig[7] ^ dig[11] ^ dig[15]);
+ //var retValue = byte0 | (byte1 << 8) | (byte2 << 16) | (byte3 << 24);
+ var retValue = new byte[] { byte3, byte2, byte1, byte0 };
+ return retValue;
+ }
}
}
diff --git a/BetaCameras/Sick.VisionaryT/VisionaryT.cs b/BetaCameras/Sick.VisionaryT/VisionaryT.cs
index 8eb3e7ce..0f33b87a 100644
--- a/BetaCameras/Sick.VisionaryT/VisionaryT.cs
+++ b/BetaCameras/Sick.VisionaryT/VisionaryT.cs
@@ -1,9 +1,9 @@
// Copyright (c) Metrilus GmbH
// MetriCam 2 is licensed under the MIT license. See License.txt for full license text.
-using Metrilus.Util;
using System;
using MetriCam2.Cameras.Internal.Sick;
+using Metrilus.Util;
namespace MetriCam2.Cameras
{
@@ -17,6 +17,8 @@ public class VisionaryT : Camera
private string ipAddress;
// device handle
private Device device;
+ private Control _control;
+
// frame buffer
private byte[] imageBuffer;
// image data contains information about the current frame e.g. width and height
@@ -38,7 +40,7 @@ private ParamDesc IPAddressDesc
{
return new ParamDesc()
{
- Description = "IP address of camera.",
+ Description = "IP address",
ReadableWhen = ParamDesc.ConnectionStates.Disconnected | ParamDesc.ConnectionStates.Connected,
WritableWhen = ParamDesc.ConnectionStates.Disconnected
};
@@ -50,37 +52,54 @@ private ParamDesc IPAddressDesc
///
public string IPAddress
{
- get { return ipAddress; }
- set { ipAddress = value; }
+ get => ipAddress;
+ set => ipAddress = value;
}
- private RangeParamDesc IntegrationTimeDesc
+
+ private ListParamDesc IntegrationTimeDesc
{
get
{
- return new RangeParamDesc(80, 4000)
+ return new ListParamDesc(typeof(VisionaryTIntegrationTime))
{
- Description = "Integration time of camera in microseconds.",
+ Description = "Integration time",
ReadableWhen = ParamDesc.ConnectionStates.Connected,
WritableWhen = ParamDesc.ConnectionStates.Connected,
- Unit = "Microseconds",
+ Unit = "μs",
};
}
}
///
- /// Integration time of ToF-sensor.
+ /// Integration time of ToF-sensor [ms].
///
- public int IntegrationTime
+ public VisionaryTIntegrationTime IntegrationTime
+ {
+ get => _control.GetIntegrationTime();
+ set => _control.SetIntegrationTime(value);
+ }
+
+ private ListParamDesc CoexistenceModeDesc
{
get
{
- return device.Control_GetIntegrationTime();
- }
- set
- {
- device.Control_SetServiceAccessMode();
- device.Control_SetIntegrationTime(value);
+ return new ListParamDesc(typeof(VisionaryTCoexistenceMode))
+ {
+ Description = "Coexistence mode",
+ ReadableWhen = ParamDesc.ConnectionStates.Connected,
+ WritableWhen = ParamDesc.ConnectionStates.Connected,
+ };
}
}
+
+ ///
+ /// Coexistence mode (modulation frequency) of ToF-sensor.
+ ///
+ public VisionaryTCoexistenceMode CoexistenceMode
+ {
+ get => _control.GetCoexistenceMode();
+ set => _control.SetCoexistenceMode(value);
+ }
+
private ParamDesc WidthDesc
{
get
@@ -97,10 +116,7 @@ private ParamDesc WidthDesc
///
/// Width of images.
///
- public int Width
- {
- get { return width; }
- }
+ public int Width => width;
private ParamDesc HeightDesc
{
@@ -118,10 +134,7 @@ private ParamDesc HeightDesc
///
/// Height of images.
///
- public int Height
- {
- get { return height; }
- }
+ public int Height => height;
#endregion
#region Constructor
@@ -176,58 +189,32 @@ protected override void LoadAllAvailableChannels()
/// This method is implicitly called by inside a camera lock.
protected override void ConnectImpl()
{
- if (ipAddress == null || ipAddress == "")
+ if (string.IsNullOrWhiteSpace(ipAddress))
{
log.Error("IP Address is not set.");
- ExceptionBuilder.Throw(typeof(MetriCam2.Exceptions.ConnectionFailedException), this, "error_connectionFailed", "IP Address is not set! Set it before connecting!");
+ ExceptionBuilder.Throw(typeof(Exceptions.ConnectionFailedException), this, "error_connectionFailed", "IP Address is not set! Set it before connecting!");
}
device = new Device(ipAddress, this, log);
- device.Connect();
- device.Control_InitStream();
- device.Control_StartStream();
+
+ _control = new Control(log, ipAddress);
+ _control.StartStream();
// select intensity channel
ActivateChannel(ChannelNames.Intensity);
SelectChannel(ChannelNames.Intensity);
- this.UpdateImpl();
+ // Call update once
+ UpdateImpl();
}
- ///
- /// Loads the intrisic parameters from the camera.
- ///
- /// Channel for which intrisics are loaded.
- /// ProjectiveTransformationZhang object holding the intrinsics.
- public override IProjectiveTransformation GetIntrinsics(string channelName)
- {
- if (channelName == ChannelNames.Intensity || channelName == ChannelNames.Distance)
- {
- ProjectiveTransformationZhang proj;
- lock (cameraLock)
- {
- proj = new ProjectiveTransformationZhang(imageData.Width,
- imageData.Height,
- imageData.FX,
- imageData.FY,
- imageData.CX,
- imageData.CY,
- imageData.K1,
- imageData.K2,
- 0,
- 0,
- 0);
- }
- return proj;
- }
- throw new ArgumentException(string.Format("Channel {0} intrinsics not supported.", channelName));
- }
///
/// Disconnects the camera.
///
/// This method is implicitly called by inside a camera lock.
protected override void DisconnectImpl()
{
- device.Control_StopStream();
+ _control.Close();
+ _control = null;
device.Disconnect();
device = null;
}
@@ -254,16 +241,16 @@ protected override CameraImage CalcChannelImpl(string channelName)
{
switch (channelName)
{
- case ChannelNames.Intensity:
- return CalcIntensity();
- case ChannelNames.Distance:
- return CalcDistance();
- case ChannelNames.ConfidenceMap:
- return CalcConfidenceMap(CalcRawConfidenceMap());
- case ChannelNames.RawConfidenceMap:
- return CalcRawConfidenceMap();
- case ChannelNames.Point3DImage:
- return Calc3D();
+ case ChannelNames.Intensity:
+ return CalcIntensity();
+ case ChannelNames.Distance:
+ return CalcDistance();
+ case ChannelNames.ConfidenceMap:
+ return CalcConfidenceMap(CalcRawConfidenceMap());
+ case ChannelNames.RawConfidenceMap:
+ return CalcRawConfidenceMap();
+ case ChannelNames.Point3DImage:
+ return Calc3D();
}
log.Error("Invalid channelname: " + channelName);
return null;
@@ -343,7 +330,7 @@ private UShortCameraImage CalcRawConfidenceMap()
for (int j = 0; j < imageData.Width; ++j)
{
// take two bytes and create integer (little endian)
- result[i, j] = (ushort) ((ushort)imageBuffer[start + 1] << 8 | (ushort)imageBuffer[start + 0]);
+ result[i, j] = (ushort)((ushort)imageBuffer[start + 1] << 8 | (ushort)imageBuffer[start + 0]);
start += 2;
}
}
@@ -362,7 +349,7 @@ private FloatCameraImage CalcConfidenceMap(UShortCameraImage rawConfidenceMap)
{
int width = rawConfidenceMap.Width;
int height = rawConfidenceMap.Height;
- float scaling = 1.0f / (float) ushort.MaxValue;
+ float scaling = 1.0f / (float)ushort.MaxValue;
FloatCameraImage confidenceMap = new FloatCameraImage(width, height);
for (int y = 0; y < height; y++)
@@ -406,7 +393,7 @@ private Point3fCameraImage Calc3D()
// we map from image coordinates with origin top left and x horizontal (right) and y vertical
// (downwards) to camera coordinates with origin in center and x to the left and y upwards (seen
// from the sensor position)
- double xp = (cx - j) / fx;
+ double xp = (cx - j) / fx;
double yp = (cy - i) / fy;
// correct the camera distortion
@@ -415,11 +402,11 @@ private Point3fCameraImage Calc3D()
double k = 1 + k1 * r2 + k2 * r4;
double xd = xp * k;
double yd = yp * k;
-
- double s0 = Math.Sqrt(xd * xd + yd * yd + 1.0);
- double x = xd * depth / s0;
+
+ double s0 = Math.Sqrt(xd * xd + yd * yd + 1.0);
+ double x = xd * depth / s0;
double y = yd * depth / s0;
- double z = depth / s0 - f2rc;
+ double z = depth / s0 - f2rc;
x /= 1000;
y /= 1000;
diff --git a/BetaCameras/Sick.VisionaryT/VisionaryTCoexistenceMode.cs b/BetaCameras/Sick.VisionaryT/VisionaryTCoexistenceMode.cs
new file mode 100644
index 00000000..1f023ed4
--- /dev/null
+++ b/BetaCameras/Sick.VisionaryT/VisionaryTCoexistenceMode.cs
@@ -0,0 +1,21 @@
+using System;
+
+namespace MetriCam2.Cameras
+{
+ ///
+ /// SICK Visionary-T Coexistence Modes or Modulation Frequencies
+ ///
+ public enum VisionaryTCoexistenceMode : byte
+ {
+ Code1 = 0,
+ Code2 = 1,
+ Code3 = 2,
+ Code4 = 3,
+ Code5 = 4,
+ Code6 = 5,
+ Code7 = 6,
+ Code8 = 7,
+
+ Automatic = 8
+ }
+}
diff --git a/BetaCameras/Sick.VisionaryT/VisionaryTIntegrationTime.cs b/BetaCameras/Sick.VisionaryT/VisionaryTIntegrationTime.cs
new file mode 100644
index 00000000..c2ec40ea
--- /dev/null
+++ b/BetaCameras/Sick.VisionaryT/VisionaryTIntegrationTime.cs
@@ -0,0 +1,22 @@
+using System;
+using System.ComponentModel;
+
+namespace MetriCam2.Cameras
+{
+ ///
+ /// SICK Visionary-T Integration Time options (in microseconds)
+ ///
+ public enum VisionaryTIntegrationTime : byte
+ {
+ [Description("500 μs")]
+ Microseconds500 = 0,
+ [Description("1000 μs")]
+ Microseconds1000 = 1,
+ [Description("1500 μs")]
+ Microseconds1500 = 2,
+ [Description("2000 μs")]
+ Microseconds2000 = 3,
+ [Description("2500 μs")]
+ Microseconds2500 = 4,
+ }
+}