public int SendForces(ForceMessage forces)
{
byte[] msg = StructureToByteArray(forces);
int bytesSent = sock.Send(msg, msg.Length);
return(bytesSent);
}
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;
}
}
public PositionMessage SendForcesReadPosition(ForceMessage forces)
{
SendForces(forces);
return(ReadPosition());
}