Главная » Мультикоптеры

IMU-сенсор 10 DOF v1 для Raspberry Pi Pico: инструкция, примеры использования и документация [Амперка / Вики]

IMU-сенсор 10 DOF v1 для Raspberry Pi Pico: инструкция, примеры использования и документация [Амперка / Вики] Мультикоптеры
На чтение 41 мин. Опубликовано
Содержание
  1. Описание алгоритма
  2. Автоматическая калибровка магнитометра
  3. Акселерометр
  4. Калибровка исходных/необработанных данных (raw data)
  5. Код для pico
  6. Код для processing
  7. Магнитометр
  8. Общие сведения о датчике mpu9250
  9. Ориентация объекта в пространстве
  10. Получение “реальных” значений
  11. Преобразователь питания rt9193-18
  12. Преобразователь питания rt9193-33
  13. Работа с arduino и mpu6050 | alexgyver
  14. Расчет углов ориентации
  15. Считывание данных с датчика mpu9250 в плату arduino mega 2560
  16. Углы ориентации
  17. Фильтрация данных

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

IMU-сенсор 10 DOF v1 для Raspberry Pi Pico: инструкция, примеры использования и документация [Амперка / Вики]

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

    $C_{XYZ}^{ENU}=begin{vmatrix} cycdot cp &-crcdot sy srcdot cycdot sp &srcdot sy crcdot cycdot sp \ sycdot cp &crcdot cy srcdot sycdot sp & -srcdot cy crcdot sycdot sp \ -sp &srcdot cp &crcdot cp end{vmatrix},$

    где $cr=cos (roll)$, $sr=sin(roll)$, $cp=cos(pitch)$, $sp=sin(pitch)$, $cy=cos(yaw)$, $sy=sin(yaw)$.

  • Используя данную матрицу можно получить выражение для истинных ускорений в ССК:

    $A_{XYZ}=left ( C_{XYZ}^{ENU} right )^{T}cdot begin{vmatrix} 0\ 0\ -1\ end{vmatrix},$

    $begin{vmatrix} 0\ 0\ -1\ end{vmatrix}$ — вектор, определяющий направление гравитационного ускорения, выраженный в единицах g, ${(C_{XYZ}^{ENU} )}^{T}$ — матрица преобразования координат из ЛСК в ССК (обратная матрице преобразования из ССК в ЛСК).

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

    $a_{XYZ}=left ( I m_{a} right )cdot A_{XYZ} b_{a} n_{a},$

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

    $roll'=atan left ( frac{a_{Y}}{a_{Z}} right ),$

    $pitch'=atanleft ( frac{-a_{X}}{sqrt{a_{Y}^{2} a_{Z}^{2}}} right ).$

  • Также необходимо сформировать матрицу пересчета в «горизонт» из этих углов, для этого воспользуемся функцией rpy2mat:

    $C_{XYZ}^{XYZ'}=rpy2matleft ( begin{bmatrix} roll'\ pitch'\ 0 end{bmatrix}^{T} right ),$

    где углы roll’ и pitch’ — это углы, рассчитанные по измерениям акселерометра, а третий угол — нулевой.

  • Возьмем вектор истинных магнитных плотностей в ЛСК ENU и пересчитаем его в ССК XYZ:

    $M_{XYZ}= {(C_{XYZ}^{ENU} )}^{T}cdot M_{ENU}.$

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

    $m_{XYZ}=S_{m}cdot M_{XYZ} b_{m} n_{m}.$

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

    $m_{XYZ'}=C_{XYZ}^{XYZ'}cdot m_{XYZ}.$

  • По «горизонтированным» измерениям магнитометра расcчитывается угол магнитного азимута:

    $yaw'=atan2left ( frac{m_{Y'}}{m_{X'}} right ).$

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

Автоматическая калибровка магнитометра

Это одна из самых простых, но в то же время важных участков кода программы. Функция magcalMPU9250(float * dest1, float * dest2) производит калибровку магнитометра в то время когда вы перемещаете его по фигуре восьмерки (цифры “8”). Она сохраняет максимальные и минимальные значения, а затем на их основе вычисляет среднее.

Акселерометр

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

$left (F=moverrightarrow{a}right )$

и силы тяжести

$left (F_{g}=moverrightarrow{g}right )$

Примем следующую модель измерений триады ортогональных (взаимно перпендикулярных) акселерометров:

где

$a_{XYZ}$

– измеряемое ускорение в ССК (собственной системе координат) XYZ,

$m_{a}$

– матрица перекоса осей и масштабных коэффициентов акселерометра,

$A_{XYZ}$

– вектор истинного ускорения в ССК XYZ,

$b_{a}$

– вектор смещения нуля акселерометра,

$n_{a}$

– шум измерений.

Матрица перекоса осей и масштабных коэффициентов выглядит следующим образом:

где элементы, расположенные по главной диагонали (

$ 1 m_{a,1,1}, 1 m_{a,2,2}, 1 m_{a,3,3}$

) — это масштабные коэффициенты и их погрешности по трём осям акселерометра, а остальные элементы матрицы — перекосы осей акселерометра.

Калибровка исходных/необработанных данных (raw data)

Получаемые с датчика MPU9250 данные должны быть откалиброваны в соответствии с оборудованием пользователя, в котором будет использоваться датчик. Калибровка магнитометра требуется для компенсации магнитного склонения (Magnetic Declination). Корректировочное значение в данном случае сильно зависит от местоположения датчика. Необходимо откалибровать две переменные: yaw и magbias.

Ниже приведенный фрагмент кода реализует калибровку переменной yaw для магнитного склонения, характерного для местности: Potheri, Chennai, India.

Данные о магнитном склонении в конкретной местности можно получить, к примеру, с помощью следующих сайтов:

В следующем фрагменте кода реализована калибровка переменной magbias, которая взята из функции magcalMPU9250(float * dest1, float * dest2) (из библиотеки, которую мы используем для работы с датчиком MPU9250).

Код для pico

imu_visualization.py
# Импортируем необходимые модулиfrom amperka_pico_imu_10dof import icm20948
importtimeimportmath
 
# Печатаем тестовую строку в консольprint("IMU-sensor Test Program")# Создаём объект для работы с IMU-сенсором
imu=icm20948.ICM20948()
 
whileTrue:
    # Считываем данных с акселерометра
    accelerometer = imu.read_accelerometer_g()# Считываем данных с гироскопа
    gyroscope = imu.read_gyroscope_rad()# Считываем данных с магнитометра
    magnetometer = imu.read_magnetometer_()
    ax = accelerometer[]
    ay = accelerometer[1]
    az = accelerometer[2]
    gx = gyroscope[]
    gy = gyroscope[1]
    gz = gyroscope[2]
    mx = magnetometer[]
    my = magnetometer[1]
    mz = magnetometer[2]# Обновляем входные данные в фильтр Маджвика
    imu.AHRS_update(gx, gy, gz, ax, ay, az, mx, my, mz)# Считываем кватернион
    quaternion = imu.read_quaternion()# Выводим кватернион в консоль для передачи в Processingprint(f"{quaternion[0]},{quaternion[1]},{quaternion[2]},{quaternion[3]}")time.sleep(0.1)

Код для processing

imu_visualization_draw.pde
importprocessing.serial.*;importtoxi.geom.*;importtoxi.processing.*;
 
// NOTE: requires ToxicLibs to be installed in order to run properly.// 1. Download from http://toxiclibs.org/downloads// 2. Extract into [userdir]/Processing/libraries//    (location may be different on Mac/Linux)// 3. Run and bask in awesomeness
 
// The serial port
Serial port;                         
 
String message;
 
float[] q =newfloat[4];
Quaternion quat =new Quaternion(1, , , );// New line character in ASCIIfinalchar newLine ='n';String[] massQ =newString[4];float[] ypr =newfloat[3];
 
void setup(){// Size form 400x400
    size(400, 400, P3D);// Open serial port// Replace "COM7" with the COM port on which your arduino is connected
    port =new Serial(this, "COM7", 9600);}
 
void draw(){// Read and parse incoming serial message
    serialEvent();// Set background to black
    background();
    printQuaternions();
    printYawPitchRoll();// Set position to centre
    translate(width /2, height /2);// Begin object
    pushMatrix();float[] axis = quat.toAxisAngle();
    rotate(axis[], axis[2], axis[3], axis[1]);// Draw main body in red
    drawBody();// Draw front-facing tip in blue
    drawCylinder();// Draw Triangles
    drawTriangles();// Draw Quads
    drawQuads();// End of object
    popMatrix();// Send character 's' to Arduino//port.write('s');}
 
void serialEvent(){// Read from port until new line (ASCII code 13)
    message = port.readStringUntil(newLine);if(message !=null){// Split message by commas and store in String array 
        massQ = split(message, ",");
        q[]=float(massQ[]);
        q[1]=float(massQ[1]);
        q[2]=float(massQ[2]);
        q[3]=float(massQ[3]);}// Print values to console
    print(q[]);
    print("t");
    print(q[1]); 
    print("t");
    print(q[2]);   
    print("t");
    print(q[3]);   
    println("t");// Set our toxilibs quaternion to new data
    quat.set(q[], q[1], q[2], q[3]);
 
}
 
void drawCylinder(){float topRadius =;float bottomRadius =20;float tall =20;int sides =8;// Begin object
    pushMatrix();
    translate(, , -120);
    rotateX(PI/2);
    fill(, , 255, 200);
 
    float angle =;float angleIncrement = TWO_PI / sides;
    beginShape(QUAD_STRIP);for(int i =; i < sides  1;  i){
        vertex(topRadius * cos(angle), , topRadius * sin(angle));
        vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle));
        angle  = angleIncrement;}
 
    endShape();
 
    // if it is not a cone, draw the circular top capif(topRadius !=){
        angle =;
        beginShape(TRIANGLE_FAN);// Center point
        vertex(, , );for(int i =; i < sides  1; i  ){
            vertex(topRadius * cos(angle), , topRadius * sin(angle));
            angle  = angleIncrement;}
        endShape();}
 
    // If it is not a cone, draw the circular bottom capif(bottomRadius !=){
        angle =;
        beginShape(TRIANGLE_FAN);// Center point
        vertex(, tall, );for(int i =; i < sides  1; i  ){
            vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle));
            angle  = angleIncrement;}
        endShape();}
    popMatrix();}
 
void drawBody(){
    fill(255, , , 200);
    box(10, 10, 200);}
 
void drawTriangles(){// Draw wings and tail fin in green
    fill(, 255, , 200);
    beginShape(TRIANGLES);// Wing top layer
    vertex(-100,  2, 30); vertex(,  2, -80); vertex(100,  2, 30);// Wing bottom layer
    vertex(-100, -2, 30); vertex(, -2, -80); vertex(100, -2, 30);// Tail left layer
    vertex(-2, , 98); vertex(-2, -30, 98); vertex(-2, , 70);// Tail right layer
    vertex(2, , 98); vertex(2, -30, 98); vertex(2, , 70);
    endShape();}
 
void drawQuads(){
    beginShape(QUADS);
    vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(, -2, -80); vertex(, 2, -80);
    vertex(100, 2, 30); vertex(100, -2, 30); vertex(, -2, -80); vertex(, 2, -80);
    vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2,  30); vertex(100, 2,  30);
    vertex(-2,   , 98); vertex(2,   , 98); vertex(2, -30, 98); vertex(-2, -30, 98);
    vertex(-2,   , 98); vertex(2,   , 98); vertex(2,   , 70); vertex(-2,   , 70);
    vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2,   , 70); vertex(-2,   , 70);
    endShape();}
 
void printQuaternions(){// Set text mode to shape
    textMode(SHAPE);
    textSize(13);
    fill(255, 255, 255);
    text("Quaternions:", 20, 20, 10);
    text(q[], 20, 40, 10);
    text(q[1], 20, 60, 10);
    text(q[2], 20, 80, 10);
    text(q[3], 20, 100, 10);}
 
void printYawPitchRoll(){// Calculate yaw/pitch/roll angles
    ypr[]= atan2(2*q[1]*q[2]-2*q[]*q[3], 2*q[]*q[] 2*q[1]*q[1]-1)*57.2;
    ypr[1]= atan2(2* q[2]* q[3]-2* q[]* q[1], 2* q[]* q[] 2* q[3]* q[3]-1)*57.2;
    ypr[2]=-atan2(2*(q[]* q[2]- q[1]* q[3]), 1-2*(q[2]* q[2]  q[1]*q[1]))*57.2;
 
    text("Yaw:", 150, 20, 10);
    text(ypr[], 150, 40, 10);
    text("Pitch:", 220, 20, 10);
    text(ypr[1], 220, 40, 10);
    text("Roll:", 290, 20, 10);
    text(ypr[2], 290, 40, 10);}

При запуске визуализации на «processinge» откроется окно с графическим отображением IMU-сенсора в виде самолёта. Самолёт на экране будет повторять перемещения IMU-сенсора в пространстве.

IMU-сенсор 10 DOF v1 для Raspberry Pi Pico: инструкция, примеры использования и документация [Амперка / Вики]

Магнитометр

Датчик, который измеряет проекцию индукции магнитного поля на ось чувствительности. Магнитометру свойственны искажения hard-iron и soft-iron. Hard-iron искажение — это аддитивный эффект, когда к измеряемому полю добавляется постоянная составляющая. Причиной может быть, например, действие постоянного магнита или собственное смещение нуля датчика.

Искажение soft-iron — мультипликативный эффект, отражающий изменение направления и/или ослабление вектора магнитной индукции. Этот эффект может быть вызван наличием металлического предмета в непосредственной близости от магнитометра или же собственными искажениями датчика — погрешностью масштабного коэффициента или перекосом его оси чувствительности.

Примем модель измерений триады магнитометров:

где

$m_{XYZ}$

– измерения магнитометра в ССК XYZ,

$S_{m}$

– диагональная матрица перекоса осей и масштабных коэффициентов (которая описывает эффект soft–iron),

$M_{XYZ}$

– вектор истинной магнитной индукции в ССК,

$b_{m}$

– смещение нулей магнитометра (описывает эффект hard–iron),

$n_{m}$

– шум измерений.


Матрица перекоса осей и масштабных коэффициентов магнитометра:

элементы, расположенные на главной диагонали (

$S_{m,1,1}, S_{m,2,2}, S_{m,3,3}$

) — масштабные коэффициенты и их погрешности по трём осям магнитометра, остальные элементы матрицы — перекосы осей магнитометра. Все элементы матрицы также учитывают эффект soft-iron.

Общие сведения о датчике mpu9250

Датчик MPU9250 реализует функции 3-осевого гироскопа, 3-осевого акселерометра и 3-осевого магнитометра, то есть он представляет собой 9-ти осевой IMU сенсор (Inertial Measurement Unit – инерционное измерительное устройство). Его внешний вид с распиновкой контактов показан на следующем рисунке.

Назначение контактов датчика MPU9250:

Обозначение контактаНазначение контакта
VCCВнешнее питание 3.3 В
GNDОбщий
SCLЛиния тактовых импульсов I2C и SPI
SDAЛиния данных для I2C или SPI
EDAЛиния данных при подключении внешних датчиков по шине I2C
ECLЛиния тактов при подключении внешних датчиков по шине I2C
AD0Для выставления адреса I2C в режиме I2C. В режиме SPI это линия данных от датчика
INTЛиния прерываний. Срабатывание настраивается при конфигурировании датчика MPU-9250
NCSВ режиме SPI – линия выбора ведомого (chip select). В режиме I2C не соединяется ни с чем
FSYNCЗависит от конфигурации

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

Рассмотрим более подробно работу составных частей датчика.

Гироскоп – это сенсор, реагирующий на изменение углов ориентации в пространстве.

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

Магнитометр представляет собой устройство для измерения интенсивности ближайшего магнитного поля, действующего на объект.

Датчик MPU9250 позволяет измерять такие параметры как угол yaw (рыскание), угол pitch (тангаж) и угол roll (крен). Графическое изображение этих параметров представлено на следующем рисунке.

Ориентация объекта в пространстве

Определим ориентацию объекта в пространстве с учётом направления на север. Для этого показания акселерометра, гироскопа и компаса используем как входные данные для фильтра Маджвика. На выходе получим кватернионы, которые для наглядности пересчитаем в самолётные углы Эйлера.

imu_9DOF.py
# Импортируем необходимые модулиfrom amperka_pico_imu_10dof import icm20948
importtimeimportmath
 
# Печатаем тестовую строку в консольprint("IMU-sensor Test Program")# Создаём объект для работы с IMU-сенсором
imu=icm20948.ICM20948()
 
 
whileTrue:
    # Считываем данных с акселерометра
    accelerometer = imu.read_accelerometer_g()# Считываем данных с гироскопа
    gyroscope = imu.read_gyroscope_rad()# Считываем данных с магнитометра
    magnetometer = imu.read_magnetometer_uT()
    ax = accelerometer[]
    ay = accelerometer[1]
    az = accelerometer[2]
    gx = gyroscope[]
    gy = gyroscope[1]
    gz = gyroscope[2]
    mx = magnetometer[]
    my = magnetometer[1]
    mz = magnetometer[2]# Обновляем входные данные в фильтр Маджвика
    imu.AHRS_update(gx, gy, gz, ax, ay, az, mx, my, mz)# Получаем из фильтра углы Эйлера: yaw, pitch и roll
    yaw = imu.get_yaw();
    pitch = imu.get_pitch();
    roll = imu.get_roll();print(f"Yaw:t{yaw}")print(f"Pitch:t{pitch }")print(f"Roll:t{roll}")print()time.sleep(0.1)

Получение “реальных” значений

Наконец, на завершающем этапе обработки данных, мы получим значения параметров yaw, pitch и roll из кватернионов.

Преобразователь питания rt9193-18

Понижающий DC-DC преобразователь RT9193-18 обеспечивает питание инерциального датчика и светодиодной индикации. Входное напряжение поступает от преобразователя RT9193-33. Выходное напряжение 1,8 В с максимальным выходным током 300 мА.

Преобразователь питания rt9193-33

Работа с arduino и mpu6050 | alexgyver

Для достижения максимальной точности измерений нужно откалибровать акселерометр и гироскоп. Калибровка акселерометра позволяет выставить “ноль” для вектора силы тяжести, а калибровка гироскопа уменьшает его “дрифт”, то есть статическое отклонение в режиме покоя. Идеально откалиброванный и лежащий горизонтально датчик должен показывать ускорение ~16384 по оси Z и нули по всем остальным осям ускорения и угловой скорости. Но это фантастика =)
Максимально правильно использовать калибровку в проекте нужно так: калибровка по запросу (кнопка, меню, и т.д.), затем запись калибровочных значений в EEPROM. При запуске – чтение и настройка оффсетов. Да, можно замерить значения по всем 6 осям в покое, сохранить их в переменные, а затем вычитать из свежих прочитанных в процессе работы. Такой способ работает для каких-то базовых операций с датчиком (определение перегрузок, тряски, наличия вращения, и т.д.). Для использования MPU6050 с целью максимально точного определения углов поворота платы такой вариант к сожалению не подходит: калибровать нужно

рекурсивно

.
Рассмотрим несколько примеров калибровки, первый – из библиотеки. Калибрует долго, но максимально точно. 

При малых вибрациях и движениях датчика в процессе калибровки (даже от громкого звука) калибровка может не закончиться

. Второй вариант – мой упрощённый алгоритм калибровки, калибрует быстро, без возможности зависнуть при тряске, но даёт менее точный результат. Я делюсь примерами, в свой проект их нужно будет переносить вручную и аккуратно =)

Калибровка из примера библиотеки

// положи датчик горизонтально, надписями вверх
// ДАЖЕ НЕ ДЫШИ НА НЕГО
// отправь любой символ в сериал, чтобы начать калибровку
// жди результатов

// --------------------- НАСТРОЙКИ ----------------------
const int buffersize = 70;     // количество итераций калибровки
const int acel_deadzone = 10;  // точность калибровки акселерометра (по умолчанию 8)
const int gyro_deadzone = 6;   // точность калибровки гироскопа (по умолчанию 2)
// --------------------- НАСТРОЙКИ ----------------------

#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
MPU6050 mpu(0x68);
int16_t ax, ay, az, gx, gy, gz;
int mean_ax, mean_ay, mean_az, mean_gx, mean_gy, mean_gz, state = 0;
int ax_offset, ay_offset, az_offset, gx_offset, gy_offset, gz_offset;

///////////////////////////////////   SETUP   ////////////////////////////////////
void setup() {
  Wire.begin();
  Serial.begin(9600);
  mpu.initialize();
  // ждём очистки сериал
  while (Serial.available() && Serial.read()); // чистим
  while (!Serial.available()) {
    Serial.println(F("Send any character to start sketch.n"));
    delay(1500);
  }
  while (Serial.available() && Serial.read()); // чистим ещё на всякий
  Serial.println("nMPU6050 Calibration Sketch");
  delay(2000);
  Serial.println("nYour MPU6050 should be placed in horizontal position, with package letters facing up");
  delay(3000);

  // проверка соединения
  Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  delay(1000);

  // сбросить оффсеты
  mpu.setXAccelOffset(0);
  mpu.setYAccelOffset(0);
  mpu.setZAccelOffset(0);
  mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(0);
  mpu.setZGyroOffset(0);
}

///////////////////////////////////   LOOP   ////////////////////////////////////
void loop() {
  if (state == 0) {
    Serial.println("nReading sensors for first time...");
    meansensors();
    state  ;
    delay(1000);
  }
  if (state == 1) {
    Serial.println("nCalculating offsets...");
    calibration();
    state  ;
    delay(1000);
  }
  if (state == 2) {
    meansensors();
    Serial.println("nFINISHED!");
    Serial.print("nSensor readings with offsets:t");
    Serial.print(mean_ax);
    Serial.print("t");
    Serial.print(mean_ay);
    Serial.print("t");
    Serial.print(mean_az);
    Serial.print("t");
    Serial.print(mean_gx);
    Serial.print("t");
    Serial.print(mean_gy);
    Serial.print("t");
    Serial.println(mean_gz);
    Serial.print("Your offsets:t");
    Serial.print(ax_offset);
    Serial.print(", ");
    Serial.print(ay_offset);
    Serial.print(", ");
    Serial.print(az_offset);
    Serial.print(", ");
    Serial.print(gx_offset);
    Serial.print(", ");
    Serial.print(gy_offset);
    Serial.print(", ");
    Serial.println(gz_offset);
    Serial.println("nData is printed as: acelX acelY acelZ giroX giroY giroZ");
    Serial.println("Check that your sensor readings are close to 0 0 16384 0 0 0");
    Serial.println("If calibration was succesful write down your offsets so you can set them in your projects using something similar to mpu.setXAccelOffset(youroffset)");
    while (1);
  }
}
///////////////////////////////////   FUNCTIONS   ////////////////////////////////////
void meansensors() {
  long i = 0, buff_ax = 0, buff_ay = 0, buff_az = 0, buff_gx = 0, buff_gy = 0, buff_gz = 0;
  while (i < (buffersize   101)) { // read raw accel/gyro measurements from device
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    if (i > 100 && i <= (buffersize   100)) { //First 100 measures are discarded
      buff_ax = buff_ax   ax;
      buff_ay = buff_ay   ay;
      buff_az = buff_az   az;
      buff_gx = buff_gx   gx;
      buff_gy = buff_gy   gy;
      buff_gz = buff_gz   gz;
    }
    if (i == (buffersize   100)) {
      mean_ax = buff_ax / buffersize;
      mean_ay = buff_ay / buffersize;
      mean_az = buff_az / buffersize;
      mean_gx = buff_gx / buffersize;
      mean_gy = buff_gy / buffersize;
      mean_gz = buff_gz / buffersize;
    }
    i  ;
    delay(2);
  }
}

void calibration() {
  ax_offset = -mean_ax / 8;
  ay_offset = -mean_ay / 8;
  az_offset = (16384 - mean_az) / 8;
  gx_offset = -mean_gx / 4;
  gy_offset = -mean_gy / 4;
  gz_offset = -mean_gz / 4;

  while (1) {
    int ready = 0;
    mpu.setXAccelOffset(ax_offset);
    mpu.setYAccelOffset(ay_offset);
    mpu.setZAccelOffset(az_offset);
    mpu.setXGyroOffset(gx_offset);
    mpu.setYGyroOffset(gy_offset);
    mpu.setZGyroOffset(gz_offset);
    meansensors();
    Serial.println("...");

    if (abs(mean_ax) <= acel_deadzone) ready  ;
    else ax_offset = ax_offset - mean_ax / acel_deadzone;
    if (abs(mean_ay) <= acel_deadzone) ready  ;
    else ay_offset = ay_offset - mean_ay / acel_deadzone;
    if (abs(16384 - mean_az) <= acel_deadzone) ready  ;
    else az_offset = az_offset   (16384 - mean_az) / acel_deadzone;
    if (abs(mean_gx) <= gyro_deadzone) ready  ;
    else gx_offset = gx_offset - mean_gx / (gyro_deadzone   1);
    if (abs(mean_gy) <= gyro_deadzone) ready  ;
    else gy_offset = gy_offset - mean_gy / (gyro_deadzone   1);
    if (abs(mean_gz) <= gyro_deadzone) ready  ;
    else gz_offset = gz_offset - mean_gz / (gyro_deadzone   1);
    if (ready == 6) break;
  }
}

Мой вариант калибровки

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 mpu;
#define BUFFER_SIZE 100
int16_t ax, ay, az;
int16_t gx, gy, gz;

void setup() {
  Wire.begin();
  Serial.begin(9600);
  mpu.initialize();
  mpu.setXAccelOffset(0);
  mpu.setYAccelOffset(0);
  mpu.setZAccelOffset(0);
  mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(0);
  mpu.setZGyroOffset(0);
  Serial.println(F("Send any character to start sketch"));

  delay(100);
  while (1) {                     //входим в бесконечный цикл
    if (Serial.available() > 0) { //если нажата любая кнопка
      Serial.read();              //прочитать (чтобы не висел в буфере)
      break;                      //выйти из цикла
    }
  }
  delay(1000);
}

void loop() {
  // выводим начальные значения
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  Serial.print(ax); Serial.print(" ");
  Serial.print(ay); Serial.print(" ");
  Serial.print(az); Serial.print(" ");
  Serial.print(gx); Serial.print(" ");
  Serial.print(gy); Serial.print(" ");
  Serial.println(gz);
  calibration();

  // выводим значения после калибровки
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  Serial.print(ax); Serial.print(" ");
  Serial.print(ay); Serial.print(" ");
  Serial.print(az); Serial.print(" ");
  Serial.print(gx); Serial.print(" ");
  Serial.print(gy); Serial.print(" ");
  Serial.println(gz);
  delay(20);
  while (1);
}

// ======= ФУНКЦИЯ КАЛИБРОВКИ =======
void calibration() {
  long offsets[6];
  long offsetsOld[6];
  int16_t mpuGet[6];

  // используем стандартную точность
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);

  // обнуляем оффсеты
  mpu.setXAccelOffset(0);
  mpu.setYAccelOffset(0);
  mpu.setZAccelOffset(0);
  mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(0);
  mpu.setZGyroOffset(0);
  delay(10);
  Serial.println("Calibration start. It will take about 5 seconds");
  for (byte n = 0; n < 10; n  ) {     // 10 итераций калибровки
    for (byte j = 0; j < 6; j  ) {    // обнуляем калибровочный массив
      offsets[j] = 0;
    }
    for (byte i = 0; i < 100   BUFFER_SIZE; i  ) { // делаем BUFFER_SIZE измерений для усреднения
      mpu.getMotion6(&mpuGet[0], &mpuGet[1], &mpuGet[2], &mpuGet[3], &mpuGet[4], &mpuGet[5]);
      if (i >= 99) {                         // пропускаем первые 99 измерений
        for (byte j = 0; j < 6; j  ) {
          offsets[j]  = (long)mpuGet[j];   // записываем в калибровочный массив
        }
      }
    }
    for (byte i = 0; i < 6; i  ) {
      offsets[i] = offsetsOld[i] - ((long)offsets[i] / BUFFER_SIZE); // учитываем предыдущую калибровку
      if (i == 2) offsets[i]  = 16384;                               // если ось Z, калибруем в 16384
      offsetsOld[i] = offsets[i];
    }
    // ставим новые оффсеты
    mpu.setXAccelOffset(offsets[0] / 8);
    mpu.setYAccelOffset(offsets[1] / 8);
    mpu.setZAccelOffset(offsets[2] / 8);
    mpu.setXGyroOffset(offsets[3] / 4);
    mpu.setYGyroOffset(offsets[4] / 4);
    mpu.setZGyroOffset(offsets[5] / 4);
    delay(2);
  }
  /*
    // выводим в порт
    Serial.println("Calibration end. Your offsets:");
    Serial.println("accX accY accZ gyrX gyrY gyrZ");
    Serial.print(mpu.getXAccelOffset()); Serial.print(", ");
    Serial.print(mpu.getYAccelOffset()); Serial.print(", ");
    Serial.print(mpu.getZAccelOffset()); Serial.print(", ");
    Serial.print(mpu.getXGyroOffset()); Serial.print(", ");
    Serial.print(mpu.getYGyroOffset()); Serial.print(", ");
    Serial.print(mpu.getZGyroOffset()); Serial.println(" ");
    Serial.println(" ");
  */
}

Мой вариант калибровки запись в EEPROM

// калибровка и запись в EEPROM
// при запуске настраиваем оффсеты
// послать символ в сериал - начать повторную калибровку
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

MPU6050 mpu;
#define BUFFER_SIZE 100
#define START_BYTE 1010
int16_t ax, ay, az;
int16_t gx, gy, gz;

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

  //----------------- ВСПОМИНАЕМ ОФФСЕТЫ ------------------------
  int offsets[6];
  EEPROM.get(START_BYTE, offsets);

  // ставим оффсеты из памяти
  mpu.setXAccelOffset(offsets[0]);
  mpu.setYAccelOffset(offsets[1]);
  mpu.setZAccelOffset(offsets[2]);
  mpu.setXGyroOffset(offsets[3]);
  mpu.setYGyroOffset(offsets[4]);
  mpu.setZGyroOffset(offsets[5]);
  //----------------- ВСПОМИНАЕМ ОФФСЕТЫ ------------------------

  Serial.println(F("Send any character to start sketch"));
  delay(100);
  while (1) {
    //входим в бесконечный цикл
    if (Serial.available() > 0) {     // если нажата любая кнопка
      Serial.read();                  // прочитать (чтобы не висел в буфере)
      break;                          // выйти из цикла
    }
  }
  delay(1000);
}

void loop() {
  calibration();
  Serial.println("Current offsets:");
  Serial.println("accX accY accZ gyrX gyrY gyrZ");
  Serial.print(mpu.getXAccelOffset()); Serial.print(", ");
  Serial.print(mpu.getYAccelOffset()); Serial.print(", ");
  Serial.print(mpu.getZAccelOffset()); Serial.print(", ");
  Serial.print(mpu.getXGyroOffset()); Serial.print(", ");
  Serial.print(mpu.getYGyroOffset()); Serial.print(", ");
  Serial.print(mpu.getZGyroOffset()); Serial.println(" ");
  Serial.println(" ");

  // выводим значения после калибровки
  Serial.println("Readings:");
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  Serial.print(ax); Serial.print(" ");
  Serial.print(ay); Serial.print(" ");
  Serial.print(az); Serial.print(" ");
  Serial.print(gx); Serial.print(" ");
  Serial.print(gy); Serial.print(" ");
  Serial.println(gz);
  delay(20);
  while (1);
}

// =======  ФУНКЦИЯ КАЛИБРОВКИ И ЗАПИСИ В ЕЕПРОМ =======
void calibration() {
  long offsets[6];
  long offsetsOld[6];
  int16_t mpuGet[6];

  // используем стандартную точность
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);

  // обнуляем оффсеты
  mpu.setXAccelOffset(0);
  mpu.setYAccelOffset(0);
  mpu.setZAccelOffset(0);
  mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(0);
  mpu.setZGyroOffset(0);
  delay(5);

  for (byte n = 0; n < 10; n  ) {     // 10 итераций калибровки
    for (byte j = 0; j < 6; j  ) {    // обнуляем калибровочный массив
      offsets[j] = 0;
    }
    for (byte i = 0; i < 100   BUFFER_SIZE; i  ) {
      // делаем BUFFER_SIZE измерений для усреднения
      mpu.getMotion6(&mpuGet[0], &mpuGet[1], &mpuGet[2], &mpuGet[3], &mpuGet[4], &mpuGet[5]);

      if (i >= 99) {                         // пропускаем первые 99 измерений
        for (byte j = 0; j < 6; j  ) {
          offsets[j]  = (long)mpuGet[j];   // записываем в калибровочный массив
        }
      }
    }

    for (byte i = 0; i < 6; i  ) {
      offsets[i] = offsetsOld[i] - ((long)offsets[i] / BUFFER_SIZE); // учитываем предыдущую калибровку
      if (i == 2) offsets[i]  = 16384;                               // если ось Z, калибруем в 16384
      offsetsOld[i] = offsets[i];
    }

    // ставим новые оффсеты
    mpu.setXAccelOffset(offsets[0] / 8);
    mpu.setYAccelOffset(offsets[1] / 8);
    mpu.setZAccelOffset(offsets[2] / 8);
    mpu.setXGyroOffset(offsets[3] / 4);
    mpu.setYGyroOffset(offsets[4] / 4);
    mpu.setZGyroOffset(offsets[5] / 4);
    delay(2);
  }

  // пересчитываем для хранения
  for (byte i = 0; i < 6; i  ) {
    if (i < 3) offsets[i] /= 8;
    else offsets[i] /= 4;
  }

  // запись в память
  EEPROM.put(START_BYTE, offsets);
}

Также в библиотеке есть готовые функции для калибровки акселя и гиро, они принимают количество итераций калибровки (до 15):

mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);

Функции блокирующие, выполняются и автоматически выставляют оффсеты, т.е. датчик сразу калибруется. Для чтения оффсетов (например для записи в EEPROM) можно воспользоваться тем же способом, что и раньше:

mpu.getXAccelOffset();
mpu.getYAccelOffset();
mpu.getZAccelOffset();
mpu.getXGyroOffset();
mpu.getYGyroOffset();
mpu.getZGyroOffset();

Расчет углов ориентации


Благодаря наличию на Земле силы тяжести, акселерометры «чувствуют» направление вниз. Их измерения используются для расчета углов крена и тангажа. Формулы для расчёта можно найти

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

можно найти формулу для расчёта магнитного азимута.

где

$atan2$

— функция полного арктангенса,

$a_{X}$

— измерения акселерометра по трём осям в ССК,

$m_{E}$

— измерения магнитометра по осям X’, Y’ (измерения магнитометров пересчитаны в плоскость).

Считывание данных с датчика mpu9250 в плату arduino mega 2560

Для работы с датчиком MPU9250 существует достаточно много библиотек, но мы будем использовать одну из самых популярных среди них: MPU-9250 Arduino Library by Kriswiner.

Когда вы сохраните библиотеку в ваш каталог Arduino, можно приступать к работе с ней. Откройте пример MPU9250BasicAHRS.ino.

Также выполните следующие соединения в схеме:

MPU9250 Breakout ——— Arduino• VIN ———————- 5V• SDA ———————– SDA (Pin 20)• SCL ———————– SCL (Pin 21)• GND ———————- GND

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

Углы ориентации

IMU-сенсор 10 DOF v1 для Raspberry Pi Pico: инструкция, примеры использования и документация [Амперка / Вики]

Будем понимать под углами ориентации объекта углы Эйлера — крен (roll), тангаж (pitch), рыскание (yaw), связывающие собственную систему координат XYZ объекта и локальную систему координат восток-север-верх (ENU — East North Up). Углы roll, pitch, yaw обозначают поворот, который нужно совершить осям XYZ чтобы перейти в оси ENU. Соответственно, нулевые углы означают, что ось X объекта смотрит на восток, ось Y объекта смотрит на север, ось Z — вверх.

Порядок поворота осей — начиная с последнего угла: сначала на yaw (вокруг оси Z), потом на pitch (вокруг оси Y), потом на roll (вокруг оси X).

Фильтрация данных

Поскольку исходные данные (raw data) содержат достаточно большое количество шума, мы будем использовать различные фильтры (Madgwick/Mahony/Kalman) чтобы преобразовать их в кватернионы (Quaternions).

Похожие дроны:

Нет похожих дронов.

Смотрите про коптеры:  Закон о беспилотниках: всё, что нужно знать пользователю БПЛА с взлётной массой выше 150 грамм
products waveshare-raspberry-pi-pico-imu-sensor-10dof-v1
Оцените статью
Радиокоптер.ру
Добавить комментарий

© 2022 Радиокоптер.ру
Adblock
detector