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;
}
}
}