Getting started with MPU-9250, Arduino Guide – Latest Open Tech From Seeed

Getting started with MPU-9250, Arduino Guide - Latest Open Tech From Seeed Роботы

Mpu-9250 introduction

Let’s get this blog started by firstly diving deeper into the MPU-9250 and giving you a better understanding of what this 9-axis MotionTracking Device is capable of.

2d diagram

Related Articles:

Configuring the mpu-9250

Once you’ve initialized the MPU-9250, you can configure it to your hearts desire. You can use setSensors to enable/disable specific sensors in the IMU. Disabling the gyroscope and magnetometer, for example, can save loads of power consumption.

language:c
// Use setSensors to turn on or off MPU-9250 sensors.
// Any of the following defines can be combined:
// INV_XYZ_GYRO, INV_XYZ_ACCEL, INV_XYZ_COMPASS,
// INV_X_GYRO, INV_Y_GYRO, or INV_Z_GYRO
imu.setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); // Enable all sensors

Detailed features

Some of the useful features MPU 9-DOF MEMS Sensor Module are listed below:

Getting started with mpu-9250

In order to get started with the MPU-9250, you’ll first need a module that’s based on it. There are a number of breakout boards, modules, etc. on the market but today, I’ll be recommending the Grove – IMU 9DOF v2.0

First, a look at its features:

Since it’s an MPU-9250 integrated module, you’ll see similarities:

Hardware configurations:

  • Step 1: Connect Grove-IMU_9DOF_v2.0 to port I2C of Grove Base Shield
  • Step 2: Plug Grove Base Shield into Seeeduino
  • Step 3: Connect Seeeduino to PC via a USB cable

It should now look something like this:

How is the mpu-9250 9-axis?

Compared to other mems sensors; Accelerometer, Gyroscope, magnetometer, that are individually sold, the MPU-9250 combines it all. Integrating two dies into a single QFN package, it packs a 3-Axis Gyroscope and accelerometer on a die and the AK8963 3-Axis magnetometer on the other.

Installation

Simply clone or download this library into your Arduino/libraries folder. The library is added as:

Methods

Mpu9250(i2c_t3 *bus, uint8_t addr) Creates a Mpu9250 object. This constructor is used for the I2C communication interface. A pointer to the I2C bus object is passed along with the I2C address of the sensor. The address will be 0x68 if the AD0 pin is grounded and 0x69 if the AD0 pin is pulled high.

Mpu9250(SPIClass *bus, uint8_t cs) Creates a Mpu9250 object. This constructor is used for the SPI communication interface. A pointer to the SPI bus object is passed along with the chip select pin of the sensor. Any pin capable of digital I/O can be used as a chip select pin.

bool Begin() Initializes communication with the sensor and configures the default sensor ranges, sampling rates, and low pass filter settings. The default accelerometer range is /- 16g and the default gyro range is /- 2,000 deg/s. The default sampling rate is 1000 Hz and the low-pass filter is set to a cutoff frequency of 184 Hz.

bool EnableDrdyInt() Enables the data ready interrupt. A 50 us interrupt will be triggered on the MPU-9250 INT pin when IMU data is ready. This interrupt is active high. This method returns true if the interrupt is successfully enabled, otherwise, false is returned.

bool DisableDrdyInt() Disables the data ready interrupt. This method returns true if the interrupt is successfully disabled, otherwise, false is returned.

bool ConfigAccelRange(const AccelRange range) Sets the accelerometer full scale range. Options are:

RangeEnum Value
/- 2gACCEL_RANGE_2G
/- 4gACCEL_RANGE_4G
/- 8gACCEL_RANGE_8G
/- 16gACCEL_RANGE_16G

True is returned on succesfully setting the accelerometer range, otherwise, false is returned. The default range is /-16g.

AccelRange accel_range() Returns the current accelerometer range.

AccelRange range = mpu9250.accel_range();

bool ConfigGyroRange(const GyroRange range) Sets the gyro full scale range. Options are:

RangeEnum Value
/- 250 deg/sGYRO_RANGE_250DPS
/- 500 deg/sGYRO_RANGE_500DPS
/- 1000 deg/sGYRO_RANGE_1000DPS
/- 2000 deg/sGYRO_RANGE_2000DPS

True is returned on succesfully setting the gyro range, otherwise, false is returned. The default range is /-2000 deg/s.

GyroRange gyro_range() Returns the current gyro range.

GyroRange range = mpu9250.gyro_range();

bool ConfigSrd(const uint8_t srd) Sets the sensor sample rate divider. The MPU-9250 samples the accelerometer and gyro at a rate, in Hz, defined by:

A srd setting of 0 means the MPU-9250 samples the accelerometer and gyro at 1000 Hz. A srd setting of 4 would set the sampling at 200 Hz. The IMU data ready interrupt is tied to the rate defined by the sample rate divider. The magnetometer is sampled at 100 Hz for sample rate divider values corresponding to 100 Hz or greater. Otherwise, the magnetometer is sampled at 8 Hz.

True is returned on succesfully setting the sample rate divider, otherwise, false is returned. The default sample rate divider value is 0, resulting in a 1000 Hz sample rate.

Смотрите про коптеры:  Топ-10 крутых роботов-животных

uint8_t srd() Returns the current sample rate divider value.

bool ConfigDlpf(const DlpfBandwidth dlpf) Sets the cutoff frequency of the digital low pass filter for the accelerometer, gyro, and temperature sensor. Available bandwidths are:

DLPF BandwidthEnum Value
184 HzDLPF_BANDWIDTH_184HZ
92 HzDLPF_BANDWIDTH_92HZ
41 HzDLPF_BANDWIDTH_41HZ
20 HzDLPF_BANDWIDTH_20HZ
10 HzDLPF_BANDWIDTH_10HZ
5 HzDLPF_BANDWIDTH_5HZ

True is returned on succesfully setting the digital low pass filters, otherwise, false is returned. The default bandwidth is 184 Hz.

DlpfBandwidth dlpf() Returns the current digital low pass filter bandwidth setting.

DlpfBandwidth dlpf = mpu9250.dlpf();

void DrdyCallback(uint8_t int_pin, void (*function)()) Assigns a callback function to be called on the MPU-9250 data ready interrupt. Input parameters are the microcontroller pin number connected to the MPU-9250 interrupt pin and the function name.

bool Read() Reads data from the MPU-9250 and stores the data in the Mpu9250 object. Returns true if data is successfully read, otherwise, returns false.

float accel_x_mps2() Returns the x accelerometer data from the Mpu9250 object in units of m/s/s. Similar methods exist for the y and z axis data.

float gyro_x_radps() Returns the x gyro data from the Mpu9250 object in units of rad/s. Similar methods exist for the y and z axis data.

float mag_x_ut() Returns the x magnetometer data from the Mpu9250 object in units of uT. Similar methods exist for the y and z axis data.

float die_temperature_c() Returns the die temperature of the sensor in units of C.

Mpu-9250 features

To better understand the features of each individual sensor, I’ve collated it in a table below:

Apart from the above features coming from the MEMS gyroscope, accelerometer, and magnetometer, the MPU-9250 consists of other additional features!

Additional Features include:

  • Smallest and thinnest QFN package for portable devices: 3x3x1mm
  • Auxiliary master I2C bus for reading data from external sensors
  • Minimal cross-axis sensitivity between the accelerometer, gyroscope and magnetometer axes
  • 512 byte FIFO buffer enables the applications processor to read the data in bursts
  • Digital-output temperature sensor
  • 10,000 g shock tolerant
  • 400kHz Fast Mode I2C for communicating with all registers
  • Serial interfaces:
    • 1MHz for communicating with all registers
    • 200MHz for reading sensor and interrupt registers

The MPU-9250 can be found in a wide array of applications as well, including:

  • Smartphones
  • Tablets
  • 3D remote controls for Internet-connected TVs and set-top boxes
  • Motion-based game controllers and 3D mice
  • Motion-based portable gaming
  • Wearable Sensors

Mpu-9250 vs mpu-9150: the improvements

Before moving on to the integrated module, let’s take a look at what’s better on the MPU-9250 as compared to its previous successor!

Smaller and consume less power

As compared to the MPU-9150, the MPU-9250 has lowered its power consumption yet decreased form factor by 44%!

MPU-9250 has Better Gyro and Compass performance

The MPU-9250 contains:

  • MPU-6500 and AK8963 magnetometer

The MPU-9150 contains:

  • MPU-6050 and AK8975 magnetometer

What do the differences mean?

  • MPU-6500 offers higher power, lower noise, and larger package as compared to the MPU-6050
  • AK8963 magnetometer has a better full-scale range than the AK8975

Overall, you would expect an incremental increase in performance as it’s the latest 9-axis device from Invensense.

Mpu9250-arduino

This library communicates with InvenSense MPU-9250 and MPU-9255 Inertial Measurement Units (IMUs) and is built for use with the Arduino IDE.

Pin configuration

MPU9250 9-DOF MEMS Sensor Module has a total of 10 pins. The pin configuration detail in tabular is mentioned below:Pin NameFunctionPin NameFunctionINTInterrupt pinECLI2C Master Serial Clock for external sensors connection pinAD0/SDOI2C Address/Serial data out pinFSYNCFrame Synchronization input pinVCCPower supply pinGNDGround pinEDAI2C Serial Data input for external sensors connection pinnCSChip selection pinSCL/SCLKI2C Serial Clock/SPI Serial Clock pinSDA/SDII2C Serial Data/SPI Serial Data pin

Reading from the dmp

The DMP streams its data to the MPU-9250’s 512-byte first-in, first-out (FIFO) buffer, so to get data from the DMP, the FIFO must be updated. You can use the fifoAvailable function, to check how full the FIFO is. Then call dmpUpdateFifo to read from the top of the FIFO.

language:c
if ( imu.fifoAvailable() > 0 ) / Check for new data in the FIFO
{
    // Use dmpUpdateFifo to update the ax, gx, qx, etc. values
    if ( imu.dmpUpdateFifo() == INV_SUCCESS )
    {
        // The following variables will have data from the top of the FIFO:
        // imu.ax, imu.ay, imu.az, -- Accelerometer
        // imu.gx, imu.gy, imu.gz -- calibrated gyroscope
        // and imu.qw, imu.qx, imu.qy, and imu.qz -- quaternions
    }
}

Reading from the mpu-9250

Once your IMU is configured to fit your needs, reading from the sensor is as simple as calling imu.update() and checking the ax, ay, az, ax, ay, az, ax, ay, and mz class variables (e.g. imu.ax, imu.gy, imu.mz, etc).

language:c
// Call update() to update the imu objects sensor data. You can specify 
// which sensors to update by OR'ing UPDATE_ACCEL, UPDATE_GYRO, 
// UPDATE_COMPASS, and/or UPDATE_TEMPERATURE.
// (The update function defaults to accel, gyro, compass, so you don't 
// have to specify these values.)
imu.update(UPDATE_ACCEL | UPDATE_GYRO | UPDATE_COMPASS);

The variables can be converted from raw, signed 16-bit values to their sensor’s respective units by passing them to the calcGyro, calcAccel, and calcMag functions:

language:c
float accelX = imu.calcAccel(imu.ax); // accelX is x-axis acceleration in g's
float accelY = imu.calcAccel(imu.ay); // accelY is y-axis acceleration in g's
float accelZ = imu.calcAccel(imu.az); // accelZ is z-axis acceleration in g's

float gyroX = imu.calcGyro(imu.gx); // gyroX is x-axis rotation in dps
float gyroY = imu.calcGyro(imu.gy); // gyroY is y-axis rotation in dps
float gyroZ = imu.calcGyro(imu.gz); // gyroZ is z-axis rotation in dps

float magX = imu.calcMag(imu.mx); // magX is x-axis magnetic field in uT
float magY = imu.calcMag(imu.my); // magY is y-axis magnetic field in uT
float magZ = imu.calcMag(imu.mz); // magZ is z-axis magnetic field in uT

Sensor orientation

This library transforms all data to a common axis system before it is returned. This axis system is shown below. It is a right handed coordinate system with the z-axis positive down, common in aircraft dynamics.

Смотрите про коптеры:  Робот - это... Что такое Робот?

Caution! This axis system is shown relative to the MPU-9250 sensor. The sensor may be rotated relative to the breakout board.

Summary

Overall, MPU-9250 is an excellent choice for your motion-sensing needs. With Invensense support really good at staying in communication while they are looking over issues, you wouldn’t have to worry when troubleshooting occurs.

However, you’ll need a module that’s based on that motion processing unit for any pairings with a microcontroller.

I highly recommend the Grove – IMU 9DOF v2.0 simply with its ease of pairing when compared to other MPU-9250 breakout boards!

Using the digital motion processor

The MPU-9250’s digital motion processor (DMP) allows you to offload tasks like quaternion calculation, step-counting, and orientation-determining off to the IMU.

Using the interrupt or data-ready

Instead of constantly polling the update function, you can use the MPU-9250’s interrupt output to tell you when new data is ready. First, in the setup area, you should call enableInterrupt. (Don’t forget to set your Arduino’s interrupt pin as an input as well!)

language:c
#define INTERRUPT_PIN 4 // MPU-9250 INT pin tied to D4
...
void setup()
{
    pinMode(INTERRUPT_PIN, INPUT_PULLUP); // Set interrupt as an input w/ pull-up resistor
    ...
    // Use enableInterrupt() to configure the MPU-9250's 
    // interrupt output as a "data ready" indicator.
    imu.enableInterrupt();

    // The interrupt level can either be active-high or low. Configure as active-low.
    // Options are INT_ACTIVE_LOW or INT_ACTIVE_HIGH
    imu.setIntLevel(INT_ACTIVE_LOW);

    // The interrupt can be set to latch until data is read, or as a 50us pulse.
    // Options are INT_LATCHED or INT_50US_PULSE
    imu.setIntLatched(INT_LATCHED);
}

You can set the interrupt’s active level to high or low, and set it to latch until the sensor is read or to send a 50µs pulse.

Once configured, simply read the status of your Arduino’s interrupt pin. If it’s active, you’ll know to update the IMU’s sensors.

language:c
if ( digitalRead(INTERRUPT_PIN) == LOW ) // If MPU-9250 interrupt fires (active-low)
{
    imu.update() // Update all sensor's
    // ... do stuff with imu.ax, imu.ay, etc.
}

Alternatively — if you don’t want to use the interrupt pin — you can check the return value of dataReady, to check if new data is available. This function returns a boolean — true if new data is ready.

language:c
if ( imu.dataReady() ) // If new IMU data is available
{
    imu.update(); // Update all sensor's
    ...
}

What you’ll need:

*Seeeduino is Seeed’s very own Arduino, built with benefits over the regular Arduino boards. 

If you already own one of the Arduino models below, it’ll work with the Grove Base Shield as well:

  • Arduino Uno
  • Arduino Mega
  • Arduino Leonardo
  • Arduino 101
  • Arduino Due

Why pick this mpu-9250 module rather than others?

Ease of pairing MPU-9250 with Arduino through Seeed’s very own Grove system

Wiring and pullups

Please refer to the MPU-9250 datasheet and your microcontroller’s pinout diagram. This library was developed using the Embedded Masters breakout board v1.1 for the MPU-9250. The data sheet for this breakout board is located here. This library should work well for other breakout boards or embedded sensors, please refer to your vendor’s pinout diagram.

Комплектация

  • 1х сенсор MPU-9250;
  • 1х СОединитель типа “ПАПА-ПАПА”;

Общие сведения

9-осевой сенсор MPU-9250 9DOF — сенсор второго поколения компании InvenSense для определения положения в пространстве, включающий в себя 3-осевой Гироскоп, 3-осевой Акселерометр и 3-осевой Компас (Магнетометр).

Питание

Для удобства подключения к Arduino воспользуйтесь Trema Shield, Trema Power Shield, Motor Shield или Trema Set Shield.

Входное напряжение питания 3,3 В или 5 В постоянного тока, подаётся на выводы VCC и GND модуля.

Подключение

Подключить сенсор к микроконтроллеру Вы можете одним из 2 способов:

  • по шине I2C: используя 4 контакта VCC, GNG, SCL, SDA
  • по шине SPI: используя 5 контактов VCC, GND, SCL, SDA, CS, SDO

Подключение mpu-9250 к arduino

#include “Wire.h”
#include “I2Cdev.h”
#include “MPU9250.h”

MPU9250 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t mx, my, mz;

void setup()
{
Wire.begin();
Serial.begin(19200);
Serial.println(“Initializing I2C devices…”);
accelgyro.initialize();
Serial.println(“Testing device connections…”);
Serial.println(accelgyro.testConnection() ? “MPU9250 connection successful” : “MPU9250 connection failed”);
}

void loop()
{
accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);

Serial.print(“a/g/m:t”);
Serial.print(ax); Serial.print(“t”);
Serial.print(ay); Serial.print(“t”);
Serial.print(az); Serial.print(“t”);
Serial.print(gx); Serial.print(“t”);
Serial.print(gy); Serial.print(“t”);
Serial.print(gz); Serial.print(“t”);
Serial.print(mx); Serial.print(“t”);
Serial.print(my); Serial.print(“t”);
Serial.println(mz);
}

Подробнее о сенсоре

Микросхема сенсора MPU-9250 9DOF состоит из 2 чипов: MPU-6500 — чип, включающий в себя 3-осевой гироскоп и акселерометр и AK8963 — чип, включающий в себя 3-осевой компас и интегрированный Digital Motion Processor (DMP).

Смотрите про коптеры:  Какой робот пылесос выбрать: ТОП-7 лучших моделей, отзывы советы перед покупкой

Данный сенсор является одним из самых миниатюрных в мире (3мм*3мм*1мм). Используется в смартфонах, планшетах, носимых датчиках и других устройствах. Если к модулю добавить барометр, получится сенсор на 10 степеней свободы.

Может использоваться для создания дронов, роботов, 3-мерных контроллеров, а так же в системах, поддерживающих управление жестами.

Обратите внимание, что в микросхеме MPU9250 определение осей у Гироскопа и Акселерометра одно, а у Магнитометра другое!

Для работы с сенсором вам понадобится библиотека IMU_10DOF. При необходимости Вы так же можете ознакомиться с DataSheet’ом.

Характеристики

  • Микросхема : MPU9250
  • Интерфейс: I2C (400кГц) / SPI (1 МГц)
  • Буфер: FIFO 512B
  • Рабочие диапазоны гироскопа: ±250, ±500, ±1000, ±2000 °/с;
  • Рабочие диапазоны акселерометра: ±2, ±4, ±8, ±16 g;
  • Рабочий диапазон магнитометра: ±4800 мкТл;
  • Напряжение питания: 2,4–3,6 В;
  • Рабочий ток: гироскоп – 3,2 мА, акселерометр – 450 мкА, магнитометр – 280 мкА;
  • Размер: 15мм х 25мм

Калибровка компаса и вывод значений осей в монитор порта.

Подключим сенсор через I2C.

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

#include <Wire.h>
#include <I2Cdev.h>
#include <MPU9250.h>
// По умолчанию адрес устройства на шине I2C - 0x68
MPU9250 accelgyro;
I2Cdev   I2C_M;
uint8_t buffer_m[6];
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t   mx, my, mz;
float heading;
float tiltheading;
float Axyz[3];
float Gxyz[3];
float Mxyz[3];
// время выполнения предварительной калибровки
#define sample_num_mdate  5000
volatile float mx_sample[3];
volatile float my_sample[3];
volatile float mz_sample[3];
static float mx_centre = 0;
static float my_centre = 0;
static float mz_centre = 0;
volatile int mx_max = 0;
volatile int my_max = 0;
volatile int mz_max = 0;
volatile int mx_min = 0;
volatile int my_min = 0;
volatile int mz_min = 0;
void setup()
{
    //подключаемся к шине I2C (I2Cdev не может сделать это самостоятельно)
    Wire.begin();
    // инициализация подключения в Мониторе порта
    // ( 38400бод выбрано потому, что стабильная работа наблюдается и при 8MHz и при 16Mhz, поэтому
    // в дальнейшем выставляйте скорость согласно ваших требований)
    Serial.begin(38400);
    // Инициализация устройства
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();
    // Подтверждение подключения
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU9250 connection successful" : "MPU9250 connection failed");
    delay(1000);
    Serial.println("     ");
    // Предварительная калибровка магнитометра
    Mxyz_init_calibrated ();
}
void loop()
{
    getAccel_Data();             // Получение значений Акселерометра
    getGyro_Data();              // Получение значений Гироскопа
    getCompassDate_calibrated(); // В этой функции происходит калибровка магнитометра
    getHeading();                // после чего мы получаем откалиброванные значения углов поворота
    getTiltHeading();            // и наклона
    Serial.println("calibration parameter: ");
    Serial.print(mx_centre);
    Serial.print("         ");
    Serial.print(my_centre);
    Serial.print("         ");
    Serial.println(mz_centre);
    Serial.println("     ");
    Serial.println("Acceleration(g) of X,Y,Z:");
    Serial.print(Axyz[0]);
    Serial.print(",");
    Serial.print(Axyz[1]);
    Serial.print(",");
    Serial.println(Axyz[2]);
    Serial.println("Gyro(degress/s) of X,Y,Z:");
    Serial.print(Gxyz[0]);
    Serial.print(",");
    Serial.print(Gxyz[1]);
    Serial.print(",");
    Serial.println(Gxyz[2]);
    Serial.println("Compass Value of X,Y,Z:");
    Serial.print(Mxyz[0]);
    Serial.print(",");
    Serial.print(Mxyz[1]);
    Serial.print(",");
    Serial.println(Mxyz[2]);
    Serial.println("The clockwise angle between the magnetic north and X-Axis:"); // "Угол поворота"
    Serial.print(heading);
    Serial.println(" ");
    Serial.println("The clockwise angle between the magnetic north and the projection of the positive X-Axis in the horizontal plane:"); // "Угол наклона"
    Serial.println(tiltheading);
    Serial.println("   ");
    Serial.println();
    delay(1000);
}
void getHeading(void)
{
    heading = 180 * atan2(Mxyz[1], Mxyz[0]) / PI;
    if (heading < 0) heading  = 360;
}
void getTiltHeading(void)
{
    float pitch = asin(-Axyz[0]);
    float roll = asin(Axyz[1] / cos(pitch));
    float xh = Mxyz[0] * cos(pitch)   Mxyz[2] * sin(pitch);
    float yh = Mxyz[0] * sin(roll) * sin(pitch)   Mxyz[1] * cos(roll) - Mxyz[2] * sin(roll) * cos(pitch);
    float zh = -Mxyz[0] * cos(roll) * sin(pitch)   Mxyz[1] * sin(roll)   Mxyz[2] * cos(roll) * cos(pitch);
    tiltheading = 180 * atan2(yh, xh) / PI;
    if (yh < 0)    tiltheading  = 360;
}
void Mxyz_init_calibrated ()
{
    Serial.println(F("Before using 9DOF,we need to calibrate the compass first. It will takes about 1 minute."));  // Перед использованием сенсора необходимо произвести калибровку компаса. Это займёт около минуты.
    Serial.print("  ");
    Serial.println(F("During  calibrating, you should rotate and turn the 9DOF all the time within 1 minute."));   // На протяжении всего времени калибровки Вам необходимо вращать сенсор во все стороны.
    Serial.print("  ");
    Serial.println(F("If you are ready, please sent a command data 'ready' to start sample and calibrate."));      // Если Вы готовы, для начала калибровки отправьте в Мониторе Порта "ready". 
    while (!Serial.find("ready"));
    Serial.println("  ");
    Serial.println("ready");
    Serial.println("Sample starting......");
    Serial.println("waiting ......");
    get_calibration_Data ();
    Serial.println("     ");
    Serial.println("compass calibration parameter ");
    Serial.print(mx_centre);
    Serial.print("     ");
    Serial.print(my_centre);
    Serial.print("     ");
    Serial.println(mz_centre);
    Serial.println("    ");
}
void get_calibration_Data ()
{
    for (int i = 0; i < sample_num_mdate; i  )
    {
        get_one_sample_date_mxyz();
        /*
        Serial.print(mx_sample[2]);
        Serial.print(" ");
        Serial.print(my_sample[2]);                            // здесь Вы можете увидеть полученные "сырые" значения.
        Serial.print(" ");
        Serial.println(mz_sample[2]);
        */
        if (mx_sample[2] >= mx_sample[1])mx_sample[1] = mx_sample[2];
        if (my_sample[2] >= my_sample[1])my_sample[1] = my_sample[2]; // Поиск максимального значения
        if (mz_sample[2] >= mz_sample[1])mz_sample[1] = mz_sample[2];
        if (mx_sample[2] <= mx_sample[0])mx_sample[0] = mx_sample[2];
        if (my_sample[2] <= my_sample[0])my_sample[0] = my_sample[2]; // Поиск минимального значения
        if (mz_sample[2] <= mz_sample[0])mz_sample[0] = mz_sample[2];
    }
    mx_max = mx_sample[1];
    my_max = my_sample[1];
    mz_max = mz_sample[1];
    mx_min = mx_sample[0];
    my_min = my_sample[0];
    mz_min = mz_sample[0];
    mx_centre = (mx_max   mx_min) / 2;
    my_centre = (my_max   my_min) / 2;
    mz_centre = (mz_max   mz_min) / 2;
}
void get_one_sample_date_mxyz()
{
    getCompass_Data();
    mx_sample[2] = Mxyz[0];
    my_sample[2] = Mxyz[1];
    mz_sample[2] = Mxyz[2];
}
void getAccel_Data(void)
{
    accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
    Axyz[0] = (double) ax / 16384;
    Axyz[1] = (double) ay / 16384;
    Axyz[2] = (double) az / 16384;
}
void getGyro_Data(void)
{
    accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
    Gxyz[0] = (double) gx * 250 / 32768;
    Gxyz[1] = (double) gy * 250 / 32768;
    Gxyz[2] = (double) gz * 250 / 32768;
}
void getCompass_Data(void)
{
    I2C_M.writeByte(MPU9150_RA_MAG_ADDRESS, 0x0A, 0x01); // активируем магнетометр
    delay(10);
    I2C_M.readBytes(MPU9150_RA_MAG_ADDRESS, MPU9150_RA_MAG_XOUT_L, 6, buffer_m);
    mx = ((int16_t)(buffer_m[1]) << 8) | buffer_m[0] ;
    my = ((int16_t)(buffer_m[3]) << 8) | buffer_m[2] ;
    mz = ((int16_t)(buffer_m[5]) << 8) | buffer_m[4] ;
    Mxyz[0] = (double) mx * 1200 / 4096;
    Mxyz[1] = (double) my * 1200 / 4096;
    Mxyz[2] = (double) mz * 1200 / 4096;
}
void getCompassDate_calibrated ()
{
    getCompass_Data();
    Mxyz[0] = Mxyz[0] - mx_centre;
    Mxyz[1] = Mxyz[1] - my_centre;
    Mxyz[2] = Mxyz[2] - mz_centre;
}

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

Adblock
detector