Датчики и измерения (работа arduino с периферией)

ADXL345 Acceleromter Calibration

Nevertheless, once we read the data, we can simply print it on the serial monitor to check whether the values are as expected. In my case, the values I was getting were not exactly as they should be, especially the Z-axis which had a noticeable error of 0.1g.

To solve this issue, we need to calibrate the accelerometer using the 3 offset calibration registers, and here’s how we can do that. So, we need to position the sensor flat, and print the RAW values without dividing them by 256.

From here now we can notice the how much the outputs are off, in my case, the Z output was around 283. That’s difference of 27 in positive. Now we need to divide this value by 4, and that will give use the number that we need to write to the Z-axis offset register. If we upload the code now, the Z-axis output will be exactly 256, or 1g as it should be.

// This code goes in the SETUP section
// Off-set Calibration
  //X-axis
  Wire.beginTransmission(ADXL345);
  Wire.write(0x1E);  // X-axis offset register
  Wire.write(1);
  Wire.endTransmission();
  delay(10);
  //Y-axis
  Wire.beginTransmission(ADXL345);
  Wire.write(0x1F); // Y-axis offset register
  Wire.write(-2);
  Wire.endTransmission();
  delay(10);
  
  //Z-axis
  Wire.beginTransmission(ADXL345);
  Wire.write(0x20); // Z-axis offset register
  Wire.write(-7);
  Wire.endTransmission();
  delay(10);

If needed we should calibrate the other axis using the same method. And just a quick note that this calibration is not permanently written to the registers. We need to do write these values to the registers at each power up of the sensor.

Once we are done with the calibration, we can now finally calculate the Roll and Pitch, or the rotation around the X-axis and the rotation around the Y axis in degrees, using these two formulas.

// Calculate Roll and Pitch (rotation around X-axis, rotation around Y-axis)
  roll = atan(Y_out / sqrt(pow(X_out, 2) + pow(Z_out, 2))) * 180 / PI;
  pitch = atan(-1 * X_out / sqrt(pow(Y_out, 2) + pow(Z_out, 2))) * 180 / PI;

For more details how these formulas work, you can check this Freescale Semiconductor application note.

Калибровка mpu6050

Калибровка гироскопа и акселерометра – это очень важный шаг. Приведенные значения для гироскопа имеют вид: “gyro_x_scalled = ”, так как для получения угла поворота относительно оси по данным угловой скорости, необходимо провести интегрирование. Если “gyro_x_scalled” содержит ошибку или неверно выбрана база, эта ошибка также интегрируется и превращается в значительную погрешность в результате. Так что в идеале измерения должны показывать нуль, если гироскоп не движется вокруг каких-либо осей координат. На практике добиться идеала практически невозможно, так что наша задача – минимизировать эту ошибку. Кроме того, для компенсации «дрифта», можно использовать акселерометр для расчета угла наклона, сравнения полученных данных с результатами гироскопа и последующей компенсацией данной погрешности. Расчет угла будет рассмотрен в этой статье отдельно ниже.

На рисунках далее показано использование функции MPU6050_OffsetCal()

непосредственно в программе в Arduino IDE.

Скетч Arduino для калибровки платы акселерометра/гироскопа MPU6050:

Результат работы скетча для калибровки в серийном мониторе

Функции в программе Arduino для работы с mpu6050

MPU6050_ReadData()

Эта функция считывает данные с акселлерометра, гироскопа и датчика температуры. После считывания данных, значения переменных (temp_scalled, accel_x_scalled, accel_y_scalled, accel_z_scalled, gyro_x_scalled, gyro_y_scalled and gyro_z_scalled) обновляются.

MPU6050_ResetWake()

Эта функция сбрасывает настройки чипа на значения по-умолчанию. Рекомендуется использовать сброс настроек перед настройкой чипа на выполнения определенной задачи.

MPU6050_SetDLPF(int BW)

Эта функция настраивает встроенный фильтр низких частот. Переменная int BW должна содержать значения (0-6). Пропускная способность фильтра будет изменяться в соответствии с представленной ниже таблицей.

int BW Пропускная способность фильтра
0 или Any бесконечность
1 184
2 94
3 44
4 21
5 10
6 5

Если int BW не в диапазоне 0-6, фильтр низких частот отключается, что соответствует установке – бесконечность.

MPU6050_SetGains(int gyro,int accel)

Эта функция используется для установки максимального значения шкалы измерений

int gyro Макс. знач.[угол/с] int accel Макс. знач. [м/с2]
250 2g
1 500 1 4g
2 1000 2 8g
3 2000 3 16g

MPU6050_ReadData()

Эта функция использует масштабные коэффициенты для расчета результата. Если не используются значения (0-3), MPU6050_ReadData() отобразит необработанные значения с датчика с погрешностью калибровки. Для получения обработанных значений, установите переменные для калибровки (accel_x_OC, accel_y_OC, accel_z_OC, gyro_x_OC, gyro_y_OC and gyro_z_OC) в нуль.

MPU6050_OffsetCal()

Эта функция позволяет откалибровать акселерометр и гироскоп. Рассчитанные значения записываются в переменные accel_x_OC, accel_y_OC, accel_z_OC, gyro_x_OC, gyro_y_OC и gyro_z_OC для дальнейшей коррекции. Для проведения калибровки необходимо расположить оси x и y axes платы MPU6050 в горизонтальной плоскости, а ось z – перпендикулярно к основанию. Даже незначительные перемещения платы во время калибровки понижают точность расчета базовой точки. Ось z калибруется относительно силя земного притяжения — 9.81 м/с2 (1g), что учтено в коде.

Водонепроницаемый ультразвуковой датчик JSN SR-04T

JSN SR-04T представляет собой модуль водонепроницаемого ультразвукового датчика, который способен измерять расстояние на дистанциях 25-450 см с точностью 2 мм. Модуль датчика состоит из двух раздельных частей. Одна часть – это датчик, который непосредственно производит передачу и прием ультразвуковых волн, а вторая часть – это плата управления. По принципу действия JSN SR-04T очень похож на датчики парковки, которые устанавливаются в бамперах автомобилей.

Модуль датчика JSN SR-04T имеет водозащищенное исполнение датчика, отличается устойчивым функционированием в сложных условиях эксплуатации и высокой точностью измерения расстояния. Он может использоваться в системах дальнометрии, предотвращения столкновения с препятствиями, автоматизированного контроля, мониторинга движения объектов, контроля трафика, системах безопасности и искусственного интеллекта и многих других применениях.

Назначение контактов (распиновка) датчика JSN SR-04T представлены на следующих рисунке и таблице.

№ контакта Название контакта Назначение контакта
1 5V Напряжение питания
2 Trig Входной контакт датчика. На него необходимо подать импульс длительностью 10 мкс чтобы запустить датчик в работу (чтобы он начал излучение ультразвуковой волны).
3 Echo Выходной контакт датчика. После приема отраженной от препятствия ультразвуковой волны на этом контакте формируется импульс высокого уровня (high), длительность которого равна времени распространения ультразвуковой волны до препятствия и обратно.
4 Gnd Общий провод (земля)

Модуль датчика JSN SR-04T во многом похож на модуль ультразвукового датчика, но по сравнению с ним у него есть ряд преимуществ:

  • в отличие от HC-SR04 у модуля JSN SR-04T непосредственно датчик не распаян на печатной плате модуля, вместо этого он закреплен на конце достаточно длинного кабеля (2,5 метра), поэтому чувствительный элемент датчика может быть удален на значительное расстояние от платы управления;
  • чувствительный элемент датчика заключен в защищённый водонепроницаемый корпус, поэтому его можно размещать в жестких условиях эксплуатации и агрессивных средах.

Но несмотря на эти преимущества имеются и некоторые недостатки. К примеру, минимальное расстояние измерения у датчика JSN SR-04T составляет 20 см, в то время как у датчика HC-SR04 оно равно 2 см. Причина этого заключается в том, что JSN SR-04T имеет только один чувствительный элемент в то время как у датчика HC-SR04 их два – один используется для передачи ультразвуковых волн, а другой для приема. В результате этого чувствительному элементу датчика JSN SR-04T приходится переключаться двумя режимами (прием/передача), а на это требуется дополнительное время. Поэтому и минимальное расстояние измерения для датчика JSN SR-04T равно 20 см, а датчик HC-SR04 может измерять значительно меньшие расстояния благодаря наличию у него отдельных передатчика и приемника.

Основные технические характеристики и особенности датчика JSN SR-04T:

  • рабочее напряжение: DC 5V (постоянного тока);
  • ток в состоянии покоя (ожидания): 5mA;
  • ток в рабочем состоянии: 30mA;
  • частота ультразвуковой волны: 40 кГц;
  • диапазон измерения расстояний: от 25 см до 4,5 м;
  • длина кабеля: 2,5 метра.

4Определение ускоренийпо трём осям акселерометра

Снимем значения с акселерометра при углах 90° и 270° и занесём в таблицу. Таблица показывает углы поворота акселерометра (столбец «A») и соответствующие им значения Zout в вольтах (столбец «B»).

Определение ускорений по трём осям акселерометра ADXL335

Для наглядности приведён график напряжений на выходе Zout в зависимости от угла поворота. Голубое поле – это область значений в спокойном состоянии (при ускорении 1g). Розовое поле на графике – это запас для того чтобы мы могли измерять ускорение до +3g и до −3g.

При угле поворота 90° на ось Z приходится нулевое ускорение. Т.е. значение 1,67 вольт – это условный ноль Z. Тогда определим ускорение так: g = Zout – Z / Sz, здесь Zout – измеренное значение в милливольтах, Z – значение при нулевом ускорении в милливольтах, Sz – чувствительность датчика по оси Z, измеренная в мВ/g.

Чувствительность акселерометра приведена в паспорте и равна в среднем 300 мВ/g или 0,3 В/g, но вообще лучше провести калибровку акселерометра и вычислить значение чувствительности конкретно для вашего датчика по формуле: Sz = Z(0°) – Z(90°) В данном случае чувствительность акселерометра по оси Z = 2,03 – 1,68 = 0,35 В/g. Аналогично чувствительность нужно будет посчитать для осей X и Y.

В столбце «С» таблицы приводится расчётное ускорение при чувствительности, равной 350 мВ/g. Как видно, расчёты практически совпадают с номинальными величинами, которые даются на первом рисунке из паспорта на датчик ADXL335, т.е. наш датчик довольно точно показывает свою ориентацию в пространстве (я показал это просто для самопроверки, дальше это не пригодится).

Тестирование

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

С акселерометром и гироскопом проще всего использовать программы 3Д рендеринга показаний, вроде ShowGY521Data, которые позволят в реальном времени увидеть, как железо позиционируется в пространстве. В случае неисправностей всегда можно подправить нулевой уровень и уменьшить чувствительность акселерометра, который также влияет на конечную модель отображения устройства.

1Принцип действияцифрового акселерометра

Акселерометры используют для определения вектора ускорения. Акселерометр ADXL335 имеет три оси, и благодаря этому он может определять вектор ускорения в трёхмерном пространстве.

Ввиду того, что сила земного притяжения – это тоже вектор, мы можем определять ориентацию акселерометра в трёхмерном пространстве относительно центра Земли.

На иллюстрации приведены рисунки из паспорта на акселерометр ADXL335. Здесь изображены координатные оси чувствительности акселерометра по отношению к геометрическому размещению корпуса устройства в пространстве, а также значения ускорений, принимаемые с 3-х каналов акселерометра в зависимости от его ориентации в пространстве. Данные приводятся для находящегося в состоянии покоя датчика, на который действует только сила земного тяготения.

Принцип снятия измерений с аналогового акселерометра ADXL335

Рассмотрим подробнее, что же показывает нам акселерометр. Пусть датчик лежит горизонтально, например, на столе. Тогда проекция вектора ускорения будет равна «1g» по оси Z, или Zout = 1g. По остальным двум осям будут нули: Xout = 0 и Yout = 0. При повороте датчика «на спину», он будет направлен в противоположную сторону относительно вектора силы тяжести, т.е. Zout = −1g. Аналогично измерения снимаются по всем трём осям. Понятно, что акселерометр может быть расположен как угодно в пространстве, поэтому со всех трёх каналов мы будем снимать отличные от нуля показания.

Если датчик сильно тряхнуть вдоль вертикальной оси Z, то значение Zout будет больше, чем «1g». Максимальное измеряемое ускорение составляет «±3g» по каждой из осей («плюс» и «минус» тут обозначают направление ускорения).

Думаю, с принципом работы акселерометра разобрались. Теперь рассмотрим схему подключения.

1Принцип действияцифрового акселерометра

Акселерометры используют для определения вектора ускорения. Акселерометр ADXL335 имеет три оси, и благодаря этому он может определять вектор ускорения в трёхмерном пространстве.

Ввиду того, что сила земного притяжения – это тоже вектор, мы можем определять ориентацию акселерометра в трёхмерном пространстве относительно центра Земли.

На иллюстрации приведены рисунки из паспорта на акселерометр ADXL335. Здесь изображены координатные оси чувствительности акселерометра по отношению к геометрическому размещению корпуса устройства в пространстве, а также значения ускорений, принимаемые с 3-х каналов акселерометра в зависимости от его ориентации в пространстве. Данные приводятся для находящегося в состоянии покоя датчика, на который действует только сила земного тяготения.

Принцип снятия измерений с аналогового акселерометра ADXL335

Рассмотрим подробнее, что же показывает нам акселерометр. Пусть датчик лежит горизонтально, например, на столе. Тогда проекция вектора ускорения будет равна «1g» по оси Z, или Zout = 1g. По остальным двум осям будут нули: Xout = 0 и Yout = 0. При повороте датчика «на спину», он будет направлен в противоположную сторону относительно вектора силы тяжести, т.е. Zout = −1g. Аналогично измерения снимаются по всем трём осям. Понятно, что акселерометр может быть расположен как угодно в пространстве, поэтому со всех трёх каналов мы будем снимать отличные от нуля показания.

Если датчик сильно тряхнуть вдоль вертикальной оси Z, то значение Zout будет больше, чем «1g». Максимальное измеряемое ускорение составляет «±3g» по каждой из осей («плюс» и «минус» тут обозначают направление ускорения).

Думаю, с принципом работы акселерометра разобрались. Теперь рассмотрим схему подключения.

3Работа с цифровым акселерометром ADXL345 по интерфейсу I2C

Временная диаграмма информационного обмена с ADXL345 по интерфейсу I2C выглядит так:

Диаграмма обмена с ADXL345 по I2C

Давайте перепишем скетч для Arduino, который будет делать всё то же самое, только с обменом по интерфейсу I2C:

Скетч для чтения данных ADXL345 по I2C (разворачивается)

#include <Wire.h>

const int adxl345 = 0x53; // I2C адрес ADXL345

void setup() {
  Serial.begin(9600);
  Wire.begin();

  // запишем адрес регистра DEVID
  Wire.beginTransmission(adxl345);
  Wire.write(byte(0x00));
  Wire.endTransmission();

  // прочитаем регистр DEVID:
  Wire.requestFrom(adxl345, 1);
  while (Wire.available()) {
    byte c = Wire.read();
    Serial.print("ID = ");
    Serial.println(c, HEX);
  }

  // переведём акселерометр в режим измерений
  Wire.beginTransmission(adxl345);
  Wire.write(byte(0x2D));
  Wire.write(byte(0x08));
  Wire.endTransmission();
}

void loop() {
  // запишем адрес начала данных по осям X, Y и Z:
  Wire.beginTransmission(adxl345);
  Wire.write(byte(0x32));
  Wire.endTransmission();

  // прочитаем 6 байтов значений XYZ:
  int i = 0;
  byte xyz;
  Wire.requestFrom(adxl345, 6);
  while (Wire.available()) {
    byte c = Wire.read();
    xyz = c;
    i++;
  }

  // посчитаем и отобразим значения X, Y, Z:
  int x = word(xyz, xyz);
  int y = word(xyz, xyz);
  int z = word(xyz, xyz);
  
  Serial.print(x);
  Serial.print("\t");
  Serial.print(y);
  Serial.print("\t");
  Serial.println(z);

  delay(100);
}

Диаграмма чтения регистра DEVID цифрового акселерометра ADXL345 при обмене по последовательному интерфейсу I2C будет в этом случае такой:

Диаграмма чтения регистра DEVID акселерометра ADXL345 по I2C

Как видно, ADXL345 возвращает нам ожидаемое значение 0xE5. А вот так будет выглядеть диаграмма чтения регистров, в которых хранятся данные по осям XYZ:

Диаграмма чтения регистров данных X, Y, Z акселерометра ADXL345 по I2C

Тут всё ещё проще, чем при работе с интерфейсом SPI.

1Принцип действияцифрового акселерометра

Акселерометры используют для определения вектора ускорения. Акселерометр ADXL335 имеет три оси, и благодаря этому он может определять вектор ускорения в трёхмерном пространстве.

Ввиду того, что сила земного притяжения – это тоже вектор, мы можем определять ориентацию акселерометра в трёхмерном пространстве относительно центра Земли.

На иллюстрации приведены рисунки из паспорта на акселерометр ADXL335. Здесь изображены координатные оси чувствительности акселерометра по отношению к геометрическому размещению корпуса устройства в пространстве, а также значения ускорений, принимаемые с 3-х каналов акселерометра в зависимости от его ориентации в пространстве. Данные приводятся для находящегося в состоянии покоя датчика, на который действует только сила земного тяготения.

Принцип снятия измерений с аналогового акселерометра ADXL335

Рассмотрим подробнее, что же показывает нам акселерометр. Пусть датчик лежит горизонтально, например, на столе. Тогда проекция вектора ускорения будет равна «1g» по оси Z, или Zout = 1g. По остальным двум осям будут нули: Xout = 0 и Yout = 0. При повороте датчика «на спину», он будет направлен в противоположную сторону относительно вектора силы тяжести, т.е. Zout = −1g. Аналогично измерения снимаются по всем трём осям. Понятно, что акселерометр может быть расположен как угодно в пространстве, поэтому со всех трёх каналов мы будем снимать отличные от нуля показания.

Если датчик сильно тряхнуть вдоль вертикальной оси Z, то значение Zout будет больше, чем «1g». Максимальное измеряемое ускорение составляет «±3g» по каждой из осей («плюс» и «минус» тут обозначают направление ускорения).

Думаю, с принципом работы акселерометра разобрались. Теперь рассмотрим схему подключения.

1Принцип действияцифрового акселерометра

Акселерометры используют для определения вектора ускорения. Акселерометр ADXL335 имеет три оси, и благодаря этому он может определять вектор ускорения в трёхмерном пространстве.

Ввиду того, что сила земного притяжения – это тоже вектор, мы можем определять ориентацию акселерометра в трёхмерном пространстве относительно центра Земли.

На иллюстрации приведены рисунки из паспорта на акселерометр ADXL335. Здесь изображены координатные оси чувствительности акселерометра по отношению к геометрическому размещению корпуса устройства в пространстве, а также значения ускорений, принимаемые с 3-х каналов акселерометра в зависимости от его ориентации в пространстве. Данные приводятся для находящегося в состоянии покоя датчика, на который действует только сила земного тяготения.

Принцип снятия измерений с аналогового акселерометра ADXL335

Рассмотрим подробнее, что же показывает нам акселерометр. Пусть датчик лежит горизонтально, например, на столе. Тогда проекция вектора ускорения будет равна «1g» по оси Z, или Zout = 1g. По остальным двум осям будут нули: Xout = 0 и Yout = 0. При повороте датчика «на спину», он будет направлен в противоположную сторону относительно вектора силы тяжести, т.е. Zout = −1g. Аналогично измерения снимаются по всем трём осям. Понятно, что акселерометр может быть расположен как угодно в пространстве, поэтому со всех трёх каналов мы будем снимать отличные от нуля показания.

Если датчик сильно тряхнуть вдоль вертикальной оси Z, то значение Zout будет больше, чем «1g». Максимальное измеряемое ускорение составляет «±3g» по каждой из осей («плюс» и «минус» тут обозначают направление ускорения).

Думаю, с принципом работы акселерометра разобрались. Теперь рассмотрим схему подключения.

Высокотемпературные и динамические среды

До появления акселерометров, рассчитанных на эксплуатацию при высокой температуре или в жестких условиях окружающей среды, некоторые разработчики вынуждены были использовать микросхемы со стандартным температурным диапазоном, выходя за пределы указанных в документации характеристик. То есть конечный пользователь должен был взять на себя ответственность и риски, связанные с аттестацией компонента для работы при повышенных температурах, что дорого и требует много времени. Известно, что герметичные корпуса весьма надежны при повышенных температурах и обеспечивают защиту от влаги и загрязнений, вызывающих коррозию. Analog Devices предлагает ряд компонентов в герметичных корпусах, характеризующихся повышенной стабильностью и высокой производительностью в широком диапазоне температур. Компания провела значительную работу по изучению производительности компонентов в пластиковых корпусах при повышенных температурах, в частности оценивалась способность выводных рамок и выводов микросхем выдерживать высокотемпературные процессы пайки и обеспечивать надежное крепление, способное противостоять сильным ударам и вибрации. В результате Analog Devices предлагает 18 акселерометров, функционирующих в диапазоне температур –40…+125 °C, среди которых можно назвать ADXL206, ADXL354/ADXL355/ADXL356/ADXL357, ADXL1001/ADXL1002, ADIS16227/ADIS16228 и ADIS16209. Большинство конкурентов не предлагают емкостные МЭМС-акселерометры, способные работать в указанных температурных пределах или в жестких условиях окружающей среды — например, в составе тяжелого промышленного оборудования или при бурении и разведке скважин.

Выполнение измерений угла наклона в очень жестких условиях окружающей среды с температурой выше +125 °C — задача чрезвычайно сложная. ADXL206 представляет собой высокоточный (точность измерения угла наклона ±0,06°) малопотребляющий МЭМС-акселерометр, предназначенный для приложений с высокотемпературными и суровыми средами, таких как бурение и разведка скважин. Этот компонент имеет керамический корпус размером 13×8×2 мм с двумя рядами расположенных по бокам выводов, который способен функционировать в температурном диапазоне −40…+175 °C с ухудшением характеристик после +175 °C и 100%-ной восстановимостью при возобновлении работы при температурах ниже +175 °C.

Для измерения угла наклона в динамических средах, где присутствует вибрация, например в сельскохозяйственной технике или беспилотных летательных аппаратах, требуются акселерометры с более широким диапазоном измерения ускорений, в частности ADXL356/ADXL357. Выполнение измерений в ограниченном диапазоне может привести к отсечке результирующего значения, что вызывает дополнительное смещение к выходному сигналу. Отсечка может возникнуть из-за того, что ось измерения присутствует в гравитационном поле 1 g, или из-за ударов с быстрым временем нарастания и медленным затуханием. При измерениях в более широком диапазоне величина отсечки сокращается, что уменьшает смещение, это в свою очередь повышает точность определения угла наклона в динамических приложениях.

На рис. 6 показаны результаты измерения в ограниченном диапазоне ускорений по оси Z акселерометра ADXL356, причем гравитационное поле 1 g уже присутствует в этом диапазоне измерений.

Рис. 6. Ошибка VRE акселерометра ADXL356, смещение по оси Z от 1 g, диапазон измеряемых ускорений ±10 g, ориентация по оси Z = 1 g

На рис. 7 даны те же измерения, но диапазон расширен с ±10 до ±40 g. Эти графики четко демонстрируют, что смещение, вызванное отсечкой, значительно уменьшается за счет расширения диапазона измеряемых ускорений акселерометра.

Рис. 7. Ошибка VRE акселерометра ADXL356, смещение по оси Z от 1 g, диапазон измеряемых ускорений ±40 g, ориентация по оси Z = 1 g

Акселерометры ADXL354/ADXL355 и ADXL356/ADXL357 обеспечивают эффективное выпрямление вибрационных колебаний, повторяемость результатов измерения на протяжении длительного времени и низкий уровень шума в компактном корпусе и идеально подходят для измерения угла наклона и крена как в статической, так и в динамической среде.

Ошибки оценивания углов ориентации

Описание алгоритма

  • Сформируем массивы случайных углов Эйлера roll, pitch, yaw. Они будут задавать наборы вариантов истинной ориентации объекта в модели.
  • Из случайных углов roll, pitch, yaw формируется матрица преобразования из ССК XYZ в ЛСК ENU:
    где , , , , , .
  • Используя данную матрицу можно получить выражение для истинных ускорений в ССК:

    — вектор, определяющий направление гравитационного ускорения, выраженный в единицах g, — матрица преобразования координат из ЛСК в ССК (обратная матрице преобразования из ССК в ЛСК).

  • Применяем модель измерения акселерометра:

  • По измерениям акселерометра рассчитываются новые углы крена и тангажа (оценки) по формулам:

  • Также необходимо сформировать матрицу пересчета в «горизонт» из этих углов, для этого воспользуемся функцией rpy2mat:
    где углы roll’ и pitch’ — это углы, рассчитанные по измерениям акселерометра, а третий угол — нулевой.
  • Возьмем вектор истинных магнитных плотностей в ЛСК ENU и пересчитаем его в ССК XYZ:
  • Применяем модель измерений магнитометра:

  • Осталось пересчитать измерения магнитометра из ССК в «горизонт»:

  • По «горизонтированным» измерениям магнитометра расcчитывается угол магнитного азимута:
  • Ошибки оценивания углов ориентации рассчитываются как разность между истинными углами roll, pitch, yaw и рассчитанными по измерениям датчиков — roll’, pitch’, yaw’.

Глобальные переменные

Данный заголовочный файл включает в себя следующие глобальные переменные:

int accel_x_OC — Содержит измерения положения акселерометра относительно оси x при калибровке

int accel_y_OC — Содержит измерения положения акселерометра относительно оси y при калибровке

int accel_z_OC — Содержит измерения положения акселерометра относительно оси z при калибровке

int gyro_x_OC — Содержит измерения положения гироскопа относительно оси x

int gyro_y_OC — Содержит измерения положения гироскопа относительно оси y

int gyro_z_OC — Содержит измерения положения гироскопа относительно оси z

float temp_scalled — Содержит абсолютное значение температуры в градусах цельсия

float accel_x_scalled — данные оси x акселерометра минус данные калибровки

float accel_y_scalled — данные оси y акселерометра минус данные калибровки

float accel_z_scalled — данные оси z акселерометра минус данные калибровки

float gyro_x_scalled — данные гироскопа относительно оси x минус данные калибровки

float gyro_y_scalled — данные гироскопа относительно оси y минус данные калибровки

float gyro_z_scalled — данные гироскопа относительно оси z минус данные калибровки

Рейтинг
( Пока оценок нет )
Понравилась статья? Поделиться с друзьями:
Электрик в доме
Добавить комментарий

;-) :| :x :twisted: :smile: :shock: :sad: :roll: :razz: :oops: :o :mrgreen: :lol: :idea: :grin: :evil: :cry: :cool: :arrow: :???: :?: :!: