public void loop()
{
stopExecuting = false;
using (RobustSerial arduinoPort = connectArduino())
using (Cls2SimSocket brunnerSocket = connectBrunner(required: false))
{
var timer = new HighPrecisionTimer.MultimediaTimer();
timer.Interval = timerMs;
timer.Resolution = 2;
//do what you need with the serial port here
try
{
arduinoPort.WaitForConnection();
logger.Info("Checking Arduino Firmware version");
CheckArduinoSketchVersion(arduinoPort, maxSeconds: 3);
_isArduinoConnected = true;
if (brunnerSocket != null)
{
_isBrunnerConnected = true;
}
timer.Elapsed += (o, e) => Communicate(arduinoPort, brunnerSocket);
timer.Start();
while (arduinoPort.IsOpen && !stopExecuting)
{
var currentPosition = new int[2];
_position.CopyTo(currentPosition, 0);
positionHistory.Enqueue(currentPosition);
var prueba = positionHistory.ToArray();
if (positionHistory.Count > 60)
{
positionHistory.Dequeue();
}
System.Threading.Thread.Sleep(100);
}
timer.Stop();
System.Threading.Thread.Sleep(500); // Give it time to the timer events to finish
}
catch (Exception ex)
{
logger.Error(ex, ex.StackTrace);
logger.Error(ex, ex.Message);
}
finally
{
_isArduinoConnected = false;
_isBrunnerConnected = false;
if (timer.IsRunning)
{
timer.Stop();
}
}
}
}
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;
}
}
private Cls2SimSocket connectBrunner(bool required)
{
try
{
Cls2SimSocket sock = new Cls2SimSocket(cls2SimHost, cls2SimPort);
sock.Connect();
return(sock);
}
catch (Exception ex)
{
if (required)
{
logger.Error(ex, ex.Message);
throw;
}
else
{
logger.Warn($"Couldn't connect to Brunner CLS2Sim: {ex.Message}");
return(null);
}
}
}
