private void ControlLoop() { //_logger.Write("TimeStamp,Throttle,pitchSP,pitchPV,pitchOP"); while (_running) { long currentTime = DateTime.Now.Ticks; long deltaTime = currentTime - _previousTime; float deltaTimeFactor = (float)deltaTime / _loopSettings.LoopUnit; _previousTime = currentTime; if (deltaTime > _maxLoopTime) { _maxLoopTime = deltaTime; } if (currentTime > (_lastRadioTime + _loopSettings.RadioLoopPeriod)) { _radio.Update(); _lastRadioTime = currentTime; } if (currentTime > (_lastSensorTime + _loopSettings.SensorLoopPeriod)) { _gyro.Update(); _lastSensorTime = currentTime; } if (currentTime > (_lastControlTime + _loopSettings.ControlAlgorithmPeriod)) { _pid.Update(_radio.Axes, _gyro.Axes, deltaTimeFactor); _lastControlTime = currentTime; } if (currentTime > (_lastMotorTime + _loopSettings.MotorLoopPeriod)) { //_motors.Update(_radio.Throttle , _pid.Axes); _lastMotorTime = currentTime; } if (currentTime > (_lastTelemetryTime + _loopSettings.TelemetryLoopPeriod)) { _lastTelemetryTime = currentTime; //_logger.Write( // currentTime + "," + // _radio.Throttle + "," + // _radio.Axes.Pitch + "," + // _gyro.Axes.Pitch + "," + // _pid.Axes.Pitch); } } Debug.Print("exiting control loop"); }
public Controller(MotorMixer mixer, AxesController pid, Gyro gyro, Accelerometer accelerometer, Radio radio, ControllerLoopSettings loopSettings, MotorSettings motorSettings, ILogger logger) { TelemetryData telemetryData = new TelemetryData(); long previousTime = DateTime.Now.Ticks; long lastRadioTime = 0; long lastSensorTime = 0; long lastControlTime = 0; long lastMotorTime = 0; long lastTelemetryTime = 0; bool systemArmed = false; Thread.Sleep(1000); gyro.Zero(); accelerometer.Zero(); while (true) { long currentTime = DateTime.Now.Ticks; if (currentTime >= (lastRadioTime + loopSettings.RadioLoopPeriod)) { lastRadioTime = currentTime; radio.Update(); systemArmed = radio.Throttle > motorSettings.MinimumOutput; if (!systemArmed) logger.Flush(); } currentTime = DateTime.Now.Ticks; if (systemArmed && (currentTime >= (lastSensorTime + loopSettings.SensorLoopPeriod))) { lastSensorTime = currentTime; gyro.Update(); accelerometer.Update(); } currentTime = DateTime.Now.Ticks; if (systemArmed && (currentTime >= (lastControlTime + loopSettings.ControlAlgorithmPeriod))) { lastControlTime = currentTime; pid.Update(radio.Axes, gyro.Axes, (float) (currentTime - previousTime)/loopSettings.LoopUnit); previousTime = currentTime; } currentTime = DateTime.Now.Ticks; if (currentTime >= (lastMotorTime + loopSettings.MotorLoopPeriod)) { if (systemArmed) mixer.Update(radio.Throttle, pid.Axes); else mixer.SetSafe(); lastMotorTime = currentTime; } currentTime = DateTime.Now.Ticks; if (systemArmed && (currentTime >= (lastTelemetryTime + loopSettings.TelemetryLoopPeriod))) { telemetryData.Update(gyro.Axes, radio.Axes, pid.Axes, currentTime); lastTelemetryTime = currentTime; logger.Write(telemetryData); } if (!systemArmed) pid.ZeroIntegral(); } }
public Controller(MotorMixer mixer, AxesController pid, Gyro gyro, Radio radio, ControllerLoopSettings loopSettings, MotorSettings motorSettings, ILogger logger) { TelemetryData telemetryData = new TelemetryData(); long previousTime = DateTime.Now.Ticks; long lastRadioTime = 0; long lastSensorTime = 0; long lastControlTime = 0; long lastMotorTime = 0; long lastTelemetryTime = 0; bool systemArmed = false; while (true) { long currentTime = DateTime.Now.Ticks; if (currentTime >= (lastRadioTime + loopSettings.RadioLoopPeriod)) { //Debug.Print((loopSettings.LoopUnit/(float)(currentTime-lastRadioTime)).ToString()); lastRadioTime = currentTime; radio.Update(); systemArmed = radio.Throttle > motorSettings.MinimumOutput; if (!systemArmed) { logger.Flush(); } } currentTime = DateTime.Now.Ticks; if (systemArmed && (currentTime >= (lastSensorTime + loopSettings.SensorLoopPeriod))) { //Debug.Print((loopSettings.LoopUnit / (float)(currentTime - lastSensorTime)).ToString()); lastSensorTime = currentTime; gyro.Update(); } currentTime = DateTime.Now.Ticks; if (systemArmed && (currentTime >= (lastControlTime + loopSettings.ControlAlgorithmPeriod))) { //Debug.Print((loopSettings.LoopUnit / (float)(currentTime - lastControlTime)).ToString()); lastControlTime = currentTime; pid.Update(radio.Axes, gyro.Axes, (float)(currentTime - previousTime) / loopSettings.LoopUnit); previousTime = currentTime; } currentTime = DateTime.Now.Ticks; if (currentTime >= (lastMotorTime + loopSettings.MotorLoopPeriod)) { //Debug.Print((loopSettings.LoopUnit / (float)(currentTime - lastMotorTime)).ToString()); if (systemArmed) { mixer.Update(radio.Throttle, pid.Axes); } else { mixer.SetSafe(); } lastMotorTime = currentTime; } currentTime = DateTime.Now.Ticks; if (systemArmed && (currentTime >= (lastTelemetryTime + loopSettings.TelemetryLoopPeriod))) { //Debug.Print((loopSettings.LoopUnit / (float)(currentTime - lastTelemetryTime)).ToString()); telemetryData.Update(gyro.Axes, radio.Axes, pid.Axes, currentTime); lastTelemetryTime = currentTime; logger.Write(telemetryData); } } }
public ControllerWithGyroDebug(MotorMixer mixer, AxesController pid, Gyro gyro, Accelerometer accelerometer,Radio radio, ControllerLoopSettings loopSettings, MotorSettings motorSettings, ILogger logger) { long lastRadioTime = 0; long lastSensorTime = 0; long lastControlTime = 0; long lastMotorTime = 0; long lastTelemetryTime = 0; bool systemArmed = false; var serialPort = new SerialPort("COM1",115200); serialPort.Open(); while (true) { long currentTime = DateTime.Now.Ticks; if (currentTime >= (lastRadioTime + loopSettings.RadioLoopPeriod)) { radio.Update(); systemArmed = radio.Throttle > motorSettings.MinimumOutput; if (!systemArmed) logger.Flush(); lastRadioTime = currentTime; } currentTime = DateTime.Now.Ticks; if (currentTime >= (lastSensorTime + loopSettings.SensorLoopPeriod)) { gyro.Update(); lastSensorTime = currentTime; } currentTime = DateTime.Now.Ticks; if (currentTime >= (lastControlTime + loopSettings.ControlAlgorithmPeriod)) { if (systemArmed) pid.Update(radio.Axes, gyro.Axes, (float)(currentTime - lastControlTime) / loopSettings.LoopUnit); lastControlTime = currentTime; } currentTime = DateTime.Now.Ticks; if (currentTime >= (lastMotorTime + loopSettings.MotorLoopPeriod)) { if (systemArmed) mixer.Update(radio.Throttle, pid.Axes); else mixer.SetSafe(); lastMotorTime = currentTime; } currentTime = DateTime.Now.Ticks; if (currentTime >= (lastTelemetryTime + loopSettings.TelemetryLoopPeriod)) { Debug.Print( //byte[] buffer = System.Text.Encoding.UTF8.GetBytes( //"\r" + currentTime + //"\tR:" + radio.Axes.Pitch +); "\rP:" + gyro.Axes.Pitch + "\rR:" + gyro.Axes.Roll + "\rY:" + gyro.Axes.Yaw //"\tP" + pid.Axes.Pitch ); //serialPort.Write(buffer, 0, buffer.Length); lastTelemetryTime = currentTime; } } }
public ControllerWithAccelerometerDebug(MotorMixer mixer, AxesController pid, Gyro gyro, Accelerometer accelerometer, Radio radio, ControllerLoopSettings loopSettings, MotorSettings motorSettings, ILogger logger) { long lastRadioTime = 0; long lastSensorTime = 0; long lastControlTime = 0; long lastMotorTime = 0; long lastTelemetryTime = 0; bool systemArmed = false; Thread.Sleep(1000); gyro.Zero(); accelerometer.Zero(); var serialPort = new SerialPort("COM1",115200); serialPort.Open(); while (true) { long currentTime = DateTime.Now.Ticks; if (currentTime >= (lastRadioTime + loopSettings.RadioLoopPeriod)) { radio.Update(); systemArmed = radio.Throttle > motorSettings.MinimumOutput; if (!systemArmed) logger.Flush(); lastRadioTime = currentTime; } currentTime = DateTime.Now.Ticks; if (currentTime >= (lastSensorTime + loopSettings.SensorLoopPeriod)) { gyro.Update(); accelerometer.Update(); lastSensorTime = currentTime; } currentTime = DateTime.Now.Ticks; if (currentTime >= (lastControlTime + loopSettings.ControlAlgorithmPeriod)) { if (systemArmed) pid.Update(radio.Axes, gyro.Axes, (float)(currentTime - lastControlTime) / loopSettings.LoopUnit); lastControlTime = currentTime; } currentTime = DateTime.Now.Ticks; if (currentTime >= (lastMotorTime + loopSettings.MotorLoopPeriod)) { if (systemArmed) mixer.Update(radio.Throttle, pid.Axes); else mixer.SetSafe(); lastMotorTime = currentTime; } currentTime = DateTime.Now.Ticks; if (currentTime >= (lastTelemetryTime + loopSettings.TelemetryLoopPeriod)) { byte[] buffer= new byte[12]; BitConverter.ToBytes(ref buffer, 0, accelerometer.Axes.Pitch); BitConverter.ToBytes(ref buffer, 4, accelerometer.Axes.Roll); BitConverter.ToBytes(ref buffer, 8, accelerometer.Axes.Yaw); serialPort.Write(buffer, 0, buffer.Length); lastTelemetryTime = currentTime; } } }
public ControllerWithLoopTimeDebug(MotorMixer mixer, AxesController pid, Gyro gyro, Radio radio, ControllerLoopSettings loopSettings, MotorSettings motorSettings, ILogger logger) { long lastRadioTime = 0; long lastSensorTime = 0; long lastControlTime = 0; long lastMotorTime = 0; long lastTelemetryTime = 0; float currentRadioFrequency = 0; float currentSensorFrequency = 0; float currentControlFrequency = 0; float currentMotorFrequency = 0; bool systemArmed = false; SerialPort serialPort = new SerialPort("COM1",115200); serialPort.Open(); while (true) { long currentTime = DateTime.Now.Ticks; if (currentTime >= (lastRadioTime + loopSettings.RadioLoopPeriod)) { currentRadioFrequency = loopSettings.LoopUnit / (float)(currentTime - lastRadioTime); radio.Update(); systemArmed = radio.Throttle > motorSettings.MinimumOutput; if (!systemArmed) logger.Flush(); lastRadioTime = currentTime; } currentTime = DateTime.Now.Ticks; if (currentTime >= (lastSensorTime + loopSettings.SensorLoopPeriod)) { currentSensorFrequency = loopSettings.LoopUnit / (float)(currentTime - lastSensorTime); gyro.Update(); lastSensorTime = currentTime; } currentTime = DateTime.Now.Ticks; if (currentTime >= (lastControlTime + loopSettings.ControlAlgorithmPeriod)) { currentControlFrequency = loopSettings.LoopUnit / (float)(currentTime - lastControlTime); if (systemArmed) pid.Update(radio.Axes, gyro.Axes, (float)(currentTime - lastControlTime) / loopSettings.LoopUnit); lastControlTime = currentTime; } currentTime = DateTime.Now.Ticks; if (currentTime >= (lastMotorTime + loopSettings.MotorLoopPeriod)) { currentMotorFrequency = loopSettings.LoopUnit / (float)(currentTime - lastMotorTime); if (systemArmed) mixer.Update(radio.Throttle, pid.Axes); else mixer.SetSafe(); lastMotorTime = currentTime; } currentTime = DateTime.Now.Ticks; if (currentTime >= (lastTelemetryTime + loopSettings.TelemetryLoopPeriod)) { float currentTelemetryFrequency = loopSettings.LoopUnit / (float)(currentTime - lastTelemetryTime); byte[] buffer = System.Text.Encoding.UTF8.GetBytes( "\r" + currentTime + "\tR: " + currentRadioFrequency + "\tS: " + currentSensorFrequency + "\tC: " + currentControlFrequency + "\tM: " + currentMotorFrequency + "\tT: " + currentTelemetryFrequency + "\t"); serialPort.Write(buffer, 0, buffer.Length); lastTelemetryTime = currentTime; } } }