YMFC-32 autonomous: STM32-квадрокоптер с GPS, барометром, компасом и телеметрией
Примечание
Автор проекта и всех материалов: J.M. Brokking (Joop Brokking), brokking.net. Ниже подробный пересказ его страниц и его файлы без изменений. Оригиналы: главная, Q&A, загрузки, медиа, обновление ПО, разбор программы настройки, тестовые квадрокоптеры; копии в Wayback Machine.
YMFC-32 autonomous — полётный контроллер квадрокоптера на STM32, написанный в Arduino IDE как учебный проект для хобби и школ. Цели: минимально необходимый и понятный код и минимальная стоимость. Вся сборка с батареей, зарядкой, пультом и телеметрией, по оценке автора, укладывается меньше чем в 200 евро. Код бесплатный, прокомментирован и разобран в видеоуроках (см. Видео и ссылки на оригинальные страницы).
Возможности: автовыравнивание (auto level), удержание высоты (altitude hold), удержание позиции по GPS (GPS hold), блокировка курса (head lock / course lock), телеметрия. Время полёта у автора на 3S 3800 мАч: 15–20 минут. Все датчики обязательны: без любого из них код работать не будет.
Файлы
YMFC-32_auto.zip— полётный контроллер и приёмник телеметрии, схемы автономного контроллера и телеметрии.YMFC-32_auto_rth.zip— версия с возвратом домой (Return To Home):YMFC-32_auto_Flight_Controller_ver-2b,YMFC-32_auto_telemetry_receiver_ver-3a.YMFC-32 Flight Monitor— прошивка с функциями маршрута (fly_waypoints.inoи др.) и приложение Windows YMFC-32 Flight Monitor (проект Visual Studio и exe).Документ 1,документ 2,надстройка STM32 для Arduino IDE,PWM→PPM для R6B.
1. Программа
Скачать пакет и установить надстройку «STM32 for Arduino» (видео автора про начало работы с STM32F103C8T6). Автор предупреждает: надстройка находится в разработке, он на неё не влияет; версия, с которой он работал, лежит в разделе загрузок.
2. Железо (список автора)
Контроллер: STM32F103C8T6, MPU-6050, GPS-модуль со встроенным компасом HMC5883L, пульт Flysky FS-i6X с приёмником IA6B (выдаёт PPM), набор резисторов, макетная плата, светодиоды 3 мм (красный, жёлтый, зелёный).
Рама: 450 с платой питания, 4 комплекта «мотор 1000 kV + винты 10x4.5 + ESC», LiPo 3S 2200 мАч 30C, XT60, зарядное 2S/3S. Автор предпочитает винты 8x4.5.
Телеметрия: клон Arduino Uno, LCD-шилд с кнопками, пищалка, беспроводные модули APC220 с USB-конвертером.
Опционально: для новичков маленький безопасный квадрокоптер JJRC H36 для тренировки в помещении.
3. Сборка
Делитель R3/R4 (1 кОм и 10 кОм): делит напряжение батареи на 11. Светодиод сигнализирует о низком напряжении, обороты автоматически поднимаются при просадке.
MPU-6050: ориентация критична, точка на корпусе смотрит на левый задний угол, иначе аппарат сразу перевернётся; ось Z вертикальна; клеить на тонкий скотч (не поролон).
Пульт и приёмник: нужны регулируемые конечные точки, субтримы и PPM-выход. Flysky T6 PPM не имеет, поэтому для него автор сделал конвертер PWM→PPM (см. YMFC-32: квадрокоптер на STM32 (Blue Pill)); FS-i6X с приёмником IA6B работает «из коробки». PPM-выход приёмника подключается к входу A0 STM32. Приёмник питается от +5 В BEC. Канал 1 – крен, 2 – тангаж, 3 – газ, 4 – рыскание (1000/1500/2000 мкс). Канал 5 – режимы полёта: 1000–1200 мкс режим 1 (автовыравнивание), 1200–1600 мкс режим 2 (удержание высоты), 1600–2000 мкс режим 3 (высота и GPS). Канал 6 – head lock: 1200–2000 мкс включает его. Функции проверяются на системе телеметрии.
Регуляторы: земля и сигнал подключены у ESC 2, 3, 4, а у ESC 1 ещё и +5 В (BEC), он питает контроллер. Если BEC нет, нужен понижающий преобразователь 7–28 В → 5 В. Моторы: 1 – B6, правый передний, против часовой; 2 – B7, правый задний, по часовой; 3 – B8, левый задний, против часовой; 4 – B9, левый передний, по часовой.
GPS и компас: модуль нужен для head lock и GPS hold; автор использует модуль от Ardupilot/APM с HMC5883L; большинство модулей на u-blox M8N. Код настраивает GPS по протоколу u-blox, поэтому подделки (не оригинальный u-blox) могут работать нестабильно. Провода по схеме (цвета у разных модулей могут отличаться, при сомнении сверяйтесь с маркировкой платы).
Барометр MS5611: для удержания высоты, чувствителен к перепадам давления с точностью около 10 см, чувствителен к свету, поэтому его нужно закрывать от солнца.
4. Программа настройки (setup)
Перед первым включением всё перепроверить. Снять винты, не подключать батарею, залить тестовую программу. Её пункты (подробный разбор у автора на отдельной странице): a — чтение импульсов приёмника, b — сканер I2C, c — сырые значения гироскопа, d — сырые значения акселерометра, e — углы IMU, f — проверка светодиодов, g — напряжение батареи, h — барометр, i — GPS, j — компас HMC5883L. Программа служит и для проверки датчиков, и для балансировки винтов, и для калибровки ESC. Каналы 1–4 приёмника должны быть настроены на 1000 / 1500 / 2000 мкс (допуск ±5).
Калибровка ESC: газ вверх, подключить батарею, после писков газ вниз, отключить батарею (читайте инструкцию к своему ESC).
5. Балансировка моторов и винтов
Так же, как в YMFC-AL: квадрокоптер с автостабилизацией на Arduino Uno: 1 запускает тест мотора 1, держать раму, полугаз, смотреть вибрации, клеить скотч на лопасти; q выход, затем моторы 2–4. Видео автора показывает процедуру с позиции 2 мин 16 с.
6. Загрузка прошивки
Перед загрузкой кода установить магнитное склонение (declination) для вашего места (карта «World Magnetic Declination 2015» в PDF у автора; остальные настройки пока можно не менять). Включить пульт и приёмник телеметрии, подать питание на контроллер через USB/FTDI. Не двигать аппарат во время калибровки (быстрое мигание). Светодиоды покажут последовательность запуска: задержка калибровки 5 секунд, калибровка гироскопа 8 секунд, затем режим полёта (1, 2, 3 – число вспышек зелёного). Светодиод GPS на плате (PC13): не мигает – нет сигнала; 5 Гц – сигнал есть; 25 Гц – фиксация (lock). После старта на телеметрии появится информация о контроллере; можно проверить переключатель режимов и head lock. На странице углов телеметрии проверить: левый бок вниз = отрицательный крен, правый бок вниз = положительный, нос вверх = положительный тангаж, нос вниз = отрицательный.
6.1 Калибровка акселерометра
Нужна после каждой заливки кода, первой сборки или аварии. Поставить аппарат на ровную поверхность и перевести оба стика в верхний левый угол; светодиод мигнёт, калибровка закончена. Если смещение акселерометра слишком большое, сработает тревога (вопрос 13 Q&A).
6.2 Калибровка компаса
Нужна после сборки, после аварии или перестановки компаса, если аппарат корректирует курс не в ту сторону, и после каждой заливки кода (EEPROM стирается). Цель — убрать смещение модуля и центрировать оси X, Y, Z. Нельзя калибровать рядом с магнитными помехами и держать при себе ферромагнитные предметы (ключи, телефон). Сначала узнайте направление на магнитный север (приложение или компас). Оба стика в верхний правый угол: красный светодиод горит постоянно (калибровка идёт). Вращать аппарат вокруг осей крена и тангажа в сторону магнитного севера, минимум два оборота по каждой оси. В конце опустить стик тангажа вниз: светодиод мигнёт, калибровка окончена. После калибровки проверить компас через телеметрию: при правильном склонении при курсе 0° должен быть направлен на географический север.
7. Первый полёт
Выбрать парк с низкой травой, никогда не летать в помещении в первый раз. Предполётный список:
MPU-6050 установлен точкой на левый задний угол?
Каналы приёмника 1–4 настроены 1000–2000 с центром 1500?
Проверена процедура старт/стоп тестовой программой?
ESC подключены к правильным портам STM32?
ESC откалиброваны программой настройки?
Моторы крутятся в правильную сторону?
Винты установлены правильно?
Винты сбалансированы?
Компас и акселерометр откалиброваны?
Углы тангажа и крена проверены по телеметрии?
Полностью заряжена ли батарея?
Подключить батарею, не двигать аппарат во время калибровки; после старта зелёный светодиод должен мигнуть один раз (режим 1). Старт: газ вниз + рыскание влево; стоп: газ вниз + рыскание вправо. Поднять газ до почти «невесомости», аппарат должен выравниваться. Первую батарею лететь как можно ниже. Центр стика газа 1400–1600 мкс (около ±20%) должен давать полёт. Не тестировать остальные режимы сразу.
Затем в коде поменять manual_takeoff_throttle = 1500 на 0 (включится автоопределение взлёта), задать правильное declination и снова залить код, затем откалибровать акселерометр и компас. На траве запустить моторы, поднять газ до половины: аппарат сам взлетит, запомнит газ как базовый для висения, после чего должен висеть при стике газа около центра (если нет, вопрос 15 Q&A).
Удержание высоты. Когда аппарат стабильно летит со стиком газа около центра, можно включить режим 2; возможно, придётся подстроить ПИД в зависимости от положения барометра.
Удержание GPS. После проверки высоты включить режим 3: аппарат держит позицию полностью автономно. Перемещать его стиками нельзя, но для безопасности пилот может «перебить» контроллер. Если что-то не так, немедленно отключить GPS hold.
Head lock. Направление «вперёд» стика — это направление носа при взлёте, а не текущее направление носа; защищает от потери ориентации.
8. Светодиоды состояния
Нормальный старт описан выше. Ошибки при запуске: 1 — MPU-6050 не отвечает; 2 — HMC5883L не отвечает; 3 — MS5611 не отвечает; 4 — не найден приёмник. Ошибки после запуска: 1 — низкая батарея; 2 — превышено время цикла; 3 — ограничение угла калибровки акселерометра; 4 — истёк таймер GPS (watchdog); 5 — manual_takeoff_throttle вне диапазона; 6 — взлёт не обнаружен; 7 — газ взлёта вне диапазона. Подробности — вопрос 13 Q&A автора (ymfc-32_auto_qanda.html).
9. Тестовые квадрокоптеры автора
Квадрокоптер 1: плата-прототип, 768 г без батареи (1077 г с батареей 3S 3800 мАч 309 г), винты 8x4.5, моторы A2212/13T 1000 kV, безымянные ESC 30A, рама DJI FlameWheel 450, пульт Flysky T6 + конвертер PWM→PPM, полёт 15 минут. Квадрокоптер 2: заводская печатная плата, 770 г (1079 г с батареей), винты 9,4x4,3, более дорогие ESC и моторы, полёт 20 минут.
10. Q&A автора (все вопросы)
После старта аппарат переворачивается. Проверить в программе настройки моторы 1–4 и направление вращения (таблица выше), ориентацию гироскопа (ось Z вертикально, точка на левый задний угол) и углы на телеметрии (левый бок вниз = отрицательный крен, нос вниз = отрицательный тангаж). Без телеметрии углы можно посмотреть в тесте
e.Моторы не стартуют или вращаются с разной скоростью. Откалибровать ESC и проверить одинаковость вращения в setup. Если моторы не запускаются вообще, увеличить
motor_idle_speed(по умолчанию 1100) и проверять без винтов.Максимальный угол. По умолчанию около 33° (см. YMFC-AL: квадрокоптер с автостабилизацией на Arduino Uno, вопрос 17): меняется множитель 15 в расчёте
pitch_level_adjustиroll_level_adjust.Время полёта и камера. 15–20 минут на 3S 3800 мАч. Камеру можно, пока вес не превышает рекомендуемую тягу; автор советует полезную нагрузку не более 200 г (иначе вибрации и «желе»).
Другой микроконтроллер. Нет, код рассчитан только на STM32F103C8T6 и для других не скомпилируется.
Другие датчики (например BMP180). Нет: поддерживаются MPU-6050, HMC5883L, MS5611 и модуль GPS u-blox. С другими датчиками нужно переписывать код, а их данные должны быть совместимы с алгоритмами.
Моторы дают тягу на холостых и отключаются на полном газе. Неверная связка мотор–винт–батарея. Моторы 1000 kV с винтами 10 дюймов подходят, но автор рекомендует 8. Для моторов 2000 kV на 3S нужны винты 5–6 дюймов.
Удержание высоты ведёт себя странно у земли (ground effect). Если барометр на высоте винтов, вокруг него создаётся зона повышенного давления. Установить барометр на несколько сантиметров выше винтов.
ESC без BEC (opto). Можно, но 5 В для датчиков, STM32, приёмника и телеметрии нужно подать внешним понижающим преобразователем 7–28 В → 5 В от основной батареи, в точку схемы, которую обычно питает BEC.
Нет сигнала канала или значения нестабильны. PPM приёмника должен быть на A0; у некоторых пультов PPM надо включать настройкой; земля приёмника должна надёжно соединяться с землёй STM32.
Ошибка загрузки «Failed to init device». Arduino IDE не смогла инициализировать STM32 по последовательному интерфейсу (stm32flash, 115200 8E1); смотреть видео автора про базовую загрузку.
Ошибки при запуске и в полёте (светодиод и телеметрия):
при запуске: 1 — MPU-6050 не отвечает (проверить I2C); 2 — HMC5883L не отвечает; 3 — MS5611 не отвечает; 4 — нет приёмника (первые 4 канала должны быть больше 1000 мкс; если в setup стабильно, а с батареей нет — плохая земля приёмника);
в полёте: 1 — низкое напряжение батареи (по умолчанию порог 10,5 В; напряжение не равно ёмкости, у плохих LiPo оно просаживается быстрее; низкое напряжение портит батарею и делает аппарат нестабильным, некоторые ESC снижают обороты — садитесь сразу); 2 — превышено время цикла (цикл фиксирован: 250 Гц, 4000 мкс, обычно виноват
Serial.print()в коде); 3 — превышен угол калибровки акселерометра (MPU-6050 установлен не плоско или вышел за заводские допуски, проверить в setup, при повторении заменить датчик); 4 — истёк таймер GPS (новая позиция каждые 200 мс = 5 Гц; если за 1000 мс нет, режим автоматически возвращается к удержанию высоты; нужно 8 и более спутников); 5 —manual_takeoff_throttleдолжен быть 1400–1600 или 0; 6 — взлёт не обнаружен (газ на ESC больше 1750, аппарат слишком тяжёл или нет винтов; проверить ручным значением 1500); 7 — газ взлёта вне диапазона 1400–1700.
Нестабильный полёт. Обычно решается подстройкой ПИД, но не всегда: причина может быть в неверной комбинации мотор–винт–батарея, сборке не по схеме или ESC для самолётов.
Не висит при стике газа в центре. При
manual_takeoff_throttle = 0автоопределение должно подстроить; можно скорректировать константу 1530 в проверке взлёта шагами по 10 или поставитьmanual_takeoff_throttleвручную (1400–1600).Другие радиомодули вместо APC220. Подойдут любые, если уровень на выводе B0 ограничен 3,3 В и скорость 9600 бит/с.
Какие винты к мотору. Большие перегружают мотор и ESC, маленькие снижают эффективность. Смотреть таблицы винтов производителей (автор ссылается на Cobra Motors).
Выключение в полёте. Чаще всего ошибка шины I2C: ненадёжная пайка, макетные платы и перемычки, плохие земли; ещё возможны помехи (отдельное видео автора) или неисправная деталь.
4S вместо 3S. Контроллер рассчитан только на 3S; для 4S нужно самому менять уровень предупреждения о батарее и компенсацию напряжения ESC.
11. Обновление: Return To Home и fail-safe
Автор рекомендует сначала проверить аппарат на исходной версии прошивки и только потом обновляться (архив YMFC-32_auto_rth.zip). Единственное обязательное изменение при переходе — правильная настройка канала 5. Возможности обновлённой версии: автовыравнивание, удержание высоты, удержание GPS с управлением позицией, возврат домой (RTH), fail-safe с возвратом, автопосадка и автовзлёт, блокировка курса, телеметрия.
Режимы по каналу 5: 1000–1200 мкс — режим 1 (автовыравнивание); 1200–1600 — режим 2 (удержание высоты); 1600–1950 — режим 3 (высота + GPS); 1950–2000 — режим 4 (Return To Home). Канал 6 (head lock) — 1200–2000. После настройки пульта режимы нужно проверить по телеметрии.
Автор показывает простую внутреннюю доработку Flysky T6: один канал и два переключателя дают все 4 режима (отдельное видео). Для Flysky i6S: в системе настройки пульта выставить центры и конечные точки через программу настройки YMFC-32 (все каналы должны иметь ход 1000–2000, первые четыре — центр 1500; использовать меню End points и Subtrim), назначить каналы и переключатели в меню Aux. channels и настроить первый микс в меню Mix. В меню fail-safe включить настройку, при которой при потере сигнала аппарат сам возвращается домой, если достаточно спутников. К светодиодам добавлена ошибка 8 после запуска: потеря связи с приёмником (fail-safe).
12. Программа настройки: что делает каждая функция
Подробный разбор автора (bain_YMFC-32_setup_explained.html, пример вывода каждого пункта есть на оригинале и в нашем архиве). Программа нужна для проверки датчиков, балансировки винтов и калибровки регуляторов. Выход из любого пункта — отправка q.
a — сигналы приёмника. Для настройки пульта: каналы 1–4 должны давать минимум 1000, центр 1500, максимум 2000 мкс (допуск ±5). Строка вывода показывает статус старта (S), крен (R), тангаж (P), газ (T), рыскание (Y), каналы 5 и 6; символы ^^^, vvv, <<<, >>> показывают направление стика, -+- — центр (от пилота, к пилоту, влево, вправо). Если стик двигает значение в обратную сторону, канал нужно инвертировать в пульте. Канал 5: 1000–1200 режим 1, 1200–1600 режим 2, 1600–2000 режим 3; канал 6: 1000–1200 обычное управление, 1200–2000 head lock. Старт проверяется движением газа вниз-влево и возвратом в центр (статус старта станет 2), останов — газ вниз-вправо.
b — сканер I2C. Перебирает адреса 1–127 и печатает те, кто ответил. Должны быть: 0x1E — компас HMC5883L, 0x68 — MPU-6050, 0x77 — барометр MS5611. Если чего-то нет — проверять проводку и исправность.
c — сырые значения гироскопа. Показывает значения после калибровки: берётся 2000 отсчётов, среднее вычитается как смещение. Неподвижный гироскоп должен давать 0 (±10). Показываются и сами значения калибровки; по даташиту MPU-6050 максимум смещения ±1310. Если везде нули — проверить подключение.
d — сырые значения акселерометра. Аппарат должен стоять ровно. Для X и Y значения не более ±500, Z около 4700 (около 1 g). Нули — проверить подключение.
e — углы IMU. Те же расчёты, что в полётной программе; акселерометр здесь ещё не откалиброван, поэтому на ровном месте углы не нулевые (калибровка делается в полётной программе). Проверить: нос вверх = положительный тангаж, нос вниз = отрицательный; левое крыло вниз = отрицательный крен, вверх = положительный. Если направление перевёрнуто, проверить ориентацию гироскопа. Также печатается температура.
f — светодиоды. Поочерёдно включает светодиоды на выводах B3 и B4 STM32 (красный и зелёный) на 3 секунды каждый.
g — напряжение батареи. Подать +5 В на провод делителя R3/R4; на мониторе должно быть около 5 В (в примере автора 4,9 В).
h — барометр. Печатает 6 калибровочных констант C1–C6 и давление; при подъёме давление должно падать, при опускании расти. На улице при ветре показания заметно колеблются.
i — GPS. Имитирует настройку u-blox и выводит сырые NMEA-строки: сначала на 9600 бод при 1 Гц, после настройки должно быть 57,6 кбит/с, 5 Гц, строки GSV отключены. Если вторая часть (57600 бод / 5 Гц) не отображается, модуль не принимает протокол u-blox и с YMFC-32 не подойдёт.
j — компас HMC5883L. При запуске выполняет самотест (подаёт малый положительный и отрицательный ток около магниторезистивных датчиков, должны получиться положительные и отрицательные показания), затем печатает значения осей X, Y, Z.
Видео автора (YouTube)
Видео проекта на канале автора MacPuffdog: плеер ведёт на оригинал, ролики лежат у автора.
Фото на сайте автора
Фотографии и рисунки к проекту лежат на сервере автора (у сайта нет HTTPS, поэтому здесь только ссылки, они открываются в новой вкладке). Копия всех файлов хранится у нас в архиве.
