****注意 Arduino IDE 使用1.6.7*****
Grove - IMU 10DOF是採用 MPU-9250 三軸陀螺儀+三軸加速度+三軸磁場 加上 BMP180 氣壓感測器所組合而成。
規格 :
供電電源：3V-5V
通信方式：標準I2C
陀螺儀範圍：±250 500 1000 2000 °/s
加速度範圍：± 2 ±4 ±8 ±16g
磁場範圍： ±4800uT
氣壓表範圍 300〜1100hPa
Library下載位置 :
https://github.com/Seeed-Studio/IMU_10DOF
這程式是用 IMU_10DOF Library 附的範例改寫。
#include <Adafruit_GFX.h>
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_ILI9341.h>
#include <TouchScreen.h>
#include "I2Cdev.h"
#include "MPU9250.h"
#include "BMP180.h"
#define TFT_RST 5 //LCM 接腳
#define TFT_DC 6
#define TFT_CS 7
#define TFT_MOSI 8
#define TFT_MISO 9
#define TFT_CLK 10
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_MOSI, TFT_CLK, TFT_RST, TFT_MISO);
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];
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;
float temperature;
float pressure;
float atm;
float altitude;
BMP180 Barometer;
void setup()
{
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
tft.begin();
tft.fillScreen(ILI9341_BLACK);
tft.setRotation(3);
// initialize device
// Serial.println("Initializing I2C devices...");
accelgyro.initialize();
Barometer.init();
tft.setCursor(0,20);
tft.print("Acceleration(g) of X,Y,Z:");
tft.setCursor(0,40);
tft.print("Gyro(degress/s) of X,Y,Z:");
tft.setCursor(0,60);
tft.print("Compass Value of X,Y,Z:");
tft.setCursor(0,80);
tft.print("Heading:");
tft.setCursor(0,100);
tft.print(" Tilt Heading:");
tft.setCursor(0,120);
tft.print("Temperature: ");
tft.setCursor(0,140);
tft.print("Pressure: ");
tft.setCursor(0,160);
tft.print("Ralated Atmosphere: ");
tft.setCursor(0,180);
tft.print("Altitude: ");
}
void loop()
{
getAccel_Data();
getGyro_Data();
getCompassDate_calibrated(); // compass data has been calibrated here
getHeading(); //before we use this function we should run 'getCompassDate_calibrated()' frist, so that we can get calibrated data ,then we can get correct angle .
getTiltHeading();
tft.fillRect(0,30,120,10,0);
tft.setCursor(0,30);
tft.print(Axyz[0]);
tft.print(" , ");
tft.print(Axyz[1]);
tft.print(" , ");
tft.print(Axyz[2]);
tft.fillRect(0,50,120,10,0);
tft.setCursor(0,50);
tft.print(Gxyz[0]);
tft.print(",");
tft.print(Gxyz[1]);
tft.print(",");
tft.print(Gxyz[2]);
tft.fillRect(0,70,120,10,0);
tft.setCursor(0,70);
tft.print(Mxyz[0]);
tft.print(",");
tft.print(Mxyz[1]);
tft.print(",");
tft.print(Mxyz[2]);
tft.fillRect(0,90,120,10,0);
tft.setCursor(0,90);
tft.print(heading);
tft.fillRect(0,110,120,10,0);
tft.setCursor(0,110);
tft.println(tiltheading);
temperature = Barometer.bmp180GetTemperature(Barometer.bmp180ReadUT()); //Get the temperature, bmp180ReadUT MUST be called first
pressure = Barometer.bmp180GetPressure(Barometer.bmp180ReadUP());//Get the temperature
altitude = Barometer.calcAltitude(pressure); //Uncompensated caculation - in Meters
atm = pressure / 101325;
tft.fillRect(0,130,120,10,0);
tft.setCursor(0,130);
tft.print(temperature, 2); //display 2 decimal places
tft.print(" deg C");
tft.fillRect(0,150,120,10,0);
tft.setCursor(0,150);
tft.print(pressure, 0); //whole number only.
tft.print(" Pa");
tft.fillRect(0,170,120,10,0);
tft.setCursor(0,170);
tft.print(atm, 4); //display 4 decimal places
tft.fillRect(0,190,120,10,0);
tft.setCursor(0,190);
tft.print(altitude, 2); //display 2 decimal places
tft.print(" m");
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 ()
{
while (!Serial.find("ready"));
get_calibration_Data ();
}
void get_calibration_Data ()
{
for (int i = 0; i < sample_num_mdate; i++)
{
get_one_sample_date_mxyz();
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]; //find max value
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]; //find min value
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); //enable the magnetometer
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;
}
上傳完成後就會顯示 IMU_10DOF個軸向的值。
沒有留言:
張貼留言