проблемы с IMU-сенсор от амперки

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

Использую Arduino UNO, вот так сенсор подключен к ней:
Сенсор | Arduino
V________+5
G_______GND
D________A4
C _______ A5

Никак не могу понять почему такое происходит.
На всякий случай кину код из примеров

Спойлер: Для Arduino

Код (C++):

// скетч для вывода кватернионов в serial-порт
// для дальнейшего графического просмотра ориентации объекта
// в среде processing «MadgwickProcessingDraw9DOF.pde»
// библиотека для работы I²C
#include <Wire.h>
// библиотека для работы с модулями IMU
#include <TroykaIMU.h>
// множитель фильтра
#define BETA 0.22
// создаём объект для фильтра Madgwick
Madgwick filter;
// создаём объект для работы с акселерометром
Accelerometer accel;
// создаём объект для работы с гироскопом
Gyroscope gyro;
// создаём объект для работы с компасом
Compass compass;
// переменные для данных с гироскопа, акселерометра и компаса
float gx, gy, gz, ax, ay, az, mx, my, mz;
// получаемые углы ориентации (Эйлера)
float yaw, pitch, roll;
// переменная для хранения частоты выборок фильтра
float fps = 100;
// калибровочные значения компаса
// полученные в калибровочной матрице из примера «compassCalibrateMatrixx»
const double compassCalibrationBias[3] = {
  524.21,
  3352.214,
  -1402.236
};
const double compassCalibrationMatrix[3][3] = {
  {1.757, 0.04, -0.028},
  {0.008, 1.767, -0.016},
  {-0.018, 0.077, 1.782}
};
void setup()
{
  // открываем последовательный порт
  Serial.begin(115200);
  // инициализация акселерометра
  accel.begin();
  // инициализация гироскопа
  gyro.begin();
  // инициализация компаса
  compass.begin();
  // калибровка компаса
  compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias);
}
void loop()
{
  // запоминаем текущее время
  unsigned long startMillis = millis();
  // считываем данные с акселерометра в единицах G
  accel.readGXYZ(&ax, &ay, &az);
  // считываем данные с гироскопа в радианах в секунду
  gyro.readRadPerSecXYZ(&gx, &gy, &gz);
  // считываем данные с компаса в Гауссах
  compass.readCalibrateGaussXYZ(&mx, &my, &mz);
  // устанавливаем коэффициенты фильтра
  filter.setKoeff(fps, BETA);
  // обновляем входные данные в фильтр
  filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);
  if (Serial.available() > 0) {
    int val = Serial.read();
    // если пришёл символ 's'
    if (val == 's') {
      float q0, q1, q2, q3;
      filter.readQuaternions(&q0, &q1, &q2, &q3);
      // выводим кватернионы в serial-порт
      Serial.print(q0);
      Serial.print(",");
      Serial.print(q1);
      Serial.print(",");
      Serial.print(q2);
      Serial.print(",");
      Serial.println(q3);
    }
  }
  // вычисляем затраченное время на обработку данных
  unsigned long deltaMillis = millis() - startMillis;
  // вычисляем частоту обработки фильтра
  fps = 1000 / deltaMillis;
}

Спойлер: Для Processing

Код (C++):

import processing.serial.*;
import toxi.geom.*;
import toxi.processing.*;
// The serial port
Serial port;                        
String message;
float[] q = new float[4];
Quaternion quat = new Quaternion(1, 0, 0, 0);
// new line character in ASCII
int newLine = 13;
String [] massQ = new String [4];
float[] ypr = new float[3];
void setup()
{
  // size form 300x300
  size(400, 400, P3D);
  // open serial port
  // replace "COM29" with the COM port on which your arduino is connected
  port = new Serial(this, "COM10", 115200);
  // wait init Serial for Arduino
  delay(1000);
}
void draw()
{
  // read and parse incoming serial message
  serialEvent();
  // set background to black
  background(0);
  printQuaternions();
  printYawPitchRoll();
  // set position to centre
  translate(width / 2, height / 2);
  // begin object
  pushMatrix();
  float[] axis = quat.toAxisAngle();
  rotate(axis[0], axis[2], axis[3], axis[1]);
  // draw main body in red
  drawBody();
  // draw front-facing tip in blue
  drawCylinder();
  // draw Triangles
  drawTriangles();
  // draw Quards
  drawQuards();
  // end of object
  popMatrix();
  // send 's' to Arduino
  port.write('s');
}
void serialEvent() { // Function for parsing serial message
  // 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[0] = float(massQ[0]);
    q[1] = float(massQ[1]);
    q[2] = float(massQ[2]);
    q[3] = float(massQ[3]);
  }
  // print values to console
  print(q[0]);
  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[0], q[1], q[2], q[3]);
}
void drawCylinder() {
  float topRadius = 0;
  float bottomRadius = 20;
  float tall = 20;
  int sides = 8;
  // begin object
  pushMatrix();
  translate(0, 0, -120);
  rotateX(PI/2);
  fill(0, 0, 255, 200);
  float angle = 0;
  float angleIncrement = TWO_PI / sides;
  beginShape(QUAD_STRIP);
  for (int i = 0; i < sides + 1; ++i) {
    vertex(topRadius*cos(angle), 0, topRadius*sin(angle));
    vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle));
    angle += angleIncrement;
  }
  endShape();
  // if it is not a cone, draw the circular top cap
  if (topRadius != 0) {
    angle = 0;
    beginShape(TRIANGLE_FAN);
    // center point
    vertex(0, 0, 0);
    for (int i = 0; i < sides + 1; i++) {
      vertex(topRadius * cos(angle), 0, topRadius * sin(angle));
      angle += angleIncrement;
    }
    endShape();
  }
  // if it is not a cone, draw the circular bottom cap
  if (bottomRadius != 0) {
    angle = 0;
    beginShape(TRIANGLE_FAN);
    // center point
    vertex(0, tall, 0);
    for (int i = 0; i < sides + 1; i++) {
      vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle));
      angle += angleIncrement;
    }
   endShape();
  }
  popMatrix();
}
void drawBody() {
  fill(255, 0, 0, 200);
  box(10, 10, 200);
}
void drawTriangles() {
  // draw wings and tail fin in green
  fill(0, 255, 0, 200);
  beginShape(TRIANGLES);
  // wing top layer
  vertex(-100,  2, 30); vertex(0,  2, -80); vertex(100,  2, 30);
  // wing bottom layer
  vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30);
  // tail left layer
  vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70);
  // tail right layer
  vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70);
  endShape();
}
void drawQuards() {
  beginShape(QUADS);
  vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(  0, -2, -80); vertex(  0, 2, -80);
  vertex( 100, 2, 30); vertex( 100, -2, 30); vertex(  0, -2, -80); vertex(  0, 2, -80);
  vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2,  30); vertex(100, 2,  30);
  vertex(-2,   0, 98); vertex(2,   0, 98); vertex(2, -30, 98); vertex(-2, -30, 98);
  vertex(-2,   0, 98); vertex(2,   0, 98); vertex(2,   0, 70); vertex(-2,   0, 70);
  vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2,   0, 70); vertex(-2,   0, 70);
  endShape();
}
void printQuaternions() {
  textMode(SHAPE); // set text mode to shape
  textSize(13);
  fill(255, 255, 255);
  text("Quaternions:", 20, 20, 10);
  text(q[0], 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[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0] * q[0] + 2 * q[1] * q[1] - 1)*57.2;
  ypr[1] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1)*57.2;
  ypr[2] = -atan2(2.0f * (q[0] * q[2] - q[1] * q[3]), 1.0f - 2.0f * (q[2] * q[2] + q[1] * q[1]))*57.2;
  text("Yaw:", 150, 20, 10);
  text(ypr[0], 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);
}

 

У меня все нормально.

хотя с этим модулем у меня проблемы.
можете попробуовать пример accel :

Код (C++):

// библиотека для работы с модулями IMU
#include <TroykaIMU.h>
// создаём объект для работы с акселерометром
Accelerometer accel(ACCEL_ADDRESS_V1);
// если напаяна перемычка, устройство доступно по новому адресу
// Accelerometer accel(ACCEL_ADDRESS_V2);

void setup()
{
  // открываем последовательный порт
  Serial.begin(115200);
  // выводим сообщение о начале инициализации
  Serial.println("Accelerometer init...");
  // инициализация акселерометра
  accel.begin();
  // устанавливаем чувствительность акселерометра
  // 2g — по умолчанию, 4g, 8g
  accel.setRange(RANGE_2G);
  // выводим сообщение об удачной инициализации
  Serial.println("Initialization completed");
}
void loop()
{
  // вывод направления и величины ускорения в м/с² по оси X
  Serial.print(accel.readAX());
  Serial.print("\t\t");
  // вывод направления и величины ускорения в м/с² по оси Y
  Serial.print(accel.readAY());
  Serial.print("\t\t");
  // вывод направления и величины ускорения в м/с² по оси Z
  Serial.print(accel.readAZ());
  Serial.print("\t\t");
  Serial.println("");
  delay(100);
  /*
  // вывод направления и величины ускорения в м/с² по X, Y и Z
  float x, y, z;
  accel.readAXYZ(&x, &y, &z);
  Serial.print(x);
  Serial.print("\t\t");
  Serial.print(y);
  Serial.print("\t\t");
  Serial.print(z);
  Serial.println("");
  delay(100);
  */
}

если положить модуль на стол и оставить в покое, что спамить в сериал будет?

 

Проблемы была именно с рысканьем, тангаж и крен, как показано у вас, и так был в норме.
Тем не менее, теперь у меня тоже все в нормально, я написал программку по-легче, все работает как должно, дрейф нуля, к сожалению, идет, но с этим уже надо программно бороться.

Спойлер: arduino

Код (C++):

#include <Wire.h>
// библиотека для работы с модулями IMU
#include <TroykaIMU.h>

// множитель фильтра
#define BETA 0.22f

// создаём объект для фильтра Madgwick
Madgwick filter;

// создаём объект для работы с акселерометром
Accelerometer accel;
// создаём объект для работы с гироскопом
Gyroscope gyro;

// переменные для данных с гироскопов, акселерометров
float gx, gy, gz, ax, ay, az;

// получаемые углы ориентации
float yaw, pitch, roll;

// переменная для хранения частоты выборок фильтра
float fps = 100;

void setup()
{
  Serial.begin(115200);
  accel.begin();
  gyro.begin();
}

void serialFlush()
{
  while(Serial.available() > 0)
  {
    Serial.read();
    //delay(1);
    delayMicroseconds(100);
  }
}
void loop()
{
  // запоминаем текущее время
  unsigned long startMillis = millis();

  // считываем данные с акселерометра в единицах G
  accel.readGXYZ(&ax, &ay, &az);
  // считываем данные с акселерометра в радианах в секунду
  gyro.readRadPerSecXYZ(&gx, &gy, &gz);
  // устанавливаем коэффициенты фильтра
  filter.setKoeff(fps, BETA);
  // обновляем входные данные в фильтр
  filter.update(gx, gy, gz, ax, ay, az);

  // получение углов yaw, pitch и roll из фильтра
  yaw =  filter.getYawDeg();
  pitch = filter.getPitchDeg();
  roll = filter.getRollDeg();

  // выводим полученные углы в serial-порт
  /*Serial.print("yaw: ");
  Serial.print(yaw);
  Serial.print("\t\t");
  Serial.print("pitch: ");
  Serial.print(pitch);
  Serial.print("\t\t");
  Serial.print("roll: ");
  Serial.println(roll);*/
  if(Serial.available() > 0)
  {
    serialFlush();
    Serial.print(yaw);
    Serial.print(", ");
    Serial.print(pitch);
    Serial.print(", ");
    Serial.println(roll);
  }
  else
    delayMicroseconds(500);
    // вычисляем затраченное время на обработку данных
    unsigned long deltaMillis = millis() - startMillis;
    // вычисляем частоту обработки фильтра
    fps = 1000 / deltaMillis;
}

 

Спойлер: processing

Код (C++):

import processing.serial.*;
import toxi.geom.*;
import toxi.processing.*;
Serial port;        
String message;
int newLine = 13;
String [] massQ = new String [4];
float x, y, z;

void setup()
{
  size(640, 480, P3D);
  port = new Serial(this, "COM10", 115200);
  delay(1000);
  //rectMode(CENTER);
}

void draw()
{
  port.write('s');
  serialEvent();
  background(0);
  text(x, 0, 12);
  text(y, 0, 24);
  text(z, 0, 36);
  stroke(255);
  translate(width / 2, height / 2);
  rotateX(2 * PI * x / 360);
  rotateY(2 * PI * -y / 360);
  rotateZ(2 * PI * z / 360);
  noFill();
  box(100);
  /*translate(width/2, height/2);
  rotate(2 * PI * q[0] / 360);
  rect(0, 0, 180, 180);
  stroke(255);
  line(0, 0, 0, width);*/
}

void serialEvent()
{
  message = port.readStringUntil(newLine);
  if (message != null)
  {
    massQ = split(message, ",");
    z = float(massQ[0]);
    x = float(massQ[1]);
    y = float(massQ[2]);
  }
}

 

 

Столкнуля с такой же проблемой: возвращается в исходное положение по рысканью.
Это "железная" проблема или софтверная, может фильтр Madgwick не правильно работает?

 

Сам спросил, сам решил. Поиски в интернетах подвердили вариант, что ошибка в фильтре Madgwick.
Если кому надо, то вылил библиотеку с фиксом бага тут https://github.com/avpdiver/SensorFusion