Блог

Я пытаюсь использовать Arduino Nano 33 BLE с экраном, чтобы создать что-то для индикатора положения самолета и гироскопа направления.

Для этой цели мне нужно было бы иметь точные углы Эйлера. Я узнал, что Nano поставляется с датчиком 9DOF, и я попытался использовать библиотеку Мэджвика для преобразования данных датчика в полезные углы.

Однако похоже, что происходит дрейф вдоль оси рыскания, а также при перемещении платы вдоль оси тангажа и рыскания фильтру требуется много времени, чтобы догнать, иногда даже несколько секунд, чтобы получить результат.

Другим решением было бы попытаться использовать Adafruit BNO055, который утверждает, что напрямую обеспечивает углы Эйлера. Однако я думаю, что более элегантным решением было бы настроить мой код, чтобы он работал с датчиком, который уже предусмотрен на Nano.

Идеи?

 #include "Arduino_LSM6DS3.h"
#include "MadgwickAHRS.h"
#include "Arduino_LSM9DS1.h"

// initialize a Madgwick filter:
Madgwick filter;
// sensor's sample rate is fixed at 104 Hz:
const float sensorRate = 104.00;

float sax, say, saz, sgx, sgy, sgz;

void setup() {
  Serial.begin(9600);
  // attempt to start the IMU:
  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU");
    // stop here if you can't access the IMU:
    while (true);
  }
  // start the filter to run at the sample rate:
  filter.begin(sensorRate);
}

long lastPrint = 0;
long nz = 0;
float x = 0, y = 0, z = 0;

void loop() {
  // values for acceleration and rotation:
  float xAcc, yAcc, zAcc;
  float xGyro, yGyro, zGyro;

  // values for orientation:
  float roll, pitch, heading;
  // check if the IMU is ready to read:
  if (IMU.accelerationAvailable() amp;amp;
      IMU.gyroscopeAvailable()) {
    // read accelerometer amp;and gyrometer:
    IMU.readAcceleration(xAcc, yAcc, zAcc);
    IMU.readGyroscope(xGyro, yGyro, zGyro);
    nz  ;
    if (nz < 500)   //hold the board still until nz is 500 for calibration
    {
      sgz  = zGyro;
      sgx  = xGyro;
      sgy  = yGyro;
      x = sgx / nz;
      y = sgy / nz;
      z = sgz / nz;
    }

    // update the filter, which computes orientation:
    filter.updateIMU(xGyro - x, yGyro - y, zGyro - z, xAcc, yAcc, zAcc);

    // print the heading, pitch and roll
    roll = filter.getRoll();
    pitch = filter.getPitch();
    heading = filter.getYaw();
    long a = millis();
    if (lastPrint   333 < millis())
    {
      lastPrint = a;
      Serial.print(nz);
      Serial.print(" Acc ");
      Serial.print(xAcc);
      Serial.print(" ");
      Serial.print(yAcc);
      Serial.print(" ");
      Serial.print(zAcc);
      Serial.print(" ");

      Serial.print("Gyr ");
      Serial.print(xGyro);
      Serial.print(" ");
      Serial.print(yGyro);
      Serial.print(" ");
      Serial.print(zGyro);
      Serial.print(" avg ");

      Serial.print(" ~~Orientation: ");
      Serial.print(heading);
      Serial.print(" ");
      Serial.print(pitch);
      Serial.print(" ");
      Serial.println(roll);
    }
  }
}
 

Дрейф рыскания, который вы видите, может быть просто потому, что вы не считываете магнитометр LSM9DS1 и не передаете его информацию в фильтр.

Вы не можете компенсировать отклонение гироскопа от курса с помощью акселерометра; акселерометр не видит отклонения от курса; здесь вступает в дело магнитометр. Магнитометр похож на компас; он может видеть направление и изменения в рыскании. Акселерометр не может, поэтому, чтобы исправить дрейф при рыскании, вам нужен магнитометр.

Все гироскопы дрейфуют, и вы должны это компенсировать. Это не просто вопрос вычитания среднего дрейфа; фильтры (дополнительные, Мэджвик, Калман и т. Д.) Используются для объединения данных гироскопа, акселерометра и магнитометра для вычисления плавных данных 3D-ориентации, которые не показывают дрейфа. То, что производители дронов называют «гироскопом», на самом деле представляет собой комбинацию гироскопов, акселерометров, магнитометров и математики.

В вашей настройке могут быть другие ошибки, я не проверял, но это фундаментально: вы обрабатываете датчик 9DoF как датчик 6DoF, и там нет ничего, чтобы компенсировать дрейф рыскания.

В библиотеке Мэджвика есть функция update() , которая будет получать информацию со всех 3 датчиков; updateIMU() можно использовать только 2.

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