Crowtail-IMU-10DOF

From Elecrow
(Difference between revisions)
Jump to: navigation, search
(Hardware)
(Programming)
Line 35: Line 35:
  
 
=== Programming  ===
 
=== Programming  ===
Download the [http://www.elecrow.com/wiki/index.php?title=File:MPU6050.zip  MPU6050] Library. Unzip and put it in the libraries file of Arduino IDE by the path: ..File/arduino IDE/Arduino/library/MPU6050. You can also copy the following program to Arduino IDE and upload to your Arduino/Crowduino.  
+
Download the [http://www.elecrow.com/wiki/index.php?title=File:IMU10lib.zip  IMU10lib] Library. Unzip and put it in the libraries file of Arduino IDE by the path: ..File/arduino IDE/Arduino/library/IMU10lib. You can also copy the following program to Arduino IDE and upload to your Arduino/Crowduino.  
 
<BR>
 
<BR>
 
<pre>
 
<pre>
  
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
 
// is used in I2Cdev.h
 
 
#include "Wire.h"
 
#include "Wire.h"
  
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
+
// I2Cdev and MPU9250 must be installed as libraries, or else the .cpp/.h files
 
// for both classes must be in the include path of your project
 
// for both classes must be in the include path of your project
 
#include "I2Cdev.h"
 
#include "I2Cdev.h"
#include "MPU6050.h"
+
#include "MPU9250.h"
 +
#include "SparkFunBME280.h"
 +
#include "Wire.h"
 +
#include "SPI.h"
  
 
// class default I2C address is 0x68
 
// class default I2C address is 0x68
Line 52: Line 53:
 
// AD0 low = 0x68 (default for InvenSense evaluation board)
 
// AD0 low = 0x68 (default for InvenSense evaluation board)
 
// AD0 high = 0x69
 
// AD0 high = 0x69
MPU6050 accelgyro;
+
MPU9250 accelgyro;
 +
I2Cdev  I2C_M;
 +
 
 +
uint8_t buffer_m[6];
 +
 
  
 
int16_t ax, ay, az;
 
int16_t ax, ay, az;
 
int16_t gx, gy, gz;
 
int16_t gx, gy, gz;
 +
int16_t  mx, my, mz;
  
#define LED_PIN 13
 
bool blinkState = false;
 
  
void setup() {
+
 
 +
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;
 +
 
 +
//float temperature;
 +
//float pressure;
 +
//float atm;
 +
//float altitude;
 +
BME280 mySensorA;
 +
 
 +
void setup()
 +
{
 
     // join I2C bus (I2Cdev library doesn't do this automatically)
 
     // join I2C bus (I2Cdev library doesn't do this automatically)
 
     Wire.begin();
 
     Wire.begin();
Line 67: Line 105:
 
     // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
 
     // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
 
     // it's really up to you depending on your project)
 
     // it's really up to you depending on your project)
     Serial.begin(38400);
+
     Serial.begin(57600);
  
 
     // initialize device
 
     // initialize device
 
     Serial.println("Initializing I2C devices...");
 
     Serial.println("Initializing I2C devices...");
 
     accelgyro.initialize();
 
     accelgyro.initialize();
 +
    //Barometer.init();
 +
   
 +
   
  
 
     // verify connection
 
     // verify connection
 
     Serial.println("Testing device connections...");
 
     Serial.println("Testing device connections...");
     Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
+
     Serial.println(accelgyro.testConnection() ? "MPU9250 connection successful" : "MPU9250 connection failed");
 +
 
 +
    delay(1000);
 +
    Serial.println("    ");
 +
 
 +
    //  Mxyz_init_calibrated ();
 +
   
 +
   
 +
    mySensorA.settings.commInterface = I2C_MODE;
 +
//mySensorA.settings.I2CAddress = 0x77;
 +
        mySensorA.settings.I2CAddress = 0x77;
 +
mySensorA.settings.runMode = 3; //  3, Normal mode
 +
mySensorA.settings.tStandby = 0; //  0, 0.5ms
 +
mySensorA.settings.filter = 0; //  0, filter off
 +
//tempOverSample can be:
 +
//  0, skipped
 +
//  1 through 5, oversampling *1, *2, *4, *8, *16 respectively
 +
mySensorA.settings.tempOverSample = 1;
 +
//pressOverSample can be:
 +
//  0, skipped
 +
//  1 through 5, oversampling *1, *2, *4, *8, *16 respectively
 +
    mySensorA.settings.pressOverSample = 1;
 +
//humidOverSample can be:
 +
//  0, skipped
 +
//  1 through 5, oversampling *1, *2, *4, *8, *16 respectively
 +
mySensorA.settings.humidOverSample = 1;
 +
 
 +
 +
 +
 +
//***Set up sensor 'B'******************************//
 +
//commInterface can be I2C_MODE or SPI_MODE
 +
//specify chipSelectPin using arduino pin names
 +
//specify I2C address.  Can be 0x77(default) or 0x76
 +
//tempOverSample can be:
 +
//  0, skipped
 +
//  1 through 5, oversampling *1, *2, *4, *8, *16 respectively
 +
//pressOverSample can be:
 +
//  0, skipped
 +
//  1 through 5, oversampling *1, *2, *4, *8, *16 respectively
 +
//humidOverSample can be:
 +
//  0, skipped
 +
//  1 through 5, oversampling *1, *2, *4, *8, *16 respectively
 +
 +
 +
 +
//***Initialize the things**************************//
 +
Serial.begin(57600);
 +
Serial.print("Program Started\n");
 +
Serial.println("Starting BME280s... result of .begin():");
 +
delay(10);  //Make sure sensor had enough time to turn on. BME280 requires 2ms to start up.
 +
//Calling .begin() causes the settings to be loaded
 +
Serial.print("Sensor A: 0x");
 +
Serial.println(mySensorA.begin(), HEX);
  
    // configure Arduino LED for
 
    pinMode(LED_PIN, OUTPUT);
 
 
}
 
}
  
void loop() {
+
void loop()
    // read raw accel/gyro measurements from device
+
{
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+
  
     // these methods (and a few others) are also available
+
     getAccel_Data();
     //accelgyro.getAcceleration(&ax, &ay, &az);
+
     getGyro_Data();
     //accelgyro.getRotation(&gx, &gy, &gz);
+
     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();
  
     // display tab-separated accel/gyro x/y/z values
+
     Serial.println("calibration parameter: ");
    Serial.print("a/g:\t");
+
    Serial.print(mx_centre);
    Serial.print(ax); Serial.print("\t");
+
    Serial.print("        ");
    Serial.print(ay); Serial.print("\t");
+
    Serial.print(my_centre);
     Serial.print(az); Serial.print("\t");
+
    Serial.print("        ");
     Serial.print(gx); Serial.print("\t");
+
    Serial.println(mz_centre);
     Serial.print(gy); Serial.print("\t");
+
    Serial.println("    ");
     Serial.println(gz);
+
 
 +
 
 +
    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);
 +
    Serial.print("Temperature: ");
 +
Serial.print(mySensorA.readTempC(), 2);
 +
Serial.print(", ");
 +
// Serial.print(mySensorB.readTempC(), 2);
 +
Serial.println(" degrees C");
 +
 
 +
Serial.print("Temperature: ");
 +
Serial.print(mySensorA.readTempF(), 2);
 +
Serial.print(", ");
 +
// Serial.print(mySensorB.readTempF(), 2);
 +
Serial.println(" degrees F");
 +
 
 +
Serial.print("Pressure: ");
 +
Serial.print(mySensorA.readFloatPressure(), 2);
 +
Serial.print(", ");
 +
// Serial.print(mySensorB.readFloatPressure(), 2);
 +
Serial.println(" Pa");
 +
 
 +
Serial.print("Altitude: ");
 +
Serial.print(mySensorA.readFloatAltitudeMeters(), 2);
 +
Serial.print(", ");
 +
// Serial.print(mySensorB.readFloatAltitudeMeters(), 2);
 +
Serial.println("m");
 +
 
 +
Serial.print("Altitude: ");
 +
Serial.print(mySensorA.readFloatAltitudeFeet(), 2);
 +
Serial.print(", ");
 +
// Serial.print(mySensorB.readFloatAltitudeFeet(), 2);
 +
Serial.println("ft");
 +
 
 +
Serial.print("Humidity: ");
 +
Serial.print(mySensorA.readFloatHumidity(), 2);
 +
Serial.print(", ");
 +
// Serial.print(mySensorB.readFloatHumidity(), 2);
 +
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 frist,It will takes about 2 minutes."));
 +
     Serial.print("  ");
 +
    Serial.println(F("During  calibratting ,you should rotate and turn the 9DOF all the time within 2 minutes."));
 +
    Serial.print(" ");
 +
    Serial.println(F("If you are ready ,please sent a command data 'ready' to start sample and calibrate."));
 +
    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]);                            //you can see the sample data here .
 +
        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]; //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;
 +
}
  
     // blink LED to indicate activity
+
void getCompassDate_calibrated ()
     blinkState = !blinkState;
+
{
     digitalWrite(LED_PIN, blinkState);
+
     getCompass_Data();
 +
     Mxyz[0] = Mxyz[0] - mx_centre;
 +
     Mxyz[1] = Mxyz[1] - my_centre;
 +
    Mxyz[2] = Mxyz[2] - mz_centre;
 
}
 
}
  
 
</pre>
 
</pre>
  
Open the Sscom32 terminal or the Serial moniter , and set the baudrate to 38400, you will see: <BR>
+
Open the Sscom32 terminal or the Serial moniter , and set the baudrate to 57600, you will see: <BR>
[[file:Accelerometer&Gyro result.png|400px]]
+
[[file:IMU-10DOF result.png|400px]]

Revision as of 09:02, 9 February 2017

Contents

Description

The crowtail-IMU-10DOF is a 10-axis sensor that can be used for motion pose monitoring, and can also read azimuth, altitude and temperature. Very suitable for use in DIY aircraft or balance robot for human-computer interaction.

The onboard MPU9255 incorporates a 3-axis accelerometer, a 3-axis gyroscope and a 3-axis magnetometer. The built-in digital motion processing engine reduces the processor load. Compared to the MPU6050, with lower power consumption, more suitable for wear equipment. Onboard BMP180 (barometric altimeter), built-in temperature sensor, can be temperature compensation.

10-axis data can be acquired only through the IIC interface.

Model:Crowtail-IMU-10DOF

400px

Specification

  • Power supply range: 3.3 to 5.5V.
  • Accelerometer resolution: 16 bits.
  • Accelerometer measurement range: ± 2, ± 4, ± 8, ± 16g.
  • Accelerometer operating current: 450mA.
  • Gyro resolution: 16 bits.
  • Gyro measurement range: ± 250, ± 500, ± 1000, ± 2000 ° / s.
  • Gyroscope operating current: 3.2mA.
  • Magnetometer resolution: 14 or 16 bits.
  • Magnetometer measurement range: ± 4800uT.
  • Magnetometer operating current: 280uA.
  • Barometric altimeter resolution: 16 to 19 bits.
  • Pressure altimeter measurement Range: 300hPa (+9000 m) to 11hPa (-500m).
  • Pressure altimeter Accuracy: 0.02hPa (0.17m).

Usage

Hardware

Connect the Crowtail-IMU-10DOF to the Crowtail base board as below:
Connection of IMU-10DOF.jpg

Programming

Download the IMU10lib Library. Unzip and put it in the libraries file of Arduino IDE by the path: ..File/arduino IDE/Arduino/library/IMU10lib. You can also copy the following program to Arduino IDE and upload to your Arduino/Crowduino.


#include "Wire.h"

// I2Cdev and MPU9250 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU9250.h"
#include "SparkFunBME280.h"
#include "Wire.h"
#include "SPI.h"

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
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;

//float temperature;
//float pressure;
//float atm;
//float altitude;
BME280 mySensorA;

void setup()
{
    // join I2C bus (I2Cdev library doesn't do this automatically)
    Wire.begin();

    // initialize serial communication
    // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
    // it's really up to you depending on your project)
    Serial.begin(57600);

    // initialize device
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();
    //Barometer.init();
    
    

    // verify connection
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU9250 connection successful" : "MPU9250 connection failed");

    delay(1000);
    Serial.println("     ");

    //  Mxyz_init_calibrated ();
    
    
    mySensorA.settings.commInterface = I2C_MODE;
	//mySensorA.settings.I2CAddress = 0x77;
        mySensorA.settings.I2CAddress = 0x77;
	mySensorA.settings.runMode = 3; //  3, Normal mode
	mySensorA.settings.tStandby = 0; //  0, 0.5ms
	mySensorA.settings.filter = 0; //  0, filter off
	//tempOverSample can be:
	//  0, skipped
	//  1 through 5, oversampling *1, *2, *4, *8, *16 respectively
	mySensorA.settings.tempOverSample = 1;
	//pressOverSample can be:
	//  0, skipped
	//  1 through 5, oversampling *1, *2, *4, *8, *16 respectively
    mySensorA.settings.pressOverSample = 1;
	//humidOverSample can be:
	//  0, skipped
	//  1 through 5, oversampling *1, *2, *4, *8, *16 respectively
	mySensorA.settings.humidOverSample = 1;

	
	
	
	//***Set up sensor 'B'******************************//
	//commInterface can be I2C_MODE or SPI_MODE
	//specify chipSelectPin using arduino pin names
	//specify I2C address.  Can be 0x77(default) or 0x76
	//tempOverSample can be:
	//  0, skipped
	//  1 through 5, oversampling *1, *2, *4, *8, *16 respectively
	//pressOverSample can be:
	//  0, skipped
	//  1 through 5, oversampling *1, *2, *4, *8, *16 respectively
	//humidOverSample can be:
	//  0, skipped
	//  1 through 5, oversampling *1, *2, *4, *8, *16 respectively
	
	
	
	//***Initialize the things**************************//
	Serial.begin(57600);
	Serial.print("Program Started\n");
	Serial.println("Starting BME280s... result of .begin():");
	delay(10);  //Make sure sensor had enough time to turn on. BME280 requires 2ms to start up.
	//Calling .begin() causes the settings to be loaded
	Serial.print("Sensor A: 0x");
	Serial.println(mySensorA.begin(), HEX);

}

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();

    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);
    	Serial.print("Temperature: ");
	Serial.print(mySensorA.readTempC(), 2);
	Serial.print(", ");
//	Serial.print(mySensorB.readTempC(), 2);
	Serial.println(" degrees C");

	Serial.print("Temperature: ");
	Serial.print(mySensorA.readTempF(), 2);
	Serial.print(", ");
//	Serial.print(mySensorB.readTempF(), 2);
	Serial.println(" degrees F");

	Serial.print("Pressure: ");
	Serial.print(mySensorA.readFloatPressure(), 2);
	Serial.print(", ");
//	Serial.print(mySensorB.readFloatPressure(), 2);
	Serial.println(" Pa");

	Serial.print("Altitude: ");
	Serial.print(mySensorA.readFloatAltitudeMeters(), 2);
	Serial.print(", ");
//	Serial.print(mySensorB.readFloatAltitudeMeters(), 2);
	Serial.println("m");

	Serial.print("Altitude: ");
	Serial.print(mySensorA.readFloatAltitudeFeet(), 2);
	Serial.print(", ");
//	Serial.print(mySensorB.readFloatAltitudeFeet(), 2);
	Serial.println("ft");	

	Serial.print("Humidity: ");
	Serial.print(mySensorA.readFloatHumidity(), 2);
	Serial.print(", ");
//	Serial.print(mySensorB.readFloatHumidity(), 2);
	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 frist,It will takes about 2 minutes."));
    Serial.print("  ");
    Serial.println(F("During  calibratting ,you should rotate and turn the 9DOF all the time within 2 minutes."));
    Serial.print("  ");
    Serial.println(F("If you are ready ,please sent a command data 'ready' to start sample and calibrate."));
    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]);                            //you can see the sample data here .
        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]; //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;
}

Open the Sscom32 terminal or the Serial moniter , and set the baudrate to 57600, you will see:
IMU-10DOF result.png

Personal tools
Namespaces

Variants
Actions
Elecrow Store
Navigation
Elecrow Products
Toolbox