private unsafe void GetVariablesOther(out ServoStatus[] servos)
{
byte[] servoSettingsArray = new byte[MaxServoCount * sizeof(ServoStatus)];
var bytesRead = (uint)_device.ControlTransfer(RequestType._0xC0, Request.REQUEST_GET_SERVO_SETTINGS, 0, 0, servoSettingsArray);
if (bytesRead != servoSettingsArray.Length)
{
throw new DataMisalignedException("Short read: " + bytesRead + " < " + servoSettingsArray.Length + ".");
}
// Put the data in to a managed array object.
servos = new ServoStatus[MaxServoCount];
fixed(byte *pointer = servoSettingsArray)
{
for (byte i = 0; i < MaxServoCount; i++)
{
servos[i] = *(ServoStatus *)(pointer + sizeof(ServoStatus) * i);
}
}
}
private unsafe void GetVariablesServo6(out MaestroVariables variables, out short[] stack, out ushort[] callStack, out ServoStatus[] servos)
{
byte[] array = new byte[sizeof(MicroMaestroVariables) + MaxServoCount * sizeof(ServoStatus)];
_device.ControlTransfer(RequestType._0xC0, Request.REQUEST_GET_VARIABLES, 0, 0, array);
fixed(byte *pointer = array)
{
// copy the variable data
MicroMaestroVariables tmp = *(MicroMaestroVariables *)pointer;
variables.StackPointer = tmp.StackPointer;
variables.CallStackPointer = tmp.CallStackPointer;
variables.Errors = tmp.Errors;
variables.ProgramCounter = tmp.ProgramCounter;
variables.ScriptDone = tmp.ScriptDone;
variables.PerformanceFlags = 0;
servos = new ServoStatus[MaxServoCount];
for (byte i = 0; i < MaxServoCount; i++)
{
servos[i] = *(ServoStatus *)(pointer + sizeof(MicroMaestroVariables) + sizeof(ServoStatus) * i);
}
stack = new short[variables.StackPointer];
for (byte i = 0; i < stack.Length; i++)
{
stack[i] = *(tmp.Stack + i);
}
callStack = new ushort[variables.CallStackPointer];
for (byte i = 0; i < callStack.Length; i++)
{
callStack[i] = *(tmp.CallStack + i);
}
}
}
/// <summary>
/// Gets an array of ServoStatus structs representing
/// the current status of all the channels.
/// </summary>
/// <remarks>If you are using the Micro Maestro and calling
/// getVariables more than once in quick succession,
/// then you can save some CPU time by just using the
/// overload that has 4 arguments.
/// </remarks>
public void getVariables(out ServoStatus[] servos)
{
if (microMaestro)
{
MaestroVariables variables;
short[] stack;
ushort[] callStack;
getVariablesMicroMaestro(out variables, out stack, out callStack, out servos);
}
else
{
getVariablesMiniMaestro(out servos);
}
}
/// <summary>
/// Gets the complete set of status information for the Maestro.
/// </summary>
/// <remarks>If you are using a Mini Maestro and do not need all of
/// the data provided by this function, you can save some CPU time
/// by using the overloads with fewer arguments.</remarks>
public unsafe void getVariables(out MaestroVariables variables, out short[] stack, out ushort[] callStack, out ServoStatus[] servos)
{
if (microMaestro)
{
// On the Micro Maestro, this function requires just one control transfer:
getVariablesMicroMaestro(out variables, out stack, out callStack, out servos);
}
else
{
// On the Mini Maestro, this function requires four control transfers:
getVariablesMiniMaestro(out variables);
getVariablesMiniMaestro(out servos);
getVariablesMiniMaestro(out stack);
getVariablesMiniMaestro(out callStack);
}
}
private unsafe void getVariablesMiniMaestro(out ServoStatus[] servos)
{
try
{
byte[] servoSettingsArray = new byte[servoCount * sizeof(ServoStatus)];
// Get the raw data from the device.
UInt32 bytesRead = controlTransfer(0xC0, (byte)uscRequest.REQUEST_GET_SERVO_SETTINGS, 0, 0, servoSettingsArray);
if (bytesRead != servoSettingsArray.Length)
{
throw new Exception("Short read: " + bytesRead + " < " + servoSettingsArray.Length + ".");
}
// Put the data in to a managed array object.
servos = new ServoStatus[servoCount];
fixed (byte* pointer = servoSettingsArray)
{
for (byte i = 0; i < servoCount; i++)
{
servos[i] = *(ServoStatus*)(pointer + sizeof(ServoStatus) * i);
}
}
}
catch (Exception e)
{
throw new Exception("Error getting channel settings from device.", e);
}
}
private unsafe void getVariablesMicroMaestro(out MaestroVariables variables, out short[] stack, out ushort[] callStack, out ServoStatus[] servos)
{
byte[] array = new byte[sizeof(MicroMaestroVariables) + servoCount * sizeof(ServoStatus)];
try
{
controlTransfer(0xC0, (byte)uscRequest.REQUEST_GET_VARIABLES, 0, 0, array);
}
catch (Exception e)
{
throw new Exception("There was an error getting the device variables.", e);
}
fixed (byte* pointer = array)
{
// copy the variable data
MicroMaestroVariables tmp = *(MicroMaestroVariables*)pointer;
variables.stackPointer = tmp.stackPointer;
variables.callStackPointer = tmp.callStackPointer;
variables.errors = tmp.errors;
variables.programCounter = tmp.programCounter;
variables.scriptDone = tmp.scriptDone;
variables.performanceFlags = 0;
servos = new ServoStatus[servoCount];
for (byte i = 0; i < servoCount; i++)
{
servos[i] = *(ServoStatus*)(pointer + sizeof(MicroMaestroVariables) + sizeof(ServoStatus) * i);
}
stack = new short[variables.stackPointer];
for(byte i = 0; i < stack.Length; i++) { stack[i] = *(tmp.stack+i); }
callStack = new ushort[variables.callStackPointer];
for (byte i = 0; i < callStack.Length; i++) { callStack[i] = *(tmp.callStack + i); }
}
}