Архитектура прошивки
Прошивка Flix это обычный скетч Arduino, реализованный в однопоточном стиле. Код инициализации находится в функции setup()
, а главный цикл — в функции loop()
. Скетч состоит из нескольких файлов, каждый из которых отвечает за определенную подсистему.
Главный цикл 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
, то соответствующая функция пропускается.
Состояние armed хранится в переменной armed
, а текущий режим — в переменной mode
.