Опыты с акселерометром и гироскопом на платформе S7V30 / Хабр

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

Предыдущие статьи по теме модуля S7V30

Открытый проект Wi‑Fi логгера по USB, CAN, RS-485, RS-232 — здесь первая публикация по модулю предшественнику нашего
Открытый проект встраиваемого модуля для IoT — схема и конструкция первой версии модуля. В текущей версии добавился только чип SPI Flash
Разработка универсального ПЛК для домашних агрегатов — здесь про то, где наш модуль применяется
Как сделать быстрый Wi‑Fi для ПЛК — здесь о том, как мы разогнали Wi‑Fi в нашем модуле
Архитектура, планирование и реализация DMA в микроконтроллерах Synergy — тут мы погружаемся в технологию быстрой пересылки данных внутри микроконтроллеров.
Делаем начальный загрузчик с криптографической защитой для Synergy на базе Azure RTOS — здесь описана первая версия нашего универсального и защищёного загрузчика.
Установка пароля на SD карту с помощью веб‑приложения на jQuery, веб‑сервера Azure RTOS и Synergy — а здесь мы нашли нетривиальное приложение для нашего модуля. С помощью него можно залочивать sd‑карты. Почти хакерский инструмент.
Открытый проект Wi‑Fi Bluetooth микророутера. Разворачиваем на модуле все необходимыме стеки чтобы по настоящему интегрировать его в IoT.

На плате модуля установлен чип ISM330DLC от STMicroelectronics — iNEMO 6-axis IMU, который совмещает трёхосный акселерометр и гироскоп, обеспечивая цифровой вывод данных для промышленных приложений.

Подключение и расположение

При работе с такими чипами важно знать его ориентацию на плате, от этого зависит корректность определения поворотов и векторов движения.

Чип подключён по шине I2C с частотой до 400 кГц, и на этой же шине работают чип зарядно��о устройства и измеритель уровня заряда. Это накладывает ограничения на пропускную способность шины: например, максимальная частота выборок по трём осям гироскопа и акселерометра составляет 1666 Гц — иначе шина просто не справится с потоком данных.

Получение и анализ данных

Для анализа удобнее всего отправлять данные с акселерометра на ПК в режиме реального времени. Модуль поддерживает сбор данных как онлайн, так и оффлайн — они могут сохраняться на внутреннюю SD‑карту. Передача данных на компьютер осуществляется через высокоскоростной USB или Wi‑Fi, используя протоколы TCP/IP, FTP и FreeMaster. Скачиавем файлы записей по FTP, живые графики и телеметрию модуля смотрим через среду FreeMaster, для глубокого анализа используем TCP подключение прямо в MATLAB.

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

Регистры и возможности чипа

Чип ISM330DLC содержит 93 доступных 8-разрядных регистра. Такое количество обусловлено тем, что чип может самостоятельно распознавать определённые типы событий, такие как свободное падение, наклон, одиночные и двойные клики и другие. Однако в этой статье мы не будем исследовать эти функции, поскольку они связаны с задача��и минимизации энергопотребления, а наша цель – точное измерение физических воздействий на чип и его ориентации.

В текущем проекте реализовано терминальное окно, которое позволяет наблюдать и изменять значения всех доступных регистров ISM330DLC. К терминалу можно подключится через клиента Telnet протокола. Например TeraTerm по URL telnet://S7V30

По нажатию последовательности кнопок 7 -> 8 -> 4 будет открыто окно с доступом к регистрам:

Здесь на экране в реальном времени отображаются данные регистров и есть возможность записа��ь любые произвольные данные в любой регистр. В нашем проекте уже реализована процедура инициализации чипа в соответствии с пользовательскими настройками. Пользовательские настройки хранятся в энергонезависимой памяти модуля и доступны также по интерфейсу telenet.

Начальный этап: калибровка

Первый шаг — это определение шумовых характеристик и дрейфа сенсора. Это критически важно для точности последующих измерений и анализа данных.

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

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

Вот график сигналов в течении 6 мин состояния покоя с вычисленными RMS шума и средним значением для каждой оси акселерометра и гироскопа:

А здесь график с тем же сигналом, но отфильтрованным бегущим средним с окном в 30 сек. График показывает дрейфы в течении 6 минут

Для сравнения посмотрим что нам обещает даташит:

Дрейф гироскопа по оси Y чуть больше обещанных 2 dps (градусов в сек). RMS шума гироскопа значительно больше заявленных 75 mdps и доходит до 785 mdps.

RMS шума акселерометра согласно вычислениям на диапазоне 2g должен быть около 7 mg, а по факту доходит до 24 mg.

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

Как считается RMS шума акселерометра

Для определения RMS шума при частоте сэмплирования 1666 Гц, учитыва��, что плотность шума акселерометра составляет $170 \, \mu g / \sqrt{\text{Hz}},$ можно использовать следующее выражение:

$\text{RMS} = \text{Noise Density} \times \sqrt{\Delta f}$

где $\Delta f$ — это полоса частот, равная частоте сэмплирования. Подставляем значения:

$\text{RMS} = 170 \times 10^{-6} \, g / \sqrt{\text{Hz}} \times \sqrt{1666 \, \text{Hz}}$

$\text{RMS} \approx 170 \times 10^{-6} \times 40.82 \approx 6.94 \times 10^{-3} \, g = 6.94 \, \text{mg}$

Таким образом, RMS шума составляет приблизительно 6.94 mg.

Срипт MATLAB для анализа шума акселерометра и гироскопа

% MATLAB скрипт для чтения двоичных данных и построения графиков акселерометра и гироскопа
% с игнорированием первой секунды данных, конвертацией в физические величины и отображением RMS для каждого сигнала

% Константы
fs = 1666; % Частота дискретизации, Гц
G = 9.80665; % Ускорение свободного падения, м/с²
PI = 3.141592653589793;

% Диапазоны акселерометра и гироскопа
ACCEL_RANGE_G = 2;   % ±2g
GYRO_RANGE_DPS = 250; % ±250°/с

% Коэффициенты масштабирования для окончательных величин
acc_g = (ACCEL_RANGE_G / 32768) * G; % м/с²/LSB
gyro_g = (GYRO_RANGE_DPS / 32768); % град/с/LSB

% Запрос файла у пользователя
[filename, pathname] = uigetfile('*.bin', 'Выберите двоичный файл данных');
if isequal(filename, 0)
    errordlg('Файл не выбран. Скрипт завершен.', 'Ошибка');
    return;
end
filepath = fullfile(pathname, filename);

% Открытие двоичного файла для чтения
fid = fopen(filepath, 'rb');
if fid == -1
    errordlg('Невозможно открыть указанный файл.', 'Ошибка');
    return;
end

% Чтение данных как int16
data = fread(fid, 'int16');

% Закрытие файла
fclose(fid);

% Проверка, что длина данных кратна 6
if mod(length(data), 6) ~= 0
    errordlg('Размер данных не кратен 6. Проверьте корректность файла данных.', 'Ошибка');
    return;
end

% Вычисление количества записей
num_records = length(data) / 6;

% Преобразование данных в матрицу N x 6
data = reshape(data, 6, num_records)';
% Столбцы соответствуют: [gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z]

% Извлечение данных гироскопа и акселерометра
gyro_data = data(:, 1:3);
acc_data = data(:, 4:6);

% Создание временного вектора с учетом частоты дискретизации
t = (0:num_records-1)' / fs; % Время в секундах

% Игнорирование первой секунды данных
ignore_samples = fs; % Количество записей, соответствующее первой секунде
if num_records <= ignore_samples
    errordlg('Длина записи меньше или равна одной секунде. Недостаточно данных для обработки.', 'Ошибка');
    return;
end
gyro_data = gyro_data(ignore_samples+1:end, :);
acc_data = acc_data(ignore_samples+1:end, :);
t = t(ignore_samples+1:end);

% Конвертация данных в физические величины
acc_data = acc_data * acc_g; % м/с²
gyro_data = gyro_data * gyro_g; % рад/с

% Расчет среднего значения для каждого сигнала
mean_acc = mean(acc_data);
mean_gyro = mean(gyro_data);

% Вычитание среднего значения из каждого сигнала
acc_data_centered = acc_data - mean_acc;
gyro_data_centered = gyro_data - mean_gyro;

% Расчет RMS для каждого сигнала относительно среднего уровня
rms_acc = rms(acc_data_centered);
rms_gyro = rms(gyro_data_centered);

% Создание фигуры на весь экран с панелью инструментов
figure('units','normalized','outerposition',[0 0 1 1], 'Toolbar', 'figure');

% Получение дескриптора панели инструментов
htoolbar = findall(gcf,'Type','uitoolbar');

% Отключение кнопки Pan и оставление только Zoom
% Поиск кнопки Pan
hpan = findall(htoolbar,'Tag','Exploration.Pan');
if ~isempty(hpan)
    delete(hpan); % Удаляем кнопку Pan
end

% Поиск кнопок Zoom
hzoom_in = findall(htoolbar,'Tag','Exploration.ZoomIn');
hzoom_out = findall(htoolbar,'Tag','Exploration.ZoomOut');

% Убедимся, что кнопки Zoom видимы
if ~isempty(hzoom_in)
    set(hzoom_in, 'Visible', 'on');
end
if ~isempty(hzoom_out)
    set(hzoom_out, 'Visible', 'on');
end

% Построение данных акселерометра и гироскопа в одном окне с отдельными графиками для каждого сигнала
ax1 = subplot(3,2,1);
plot(t, acc_data(:, 1), 'r');
xlabel('Время, сек');
ylabel('acc\_x (м/с²)');
title(['Акселерометр - acc\_x (Mean = ' num2str(mean_acc(1)) ', RMS = ' num2str(rms_acc(1)) ')']);
grid on;

ax2 = subplot(3,2,3);
plot(t, acc_data(:, 2), 'b');
xlabel('Время, сек');
ylabel('acc\_y (м/с²)');
title(['Акселерометр - acc\_y (Mean = ' num2str(mean_acc(2)) ', RMS = ' num2str(rms_acc(2)) ')']);
grid on;

ax3 = subplot(3,2,5);
plot(t, acc_data(:, 3), 'g');
xlabel('Время, сек');
ylabel('acc\_z (м/с²)');
title(['Акселерометр - acc\_z (Mean = ' num2str(mean_acc(3)) ', RMS = ' num2str(rms_acc(3)) ')']);
grid on;

ax4 = subplot(3,2,2);
plot(t, gyro_data(:, 1), 'r');
xlabel('Время, сек');
ylabel('gyro\_x (град/с)');
title(['Гироскоп - gyro\_x (Mean = ' num2str(mean_gyro(1)) ', RMS = ' num2str(rms_gyro(1)) ')']);
grid on;

ax5 = subplot(3,2,4);
plot(t, gyro_data(:, 2), 'b');
xlabel('Время, сек');
ylabel('gyro\_y (град/с)');
title(['Гироскоп - gyro\_y (Mean = ' num2str(mean_gyro(2)) ', RMS = ' num2str(rms_gyro(2)) ')']);
grid on;

ax6 = subplot(3,2,6);
plot(t, gyro_data(:, 3), 'g');
xlabel('Время, сек');
ylabel('gyro\_z (град/с)');
title(['Гироскоп - gyro\_z (Mean = ' num2str(mean_gyro(3)) ', RMS = ' num2str(rms_gyro(3)) ')']);
grid on;

% Синхронизация оси X для всех графиков
linkaxes([ax1, ax2, ax3, ax4, ax5, ax6], 'x');

% Фильтрация данных с помощью скользящего среднего с окном 30 секунд
window_size = 30 * fs; % количество отсчетов в окне 30 секунд
acc_data_filtered = movmean(acc_data, window_size);
gyro_data_filtered = movmean(gyro_data, window_size);

% Создание новой фигуры для отображения отфильтрованных данных акселерометра и гироскопа
figure('units','normalized','outerposition',[0 0 1 1], 'Toolbar', 'figure');

ax7 = subplot(3,2,1);
plot(t, acc_data_filtered(:, 1), 'r');
xlabel('Время, сек');
ylabel('acc\_x (м/с²) - Фильтрованные');
title('Акселерометр (фильтрованные) - acc\_x');
grid on;

ax8 = subplot(3,2,3);
plot(t, acc_data_filtered(:, 2), 'b');
xlabel('Время, сек');
ylabel('acc\_y (м/с²) - Фильтрованные');
title('Акселерометр (фильтрованные) - acc\_y');
grid on;

ax9 = subplot(3,2,5);
plot(t, acc_data_filtered(:, 3), 'g');
xlabel('Время, сек');
ylabel('acc\_z (м/с²) - Фильтрованные');
title('Акселерометр (фильтрованные) - acc\_z');
grid on;

ax10 = subplot(3,2,2);
plot(t, gyro_data_filtered(:, 1), 'r');
xlabel('Время, сек');
ylabel('gyro\_x (град/с) - Фильтрованные');
title('Гироскоп (фильтрованные) - gyro\_x');
grid on;

ax11 = subplot(3,2,4);
plot(t, gyro_data_filtered(:, 2), 'b');
xlabel('Время, сек');
ylabel('gyro\_y (град/с) - Фильтрованные');
title('Гироскоп (фильтрованные) - gyro\_y');
grid on;

ax12 = subplot(3,2,6);
plot(t, gyro_data_filtered(:, 3), 'g');
xlabel('Время, сек');
ylabel('gyro\_z (град/с) - Фильтрованные');
title('Гироскоп (фильтрованные) - gyro\_z');
grid on;

% Синхронизация оси X для всех графиков фильтрованных данных
linkaxes([ax7, ax8, ax9, ax10, ax11, ax12], 'x');

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

Можем ли мы этим IMU чипом определить поворот на доли градуса

Проведем эксперимет по повороту модуля на доли градуса по разным осям. Просто кладем модуль на жесткую рейку длиной 1.9 м с приподнятым одним концом на 25 мм. Через 2 мин опускаем приподнятый конец в горизонтальное положение. Это будет поворот на 0.75 градуса. Частота сэмплирования при этом 1666 Гц. Данные фильтруются бегущим средним с окном в 1 сек.

Вот график реакции на поворот 0.75 градуса в течении 1 сек по оси X

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

Визуализация ориентации в пространстве в реальном времени

Теперь попытаемся реализовать на основе акселерометра и гироскопа визуализацию ориентации в пространстве нашего модуля. Используем для этого компонент Simulation 3D Actor среды Simulink и прямое соединение с модулем по протоколу TCP/IP. Как было показано в моих предыдущих статьях среда matlab вполне справляется с управлением внешними устройствами в реальном времени с тактом в 10 мс. В данном случае также выбираем такт 10 миллисекунд.

Сначала строим подсистему приёма данных

Это подсистема подключается по протоколу tcp/ip на порт 3333 к модулю и получает от него сырые данные в виде 6 двухбайтных слов. Далее идёт синхронизация с тактом системы и демультиплексирование данных на канала акселерометра и гироскопа. К каналам акселерометра и гироскопа применяются множители для приведения величин к физическим значениям.

Построив приемник в подсистеме Receiver, достариваем остальную систему

Подсистема Data Accumulation and Averaging собирает и усредняет данные от гироскопа за первую секунду записи. Три полученный константы будут применены для корректировки смещения в данных от гироскопа. Спустя секунду от начала записи включается подсистема 3D Orientation Estimation.

Основную работу в этой подсистеме выполняет блок Complementary Filter. Комплементарный фильтр — это простой, линейный фильтр, подходящий для задач, где требуется только базовая стабилизация и ориентация. В Simulink есть и более сложные алгоритмы оценки ориентации, но пока их трогать не будем. Даже с комплементарным фильтром результат получился довольно интересный.

На этом заканчиваю мой короткий обзор и представляю перечень использованных для этой статьи инструментов:

Altium Designer 24.10.1 — схема и плата
SolidWorks — 3D модель в формате STL для Simulation 3D Actor
Matlab 2024A — расчеты, графики и 3D визуализация
ChatGPT 4o — все скрипты для MATLAB

Прошивка для модуля S7V30 находится здесь.
Модель MATLAB Simulink здесь

Опыты с акселерометром и гироскопом на платформе S7V30