public static void Main() { SDCard sdCard = new SDCard(); //todo: need sdCard mode detection ILogger logger = new PersistenceWriter(new FileStream(@"\SD\telemetry.bin", FileMode.CreateNew), new TelemetryFormatter()); //ILogger logger = new DebugLogger(); // use for debugging //ILogger logger = new NullLogger(); // use for release //Assumes one type of speed controller on quad //ISpeedControllerSettings speedControllerSettings = new HobbyKingSs2530Settings(); //IPWM pwmFront = new SecretLabsPWM(Pins.GPIO_PIN_D9); //SpeedController electricSpeedControllerFront = new SpeedController(speedControllerSettings, pwmFront); //IPWM pwmRear = new SecretLabsPWM(Pins.GPIO_PIN_D8); //SpeedController electricSpeedControllerRear = new SpeedController(speedControllerSettings, pwmRear); //IPWM pwmRight = new SecretLabsPWM(Pins.GPIO_PIN_D8); //SpeedController electricSpeedControllerRight = new SpeedController(speedControllerSettings, pwmRight); //IPWM pwmLeft = new SecretLabsPWM(Pins.GPIO_PIN_D8); //SpeedController electricSpeedControllerLeft = new SpeedController(speedControllerSettings, pwmLeft); MotorMixer mixer = null; PIDSettings[] pidSettings = GetPIDSettings(); AxesController axesController = new AxesController(pidSettings[0], pidSettings[1], pidSettings[2], true); Gyro gyro = new DefaultGyro(new AircraftPrincipalAxes()); Radio radio = new DefaultRadio(null, null); ControllerLoopSettings loopSettings = GetLoopSettings(); Controller controller = new Controller(mixer, axesController, gyro, radio, loopSettings, GetMotorSettings(), logger); }
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); } } }