private void UpdatePosition(PositionMessage positionMessage)
{
int x = BrunnerPosition2Arduino(positionMessage.aileron);
int y = BrunnerPosition2Arduino(positionMessage.elevator);
if (x != _position[0])
{
_position[0] = x;
axisHasMoved[0] = true;
}
if (y != _position[1])
{
_position[1] = y;
axisHasMoved[1] = true;
}
if (ticksToNextPositionChange < ((forcedArduinoCalculation - minimumTimeBetweenCalculations) / timerMs))
{
// if both axes have moved and at least minimumTimeBetweenCalculations has passed
if (axisHasMoved[0] && axisHasMoved[1])
{
Interlocked.Exchange(ref ticksToNextPositionChange, 0);
}
// If only one of the two axes has moved, we might want to wait up to 10ms to see if we get an update for the other
// some times we get an update for only one of the two axes
else if (axisHasMoved[0] || axisHasMoved[1])
{
Interlocked.Exchange(ref ticksToNextPositionChange, Math.Min((9 / timerMs) + 1, ticksToNextPositionChange));
}
}
}
public PositionMessage ReadPosition()
{
PositionMessage position = new PositionMessage();
int len = Marshal.SizeOf(position);
byte[] bytes = sock.Receive(ref remoteEP);
ByteArrayToPositionMessage(bytes, ref position);
return(position);
}
static void ByteArrayToPositionMessage(byte[] bytearray, ref PositionMessage obj)
{
int len = Marshal.SizeOf(obj);
IntPtr i = Marshal.AllocHGlobal(len);
Marshal.Copy(bytearray, 0, i, len);
obj = (PositionMessage)Marshal.PtrToStructure(i, obj.GetType());
Marshal.FreeHGlobal(i);
}
private void Communicate(RobustSerial arduinoPort, Cls2SimSocket brunnerSocket)
{
bool lockTaken = false;
Interlocked.Decrement(ref ticksToNextPositionChange);
Monitor.TryEnter(lockObject, TimeSpan.FromMilliseconds(1), ref lockTaken);
try
{
if (lockTaken)
{
// wait for receiving new position
if (brunnerSocket != null)
{
// Notice we change the order of Aileron,Elevator
ForceMessage forceMessage = new ForceMessage(
ArduinoForce2Brunner(force[1]), ArduinoForce2Brunner(force[0]));
if (delaySeconds > 0)
{
forceMessage.aileron = 0;
forceMessage.elevator = 0;
}
PositionMessage positionMessage = brunnerSocket.SendForcesReadPosition(forceMessage);
UpdatePosition(positionMessage);
}
// send the current position to the Arduino
if (arduinoPort.semaphore >= 1 && ticksToNextPositionChange <= 0)
{
arduinoPort.WriteOrder(Order.POSITION);
arduinoPort.WriteInt16(this.position[0]);
arduinoPort.WriteInt16(this.position[1]);
Interlocked.Exchange(ref ticksToNextPositionChange, (forcedArduinoCalculation / timerMs) + 1);
axisHasMoved[0] = false;
axisHasMoved[1] = false;
}
if (arduinoPort.semaphore >= 1)
{
arduinoPort.WriteOrder(Order.FORCES);
}
}
}
catch (Exception ex)
{
logger.Error(ex, ex.StackTrace);
logger.Error(ex, ex.Message);
stopExecuting = true;
}
}
