public ControllerLoop(InertialMeasurement inertialMeasurement, Pilot pilot, VectorTranslationalCommandConsumer vectorTranslationalCommandConsumer, VectorTranslationalCommandProvider vectorTranslationalCommandProvider, LoopSettings loopSettings)
{
long previousTime = DateTime.Now.Ticks;
long lastRadioTime = 0;
long lastSensorTime = 0;
long lastControlTime = 0;
long lastMotorTime = 0;
inertialMeasurement.Update();
bool systemArmed = false;
Thread.Sleep(1000);
inertialMeasurement.Zero();
VectorTranslationalCommand vectorTranslationalCommand = null;
while (true)
{
long currentTime = DateTime.Now.Ticks;
if (currentTime >= (lastRadioTime + loopSettings.RadioLoopPeriod))
{
lastRadioTime = currentTime;
pilot.Update();
systemArmed = true; // pilot.GetCommand().Throttle > motorSettings.MinimumOutput;
//if (!systemArmed)
// logger.Flush();
}
currentTime = DateTime.Now.Ticks;
if (systemArmed && (currentTime >= (lastSensorTime + loopSettings.SensorLoopPeriod)))
{
lastSensorTime = currentTime;
inertialMeasurement.Update();
}
currentTime = DateTime.Now.Ticks;
if (systemArmed && (currentTime >= (lastControlTime + loopSettings.ControlAlgorithmPeriod)))
{
lastControlTime = currentTime;
vectorTranslationalCommand = vectorTranslationalCommandProvider.GetCommand(pilot, inertialMeasurement, (float)(currentTime - previousTime) / loopSettings.LoopUnit);
previousTime = currentTime;
}
currentTime = DateTime.Now.Ticks;
if (currentTime >= (lastMotorTime + loopSettings.MotorLoopPeriod))
{
if (systemArmed)
vectorTranslationalCommandConsumer.ProcessCommand(vectorTranslationalCommand); //.Update(radio.Throttle, pid.Axes);
else
//mixer.SetSafe();
lastMotorTime = currentTime;
}
}
}
public abstract VectorTranslationalCommand GetCommand(Pilot pilot, InertialMeasurement inertialMeasurement,
double time);
