Сборка устройства и код для получения данных

 

Как было сказано выше, беспилотный дрон управляется четырьмя основными каналами - газ, рыскание, крен и тангаж. Полетный контроллер принимает на своих входах значения каждого канала как целое число размером 1 байт (от 0 до 255 для газа и от -127 до 127 для рыскания, крена и тангажа). Значит первая цель - получить эти значения как переменные в микроконтроллере. В скетче можно сразу объявить эти переменные и назвать их throttle, yaw, pitch и roll. В этих переменных будут хранится необработанные показатели датчиков. Для данных, прошедших обработку и готовых к отправке, объявим еще четыре переменных: sendThrottle, sendYaw, sendPitch, sendRoll.

Для получения значения газа будет использоваться ультразвуковой дальномер HC-SR04. Необходимо подключить его к выводам 5V и GND для обеспечения питания и два логических вывода соединить с любыми цифровыми выводами Arduino. Подсоединим вывод trig к цифровому выводу с номером 8 и определим его в программе как TRIGGER_PIN, а echo - к выводу 9 и определим, как ECHO_PIN. Для работы с дальномером будет использоваться библиотека NewPing.h. После ее импортирования следует ввести команду NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE), где MAX_DISTANCEустановим как 200 - это максимальное измеряемое значение в сантиметрах. Эта команда создает объект sonar, методами которого можно получить расстояние, измеряемое дальномером. Теперь в функции loop можно присвоить переменной throttle значение sonar.ping и ее значение будет пропорционально расстоянию до препятствия. В этом можно убедиться, если вывести значение переменной в монитор серийного порта. В конце функции loop также необходимо ввести задержку в 5мс для того чтобы периоды опроса не «наслаивались» друг на друга и не возникало ошибочных значений.

В массиве представленных значений присутствует достаточно сильный шум, который может негативно повлиять на результаты их анализа. Необходимо включить в программу фильтрацию для переменной throttle. Имеет смысл в данной работе использовать упрощенный фильтр Калмана. Принцип его работы приведен выше. Функция фильтра принимает два аргумента - стандартное отклонение величины от математического ожидания и скорость реакции на изменение. Первый аргумент считается опытным путем - для этого нужно создать массив данных достаточного размера на выходе датчика без изменения внешних условий, а затем найти стандартное отклонение по генеральной совокупности. Можно модифицировать программу для выполнения этого действия, но удобнее выполнить его в программе MicrosoftExcel с помощью функции СТАНДОТКЛОН.Г. Из выборки около 750 элементов получаем стандартное отклонение 21,47. Запишем это значение при объявлении констант в начале кода. Подбор скорости реакции на изменение производится вручную, эмпирическим путем. Нужно выставить значение достаточно малое, чтобы дрон быстро воспринимал команду изменения газа и достаточно большую, чтобы отфильтровать лишние значения. Для тестирования добавим в программу функцию с алгоритмом упрощенного фильтра Калмана, которая принимает значение и возвращает его отфильтрованным. Сразу после получения переменной throttle в функции loop вызовем фильтрующую функцию, в качестве аргумента которой будет значение throttle и значение которой будет снова присваиваться переменной throttle. Далее эмпирическим путем определено, что оптимальный коэффициент для данной задачи - это число 0,02.

Теперь произведем аналогичные действия для остальных трех каналов, а именно считаем и отфильтруем данные с датчиков. Показания рыскания, крена и тангажа будут считываться с инерционного измерительного устройства, который имеет в составе гироскоп, акселерометр, барометр и компас. Оно так же нуждается в питании с напряжением 5В и имеет два логических выхода. Передача данных осуществляется по протоколу I2C, который поддерживается Arduino, поэтому для передачи большого количества данных достаточно двух проводов. Подсоединим устройство к I2C-совместимым портам ArduinoMega, а именно 20 и 21. Они помечены метками SDA и SCL.

Для работы с инерционным измерительным устройством необходимо импортировать в скетч две библиотеки - Wire.h для работы с протоколом I2C и TroykaIMU.h для работы с самим устройством. Библиотека написана по принципам объектно-ориентированного программирования, поэтому для дальнейшей работы, вне функций необходимо создать по объекту для каждого использованного модуля, а именно объекты классов Accelerometer, Gyroscope и Compass. Все полученные величины будут проходить через фильтр Магвика, поэтому для него тоже создать объект класса Magwick. Дадим ему имя filter, а объектам для акселерометра, гироскопа и компаса - accel, gyro и compass соответсвенно.

Далее объявим переменные для данных с акселерометра, гироскопа и компаса - каждый из них ориентируется в трехмерном пространстве и поэтому выдает контроллеру по три значения за раз. Назовем их ax, ay, az, gx, gy, gz, mx, my, mz соответственно. Также необходимо объявить переменную fps, которая будет хранить частоту выборок фильтра. В конце каждого цикла она будет обновляться, но для первого цикла ей при инициализации нужно присвоить значение, например, 100. Далее инициализируем массивы постоянных величин, полученные в калибровочной матрице из примеров на сайте производителя [12]. Это наборы констант для калибровки компаса и compassCalibrationBias представляет собой одномерный массив из трех элементов, а compassCalibrationMatrix - двухмерный массив из девяти элементов.

В функции setup необходимо произвести инициализацию всех элементов инерционного измерительного устройства и откалибровать компас. В начале функции выведем в монитор порта сообщение «Begin init». Затем нужно для объектов accel, gyro и compass запустить метод.begin. Для объекта compass также запускаем метод calibrateMatrix с аргументами compassCalibrationBias и compassCalibrationMatrix, он откалибрует компас для данного запуска устройства. В конце функции выводим в монитор порта сообщение об удачной инициализации, его можно будет увидеть только если все методы завершили свою работу без ошибок.

В функции loop нужно сначала вычислить частоту обработки фильтра, fps. Для этого в функции, после всех команд, связанных с ультразвуковым дальномером нужно записать в переменную startMillis значение времени, прошедшего с начала работы контроллера с помощью функции millis, которая возвращает это время. В конце функции, но перед командой задержки delay нужно сначала вычислить затраченное время на обработку данных, millis - startMillis и записать его в новую переменную, deltaMillis. Затем вычисляем частоту обработки фильтра в герцах, для этого нужно величину, обратную deltaMillis домножить на 1000, так как время в переменной deltaMillis измеряется в миллисекундах. Между присваиванием startMillis и вычислением fps находится весь остальной код, обрабатывающий информацию с инерционного измерительного устройства.

Начинается он со считывания данных с датчиков. Метод readGXYZ объекта accel считывает данные с акселерометра в единицах G и записывает их в переменные ax, ay, az с помощью указателей. Метод readRadPerSecXYZ объекта gyro считывает данные с гироскопа в радианах в секунду и записывает их в переменные gx, gy, gz. Метод readCalibrateGaussXYZ объекта compass считывает данные с компаса в Гауссах и записывает их в переменные mx, my, mz.

После того, как данные получены, библиотека позволяет поместить их в объект фильтра и извлекать из него конкретные значения рыскания, крена и тангажа. Для начала нужно обновить коэффициенты фильтра - это делается с помощью метода setKoeff соответствующего объекта. Метод принимает два аргумента - вычисленное в предыдущем цикле значение fps и числовой коэффициент BETA, в примерах производителя равный 0,22. При необходимости его можно будет изменять эмпирическим путем, вне функций запишем это число в константу BETA с помощью команды #define.

Далее с помощью метода update обновляем входные данные в фильтр. Аргументами служат значения g* a* и z*, полученные ранее. Теперь из объекта фильтра методами getYawDeg, getPitchDeg и getRollDeg можно получить значения рыскания, тангажа и крена и записать их в соответствующие переменные yaw, pitch и deg. После вывода их в монитор серийного порта, можно наблюдать следующее.

 

Рис. 14. Данные из монитора порта

 

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

Для исправления обеих вышеописанных проблем добавим к схеме кнопку запуска. Она должна замыкать вывод питания 5В и один из логических выводов. Объявим номер ее вывода как константу с именем BUTTON_PIN и функцией pinMode зададим выводу значение INPUT_PULLUP, то есть вход с включением подтягивающего резистора, встроенного в плату Arduino. Создадим функцию, в которую включены различные типы отработки нажатий кнопок. В данном коде будут использоваться только одинарные нажатия с применением защиты против дребезга контактов, но функция поддерживает так же двойные нажатия, нажатия с удержанием и т.д. Все эти режимы могут пригодиться при дальнейшей разработке программы. Присвоим одной из переменных, button1S значение digitalRead (BUTTON_PIN), а затем вызовем функцию обработки кнопок. После этого нужно поставить условие, было ли выполнено одинарное нажатие, то есть является ли выражение button1P истиной. Если да, то этой переменной следует задать значение false и выполнить необходимые действия, а именно «перевернуть» флаг check (то есть присвоить значение true переменной check, если до этого она имела значение false и наоборот) и задать переменной yawCenter значение yaw. Это нулевой отсчет рыскания, теперь в программе ориентация будет приходиться не на север, а на градус, записанный в переменную yawCenter при нажатии кнопки. Также в блоке, в котором происходит печать значений в монитор порта, добавим условие: при значении check равному false пусть в начале каждой строки печатается восклицательный знак - это будет символизировать то, что устройство находится в режиме ожидания и не передает данные.

 

4.2 Преобразование данных

 

Приступим к преобразованию данных. Как уже говорилось выше, каждое значение должно занимать один байт и газ должен принимать значения от 0 до 255, рыскание, тангаж и крен - от -127 до 127. Причем канал тангажа должен быть инвертирован, то есть при наклоне вперед выдавать отрицательное значение, а при наклоне назад - положительное.

Начнем с преобразования рыскания. Как оговаривалось ранее, его значение центрируется, то есть задается определенный угол yawCenter, который считается нулевым. Значит для получения конечного результата следует пользоваться не углом yaw, а углом yaw - yawCenter, он равен нулю если датчик находится в положении, при котором была нажата кнопка пуска и отклоняется от нуля при изменении этого положения. Нас интересует изменение положения максимум на 40 градусов в каждую сторону, на больший угол повернуть руку трудно физически. Экспериментально выявлено, что значение угла является положительным при повороте влево и отрицательным при повороте вправо. Значит нужно составить такую функцию sendYaw (yaw - yawCenter), чтобы

·   При yaw - yawCenter = 0, sendYaw = 0;

·   При yaw - yawCenter = 40, sendYaw = -127;

·   При yaw - yawCenter = -40, sendYaw = 127;

Решив простую систему уравнений, выясняем, что такой функцией является sendYaw = -3,175 (yaw - yawCenter).

Теперь данные изменяются в нужном виде, но значения могут быть больше 127 и меньше -127. Можно исправить это простыми условиями: если sendYaw> 127, значит sendYaw нужно принудительно присвоить значение 127. Если же sendYaw< -127, значит sendYaw нужно принудительно присвоить значение -127.тангажом и креном необходимо произвести аналогичные преобразования, учитывая характер показаний с датчиков.

Для тангажа:

·   При pitch = 180 или -180, sendPitch = 0;

·   При pitch = 140, sendPitch = -127;

·   При pitch = -140, sendPitch = 127;

Получившееся уравнение:= 3,175 * pitch - 571,5, если -180 <= pitch < -140;

sendPitch = 3,175 * pitch + 571,5, если 140 < pitch <= 180;= 127, если 0 > pitch >= -140;

sendPitch = -127, если 0 <= pitch <= 140;

Для крена:

·   При roll = 180 или -180, sendRoll = 0;

·   При roll = -140, sendRoll = -127;

·   При roll = 140, sendRoll = 127;

Получившееся уравнение:

sendRoll= -3,175 * roll - 571,5, если -180 <= roll < -140;= -3,175 * roll + 571,5, если 140 < roll <= 180;= 127, если 0 < roll =< 140;= -127, если 0 >= roll >= -140;

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

·   При throttle = 400, sendThrottle = 0;

·   При throttle = 2000, sendThrottle = 255;

Тогда,= 0.159375 * throttle - 63,75, если 400 < throttle < 2000;= 0, если throttle <= 400; = 255, еслиthrottle>= 2000;

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