Архитектура прошивки

Прошивка Flix это обычный скетч Arduino, реализованный в однопоточном стиле. Код инициализации находится в функции setup(), а главный цикл — в функции loop(). Скетч состоит из нескольких файлов, каждый из которых отвечает за определенную подсистему.

Firmware dataflow diagram

Главный цикл loop() работает на частоте 1000 Гц. Передача данных между подсистемами происходит через глобальные переменные:

  • t (float) — текущее время шага, с.
  • dt (float) — дельта времени между текущим и предыдущим шагами, с.
  • gyro (Vector) — данные с гироскопа, рад/с.
  • acc (Vector) — данные с акселерометра, м/с2.
  • rates (Vector) — отфильтрованные угловые скорости, рад/с.
  • attitude (Quaternion) — оценка ориентации (положения) дрона.
  • controlRoll, controlPitch, ... (float[]) — команды управления от пилота, в диапазоне [-1, 1].
  • motors (float[]) — выходные сигналы на моторы, в диапазоне [0, 1].

Исходные файлы

Исходные файлы прошивки находятся в директории flix. Основные файлы:

  • flix.ino — основной файл Arduino-скетча. Определяет некоторые глобальные переменные и главный цикл.
  • imu.ino — чтение данных с датчика IMU (гироскоп и акселерометр), калибровка IMU.
  • rc.ino — чтение данных с RC-приемника, калибровка RC.
  • estimate.ino — оценка ориентации дрона, комплементарный фильтр.
  • control.ino — подсистема управления, трехмерный двухуровневый каскадный ПИД-регулятор.
  • motors.ino — выход PWM на моторы.
  • mavlink.ino — взаимодействие с QGroundControl или pyflix через протокол MAVLink.

Вспомогательные файлы:

  • vector.h, quaternion.h — библиотеки векторов и кватернионов.
  • pid.h — ПИД-регулятор.
  • lpf.h — фильтр нижних частот.

Подсистема управления

Состояние органов управления обрабатывается в функции interpretControls() и преобразуется в команду управления, которая включает следующее:

  • attitudeTarget (Quaternion) — целевая ориентация дрона.
  • ratesTarget (Vector) — целевые угловые скорости, рад/с.
  • ratesExtra (Vector) — дополнительные (feed-forward) угловые скорости, для управления рысканием в режиме STAB, рад/с.
  • torqueTarget (Vector) — целевой крутящий момент, диапазон [-1, 1].
  • thrustTarget (float) — целевая общая тяга, диапазон [0, 1].

Команда управления обрабатывается в функциях controlAttitude(), controlRates(), controlTorque(). Если значение одной из переменных установлено в NAN, то соответствующая функция пропускается.

Control subsystem diagram

Состояние armed хранится в переменной armed, а текущий режим — в переменной mode.