public ControllerWithAsyncronousTelemetry(
            MotorMixer mixer,
            AxesController pid,
            Gyro gyro,
            Accelerometer accelerometer,
            Radio radio,
            ControllerLoopSettings loopSettings,
            MotorSettings motorSettings,
            ILogger logger)
        {
            _mixer = mixer;
            _pid = pid;
            _gyro = gyro;
            _accelerometer = accelerometer;
            _radio = radio;
            _loopSettings = loopSettings;
            _motorSettings = motorSettings;
            _logger = logger;
            _serialPort.DataReceived += new SerialDataReceivedEventHandler(serialPort_DataReceived);
            _serialPort.Open();

            Thread.Sleep(1000);
            gyro.Zero();
            accelerometer.Zero();
            Start();
        }
Esempio n. 2
0
        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();
            }
        }
Esempio n. 3
0
        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;
                }
            }
        }