GitHub – lobodol/drone-flight-controller: A quadcopter flight controller based on Arduino Uno

GitHub - lobodol/drone-flight-controller: A quadcopter flight controller based on Arduino Uno Квадрокоптеры
Автор На чтение 39 мин Просмотров 28 Опубликовано
Содержание
  1. Программирование и настройка контроллера аэродинамической скорости на основе arduino
  2. Математика стабилизации, пид-регуляторы (pid)
  3. Устройство бортового компьютера

Программирование и настройка контроллера аэродинамической скорости на основе arduino

Эта статья является окончанием серией публикаций начатой со статьи Стабилизация крыла HK Mini-Sonic. Здесь объясняется, как запрограммировать и настроить контроллер аэродинамической скорости, построенный на основе стабилизатора Guardian Eagle Tree,  пропеллерного датчика и платы Arduino Pro Micro.

GitHub - lobodol/drone-flight-controller: A quadcopter flight controller based on Arduino Uno

Как залить прошивку в контроллер.

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

– Загрузка программы производится через MicroUSB шнур с Windows компьютера. У меня установлена операционная система Windows 7, но и более новые версии операционной системы так же подходят.

– Сначала нужно установить драйвер платы Arduino для Windows. В большинстве случаев, при подсоединении платы через шнур MicroUSB к компьютеру, драйвер устанавливается автоматически. Но если этого не происходит, то необходимо установить его вручную – в интернете есть много примеров, как это делается.

– Необходимо скачать и установить на Windows компьютер последнюю версию стандартной и создать в ней пустой скетч.

– Открыть страницу проекта в депозитарии github и скопировать в окно скетча. Сохранить скетч на диск.

– Проверить подключение платы Arduino – В среде разработки Arduino, в настройках меню “Инструменты” выбрать плату “Arduino Leonardo”. Если драйвер платы установлен правильно, после этого должна появиться новая строка типа “Порт: COM4 (Arduinon Leonardo)”. Если такая строка не появилась, ситуацию можно попробовать исправить отсоединением-соединением платы и рестартом программы среды разработки.

– Выполнить команду “Файл->Вгрузить”. Во время загрузки плате Arduino должн мигать RX светодиод, подтверждающий передатчу данных – через несколько секунд контроллер будет запрограммирован. При первом  запуске программы программы, контроллер записывает предустановленные параметры стабилизации в память EEPROM – после этого он должен издать звуковой сигнал длинной в 3 сек., последующие включения и программирования контроллера не должны производить такого эффекта.

Все – прошивка залита в контроллер, можно отсоединить плату от компьютера.

Как работает программа контроллера?

Для работы с контроллером совсем не надо разбираться в программировании и понимать математические алгоритмы стабилизации, использованные в программе. Но лучше иметь хотя бы общее представление:
– На плату Arduino приходит 3 импульсных ШИМ сигнала с приемника (THR_IN, GAIN_IN, MOD_IN) и один импульсный сигнал c фотодиода оптопары – PROP_IN.
– Плата Arduino запрограммированна таким образом, что эти сигналы вызывают прерывания процессора. Это значит, что фронт или спад каждого сигнал независимо запускает специальную короткую подпрограмму – обработчик прерывания или вектор. Эти подпрограммы и занимаются измерением длительности сигналов управления и считают импульсы с пропеллерного датчика. Эти же эти подпрограммы, используя встроенный в Ардуино 32-битный таймер, формируют выходные импульсы THR_OUT, GAIN_OUT, а так же управляют включением-выключением зуммера и светодиода.

– Для вычисления выходных значений тяги и коэффициента усиления Guardian используются простейшие формулы из теории управления систем с обратной связью. Самые важные параметры этих формул записаны в энергонезависимую память контроллера Ардуино – EEPRPOM. Они могут быть изменены без перезаливки прошивки, непосредственно с пульта управления моделью с помощью “режима программирования параметров”.

Настройка контроллера:

Контроллер управляется следующими каналами передатчика.
– Сигнал Thr – тяга двигателя.
– Сигнал Gain формируется одним из потенциометров, расположенных на панели передатчика.
– Сигнал Mod  – поступает с 3-х-позицинного переключателя, формирующего три длительности импульса управления 1.1 мс, 1.5 мс, 1.9 мс которые соответствуют трем режимам стабилизации Guardian: 2D – 3D – X. Верхнее положение переключателя X – “стабилизация выключена” должно соответствовать максимальной длительности импульса.

Режимы работы контроллера

У контроллера есть 4 основных режима работы:
– Режим пилотажа.
– Сквозной режим.
– Режим калибровки.
– Режим программирования параметров.

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

Вход режим пилотажа происходит, когда при включении питания контроллера, тяга модели выключена (рычажок тяги находится в нижнем положении).

Вход в любой режим настройки осуществляется включением питания контроллера в момент, когда тяга не выключенна (рычажок тяги находится в центральном положении). Выбор режима настройки определяется положением 3-х позиционного переключателя:
– Верхнее положение (X) – сквозной режим, подтверждение – один бип.
– Среднее положение (3D) – режим калибровки, подтверждение – два бипа.
– Нижнее положение (2D) – режим программирования параметров, подтверждение – три бипа.

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

Режим пилотажа

При входе в режим пилотажа контроллер издает длинный прерывистый сигнал и включает-выключает светодиод. В пилотажном режиме положение 3-х позиционного переключателя определяет, какая мода управления – Х, 3D, 2D будет выбрана. Переключение между модами управления можно осуществлять в любой момент времени. Потенциометр Gain отключен от управления (за исключением “Экспертного пилотажного режима”, см. ниже).  Из режима пилотажа нельзя перейти в любой другой режим без выключения питания контроллера.

Сквозной режим

В этом режиме сигналы Thr, Gain, Mod просто копируются контроллером скорости и передаются неизменными на контроллер тяги модели и стабилизатор Guardian. Во многом, это похоже на поведение контроллера в пилотажной моде “без стабилизации”, но в этом режиме сигнал с 3-х позиционного переключателя передается непосредственно на стабилизатор Guardian и не вызывает переключения пилотажных мод управления. Данный режим необходим, чтобы настроить параметры контроллера тяги модели и стабилизатора Guardian с помощью пульта управления (действия при настройке описаны в документации этих устройств).

Режим калибровки

Для эффективной работы стабилизатора скорости ему нужно точно знать диапазон входных сигналов управления Thr и Gain (Сигнал Mod передает дискретную информацию и не нуждается в калибровке).

Режим калибровки обозначается часто мигающим светодиодом контроллера. Во время калибровки нужно поднять до максимума и опустить до минимума рычажок тяги, а так же провернуть потенциометр сигнала Gain от минимального до максимального положения. После этих действий рычаг тяги надо перевести в минимальное положение и подождать 5 сек. Если калибровка произведена правильно, то данные о временных характеристиках импульсов этих сигналов будут записаны контроллером в память EEPROM. Контроллер при этом издаст длинный звуковой сигнал. Если калибровка произведена неправильно, то данные о диапазонах сигналов управления не будут обновлены и контроллер, перейдя в пилотажный режим, продолжит использовать старые значения.

Режим программирования параметров

Для правильного функционирования контроллера необходимо установить параметры стабилизации. В текущей версии прошивки их шесть, они хранятся в энергонезависимой памяти Ардуино EEPROM и могут быть перепрограммированы непосредственно с пульта управления моделью. Программирование параметров здесь во многом похоже на настройку параметров контроллера бесколлекторных двигателей (если не совсем понятно, как эта настройка делается – поищите соответствующее видео на youtube).

Список параметров:

1 – Коэффициент подавления стабилизации гироскопа аэродинамической скоростью (GAIN_SUP_PARAM). Влияет на стабилизацию полета в зоне высокой аэродинамической скорости – чем параметр больше, тем сильнее скорость подавляет усиление в цепи обратной связи гироскопа-стабилизатора. Предустановленное значение – 0.32.
2 – Максимальный коэффициент стабилизации гироскопа (GAIN_MAX_PARAM). Влияет на полет на низких скоростях. Предустановленное значение – 1.0.
3 – Напряжение включения звуковой сигнализации низкого напряжения аккумулятора (LOW_BATT_PARAM). Диапазон значений 0.0 – 1.0 соответствует напряжениям 5.0V – 7.5V. предустановленное значение – 0.85 (6.8 V).
4 – Коэффициент чувствительности датчика аэродинамической скорости, определяет, как частота вращения пропеллерного датчика преобразуется в коэффициент управления стабилизацией (PROP_PARAM). Этот коэффициент подбирается для каждого датчика индивидуально. Для быстро вращающихся пропеллеров нужно устанавливать низкий коэффициент, для медленно вращающихся – высокий. Предустановленное значение 0.73 соответствует стандартной геометрии датчика, описанного в статье Изготовление пропеллерного датчика аэродинамической скорости.
5 – Коэффициент подавления тяги двигателя модели аэродинамической скоростью (THR_SUP_PARAM), параметр активен только в 2D режиме . Тяга двигателя модели будет автоматически будет уменьшена, если аэродинамическая скорость очень высокая или увеличена, если скорость слишком низкая и величина этих “корректирующих” изменений тяги будет пропорционально этому коэффициенту. Предустановленное значение – 0.39.
6 – Величина минимальной стабилизированной тяги, активен только в 2D режиме (THR_MIN_PARAM). Это вспомогательный параметр. Данный параметр предотвращает остановку бесколлекторного двигателя модели, не давая контроллеру уменьшить число оборотов двигателя до нуля и тем самым дав ему возможность перестать вращаться. Диапазон значений 0.0 – 1.0 соответствует диапазону тяги двигателя 0 – 20%. Предустановленное значение – 0.34 (6.4% тяги).

Выбор и программирование параметра

– Как выбрать параметр для установки значения? – Номер параметра соответствует некоторому положению рычажка тяти – минимальное значение тяги это 0 (выход) максимальное – 6. Чтобы установить значение параметра нужно перемещать рукоятку тяги по всему диапазону и найти необходимое положение – контроллер сообщит о номере параметра короткими бипами.
– Как установить и сохранить значение параметра? – Установка значения производится потенциометром канала Gain. Все параметры могут быть установлены в диапазоне 0.0 – 1.0. Если вращать потенциометр – светодиод на контроллере загорается, когда значение, установленное потенциометром параметра, становится выше “старого” значения, записанного в EEPROM. Так же, в момент совпадения нового и старого значения, зуммер производит короткий щелчок. Таким образом можно понять, какое значение параметра уже установлено и уменьшить или увеличить его. Запись нового значения в EEPROM производится кратковременным переключением 3-позицинного переключателя вверх-вниз-вверх (X)-(2D)-(X) . В подтверждение записи зуммер произведет короткий вибрирующий звуковой сигнал.

“Экспертный” пилотажный режим.

Для более точного подбора значения любого параметра у контроллера есть возможность “присвоить” ему потенциометр сигнала Gain и управлять им непосредственно в полете и после серии экспериментов записать EEPROM наиболее подходящее значение.
– Как включить экспертный режим и “присвоить” потенциометр параметру? Присвоение можно сделать непосредственно в режиме программирования параметров, непосредственно после записи этого параметра в EEPROM – если сразу после записи параметра в EEPROM переключить 3-х позиционный переключатель вверх-вниз-вниз еще раз (это необходимо сделать в течении 1 сек. после первого переключения), то параметр дополнительно будет ассоциирован с потенциометром. Для подтверждения, контроллер издаст длинный звуковой бип, подтверждающий это присвоение.

GitHub - lobodol/drone-flight-controller: A quadcopter flight controller based on Arduino Uno

– Как записать значение “присвоенного” параметра в EEPROM? – Это не делается автоматически – нужно перейти в режим программирования параметров, выбрать положение рычажка тяги, соответствующее номеру параметра и, не трогая потенциометра, произвести запись значения параметра 3-х позиционным переключателем.

– Как завершить экспертный режим и “отключить” потенциометр от параметра? – для этого достаточно просто снова войти в меню программирования.

Экспертный пилотажный режим сохраняется даже после выключении питания контроллера, – при его активации, при каждом входе в пилотажный режим, после длинного вибрирующего сигнала, контроллер короткими бипами будет сообщать номер ассоциированного с потенциометром параметра.

Настройка и проверка контроллера.

Итак, программа залита в контроллер. Контроллер установлен в стенд, описанный в предыдущей статье Контроллер пропеллерного датчика аэродинамической скорости на основе Arduino , передатчик включен, тяга выставлена в ноль, 3-х позиционник поднят в режим пилотирования без стабилизации (Х).

GitHub - lobodol/drone-flight-controller: A quadcopter flight controller based on Arduino Uno

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

Проверка подсоединения стабилизатора Guardian – Переводим переключатель в 2D или 3D моду, когда гироскоп включен, – при покачивании стабилизатора сервопривод на стенде должен шевелить ‘рукой’.

Калибровка – Выключить питание контроллера, поставить переключатель в среднюю позицию (3D) и включить питание – контроллер войдет в режим калибровки. Откалибровать тягу, потом откалибровать потенциометр. Переместить рычаг тяги в нижнее положение и подождать 5 сек – контроллер должен издать длинный сигнал и перейти в режим пилотажа – калибровка закончена.

Проверка работы авто-подстройки усиления гироскопа – Перевести контроллер в режим 2D пилотажа, слегка наклонить Guardian, чтобы сервопривод изменил положение ‘руки’ и несильно подуть в пропеллерный датчик. При вращении пропеллера, коэффициент обратной связи, поступающия на  Guardian, должен уменьшиться и сервопривод так же должен уменьшить угол отклонения ‘руки’ от нейтрального положения.

Проверка работы стабилизатора скорости – Перевести контроллер в 2D режим. Включить тягу, примерно 20% от максимальной мощности. Подуть в пропеллерный датчик – тяга должна заметно уменьшиться.

В целом, если все действия прошли успешно – контроллер уже готов к полетам и его можно устанавливать в модель. Конечно, параметры стабилизатора будут неидеальными, но более точно их значения можно подобрать только в реальных полевых испытаниях.

Способы устранения проблем

Правильно собранный контроллер практически не нуждается в настройке. Что же делать, если он работает не так, как написано выше? – Ниже несколько диагностических советов.

Нет звуковых сигналов, или не работает светодиод – проверить подключение зуммера или светодиода – здесь возможны обрыв или ошибка полярности.

Если при ри включенном передатчике, светодиод на контроллере мигает с 1 сек. периодом – значит значит сработала система детектирования потери сигналов управления – с приемника не приходит хотя бы один из сигналов THR_IN, GAIN_IN, MOD_IN. Необходимо проверить работу приемника и наличие всех управляющих сигналы на входах платы Ардуино.

Если все работает, но не вращается мотор – проверить подсоединение контроллера бесколлекторного двигателя к выходному сигналу платы Ардуино GAIN_OUT.

Если контроллер не реагирует на вращение пропеллера датчика аэродинамической скорости – скорее всего не работает или неправильно подключена оптопара – проверить напряжение на ножках HOA1874-012. С помощью осциллографа посмотреть, правильно ли подано питание на ножки светодиода и поступают ли импульсы с фотодиода при вращении пропеллера датчика.

Установка контроллера в модель

Я испробовал несколько конфигураций установки контроллера на крыло HK Mini Sonic, но наиболее удачное расположение потребовало изготовления специального компартмента для приемника, как показоно на фотографии. При установке, стабилизатор Guardian и приемник крепятся к корпусу модели толстой двусторонней клейкой лентой.

GitHub - lobodol/drone-flight-controller: A quadcopter flight controller based on Arduino Uno

После установки, отделение приемника заклеивается сверху прозрачным скотчем.

Заключение

Зачем я написал эти несколько статей?
– Я хотел поделиться опытом – нет большого смысла делать такую сложную работу только для себя. Теперь после публикации всех пяти статей, я смогу не объяснять сто раз знакомым авиамоделистам, как это устройство работает :).
– Я давно задался идеей как-то использовать платы Arduino в авиа-моделировании. Теперь, начинание может быть продолжено – прошивка контроллера выложена в общественное пользование, она более-менее универсальна и понятна – знакомые с программированием моделисты могут придумывать ее дальнейшие модификации.
– И наконец – пропеллерный датчик скорости испытан, его характеристики, способы изготовления и подключения понятны – у него так же появились шансы быть востребованным.

Математика стабилизации, пид-регуляторы (pid)

Если вы решили заняться мультикоптерами, то рано или поздно вам придется столкнуться с настройкой ПИД-регулятора, поскольку этот математический аппарат применяется почти во всех задачах стабилизации: стабилизация углов квадрокоптера в воздухе, полет и удержание позиции по GPS, удержание высоты по барометру, бесколлекторные механизмы стабилизации видеокамеры в полете (подвес камеры).

Вы приобретаете двухосевой подвес для камеры, ставите туда, например, GoPro, включаете и вместо стабилизации получаете конвульсии, вибрации и дергания, хотя все датчики откалиброваны и механические проблемы устранены. Причина — неверные параметры ПИД-регуляторов.

Вы собираете мультикоптер, калибруете датчики, регуляторы, радио, все проверяете, пытаетесь взлететь, а он такой унылый в воздухе, что его даже легким ветерком переворачивает. Или наоборот: он такой резкий, что внезапно срывается с места и крутит тройное сальто без разрешения. Причина все та же: параметры ПИД-регуляторов.

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

Кроме того, мы же собираемся писать собственную систему стабилизации квадрокоптера! Предлагаю вместе со мной самим заново «изобрести» и «на пальцах» понять формулу ПИД-регулятора. Для тех, кому больше нравится сухой математический язык, я рекомендую Википедию, английскую статью, т.к. в русской пока не так подробно изложен материал.

Будем рассматривать квадрокоптер в двумерном пространстве, где у него есть только один угол — угол крена, и два мотора: левый и правый.

В полетный контроллер непрерывно поступают команды с земли: «крен 30 градусов», «крен -10 градусов», «крен 0 градусов (держать горизонт)»; его задача — как можно быстрее и точнее их выполнять с помощью моторов с учетом: ветра, неравномерного распределения веса квадрокоптера, неравномерного износа моторов, инерции квадрокоптера и т.п.

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

На Adruino вполне можно одну итерацию цикла обработки и управления уместить в 10 миллисекунд. Это значит, что раз в 10 миллисекунд будут считываться показания углов квадрокоптера, и на их основе будут отправляться управляющие сигналы к моторам. Эти 10 миллисекунд называют периодом регулирования. Понятно, что чем он меньше, тем чаще и точнее происходит регулирование.

Уровень газа поступает из приемника в контроллер. Обозначим его Программируем квадрокоптер на Arduino (часть 1). Напомню, что это среднее арифметическое между скоростями вращения всех моторов, выраженное в процентах от максимальной скорости вращения. Если Программируем квадрокоптер на Arduino (часть 1) и Программируем квадрокоптер на Arduino (часть 1) — скорости вращения левого и правого моторов, то:где Программируем квадрокоптер на Arduino (часть 1) — реакция квадрокоптера (усилие), которое создает момент вращения за счет того, что левый мотор вращается на Программируем квадрокоптер на Arduino (часть 1) быстрее, чем газ, а правый — на столько же медленнее. Программируем квадрокоптер на Arduino (часть 1) может принимать и отрицательные значения, тогда правый мотор закрутится быстрее. Если мы научимся вычислять эту величину на каждой итерации цикла обработки, значит мы сможем управлять квадрокоптером. Понятно, что Программируем квадрокоптер на Arduino (часть 1) как минимум должно зависеть от текущего угла крена (Программируем квадрокоптер на Arduino (часть 1)) и желаемого угла крена (Программируем квадрокоптер на Arduino (часть 1)), который поступает с пульта управления.Представим ситуацию: поступает команда «держать горизонт» (Программируем квадрокоптер на Arduino (часть 1) = 0), а квадрокоптер имеет крен влево:Программируем квадрокоптер на Arduino (часть 1) — разность (ошибка) между Программируем квадрокоптер на Arduino (часть 1) и Программируем квадрокоптер на Arduino (часть 1), которую контроллер стремится минимизировать.

Чем больше разность между желаемым углом крена и текущим, тем сильнее должна быть реакция, тем быстрее левый мотор должен закрутиться относительно правого. Если это записать с использованием наших обозначений:

Здесь P — коэффициент пропорциональности. Чем он больше, тем сильнее будет реакция, тем резче квадрокоптер будет реагировать на отклонение от требуемого угла крена. Эта интуитивно понятная и простая формула описывает работу пропорционального регулятора.

За несколько десятков миллисекунд (несколько итераций цикла обработки) под воздействием пропорционального регулятора квадрокоптер вернется в требуемое (в данном случае горизонтальное) положение. Все это время ошибка Программируем квадрокоптер на Arduino (часть 1) и усилие Программируем квадрокоптер на Arduino (часть 1) будут иметь один и тот же знак, хоть и становиться все меньше по модулю. Набрав какую-то скорость поворота (угловую скорость) квадрокоптер просто перевалится на другой бок, ведь никто его не остановит в требуемом положении. Все равно что пружина, которая всегда стремится вернуться в начальное положение, но если ее оттянуть и отпустить — будет колебаться, пока трение не возьмет верх. Конечно, на квадрокоптер тоже будет действовать трение, но практика показывает, что его не достаточно.По этой причине в пропорциональный регулятор нужно добавить еще одно слагаемое, которое будет тормозить вращение квадрокоптера и препятствовать перерегулированию (переваливанию в противоположную сторону) — своего рода имитация трения в вязкой среде: чем быстрее поворачивается квадрокоптер, тем сильнее надо пытаться его остановить, конечно, в разумных пределах. Скорость вращения (скорость изменения ошибки ) обозначим как Программируем квадрокоптер на Arduino (часть 1), тогда:

где D — настраиваемый коэффициент: чем он больше, тем сильнее останавливающее усилие. Из школьного курса физики всплывают смутные воспоминания, что скорость изменения любой величины — производная этой величины по времени:

И вот пропорциональный регулятор превращается в пропорционально-дифференциальный (пропорциональное слагаемое и дифференциальное):

Ошибку Программируем квадрокоптер на Arduino (часть 1) вычислить легко, ведь на каждой итерации мы знаем Программируем квадрокоптер на Arduino (часть 1) и Программируем квадрокоптер на Arduino (часть 1); P и D — настраиваемые перед запуском параметры. Для вычисления производной (скорости изменения Программируем квадрокоптер на Arduino (часть 1)) необходимо хранить предыдущее значениеПрограммируем квадрокоптер на Arduino (часть 1), знать текущее значение Программируем квадрокоптер на Arduino (часть 1) и знать время, которое прошло между измерениями (период регулирования). И вот она — физика шестого класса школы (скорость = расстояние / время):Программируем квадрокоптер на Arduino (часть 1) — период регулирования; Программируем квадрокоптер на Arduino (часть 1) — значение ошибки с предыдущей итерации цикла регуляции. Кстати, эта формула — простейший способ численного дифференцирования, и он нам здесь вполне подойдет.

Теперь у нас есть пропорционально-дифференциальный регулятор в плоском «бикоптере», но осталась еще одна проблема. Пусть левый край будет весить чуть больше правого, или, что то же самое, левый мотор работает чуть хуже правого. Квадрокоптер чуть наклонен влево и не поворачивается обратно: дифференциальное слагаемое равно нулю, а пропорциональное слагаемое хоть и принимает положительное значение, но его не хватает, чтобы вернуть квадрокоптер в горизонтальное положение, ведь левый край весит чуть больше правого. Как следствие — квадрокоптер будет все время тянуть влево.

Необходим механизм, который бы отслеживал такие отклонения и исправлял их. Характерной особенностью таких ошибок является то, что они прявляют себя со временем. На помощь приходит интегральное слагаемое. Оно хранит сумму всех ошибкок Программируем квадрокоптер на Arduino (часть 1) по всем итерациям цикла обработки. Как же это поможет? Если пропорционального слагаемого не достаточно, чтобы исправить маленькую ошибку, но она все равно есть — постепенно, со временем, набирает силы интегральное слагаемое, увеличивая реакцию Программируем квадрокоптер на Arduino (часть 1) и квадрокоптер принимает требуемый угол крена.Тут есть нюанс. Предположим Программируем квадрокоптер на Arduino (часть 1) равна 1 градусу, цикл регулирования — 0.1с. Тогда за одну секунду сумма ошибок примет значение 10 градусов. А если цикл обработки — 0.01с, то сумма наберет аж 100 градусов. Чтобы за одно и тоже время интегральное слагаемое набирало одно и тоже значение при разных периодах регулирования, полученную сумму будем умножать на сам период регулирования. Легко посчитать, что в обоих случаях из примера получается сумма в 1 градус. Вот оно — интегральное слагаемое (пока без настраиваемого коэффициента):Эта формула — не что иное, как численный интеграл по времени функции Программируем квадрокоптер на Arduino (часть 1) в интервале от нуля до текущего момента. Именно поэтому слагаемое называется интегральным:

где T — текущий момент времени.Пришло время записать окончательную формулу пропорционально-интергрально-дифференциального регулятора:

где Программируем квадрокоптер на Arduino (часть 1) — один из настраиваемых параметров, которых теперь трое: Программируем квадрокоптер на Arduino (часть 1). Эта формула удобна в применении из программного кода, а вот формула, которая приводится в учебниках:

Существует несколько ее вариаций, например, можно ограничить модуль интегрального слагаемого, чтобы он не превысил определенный допустимый порог (мы так и будем делать).

Устройство бортового компьютера

Компоненты устройства:1. Платформа Arduino2. Модуль гироскопа акселерометра высотомера3. Модуль записи на SD-карте SD-карта4. Батарея питания5. Тумблер отключения питания6. Кнопка для сброса высоты7. Светодиоды-индикаторы взвода и зажигания8. Реле для замыкания внешней цепи при зажигании9. Несколько резисторов, указанных на схеме

Принципы работы устройства:1. Устройство непрерывно замеряет текущую высоту с помощью датчика высоты, и вычисляет среднее значение за последние 16 измерений — скользящее среднее (это нужно для сглаживания колебаний измерения, так как они довольно сильно «скачут»).2.

Если нажата кнопка сброса высоты, текущее скользящее среднее высоты берётся в качестве начала отсчёта, и, в дальнейшем измеряется так называемая «высота от точки старта» — это высота за вычетом данного начала отсчёта. Соответственно, в месте, где кнопка была нажата, будет 0 этой высоты.3.

В ходе вычислений замеряется максимальная «высота от точки старта», после достижения которой 10 метров (например), устройство переходит в состояние «взвод», максимальная высота продолжает замеряться (любая измеренная высота больше максимальной — объявляется новой максимальной), при этом зажигается светодиод взвода4.

Если, находясь в состоянии «взвод» замерянная высота окажется меньше максимальной на 1 метр (например), устройство переходит в состояние «запуск», замыкая реле, и зажигая светодиод запуска. Светодиод взвода при этом гасится. Реле может выполнить любую работу, например, подать напряжение на сервопривод, выпускающий парашют.5.

Общая схема подключения компонентов:

Модули, необходимые для приложения:

Для работы программы я собрал несколько библиотек, либо не входящих в репозиторий Arduino, либо других версий, которые удалось заставить работать.

Библиотека BMP085:BMP085.cppBMP085.h

Библиотека HMC5883LHMC5883L.cppHMC5883L.h

Библиотека I2CI2Cdev.cpp I2Cdev.h

Библиотека MPU6050MPU6050.cpp MPU6050.h

Все эти модули можно найти вместе с основным файлом программы в репозитории.

Код приложения: 

#include <Wire.h>#include "BMP085.h"#include "I2Cdev.h"#include "MPU6050.h"#include "HMC5883L.h"#include <SPI.h>#include <Wire.h>#include "BMP085.h"#include "I2Cdev.h"#include "MPU6050.h"#include "HMC5883L.h"#include <SPI.h>#include <SD.h>
 
#define TRUE 1#define FALSE 0
 
File myFile;
 
HMC5883L compass;
BMP085 pressure_m;
MPU6050 accelgyro;
 
//Контакт, куда подключено рэле#define EXEC_PIN 3//Контакт, куда подключена кнопка "сброс высоты"#define RESET_ALT_PIN 6//Контакт, куда подключен светодиод "взвод" (красный)#define ARMED_PIN 9//Время в течении которого надо держать рэле замкнутым, в миллисекундах#define FIRE_TIME 6000//Количество итераций для вычисления скользящего среднего высоты (лучше, чтобы было степенью двойки)#define RUN_ALT_NUM 16
 
//Контакт, куда подключена кнопка "положение X"#define POS_X_PIN 7//Контакт, куда подключена кнопка "положение Y"#define POS_Y_PIN 8//Контакт, куда подключена кнопка "положение Z"#define POS_Z_PIN 10
 
 
int16_t ax, ay, az;int16_t gx, gy, gz;
bool blinkState =false;double dt =0;long counter =0;double t0 =-1;double millis_t =0;double io_t =0;double io_dt =40;double fired_millis_t =0;
 
float base_altitude =0;
 
float run_alt[RUN_ALT_NUM];int run_alt_index =0;int last_run_alt_index =0;int run_alt_count =0;float run_alt_summ =0;
 
bool is_prepared = FALSE;
bool is_armed = FALSE;
bool is_fired = FALSE;
bool is_led_armed = FALSE;
bool is_led_fired = FALSE;
bool is_done = FALSE;
 
float arming_altitude =10;float fire_minus_altitude =1;float max_altitude =0;
 
char filename[]="GY88_000.TXT";
 
void reset_running_alt();void reset_alt(float current_alt);
 
/**
 * Сгенерировать имя файла
 */void generateFileName(){for(int i =0; i<1000; i  ){
    filename[5]= i/100 '0';
    filename[6]=(i%100)/10 '0';
    filename[7]=(i%10) '0';if(SD.exists(filename))continue;
    Serial.print("File: ");
    Serial.println(filename);break;}}
 
/**
 * Отладочная функция, вычисляет сумму скользящего среднего "по-настоящему"
 */float debug_get_alt_summ(){float summ =0;for(int i=0; i< run_alt_count; i  ){
    summ  = run_alt[i];}return summ;}
 
void setup(){//Настраиваем режимы работы для цифровых контактов
    pinMode(EXEC_PIN, OUTPUT);
    pinMode(RESET_ALT_PIN, INPUT);
    pinMode(ARMED_PIN, OUTPUT);//Сбрасываем значения для скользящего среднего по высоте
    reset_running_alt();
 
    //Включаем последовательный порт на максимальную скорость
    Serial.begin(230400);//while (!Serial) {;// wait for serial port to connect. Needed for native USB port only//}    
 
    Serial.print("Initializing SD card...");
 
    if(!SD.begin(4)){
      Serial.println("initialization failed!");return;}
    Serial.println("initialization done.");
 
    //Сгенерируем имя файла
    generateFileName();
    myFile = SD.open(filename, O_WRITE | O_CREAT);
 
    //Включаем протокол Wire
    Wire.begin();
    TWBR =24;
    Serial.println("Initializing I2C devices...");//инициализируем акселерометр с гироскопом
    accelgyro.initialize();
    accelgyro.setMasterClockSpeed(13);
    accelgyro.setI2CMasterModeEnabled(true);//инициализируем компас
    compass = HMC5883L();
    setupHMC5883L(); 
 
    //проверяем содеинение
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection()?"MPU6050 connection successful":"MPU6050 connection failed");
 
    //калибруем датчик давления
    pressure_m.bmp085Calibration();}
 
void loop(){
  millis_t = millis();//Считаем температуру (её знает датчик давления)float temperature = pressure_m.bmp085GetTemperature();//MUST be called first//Считаем давлениеfloat pressure = pressure_m.bmp085GetPressure();//Вычислим высоту над уровнем моряfloat altitude = pressure_m.calcAltitude(pressure);float altitude_b = altitude - base_altitude;
 
  //Проведём вычисления для скользящего среднего по высоте
  run_alt[run_alt_index]= altitude_b;//run_alt_summ  = altitude_b;if(run_alt_count == RUN_ALT_NUM){
    last_run_alt_index =(run_alt_index ==0)? RUN_ALT_NUM -1: run_alt_index -1;//run_alt_summ -= run_alt[last_run_alt_index];}else{
    run_alt_count  =1;}
  run_alt_index =(run_alt_index  1== RUN_ALT_NUM)?0: run_alt_index  1;
 
  //Временно вычисляем медленным методом
  run_alt_summ = debug_get_alt_summ();
 
  float run_altitude = run_alt_summ /(float) run_alt_count;
 
  //Вычислим dt (интервал между замерами в миллисекундах)if(t0 <0){
    t0 = millis_t;}else{
    dt = millis_t - t0;
    t0 = millis_t;}
 
  if(run_altitude > max_altitude){
    max_altitude = run_altitude;}
 
 
  if(millis_t - io_t > io_dt){
    io_t = millis_t;if(digitalRead(RESET_ALT_PIN)== HIGH){
      reset_alt(altitude);}
 
 
 
    if(!is_done){if(!is_fired){if(is_prepared){if(!is_armed){if(max_altitude > arming_altitude){
              is_armed = TRUE;}}else{if(max_altitude - run_altitude > fire_minus_altitude){
              is_armed = FALSE;
              is_fired = TRUE;
              fired_millis_t = millis_t;}}}}else{if(millis_t - fired_millis_t > FIRE_TIME){
          is_fired = FALSE;
          is_done = TRUE;
          is_prepared = FALSE;}}}
 
    if(is_armed != is_led_armed){
      is_led_armed = is_armed;
      digitalWrite(ARMED_PIN, is_armed ? HIGH : LOW);}
 
    if(is_fired != is_led_fired){
      is_led_fired = is_fired;
      digitalWrite(EXEC_PIN, is_fired ? HIGH : LOW);}
 
  }
 
  //Интервал между замерами
  Serial.print("dt:"); Serial.print(dt /1000,3);
  myFile.print("dt:"); myFile.print(dt /1000,3);//Сколько времени прошло с запуска?
  Serial.print(" tm:"); Serial.print(millis_t);
  myFile.print(" tm:"); myFile.print(millis_t);//Температура, в градусах цельсия
  Serial.print(" t:"); Serial.print(temperature,2); 
  myFile.print(" t:"); myFile.print(temperature,2);//Давление, в паскалях
  Serial.print(" p:"); Serial.print(pressure,0); 
  myFile.print(" p:"); myFile.print(pressure,0);//Высота над уровнем моря, в метрах
  Serial.print(" alt:"); Serial.print(altitude,2); 
  myFile.print(" alt:"); myFile.print(altitude,2);//Высота над базовым уровнем, в метрах
  Serial.print(" altb:"); Serial.print(altitude_b,2); 
  myFile.print(" altb:"); myFile.print(altitude_b,2);//Высота над базовым уровнем, в метрах, скользящее среднее
  Serial.print(" altr:"); Serial.print(run_altitude,2); 
  myFile.print(" altr:"); myFile.print(run_altitude,2);//Высота над базовым уровнем, в метрах, максимальная
  Serial.print(" Malt:"); Serial.print(max_altitude,2); 
  myFile.print(" Malt:"); myFile.print(max_altitude,2);
 
  //Статусы:
  Serial.print(" P"); Serial.print(is_prepared ?"1":"0"); 
  myFile.print(" P"); myFile.print(is_prepared ?"1":"0");
  Serial.print(" A"); Serial.print(is_armed ?"1":"0"); 
  myFile.print(" A"); myFile.print(is_armed ?"1":"0");
  Serial.print(" F"); Serial.print(is_fired ?"1":"0"); 
  myFile.print(" F"); myFile.print(is_fired ?"1":"0");
  Serial.print(" D"); Serial.print(is_done ?"1":"0"); 
  myFile.print(" D"); myFile.print(is_done ?"1":"0");
 
  //Считаем значения с магнитометра
  MagnetometerScaled scaled = compass.ReadScaledAxis();//Считаем значения с акселерометра и гироскопа
  accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
 
  //Данные акселерометра:
  Serial.print(" ax:"); Serial.print(ax);
  Serial.print(" ay:"); Serial.print(ay);
  Serial.print(" az:"); Serial.print(az);
  myFile.print(" ax:"); myFile.print(ax);
  myFile.print(" ay:"); myFile.print(ay);
  myFile.print(" az:"); myFile.print(az);//Данные гироскопа
  Serial.print(" wx:"); Serial.print(gx);
  Serial.print(" wy:"); Serial.print(gy);
  Serial.print(" wz:"); Serial.print(gz);
  myFile.print(" wx:"); myFile.print(gx);
  myFile.print(" wy:"); myFile.print(gy);
  myFile.print(" wz:"); myFile.print(gz);//Данные магнитометра    
  Serial.print(" cx:"); Serial.print(scaled.XAxis,3);
  Serial.print(" cy:"); Serial.print(scaled.YAxis,3);
  Serial.print(" cz:"); Serial.print(scaled.ZAxis,3);
  Serial.println();
  myFile.print(" cx:"); myFile.print(scaled.XAxis,3);
  myFile.print(" cy:"); myFile.print(scaled.YAxis,3);
  myFile.print(" cz:"); myFile.print(scaled.ZAxis,3);
  myFile.println();
 
  myFile.flush();}
 
//Настройка модуля HMC5883L (компаса)void setupHMC5883L(){//Setup the HMC5883L, and check for errorsint error;  
  error = compass.SetScale(1.3);//Set the scale of the compass.if(error !=0) Serial.println(compass.GetErrorText(error));//check if there is an error, and print if so
 
  error = compass.SetMeasurementMode(Measurement_Continuous);// Set the measurement mode to Continuousif(error !=0) Serial.println(compass.GetErrorText(error));//check if there is an error, and print if so}
 
/**
 * Сброс высоты и перевод в состояние готовности - выполняется по нажатию кнопки
 */void reset_alt(float current_alt){
  base_altitude = current_alt;
  is_prepared = TRUE;
  is_armed = FALSE;
  max_altitude =0;  
  reset_running_alt();}
 
/**
 * Сброс скользящего среднего для высоты
 */void reset_running_alt(){
  run_alt_index =0;
  run_alt_count =0;
  run_alt_summ =0;
  last_run_alt_index =0;for(int i =0; i<RUN_ALT_NUM; i  ){
      run_alt[i]=0;}}

#include <Wire.h>
#include “BMP085.h”
#include “I2Cdev.h”
#include “MPU6050.h”
#include “HMC5883L.h”
#include <SPI.h>
#include <Wire.h>
#include “BMP085.h”
#include “I2Cdev.h”
#include “MPU6050.h”
#include “HMC5883L.h”
#include <SPI.h>
#include <SD.h>

#define TRUE 1
#define FALSE 0

File myFile;

HMC5883L compass;
BMP085 pressure_m;
MPU6050 accelgyro;

//Контакт, куда подключено рэле
#define EXEC_PIN 3
//Контакт, куда подключена кнопка “сброс высоты”
#define RESET_ALT_PIN 6
//Контакт, куда подключен светодиод “взвод” (красный)
#define ARMED_PIN 9
//Время в течении которого надо держать рэле замкнутым, в миллисекундах
#define FIRE_TIME 6000
//Количество итераций для вычисления скользящего среднего высоты (лучше, чтобы было степенью двойки)
#define RUN_ALT_NUM 16

//Контакт, куда подключена кнопка “положение X”
#define POS_X_PIN 7
//Контакт, куда подключена кнопка “положение Y”
#define POS_Y_PIN 8
//Контакт, куда подключена кнопка “положение Z”
#define POS_Z_PIN 10

int16_t ax, ay, az;
int16_t gx, gy, gz;
bool blinkState = false;
double dt = 0;
long counter = 0;
double t0 = -1;
double millis_t = 0;
double io_t = 0;
double io_dt = 40;
double fired_millis_t = 0;

float base_altitude = 0;

float run_alt[RUN_ALT_NUM];
int run_alt_index = 0;
int last_run_alt_index = 0;
int run_alt_count = 0;
float run_alt_summ = 0;

bool is_prepared = FALSE;
bool is_armed = FALSE;
bool is_fired = FALSE;
bool is_led_armed = FALSE;
bool is_led_fired = FALSE;
bool is_done = FALSE;

float arming_altitude = 10;
float fire_minus_altitude = 1;
float max_altitude = 0;

char filename[] = “GY88_000.TXT”;

void reset_running_alt();
void reset_alt(float current_alt);

/**
* Сгенерировать имя файла
*/
void generateFileName()
{
for (int i = 0; i< 1000; i )
{
filename[5] = i/100 ‘0’;
filename[6] = (i0)/10 ‘0’;
filename[7] = (i) ‘0’;
if (SD.exists(filename)) continue;
Serial.print(“File: “);
Serial.println(filename);
break;
}
}

/**
* Отладочная функция, вычисляет сумму скользящего среднего “по-настоящему”
*/
float debug_get_alt_summ()
{
float summ = 0;
for (int i=0; i< run_alt_count; i )
{
summ = run_alt[i];
}
return summ;
}

void setup(){
//Настраиваем режимы работы для цифровых контактов
pinMode(EXEC_PIN, OUTPUT);
pinMode(RESET_ALT_PIN, INPUT);
pinMode(ARMED_PIN, OUTPUT);
//Сбрасываем значения для скользящего среднего по высоте
reset_running_alt();

//Включаем последовательный порт на максимальную скорость
Serial.begin(230400);
//while (!Serial) {
; // wait for serial port to connect. Needed for native USB port only
//}

Serial.print(“Initializing SD card…”);

if (!SD.begin(4)) {
Serial.println(“initialization failed!”);
return;
}
Serial.println(“initialization done.”);

//Сгенерируем имя файла
generateFileName();
myFile = SD.open(filename, O_WRITE | O_CREAT);

//Включаем протокол Wire
Wire.begin();
TWBR = 24;
Serial.println(“Initializing I2C devices…”);
//инициализируем акселерометр с гироскопом
accelgyro.initialize();
accelgyro.setMasterClockSpeed(13);
accelgyro.setI2CMasterModeEnabled(true);
//инициализируем компас
compass = HMC5883L();
setupHMC5883L();

//проверяем содеинение
Serial.println(“Testing device connections…”);
Serial.println(accelgyro.testConnection() ? “MPU6050 connection successful” : “MPU6050 connection failed”);

//калибруем датчик давления
pressure_m.bmp085Calibration();
}

void loop()
{
millis_t = millis();
//Считаем температуру (её знает датчик давления)
float temperature = pressure_m.bmp085GetTemperature(); //MUST be called first
//Считаем давление
float pressure = pressure_m.bmp085GetPressure();
//Вычислим высоту над уровнем моря
float altitude = pressure_m.calcAltitude(pressure);
float altitude_b = altitude – base_altitude;

//Проведём вычисления для скользящего среднего по высоте
run_alt[run_alt_index] = altitude_b;
//run_alt_summ = altitude_b;
if (run_alt_count == RUN_ALT_NUM)
{
last_run_alt_index = (run_alt_index == 0) ? RUN_ALT_NUM – 1 : run_alt_index – 1;
//run_alt_summ -= run_alt[last_run_alt_index];
}
else
{
run_alt_count =1;
}
run_alt_index = (run_alt_index 1 == RUN_ALT_NUM) ? 0 : run_alt_index 1;

//Временно вычисляем медленным методом
run_alt_summ = debug_get_alt_summ();

float run_altitude = run_alt_summ / (float) run_alt_count;

//Вычислим dt (интервал между замерами в миллисекундах)
if (t0 < 0)
{
t0 = millis_t;
}
else
{
dt = millis_t – t0;
t0 = millis_t;
}

if (run_altitude > max_altitude)
{
max_altitude = run_altitude;
}

if (millis_t – io_t > io_dt)
{
io_t = millis_t;
if (digitalRead(RESET_ALT_PIN) == HIGH)
{
reset_alt(altitude);
}

if (!is_done)
{
if (!is_fired)
{
if (is_prepared)
{
if (!is_armed)
{
if (max_altitude > arming_altitude)
{
is_armed = TRUE;
}
}
else
{
if (max_altitude – run_altitude > fire_minus_altitude)
{
is_armed = FALSE;
is_fired = TRUE;
fired_millis_t = millis_t;
}
}
}
}
else
{
if (millis_t – fired_millis_t > FIRE_TIME)
{
is_fired = FALSE;
is_done = TRUE;
is_prepared = FALSE;
}
}
}

if (is_armed != is_led_armed)
{
is_led_armed = is_armed;
digitalWrite(ARMED_PIN, is_armed ? HIGH : LOW);
}

if (is_fired != is_led_fired)
{
is_led_fired = is_fired;
digitalWrite(EXEC_PIN, is_fired ? HIGH : LOW);
}

}

//Интервал между замерами
Serial.print(“dt:”); Serial.print(dt / 1000, 3);
myFile.print(“dt:”); myFile.print(dt / 1000, 3);
//Сколько времени прошло с запуска?
Serial.print(” tm:”); Serial.print(millis_t);
myFile.print(” tm:”); myFile.print(millis_t);
//Температура, в градусах цельсия
Serial.print(” t:”); Serial.print(temperature, 2);
myFile.print(” t:”); myFile.print(temperature, 2);
//Давление, в паскалях
Serial.print(” p:”); Serial.print(pressure, 0);
myFile.print(” p:”); myFile.print(pressure, 0);
//Высота над уровнем моря, в метрах
Serial.print(” alt:”); Serial.print(altitude, 2);
myFile.print(” alt:”); myFile.print(altitude, 2);
//Высота над базовым уровнем, в метрах
Serial.print(” altb:”); Serial.print(altitude_b, 2);
myFile.print(” altb:”); myFile.print(altitude_b, 2);
//Высота над базовым уровнем, в метрах, скользящее среднее
Serial.print(” altr:”); Serial.print(run_altitude, 2);
myFile.print(” altr:”); myFile.print(run_altitude, 2);
//Высота над базовым уровнем, в метрах, максимальная
Serial.print(” Malt:”); Serial.print(max_altitude, 2);
myFile.print(” Malt:”); myFile.print(max_altitude, 2);

//Статусы:
Serial.print(” P”); Serial.print(is_prepared ? “1” : “0”);
myFile.print(” P”); myFile.print(is_prepared ? “1” : “0”);
Serial.print(” A”); Serial.print(is_armed ? “1” : “0”);
myFile.print(” A”); myFile.print(is_armed ? “1” : “0”);
Serial.print(” F”); Serial.print(is_fired ? “1” : “0”);
myFile.print(” F”); myFile.print(is_fired ? “1” : “0”);
Serial.print(” D”); Serial.print(is_done ? “1” : “0”);
myFile.print(” D”); myFile.print(is_done ? “1” : “0”);

//Считаем значения с магнитометра
MagnetometerScaled scaled = compass.ReadScaledAxis();
//Считаем значения с акселерометра и гироскопа
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

//Данные акселерометра:
Serial.print(” ax:”); Serial.print(ax);
Serial.print(” ay:”); Serial.print(ay);
Serial.print(” az:”); Serial.print(az);
myFile.print(” ax:”); myFile.print(ax);
myFile.print(” ay:”); myFile.print(ay);
myFile.print(” az:”); myFile.print(az);
//Данные гироскопа
Serial.print(” wx:”); Serial.print(gx);
Serial.print(” wy:”); Serial.print(gy);
Serial.print(” wz:”); Serial.print(gz);
myFile.print(” wx:”); myFile.print(gx);
myFile.print(” wy:”); myFile.print(gy);
myFile.print(” wz:”); myFile.print(gz);
//Данные магнитометра
Serial.print(” cx:”); Serial.print(scaled.XAxis, 3);
Serial.print(” cy:”); Serial.print(scaled.YAxis, 3);
Serial.print(” cz:”); Serial.print(scaled.ZAxis, 3);
Serial.println();
myFile.print(” cx:”); myFile.print(scaled.XAxis, 3);
myFile.print(” cy:”); myFile.print(scaled.YAxis, 3);
myFile.print(” cz:”); myFile.print(scaled.ZAxis, 3);
myFile.println();

myFile.flush();
}

//Настройка модуля HMC5883L (компаса)
void setupHMC5883L(){
//Setup the HMC5883L, and check for errors
int error;
error = compass.SetScale(1.3); //Set the scale of the compass.
if(error != 0) Serial.println(compass.GetErrorText(error)); //check if there is an error, and print if so

error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
if(error != 0) Serial.println(compass.GetErrorText(error)); //check if there is an error, and print if so
}

/**
* Сброс высоты и перевод в состояние готовности – выполняется по нажатию кнопки
*/
void reset_alt(float current_alt)
{
base_altitude = current_alt;
is_prepared = TRUE;
is_armed = FALSE;
max_altitude = 0;
reset_running_alt();
}

/**
* Сброс скользящего среднего для высоты
*/
void reset_running_alt()
{
run_alt_index = 0;
run_alt_count = 0;
run_alt_summ = 0;
last_run_alt_index = 0;
for (int i =0; i<RUN_ALT_NUM; i )
{
run_alt[i]=0;
}
}

Данные, выдаваемые программой, должны быть примерно такими:

dt:0.048 tm:5760.00 t:26.80 p:100898 alt:35.61 altb:35.61 altr:35.60 Malt:35.60 P0 A0 F0 D0 ax:-1084 ay:16 az:-15708 wx:-282 wy:66 wz:-68 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.047 tm:5807.00 t:26.80 p:100898 alt:35.61 altb:35.61 altr:35.61 Malt:35.61 P0 A0 F0 D0 ax:-1132 ay:100 az:-16276 wx:-282 wy:213 wz:-81 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.048 tm:5855.00 t:26.80 p:100898 alt:35.61 altb:35.61 altr:35.63 Malt:35.63 P0 A0 F0 D0 ax:-1016 ay:16 az:-15740 wx:-279 wy:149 wz:-78 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.047 tm:5902.00 t:26.80 p:100895 alt:35.86 altb:35.86 altr:35.66 Malt:35.66 P0 A0 F0 D0 ax:-1152 ay:-48 az:-15596 wx:-247 wy:60 wz:-60 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.047 tm:5949.00 t:26.80 p:100904 alt:35.11 altb:35.11 altr:35.64 Malt:35.66 P0 A0 F0 D0 ax:-1080 ay:-52 az:-15704 wx:-276 wy:54 wz:-61 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.047 tm:5996.00 t:26.80 p:100902 alt:35.27 altb:35.27 altr:35.62 Malt:35.66 P0 A0 F0 D0 ax:-1152 ay:-44 az:-15652 wx:-290 wy:69 wz:-79 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.047 tm:6043.00 t:26.80 p:100900 alt:35.44 altb:35.44 altr:35.60 Malt:35.66 P0 A0 F0 D0 ax:-1128 ay:-8 az:-15496 wx:-281 wy:37 wz:-70 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.047 tm:6090.00 t:26.80 p:100897 alt:35.69 altb:35.69 altr:35.60 Malt:35.66 P0 A0 F0 D0 ax:-1048 ay:24 az:-15620 wx:-280 wy:59 wz:-75 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.047 tm:6137.00 t:26.80 p:100902 alt:35.27 altb:35.27 altr:35.55 Malt:35.66 P0 A0 F0 D0 ax:-1164 ay:0 az:-15736 wx:-292 wy:73 wz:-69 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.047 tm:6184.00 t:26.80 p:100899 alt:35.52 altb:35.52 altr:35.56 Malt:35.66 P0 A0 F0 D0 ax:-1088 ay:204 az:-15404 wx:-296 wy:-9 wz:-38 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.048 tm:6232.00 t:26.80 p:100897 alt:35.69 altb:35.69 altr:35.57 Malt:35.66 P0 A0 F0 D0 ax:-1220 ay:-340 az:-15976 wx:-261 wy:32 wz:-106 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.048 tm:6280.00 t:26.80 p:100902 alt:35.27 altb:35.27 altr:35.59 Malt:35.66 P0 A0 F0 D0 ax:-880 ay:144 az:-15828 wx:-289 wy:120 wz:-76 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.047 tm:6327.00 t:26.80 p:100906 alt:34.94 altb:34.94 altr:35.53 Malt:35.66 P0 A0 F0 D0 ax:-1300 ay:52 az:-15292 wx:-274 wy:97 wz:-71 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.047 tm:6374.00 t:26.80 p:100903 alt:35.19 altb:35.19 altr:35.50 Malt:35.66 P0 A0 F0 D0 ax:-1072 ay:-76 az:-15452 wx:-264 wy:55 wz:-74 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.047 tm:6421.00 t:26.80 p:100905 alt:35.02 altb:35.02 altr:35.44 Malt:35.66 P0 A0 F0 D0 ax:-1040 ay:-4 az:-15844 wx:-297 wy:70 wz:-80 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.047 tm:6468.00 t:26.80 p:100901 alt:35.36 altb:35.36 altr:35.40 Malt:35.66 P0 A0 F0 D0 ax:-1108 ay:64 az:-15644 wx:-294 wy:88 wz:-65 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.047 tm:6515.00 t:26.80 p:100901 alt:35.36 altb:35.36 altr:35.39 Malt:35.66 P0 A0 F0 D0 ax:-1136 ay:80 az:-15628 wx:-287 wy:75 wz:-62 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.047 tm:6562.00 t:26.80 p:100903 alt:35.19 altb:35.19 altr:35.36 Malt:35.66 P0 A0 F0 D0 ax:-1120 ay:-136 az:-15608 wx:-256 wy:37 wz:-76 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.047 tm:6609.00 t:26.80 p:100895 alt:35.86 altb:35.86 altr:35.38 Malt:35.66 P0 A0 F0 D0 ax:-1032 ay:-8 az:-15600 wx:-283 wy:81 wz:-68 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.048 tm:6657.00 t:26.80 p:100900 alt:35.44 altb:35.44 altr:35.35 Malt:35.66 P0 A0 F0 D0 ax:-1112 ay:-16 az:-15612 wx:-283 wy:59 wz:-77 cx:24508.800 cy:7121.720 cz:3551.200
dt:0.048 tm:6705.00 t:26.80 p:100901 alt:35.36 altb:35.36 altr:35.37 Malt:35.66 P0 A0 F0 D0 ax:-1084 ay:64 az:-15624 wx:-282 wy:73 wz:-66 cx:24508.800 cy:7121.720 cz:3551.200

Что означают данные:dt — интервал времени в миллисекундах с прошлого замераtm — количество миллисекунд, прошедшее с включения устройстваt — температура воздухаp — давление воздухаalt — высота по данным датчика-высотомераaltb — высота за вычетом «базовой» высоты, полученной при нажатии кнопкиaltr — скользящее среднее по последним 16 замерам высота altbЗначения-флаги (могут принимать значение 0 или 1, что означает НЕТ и ДА)

P — Была ли нажата кнопка установки базовой высоты, т.е. подготовлено ли устройство? (Prepared)A — Взведено ли устройство? Т.е. пройдена ли точка минимальной необходимой для взвода высоты (10 метров) (Armed)F — Активировано ли реле? т.е. выполнено ли условие снижения не 1 метр относительно максимальной набранной высоты?

(Fired)D — Закончена ли работа устройства? Этот флаг включается, когда выключается флаг F. После этого устройство больше никаких функций выполнять не будет, и для работы с ним его потребуется либо перезагрузить либо выключить и включить снова.

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

Смотрите про коптеры:  Беспилотники, беспилотные летательные аппараты - виды и модели беспилотников
adxl345 Arduino bma180 bmp085 crius itg3200 multiwii
Оцените статью
Радиокоптер.ру
Добавить комментарий

© 2024 Радиокоптер.ру