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

Как я указал выше, полезность кренодифферентометра для корабля очевидна. А это значит, что крендодифферентометры (ну или только кренометры) давным-давно выпускаются. Причём, как механические, так и электронные.


Механический кренометр.


Электронный кренодифферентометр.

Я, по понятным причинам, буду делать прототип электронного кренодифферентометра.

Для построения электронного кренодифферентометра нам в общем случае нужно знать крен и дифферент корабля в текущий момент времени. Хотелось бы, конечно, использовать для этого обычные механические гироскопы с электронным устройством списывания угла, но стоимость такого устройства окажется весьма и весьма большой. Между тем, стоимость электронного кренометра Delta 401 (на картинке выше его блок индикации) со всей необходимой сертификацией составляла (сколько сейчас составляет, я не знаю) около 350 тыс. рублей. Это слишком мало для кренометра с механическими гироскопами. Да и габариты датчика у этого кренометра очень небольшие. Так что же там внутри могло бы быть? Вариант купить это устройство и посмотреть, увы, отпадает – мне будет сильно жалко 350 тыс. рублей. Тем не менее, мы можем предположить, что внутри датчика как минимум установлены микромеханические акселерометры и гироскопы. Так это или нет, я сказать не берусь, но свой вариант кренометра я решил попробовать построить именно на таких штуках.

Из широкодоступных (и по цене и по наличию) любителю на алиэкспресс присутствует модуль GY-521 на известной микросхеме MPU6050. Микросхема эта представляет собой сборку трёх микромеханических гироскопов и акселерометров с дополнительным термодатчиком.

Параметры микросхемы MPU6050 следующие:

• Диапазоны работы акселерометра: ±2g, ±4g, ±6g, ±8g, ±16g;
• Диапазоны работы для гироскопа: ±250°/с, ±500°/с, ±1000°/с, ±2000°/с.

Подключается эта микросхема по шине I2C.


Модуль GY-521 с MPU6050.

Для кренодифферентометра я выбрал диапазон работы акселерометра как ±2g, а гироскопа ±250°/с. Думаю, этого вполне будет достаточно для относительно медленно качающегося корабля.

Микромеханический гироскоп имеет заметный дрейф и без акселерометра не позволит точно измерять угол, поэтому чтобы на основании трёх акселерометров и гироскопов получить углы крена и дифферента, потребуется каким-то образом выполнять совмещение показаний акселерометров и гироскопов. Иными словами, нам нужен какой-то алгоритм выработки угловой информации. Такие алгоритмы, естественно, существуют. Например, AB-фильтр, фильтры Махони, Маджвика или Калмана. Для своего устройства я взял фильтр Маджвика, благо существует готовый пример простой программы тут. Я не скажу, что понимаю, как именно работает этот фильтр, но, к счастью, приведённая по ссылке программа фильтра этого и не требует. Перед подачей показаний акселерометров на фильтр их стоит откалибровать (узнать смещение ноля и масштабные коэффициенты), например, по данной методике. Испытания на качающемся стенде показали, что после калибровки акселерометров фильтр даёт корректные углы крена и дифферента.

Результатом работы фильтра будут два необходимых нам угла. Почему два, а не три? А всё дело в том, что третий угол (рысканье) нельзя получить только по акселерометрам и гироскопам – у фильтра нет вектора, по которому он мог бы корректировать курс. Для крена и дифферента такой вектор даётся акселерометром, показывающим направление действия силы тяжести в моменты, когда движения с ускорением не происходит. А вот для рысканья акселерометры никакой информации не дают, поэтому без датчика курса фильтр рысканье выдать не может. Таким датчиком можно взять магнитный компас, но в MPU6050 его нет (но он есть в других моделях серии MPU). Так как рысканье мне не требуется, то я с магнитным компасом не заморачивался.


Углы сразу после фильтра Маджвика.

Так как корабль не должен качаться чаще, чем с частотой 1 Гц, полученные углы отправляются на цифровой ФНЧ с частотой среза 1 Гц. И вот уже углы после ФНЧ используются в определении параметров колебаний по следующему алгоритму.

Имея два угла, просто ищем для каждого верхние максимумы и нижние минимумы и время их нахождения. При этом я считаю, что корабль будет при качке переходить через ноль по углам (но я могу и ошибаться — для этого случая я делал и вариант с учётом колебаний относительно вычисленного текущего статического значения угла), что позволяет мне, находясь в нижней половине по углу (угол отрицательный), обновлять текущий минимум, сбросив текущий максимум (он в другой половине по углу) в ноль (как минимальное значение максимума), а, находясь в верхней половине (угол положительный), обновлять максимум, сбросив текущий минимум в ноль. Возможно, звучит путано, но, на самом деле, всё очень просто. Таким образом, половины чередуются, и в момент перехода через ноль (с некоторым гистерезисом в 0.5 градуса) достигнутые максимум/минимум обновляют запомненные значения максимумов и минимумов и время, когда они были найдены. Полусумма этих запомненных значений даёт статический угол, полуразность даёт амплитудное значение, а интервалы времени (для максимумов свой, для минимумов, соответственно, свой) период колебаний. Может случиться ситуация, когда корабль вдруг перестал колебаться через ноль. В этом случае спустя 50 секунд показания периода, амплитуды и статического угла обнуляются.


Определение параметров углов.

Для построения прототипа кренодифферентометра я взял палату STM32F407 Discovery, подключил к ней по I2C модуль GY-521 и дисплей с разрешением 320x240.

Выглядит дисплей вот так:


А работает всё это вот так:

Видео работы прототипа кренодифферентометра.

Исходный код программы прототипа кренодифферентометра.

В целом, мой прототип оказался вполне работоспособным, хотя реальные испытания на корабле я не проводил за отсутствием у меня в наличии корабля. Также непонятно, сможет ли фильтр Маджвика правильно выдавать углы при реальном движении судна (ведь акселерометры могут всё время вносить постоянное смещение).

К сожалению, я смог придумать только такой вот алгоритм работы кренодифферентометра. Для определения параметров колебаний я попробовал также использовать БПФ, но результат оказался совсем неудовлетворительный. Если кто знает, как ещё можно получить углы и параметры колебаний, буду рад, если вы мне подкинете идею.