Как мы верифицированный полетный контроллер для квадрокоптера написали. На Ada

Однажды на новогодних каникулах, лениво листая интернет, бракоделы в нашем* R&D офисе заметили видео с испытаний прототипа роботакси. Комментатор отзывался восторженным тоном – революция, как-никак. Здорово, да, но время сейчас такое – кругом революции, и ИТ их возглавляет.

Но тренированное ухо расслышало в шуме с испытательной площадки еще кое-что. Контроллер скорости (штука для управления тягой винтов) сыграл мелодию при старте, как это любят делать пилоты дронов, которые часто используют полётный контроллер Betaflight. Неужели там бета-флайт? Похоже, что да. Ну, или какая-то из ее разновидностей — открытых полетных контроллеров всего около полдюжины.

Перед глазами побежали флешбеки, где-то из глубин подсознания всплыла забытая уже информация о прошивках для Тойоты на миллионы тысяч строк Си и 2 тысячи глобальных переменных (Toyota: 81564 ошибки в коде).

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

Но ведь можно иначе? Можно, решили мы!

И решили это доказать. На Avito был куплен акробатический FPV-“квадрик” на базе STM32F405, для отладки — Discovery-плата для этого же контроллера, а дальше все как в тумане..

Так как же быть иначе?

После быстрого совещания возникли вот такие мысли:

• нам нужен другой подход

• язык и подход должны друг друга дополнять

• академический подход не подойдет, нужны практические применения.

В качестве нового подхода решили, что лучше всего опираться на возможность возможность верификации ПО — до необходимого уровня, без злоупотреблений. Но для языка типа С доступных промышленных зрелых решений не существует, только прототипы [FC] и рекомендации.

При выборе языка мы поставили себе вот какими требования:

• это должно быть что-то близкое к embedded

• Нам нужен хороший богатый runtime с возможностями RTOS, но при этом брать и интегрировать RTOS не хочется

• Он не должен заметно уступать в производительности тому, что используется сейчас.

Оказалось, что из практических инструментов в эти требования хорошо подходит один очень старый, незаслуженно забытый инструмент. Да, это Ada. А точнее, его модерновое, регулярно обновляемое ядро SPARK. В [SRM] описаны основные отличия SPARK от Ada, их не так много.

Что такое SPARK, будет ясно дальше, мы покажем, как именно оно было применено, почему Ада понравилась больше, чем С, как работает прувер, и почему мы при этом ничего не потеряли, а только приобрели. И почему мы не взяли Rust.

Иной подход

Иной подход это верификация ПО. Обычно при этих словах люди начинают думать об абстрактных монадах, академический манускриптах, докладах на конференциях и пыльных трудах института системного программирования РАН, которые как будто бы от жизни отстают лет на 30, а то и 50. Но оказалось, что не все так плохо.

Прежде всего, верификация НЕ является гарантией того, что программа не содержит ошибок, а является только проверкой, что программа гарантирует некоторые свойства. А уже дело программиста таким образом обеспечить контроль свойств, чтобы получить нужные результаты.

В случае с SPARK, верификация базово предоставляет нам гарантии:

• отсутствия переполнения массивов и переменных

• отсутствия выхода за границы в типах и диапазонах

• отсутствия разыменования null-указателей

• отсутствие выброса исключений.

• гарантию неприменения инструментов, проверку которых выполнить нельзя.

• гарантию выполнения всех инвариантов, которые мы опишем. А опишем мы много!

  Круто, да?

Для описания инвариантов в языке SPARK предусмотрены специальные расширяющие конструкции языка, описывающие контракты процедур, структур данных, и даже циклов. Контрактом можно указать, например, что данная функция не может обращаться к глобальным переменным, или модифицировать глобальное состояние. 

SPARK также учитывает ограничения на типы, которые описаны в Ada. В случае обычного исполнения ошибка несоответствия типов упадет в Runtime; SPARK же позволяет статически доказать, что ограничения на типы не могут быть нарушены никаким потоком исполнения.

Например:

Или другой пример:

Компилятор и верификатор не дадут создать такой код, который приведет к присвоению значений, нарушающих ограничения и предикаты.

Отдельный плюс SPARK в том, что система позволяет “натягивать” гарантии на программу поэтапно, то есть программа может быть частично верифицированной. То есть часть модулей можно объявить верифицируемыми, а часть — (пока) нет.

Сам SPARK делит верификацию на уровни: от «каменного» (Stone level) через «Бронзовый» и «Серебряный» уровни до «Золотого» (Gold) и «Платинового». Каждый из уровней усиливает гарантии:

Мы остановились на уровне Gold, потому что наш квадрокоптер все-таки не Boing 777 MAX. 

Как работает верификация в SPARK: прувер собирает описание контрактов и типов, на их основе генерирует правила и ограничения, и далее передает их в солвер (SMT — Z3), который проверяет выполнимость ограничений. Результат решения прувер привязывает к конкретным строкам, в которых возникает невыполнимость.

Более подробно можно почитать в [SUG]

Иной язык

Несмотря на то, что сейчас «рулят» си-подобные ECMA-языки, мы нормально отнеслись к тому, что от этого придется отказаться. Более того, кажется, что чем больше программа, тем больше вредит укорочение ключевых слов и конструкций. Что касается Rust, то он — субъективно — в отношении минимализма издалека сильно напоминает Perl, к сожалению.

И наоборот, по ощущениям, пространные, многобуквенные конструкции раздражают, когда разум летит вперед, но не мешают, когда во главу угла ставится надежность, понятность, и легкость чтения. В этом смысле Ada (а SPARK это подмножество Ada) вполне хорош. Теперь давайте посмотрим, что же язык Ada может дать embedded-разработчику.

 Профили

Сам язык и стандартная библиотека позволяет определять и использовать так называемые «профили». Профиль это набор ограничений, выполнение которых контролирует компилятор. Например, можно определить ограничение «нельзя использовать динамическую память». Или «нельзя бросать исключения». Или «не более двух активных потоков». Или «нельзя использовать объекты синхронизации». Все это помогает контролировать целостность состояния программы, и ее валидность.

Мы используем профиль Ravenscar, специально для embedded-разработки. Он включает пару дюжин ограничений, которые делают вашу разработку для микроконтроллеров более удобной и верифицируемой: нельзя на ходу переназначать приоритеты задач-потоков, переключать обработчики прерываний, сложные объекты из stdlib-ы и такое.

Вот ограничения профиля Ravescar, для примера

 Runtime

Когда в embedded необходимо создать более-менее сложное приложение, там всегда заводится RTOS, и ее выбор и интеграция это отдельная песня. В случае с Ada с этим больше нет сложностей — сама Ada включает в себя минимальную исполняемую среду с вытесняющим планировщиком потоков (в Ada это tasks, задачи), с интегрированными в сам язык средствами синхронизации (семафоры, рандеву, называются entries) и даже средствами избегания дедлоков и инверсии приоритетов. Это оказалось очень удобно для квадрокоптера, как станет понятно ниже.

Для embedded-разработчика доступны на выбор также разные рантаймы:

• zero-footprint — с минимальными ограничениями и даже без многопоточности; зато минимальная программа не превышает пары килобайт, влезает даже в TO MSP430

• small footprint — доступна большая часть функций Ada, но и требования побольше, несколько десятков килобайт RAM

• full ravenscar — доступны все функции в рамках профиля Ravenscar/Extended Ravenscar

Вот пример описания пустой задачи

Хочется обратить внимание, что эти задачи — это по сути легковесные green threads, так как под ними нет никаких настоящих потоков не существует. Поэтому мы не страдаем от отсутствия корутин, ведь задачи не тяжелее них, зато встроены в язык.

Кроме этого, в Ada есть достаточно мощная stdlib для ядра STM32, включая полную реализацию рантайма. Возможно, и для вашей архитектуры она тоже есть.

Почему не “rustRustRUST”!

Когда мы говорим, про гарантии в языках программирования, сразу вспоминается Rust и его гарантии относительно указателей. Почему тут не он? Нам кажется, что Spark мощнее.

Ada не очень любит указатели — там они называются access types, и в большинстве случаев там они не нужны, но если нужны, то — в Spark также есть проверки владения, как в Rust. Если вы аллоцировали объект по указателю, то простое копирование указателя означает передачу владения (которую проконтролирует компилятор/верификатор), а передачу во временное владение (или доступ на чтение) верификатор также понимает и контролирует.

В общем, концепция владения объектом по указателю, и уровень доступа через этот указатель — есть не только в Rust, и его преимуществами можно пользоваться и в других инструментах, в частности, в Ada/SPARK. Подробно можно почитать в [UPS]

Вот пример кода с владением

Почему мы пишем, что в Ada/SPARK не нужны указатели? В отличие от Си, где базовым инструментом является указатель (хочешь ссылку — вот указатель, хочешь адрес в памяти — вот указатель, хочешь массив — вот указатель — ну вы поняли?), в Ada для всего этого есть строгий тип. Если не хватает встроенных операций, их допустимо переопределять (например, реализовать инлайновый автоинкремент), аналогично можно создать placement constructor, используя т.н. limited-типы — типы, которые компилятор запрещает копировать.

Если уже и этого мало, есть интероп с СИ – то есть код можно компилировать совместно, и слинковать на этапе сборки. Но в этом случае гарантии поведения модуля как черного ящика остаются на разработчике. Для интеропа используются атрибуты — вот так, например, мы оборачиваем функцию на Си в доступ для Ada.

Для соблюдения нужного layout или битности в коде также не нужны указатели: Ада при необходимости позволяет детально описать, как именно структура будет располагаться в памяти. Минус ошибки на конвертации из логического в физическое представления и обратно — прощайте битовые сдвиги, сложения на кольце, арифметика указателей.

IDE

Для работы доступна вполне приятная и удобная IDE, но всегда можно использовать и VSCode с плагинами, и другие текстовые редакторы.

О производительности и надежности

Вполне валидным аргументом может быть вопрос с эффективностью ПО. Что касается эффективности, то в интернете доступно свежее исследование [EFF], из которого хочется привести табличку, показывающую, что “старичок» Ada еще огого:

Если говорить о надежности, то SPARK/Ada известен как один из языков с наименьшим количеством ошибок. В планируемом на 21 запуске кубсатов [LIC] полетное ПО планируется реализовывать на Ada, предыдущий спутник BasiLEO тоже на Ada был единственным среди 12, кому удалось приступить к планируемой миссии.

А теперь — о самом полетном контроллере

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

Структурная схема управляющего ПО показана на рисунке

Как видно из рисунка, ПО состоит из двух частей:

• Veriflight — собственно, верифицированный полетный контроллер с алгоритмами.

• Veriflight_board — библиотеки поддержи и адаптации для конкретной платы, которая оказалась у нас в наличии — не верифицированная. Но она и не содержит особой логики, кроме управления устройствами микроконтроллера.

Так как тратить много времени не хотелось, то драйвер для USB в STM32 был взят прямо нативный и при помощи Interop был слинкован с оберткой на Ada.

Плата оказалась оснащена минимальным количеством периферийных устройств: 

• STM32F405 микроконтроллер на 168 МГц (192кб RAM, 1Mб flash)

• трансивером S.BUS на USART1

• 6-осевым гиро-акселерометром без магнитного компаса

• токовыми усилителями PWM

• USB-интерфейсом, PHY-часть которого реализована на самом микроконтроллере платы.

Полетный контроллер реализован по простой схеме и «крутит» 2 цикла:

1. внешний

2. внутренний

Внешний цикл это цикл опроса периферии (CMD task на рисунке) в ожидании команд с радиопередатчика. Если команды нет, он передает признаки «сохраняем высоту, держим горизонт». Если команда с пульта есть, передаем ее — целевой угол наклона, целевую мощность на пропеллеры. Частота внешнего цикла 20 Гц.

Внутренний цикл — цикл опроса гиро-акселерометра и распределения мощности на двигатели. Цикл оборудован 3 PID-регуляторами, и математикой Махони для расчета текущего положения по сигналам с гироскопов. В расчетах внутри используем кватернионы, для генерации управляющего сигнала — углы Эйлера. Частота размыкания внутреннего цикла — 200 Гц. Да, Ада без проблем успевает диспетчеризировать с такой скоростью.

Так как у нас нет ни баровысотомера, и другого способа измерить высоту, то для грубого удержания высоты используем интеграцию вертикальной скорости.

Внутренний цикл реализует опрос PID и стабилизацию аппарата:

• Считали затребованные пилотом углы

• Запросили у математики расчетные углы положения

• Нашли расхождение между желаемыми и настоящими

• Пересчитали текущее положение на основании сигналов с гиро-акселерометров

• Зарядили PID-регуляторы на новую коррекцию, если пришли новые затребованные углы

• Запросили у PID-пакетов текущие импульсы коррекции

• На основании них, а также запрошенной пилотом мощности на двигатели, сформировали необходимое распределение скоростей вращения на двигателях

Забавно, что большинство опен-сорсных реализаций Махони (для Arduino и не только) — на Cи и Wiring оказались содержащими разнообразные баги. Это мешало системе заработать. После того, как было выпито пол-ящика лимонада и съедена корзина круассанов, алгоритм воссоздали с нуля по описанию из [MHN], и система тут же заработала.

Данная схема, как и любая упрощенная модель, испытывает сложности при приближениях параметров к предельным. Здесь это 90 по крену и тангажу — при их превышении для безопасности реализовано отключение двигателей (disarm).

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

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

Статья получилась неожиданно длинная, поэтому придется разбить ее на две части.  Мы в первой части постарались познакомить вас с инструментами предков, которые на удивление неплохи: по крайней мере, мы сделали выводы, что дальнейшие проекты для эмбеддед хочется делать на Ada, а не на Си.

Итог на текущий момент

Квадрокоптер с прошивкой на Ada/SPARK прошел тесты на подпружиненном стенде и полетах в закрытом помещении, собираются логи, схема стабилизации работает в соответствии с ожиданиями, но в рамках ограничений на углы маневров, как описано выше.

Мы в R&D продолжаем познавать верификацию ПО и с нетерпением ждем схода снега в Москве, чтобы в следующей статье поделиться результатами испытаний, подгонки и видео тестовых полётов ПО в уличных условиях, которое, кажется, статически проверено на то, что не содержит ошибок. 

И конечно, на время испытательных полетов все runtime-проверки все равно останутся включены, хотя конечный итоговый результат — проверки не нужны, так как заведомо известно, что они выполняются.

Для себя мы сделали вывод, что для embedded будем стараться писать только на Ada.

Если вы также считаете, что современная robotics и automotive это слишком важные вещи, чтобы позволить себе переполнения буфера и разыменование нуля и «ой программисты не написали тесты», пишите, комментируйте, присоединяйтесь: пора сделать ПО надёжнее, потому что оно вокруг нас везде.

Литература для дальнейшего изучения

[SUG] SPARK user guide https://docs.adacore.com/spark2014-docs/html/ug/index.html

[SRM] SPARK reference manual (https://docs.adacore.com/live/wave/spark2014/html/spark2014_rm/index.html)

[FC] Frama-C — платформа для модульного анализа кода С https://frama-c.com/ 

[UPS] https://blog.adacore.com/using-pointers-in-spark

[MHN] https://nitinjsanket.github.io/tutorials/attitudeest/mahony 

[EFF] https://greenlab.di.uminho.pt/wp-content/uploads/2017/10/sleFinal.pdf

[LIC] https://en.wikipedia.org/wiki/Lunar_IceCube