Гироскоп с помощью Arduino 101 | АрдуиноПлюс

Гироскоп с помощью Arduino 101 | АрдуиноПлюс Роботы
На чтение 23 мин. Просмотров 54 Опубликовано
Содержание
  1. Mpu6050 arduino code
  2. Работа с arduino и mpu6050 | alexgyver
  3. Связь
  4. Сенсоры

Mpu6050 arduino code

Here’s the Arduino code for reading the data from the MPU6050 sensor. Below the code you can find a detail description of it.

#include<Wire.h>constint MPU = 0x68; float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float elapsedTime, currentTime, previousTime;
int c = 0;
voidsetup(){
  Serial.begin(19200);
  Wire.begin();                      Wire.beginTransmission(MPU);       Wire.write(0x6B);                  Wire.write(0x00);                  Wire.endTransmission(true);        
  calculate_IMU_error();
  delay(20);
}
voidloop(){
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); 
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; 
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; 
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; 
  accAngleX = (atan(AccY / sqrt(pow(AccX, 2)   pow(AccZ, 2))) * 180 / PI) - 0.58; 
  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2)   pow(AccZ, 2))) * 180 / PI)   1.58; 
  previousTime = currentTime;        
  currentTime = millis();            
  elapsedTime = (currentTime - previousTime) / 1000; Wire.beginTransmission(MPU);
  Wire.write(0x43); Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); 
  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; 
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
  
  GyroX = GyroX   0.56; 
  GyroY = GyroY - 2; 
  GyroZ = GyroZ   0.79; 
  gyroAngleX = gyroAngleX   GyroX * elapsedTime; 
  gyroAngleY = gyroAngleY   GyroY * elapsedTime;
  yaw =  yaw   GyroZ * elapsedTime;
  
  roll = 0.96 * gyroAngleX   0.04 * accAngleX;
  pitch = 0.96 * gyroAngleY   0.04 * accAngleY;
  
  Serial.print(roll);
  Serial.print("/");
  Serial.print(pitch);
  Serial.print("/");
  Serial.println(yaw);
}
voidcalculate_IMU_error(){
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    
    AccErrorX = AccErrorX   ((atan((AccY) / sqrt(pow((AccX), 2)   pow((AccZ), 2))) * 180 / PI));
    AccErrorY = AccErrorY   ((atan(-1 * (AccX) / sqrt(pow((AccY), 2)   pow((AccZ), 2))) * 180 / PI));
    c  ;
  }
  
  AccErrorX = AccErrorX / 200;
  AccErrorY = AccErrorY / 200;
  c = 0;
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    GyroX = Wire.read() << 8 | Wire.read();
    GyroY = Wire.read() << 8 | Wire.read();
    GyroZ = Wire.read() << 8 | Wire.read();
    
    GyroErrorX = GyroErrorX   (GyroX / 131.0);
    GyroErrorY = GyroErrorY   (GyroY / 131.0);
    GyroErrorZ = GyroErrorZ   (GyroZ / 131.0);
    c  ;
  }
  
  GyroErrorX = GyroErrorX / 200;
  GyroErrorY = GyroErrorY / 200;
  GyroErrorZ = GyroErrorZ / 200;
  Serial.print("AccErrorX: ");
  Serial.println(AccErrorX);
  Serial.print("AccErrorY: ");
  Serial.println(AccErrorY);
  Serial.print("GyroErrorX: ");
  Serial.println(GyroErrorX);
  Serial.print("GyroErrorY: ");
  Serial.println(GyroErrorY);
  Serial.print("GyroErrorZ: ");
  Serial.println(GyroErrorZ);
}
Code language:Arduino(arduino)

Code Description: So first we need to include the Wire.h library which is used for the I2C communication and define some variables needed storing the data.

In the setup section, we need initialize the wire library and reset the sensor through the power management register. In order to do that we need to take a look at the datasheet of the sensor from where we can see the register address.

Also, if we want, we can select the Full-Scale Range for the accelerometer and the gyroscope using their configuration registers. For this example, we will use the default – 2g range for the accelerometer and 250 degrees/s range for the gyroscope, so I will leave this part of the code commented.

Wire.beginTransmission(MPU);
  Wire.write(0x1C);                  Wire.write(0x10);                  Wire.endTransmission(true);
  Wire.beginTransmission(MPU);
  Wire.write(0x1B);                   Wire.write(0x10);                   Wire.endTransmission(true);
  */
Code language:Arduino(arduino)

In the loop section we start by reading the accelerometer data. The data for each axis is stored in two bytes or registers and we can see the addresses of these registers from the datasheet of the sensor.

In order to read them all, we start with the first register, and using the requiestFrom() function we request to read all 6 registers for the X, Y and Z axes. Then we read the data from each register, and because the outputs are twos complement, we combine them appropriately to get the correct values.

Wire.beginTransmission(MPU);
  Wire.write(0x3B); Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); 
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; 
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; 
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; 
Code language:Arduino(arduino)

In order to get output values from -1g to 1g, suitable for calculating the angles, we divide the output with the previously selected sensitivity.

Finally, using these two formulas, we calculate the roll and pitch angles from the accelerometer data. 


  accAngleX = (atan(AccY / sqrt(pow(AccX, 2)   pow(AccZ, 2))) * 180 / PI) - 0.58; 
  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2)   pow(AccZ, 2))) * 180 / PI)   1.58; 
Code language:Arduino(arduino)

Next, using the same method we get the gyroscope data.

We read the six gyroscope registers, combine their data appropriately and divide it by the previously selected sensitivity in order to get the output in degrees per second.


  previousTime = currentTime;        
  currentTime = millis();            
  elapsedTime = (currentTime - previousTime) / 1000; Wire.beginTransmission(MPU);
  Wire.write(0x43); Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); 
  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; 
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
Code language:Arduino(arduino)

Here you can notice that I correct the output values with some small calculated error values, which I will explain how we get them in a minute. So as the outputs are in degrees per second, now we need to multiply them with the time to get just degrees. The time value is captured before each reading iteration using the millis() function.


  GyroX = GyroX   0.56; 
  GyroY = GyroY - 2; 
  GyroZ = GyroZ   0.79; 
  gyroAngleX = gyroAngleX   GyroX * elapsedTime; 
  gyroAngleY = gyroAngleY   GyroY * elapsedTime;
  yaw =  yaw   GyroZ * elapsedTime;
Code language:Arduino(arduino)

Finally, we fuse the accelerometer and the gyroscope data using a complementary filter. Here, we take 96% of the gyroscope data because it is very accurate and doesn’t suffer from external forces. The down side of the gyroscope is that it drifts, or it introduces error in the output as the time goes on.


  roll = 0.96 * gyroAngleX   0.04 * accAngleX;
  pitch = 0.96 * gyroAngleY   0.04 * accAngleY;
Code language:Arduino(arduino)

However, as we cannot calculate the Yaw from the accelerometer data, we cannot implement the complementary filter on it.

Смотрите про коптеры:  Роботы пылесосы самсунг с влажной уборкой отзывы владельцев

Before we take a look at the results, let me quickly explain how to get the error correction values.  For calculate these errors we can call the calculate_IMU_error() custom function while the sensor is in flat still position. Here we do 200 readings for all outputs, we sum them and divide them by 200.

voidcalculate_IMU_error(){
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    
    AccErrorX = AccErrorX   ((atan((AccY) / sqrt(pow((AccX), 2)   pow((AccZ), 2))) * 180 / PI));
    AccErrorY = AccErrorY   ((atan(-1 * (AccX) / sqrt(pow((AccY), 2)   pow((AccZ), 2))) * 180 / PI));
    c  ;
  }
  
  AccErrorX = AccErrorX / 200;
  AccErrorY = AccErrorY / 200;
  c = 0;
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    GyroX = Wire.read() << 8 | Wire.read();
    GyroY = Wire.read() << 8 | Wire.read();
    GyroZ = Wire.read() << 8 | Wire.read();
    
    GyroErrorX = GyroErrorX   (GyroX / 131.0);
    GyroErrorY = GyroErrorY   (GyroY / 131.0);
    GyroErrorZ = GyroErrorZ   (GyroZ / 131.0);
    c  ;
  }
  
  GyroErrorX = GyroErrorX / 200;
  GyroErrorY = GyroErrorY / 200;
  GyroErrorZ = GyroErrorZ / 200;
  Serial.print("AccErrorX: ");
  Serial.println(AccErrorX);
  Serial.print("AccErrorY: ");
  Serial.println(AccErrorY);
  Serial.print("GyroErrorX: ");
  Serial.println(GyroErrorX);
  Serial.print("GyroErrorY: ");
  Serial.println(GyroErrorY);
  Serial.print("GyroErrorZ: ");
  Serial.println(GyroErrorZ);
}
Code language:Arduino(arduino)

Работа с 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):

Смотрите про коптеры:  Roborock s5 max прикольные озвучки

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

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

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

Связь

Радиоуправление (RC)

Управление посредством радиосвязи обычно включает в себя RC передатчик/RC transmitter (в беспилотном хобби — радиоаппаратура управления/пульт) и RC приёмник (RC receiver). Для взаимодействия с БПЛА пользователю потребуется как минимум четырёх (и более) канальный RC передатчик. По умолчанию первые четыре канала связаны с:

Гироскоп с помощью Arduino 101 | АрдуиноПлюс

Все остальные имеющиеся каналы могут быть задействованы для таких действий как:

  • Арминг (Arming или Arm)/Дизарминг (Disarming или Disarm) — постановка/снятие с охраны моторов.
  • Управление подвесом (панорамирование вверх/вниз, вращение по часовой стрелке/против часовой стрелки, зуммирование)
  • Смена режимов полёта (ACRO/ANGLE и т.д.)
  • Активировать/Задействовать полезную нагрузку (парашют, зуммер или другое устройство)
  • Любое другое применение

Гироскоп с помощью Arduino 101 | АрдуиноПлюс

Большинство пользователей (пилотов БПЛА) предпочитают именно ручное управление, это ещё раз доказывает, что пилотирование при помощи аппаратуры управления по прежнему является выбором номер один. Сам по себе RC приёмник просто передаёт поступающие от RC передатчика значения, а значит не может управлять беспилотником. RC приёмник должен быть подключен к контроллеру полёта, который в свою очередь должен быть запрограммирован для приёма RC сигналов. На рынке очень мало полётных контроллеров, которые принимают входящие радиокоманды от приёмника на прямую, а большинство ПК даже обеспечивают питание приёмника от одного из контактных выводов. Дополнительные соображения при выборе пульта дистанционного управления включают в себя:

  • Не все RC передатчики могут обеспечить полный диапазон RC сигналов от 500мс до 2500мс; некоторые искусственно ограничивают этот диапазон, так как большинство используемых RC предназначены для радиоуправляемых автомобилей, самолётов и вертолётов.
  • Дальность/Макс. воздушный радиус действия (измеряется в футах или метрах) RC-системы — практически никогда не предоставляются производителями, поскольку на этот параметр влияют множество факторов, таких как помехи, температура, влажность, заряд батареи и другие.
  • Некоторые RC-системы имеют приёмник, который также имеет встроенный передатчик для передачи данных от датчика (например, GPS-координат), которые в последствии будут отображаться на ЖК-дисплее RC передатчика.
Смотрите про коптеры:  Робот пылесос Panda X500 Series

Bluetooth

Bluetooth и более поздние продукты BLE (Bluetooth Low Energy) изначально предназначались для передачи данных между устройствами без заморочек сопряжения или согласования частот. Некоторые имеющиеся на рынке контроллеры полёта могут отправлять и получать данные по беспроводной связи через соединение Bluetooth, что упрощает поиск неисправностей в полевых условиях.

Гироскоп с помощью Arduino 101 | АрдуиноПлюс

Wi-Fi

Управление по Wi-Fi обычно достигается посредством Wi-Fi роутера, компьютера (в том числе ноутбук, десктоп, планшет) или смартфон. Wi-Fi в состоянии справится как с передачей данных, так и с передачей видеопотока, но одновременно с этим эту технологию сложнее настроить/реализовать. Как и для всех Wi-Fi устройств, расстояние удаления ограничено Wi-Fi передатчиком.

Гироскоп с помощью Arduino 101 | АрдуиноПлюс

Радиочастота (RF или РЧ)

Радиочастотное (РЧ) управление в этом контексте относится к беспроводной передаче данных с компьютера или микроконтроллера на летательный аппарат с использованием РЧ передатчика/Приёмника (или двухполосного приёмопередатчика). Использование обычного радиочастотного блока, подключенного к компьютеру, позволяет осуществлять двухполосную связь на большие расстояния с высокой «плотностью» данных (обычно в последовательном формате).

Гироскоп с помощью Arduino 101 | АрдуиноПлюс

Смартфон

Хоть это и не тип связи, самого вопроса, как управлять дроном используя смартфон, достаточно, чтобы уделить ему отдельный раздел. Современные смартфоны это по сути мощные компьютеры, которые по случайному совпадению могут также совершать телефонные звонки. Почти все смартфоны имеют встроенный модуль Bluetooth, а также модуль WiFi, каждый из которых используется для управления дроном и/или получения данных и/или видео.

Гироскоп с помощью Arduino 101 | АрдуиноПлюс

Инфракрасное излучение (Infrared (IR))

Инфракрасная связь (то что можно найти в каждом телевизионном пульте дистанционного управления) редко используется для управления дронами, так как даже в обычных комнатах (не говоря уже об открытом пространстве) присутствует так много инфракрасных помех, что они не очень надёжны. Несмотря на то, что технологию можно использовать для управления БПЛА, не может быть предложена как основной вариант.

Гироскоп с помощью Arduino 101 | АрдуиноПлюс

Сенсоры

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

  • Акселерометр: Как следует из названия, акселерометры измеряют линейное ускорение по трем осям (назовём их: X, Y и Z). Обычно измеряется в «G (на рус. Же)». Стандартное (нормальное) значение, составляет g = 9.80665 м/с². Для определения положения, выход акселерометра может быть интегрирован дважды, правда из-за потерь на выходе объект может быть подвержен дрейфу. Самой значимой характеристикой трёхосевых акселерометров является то, что они регистрируют гравитацию, и как таковые, могут знать, в каком направлении «спуск». Это играет главную роль в обеспечении стабильности многороторного БЛА. Акселерометр должен быть установлен на контроллере полёта так, чтобы линейные оси совпадали с основными осями беспилотника.

Гироскоп с помощью Arduino 101 | АрдуиноПлюс

  • Гироскоп: Гироскоп измеряет скорость изменения углов по трём угловым осям (назовём их: альфа, бета и гамма). Обычно измеряется в градусах в секунду. Обратите внимание, что гироскоп не измеряет абсолютные углы напрямую, но вы можете выполнить итерацию, чтобы получить угол, который, как и у акселерометра, способствует дрейфу. Выход реального гироскопа имеет тенденцию быть аналоговым или I2C, но в большинстве случаев вам не нужно беспокоиться об этом, так как все поступающие данные обрабатываются кодом контроллера полёта. Гироскоп должен быть установлен так, чтобы его оси вращения совпадали с осями БПЛА.

Гироскоп с помощью Arduino 101 | АрдуиноПлюс

  • Инерционный измерительный блок (IMU): IMU — по сути, это небольшая плата, которая содержит как акселерометр, так и гироскоп (обычно многоосевые). Большинство из них включают трёхосевой акселерометр и трёхосевой гироскоп, другие могут включать дополнительные сенсоры, например трёхосевой магнитометр, обеспечивающий в общей сложности 9 осей измерения.

Гироскоп с помощью Arduino 101 | АрдуиноПлюс

  • Компас/Магнитометр: Электронный магнитный компас способный определять магнитное поле Земли и использовать эти данные для определения направления компаса беспилотника (относительно северного магнитного полюса). Этот сенсор почти всегда присутствует, если система имеет GPS вход и доступно от одной до трех осей.

Гироскоп с помощью Arduino 101 | АрдуиноПлюс

  • Давление/Барометр: Так как атмосферное давление изменяется по мере удаления от уровня моря, можно использовать сенсор давления, чтобы получить довольно точные показания высоты БПЛА. Для расчёта максимально точной высоты, большинство контроллеров полёта получают данные одновременно от сенсора давления и спутниковой системы навигации (GPS). При сборке обратите внимание, что предпочтительнее, чтобы отверстие в корпусе барометра было накрыто куском поролона, это уменьшить отрицательное влияние ветра на чип.

Гироскоп с помощью Arduino 101 | АрдуиноПлюс

  • Расстояние: Датчики расстояния все чаще используются на беспилотниках, поскольку GPS-координаты и датчики давления не могут рассказать вам, насколько далеко вы находитесь от земли (холма, горы или здания), либо столкнётесь ли вы с объектом или нет. Датчик расстояния, обращенный вниз, может быть основан на ультразвуковой, лазерной или лидарной технологии (ИК-сенсоры могут испытывать проблемы в работе при солнечном свете). Датчики расстояния редко входят в стандартный комплект полётного контроллера.

Гироскоп с помощью Arduino 101 | АрдуиноПлюс

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

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

Оцените статью
Радиокоптер.ру
Добавить комментарий

© 2023 Радиокоптер.ру