Software:Quadrocopter

На Квадрокоптере стоит Arduino, на которой с фиксированной частотой выполняется алгоритм стабилизации и обмена данными.

Алгоритм стабилизации корректирует мощности моторов для того, чтобы поддерживать горизонтальное положение. Характеристикой отклонения от этого положения являются два угла. Для получения этих углов используется датчик MPU-6050, который расчитывает их из показаний гироскопа и акселерометра.

Поскольку выполнять новую итерацию алгоритма стоит только при наличии нового значения угла с датчика, а он предоставляет эти данные с фиксированной частотой, именно датчик и задает частоту выполнения алгоритма.

Каждый раз, когда появляются новые данные, датчик изменяет напряжение на одном из выводов, на что реагирует Arduino. Угол считывается и обрабатывается, а также выполняется обмен данными с компьютером. Все это должно произойти до появления следующих данных у датчика, чтобы частота выполнения оставалась постоянной.

В этой статье описана начальная стадия работы программы, в которой создаются объекты, рассказано о том, как Arduino реагирует на прерывание от датчика. Также есть описание одной итерации алгоритма.

Файл проекта quadrocopter_oop.ino

При инициализации в функции setup() создается объект класса Quadrocopter, конструктор которого создает объект класса MPU6050DMP.

Конструктор класса датчика назначает функцию dmpDataReady() прерыванию от датчика.

Теперь, каждый раз, когда появляются новые данные, вызывается эта функция. Ее назначение — выставить переменную

 mpuInterrupt = true;

Поэтому ненулевое значение этой переменной говорит о том, что нужно выполнить итерацию алгоритма.

В бесконечном цикле loop() выполняется вызов Quadrocopter::iteration().

Если новых данных с датчика нет (то есть, mpuInterrupt == false), на данной итерации loop() не происходит ничего. В противном случае совершается итерация алгоритма. После выполнения значение mpuInterrupt сбрасывается.

Далее описание одной итерации алгоритма, которая реализована в методе Quadrocopter::iteration().

Файл Quadrocopter.cpp

В связи с особенностью датчика, данные можно считывать не совсем сразу после того, как было получено прерывание. Поэтому порядок выполнения действий следующий:

1. Обмен данными с компьютером
2. Считывание угла с датчика
3. Обработка угла (одна итерация PID)
4. Отправка новых значений мощности на моторы

Далее разобран код из файла Quadrocopter.cpp для наглядности.

void Quadrocopter::iteration()
{
    //Проверяется значение переменной mpuInterrupt
    if(MPU->getNewData())
    {
        { // Выполняется обмен данными с компьютером
            processSerialRx();
            processSerialTx();
        }

        { // Чтение угла с сенсора
            MPU->iteration();
            processSensorsData();
        }

        { // Выполняется итерация PID, значения мощностей передаются на моторы
            processCorrection();
            processMotors();
        }

        // Сбрасывается значение mpuInterrupt
        MPU->resetNewData();
    }
}