Software

Эта статья отображает текущее состояние кода. Очевидно, что некоторые вещи стоит переделать.

На данный момент для полета необходимы 3 устройства: arduino на квадрокоптере, компьютер с Bluetooth и arduino для джойстика. Джойстик подключается к компьютеру через USB, программа пересылает данные с джойстика на квадрокоптер через Bluetooth и запрашивает состояние.

Table of Contents Код для квадрокоптера Глобальные переменные Функция setup() Функция loop() Класс Accelerometer Класс Gyroscope Класс Motor Класс MotorController Код для компьютера Код для джойстика

Код для квадрокоптера

Используется подход arduino (функции setup, loop), в дальнейшем планируется использование таймеров и прерываний от UART для лучшего распределения времени.

Работа с датчиками реализована через классы Accelerometer и Gyroscope.

Класс TimerCount позволяет засечь время между итерациями loop()

Структура RVector3D реализует трехмерный вектор:

struct RVector3D {
    double x, y, z;
};

Макросы DEBUG_* используются для включения/выключения режимов отладки:

• DEBUG_NO_MOTORS отключает задержку в несколько секунд перед включением. Удобно для отладки кода (без моторов)
• DEBUG_NO_GYROSCOPE отключает запрос данных с гироскопа (возвращаемое значение — (0, 0, 0))
• DEBUG_NO_GYROSCOPE отключает запрос данных с акселлерометра (возвращаемое значение — (0, 0, 0))
• DEBUG_SERIAL включает последовательный порт
• DEBUG_SERIAL_HUMAN включает команды, которые удобно выполнять, используя Serial Monitor (wasd как джойстик, цифры для мощности)

Коммуникация с компьютером осуществляется через последовательный порт (Serial) на скорости 115200.

Глобальные переменные

• throttle — вектор длины 1, который содержит данные о запрашиваемом (вручную) наклоне. Зависит от позиции джойстика.
• angle содержит два угла поворота системы координат, связанной с квадрокоптером относительно g.
• angle_period — значение RC для RC-фильтра, который используется для получения угла (angle).
• MController, Accel, Gyro, TCount — указатели на объекты соответствующих классов.
• serial_type — текущий режим работы с последовательным портом. В режиме SERIAL_DEFAULT последовательный порт готов принимать команды, в режим SERIAL_RESTORE производится переход после дополнительного приема данных (например, при получении нескольких байт значений одной величины).

Функция setup()

Создаются объекты классов датчиков, инициализируется последовательный порт,

Функция loop()

• Производится запрос данных с датчиков:

RVector3D accel_data = Accel->get_readings();
RVector3D gyro_data = Gyro->get_readings();

• Высчитывается угол с помощью RC-фильтра:

angle_alpha = dt / (dt + angle_period / (2 * MPI));        
angle.x = (angle.x + gyro_data.x * dt) * (1 - angle_alpha) + accel_data.y * angle_alpha;
angle.y = (angle.y + gyro_data.y * dt) * (1 - angle_alpha) - accel_data.x * angle_alpha;

• Вычисляется коррекция throttle, вызванная случайными отклонениями:

RVector3D accel_correction = MController->get_accelerometer_correction(angle, accel_data);
RVector3D gyro_correction = MController->get_gyroscope_correction(gyro_data);

• При наличии входящих символов и включенном DEBUG_SERIAL, они обрабатываются (в этом блоке кода происходит только прием данных: корректировки с джойстика и мощности, реализация остальных случаев далее)
• Находится throttle_corrected — итоговый вектор управления:

RVector3D throttle_corrected = throttle;
throttle_corrected += gyro_correction;
throttle_corrected += accel_correction;
throttle_corrected /= throttle_corrected.module();
throttle_corrected *= MController->get_throttle_abs();

• В случае, если пришел запрос на передачу состояния, выполняется ответ на него.
• Меняется скорость моторов

Класс Accelerometer

Класс Gyroscope

Класс Motor

Классом, содержащим алгоритм инициализации моторов и методы управления ими, является MotorController. Он имеет конструктор вида:

MotorController(const int motor_control_pins[N_MOTORS]);

где N_MOTORS (= 4) — число двигателей в модели
motor_control_pins — массив номеров выводов Arduino, к которым подключены контроллеры двигателей. Выводы должны поддерживать ШИМ (PWM, '~').

Класс MotorController

• Управление скоростью осуществляется посредством метода

void speedChange(RVector3D throttle_vec);

Код для компьютера

Код для джойстика