Revision 0:2a7221ee9861, committed 2018-03-21

Comitter:

daqn

Date:

Wed Mar 21 07:58:57 2018 +0000

Commit message:

i dont know what should i write here

Changed in this revision

diff -r 000000000000 -r 2a7221ee9861 Eigen.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Eigen.lib	Wed Mar 21 07:58:57 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/ykuroda/code/Eigen/#13a5d365ba16

diff -r 000000000000 -r 2a7221ee9861 MPU9250/MPU9250.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250/MPU9250.cpp	Wed Mar 21 07:58:57 2018 +0000
@@ -0,0 +1,388 @@
+/** Daqn change log
+ * 2018/02/16 :  Replace words "MPU6050" with "MPU9250".
+ * 2018/02/16 :  L57, flip T/F by adding "!".
+ * 2018/02/21 :  L
+ */
+
+/**
+ * Includes
+ */
+#include "MPU9250.h"
+
+MPU9250::MPU9250(PinName sda, PinName scl) : connection(sda, scl) {
+    this->setSleepMode(false);
+    
+    //Initializations:
+    currentGyroRange = 0;
+    currentAcceleroRange=0;
+    alpha = ALPHA;
+}
+
+//--------------------------------------------------
+//-------------------General------------------------
+//--------------------------------------------------
+
+void MPU9250::write(char address, char data) {
+    char temp[2];
+    temp[0]=address;
+    temp[1]=data;
+    
+    connection.write(MPU9250_ADDRESS * 2,temp,2);
+}
+
+char MPU9250::read(char address) {
+    char retval;
+    connection.write(MPU9250_ADDRESS * 2, &address, 1, true);
+    connection.read(MPU9250_ADDRESS * 2, &retval, 1);
+    return retval;
+}
+
+void MPU9250::read(char address, char *data, int length) {
+    connection.write(MPU9250_ADDRESS * 2, &address, 1, true);
+    connection.read(MPU9250_ADDRESS * 2, data, length);
+}
+
+void MPU9250::setSleepMode(bool state) {
+    char temp;
+    temp = this->read(MPU9250_PWR_MGMT_1_REG);
+    if (state == true)
+        temp |= 1<<MPU9250_SLP_BIT;
+    if (state == false)
+        temp &= ~(1<<MPU9250_SLP_BIT);
+    this->write(MPU9250_PWR_MGMT_1_REG, temp);
+}
+
+bool MPU9250::testConnection( void ) {
+    char temp;
+    temp = this->read(MPU9250_WHO_AM_I_REG);
+    return !(temp == (MPU9250_ADDRESS & 0xFE));  // Daqn 2018/02/16
+}
+
+void MPU9250::setBW(char BW) {
+    char temp;
+    BW=BW & 0x07;
+    temp = this->read(MPU9250_CONFIG_REG);
+    temp &= 0xF8;
+    temp = temp + BW;
+    this->write(MPU9250_CONFIG_REG, temp);
+}
+
+void MPU9250::setI2CBypass(bool state) {
+    char temp;
+    temp = this->read(MPU9250_INT_PIN_CFG);
+    if (state == true)
+        temp |= 1<<MPU9250_BYPASS_BIT;
+    if (state == false)
+        temp &= ~(1<<MPU9250_BYPASS_BIT);
+    this->write(MPU9250_INT_PIN_CFG, temp);
+}
+
+//--------------------------------------------------
+//----------------Accelerometer---------------------
+//--------------------------------------------------
+
+void MPU9250::setAcceleroRange( char range ) {
+    char temp;
+    range = range & 0x03;
+    currentAcceleroRange = range;
+    
+    temp = this->read(MPU9250_ACCELERO_CONFIG_REG);
+    temp &= ~(3<<3);
+    temp = temp + (range<<3);
+    this->write(MPU9250_ACCELERO_CONFIG_REG, temp);
+}
+
+int MPU9250::getAcceleroRawX( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU9250_ACCEL_XOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+int MPU9250::getAcceleroRawY( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU9250_ACCEL_YOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+int MPU9250::getAcceleroRawZ( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU9250_ACCEL_ZOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+void MPU9250::getAcceleroRaw( int *data ) {
+    char temp[6];
+    this->read(MPU9250_ACCEL_XOUT_H_REG, temp, 6);
+    data[0] = (int)(short)((temp[0]<<8) + temp[1]);
+    data[1] = (int)(short)((temp[2]<<8) + temp[3]);
+    data[2] = (int)(short)((temp[4]<<8) + temp[5]);
+}
+
+void MPU9250::getAccelero( float *data ) {
+    int temp[3];
+    this->getAcceleroRaw(temp);
+    if (currentAcceleroRange == MPU9250_ACCELERO_RANGE_2G) {
+        data[0]=(float)temp[0] / 16384.0 * 9.81;
+        data[1]=(float)temp[1] / 16384.0 * 9.81;
+        data[2]=(float)temp[2] / 16384.0 * 9.81;
+        }
+    if (currentAcceleroRange == MPU9250_ACCELERO_RANGE_4G){
+        data[0]=(float)temp[0] / 8192.0 * 9.81;
+        data[1]=(float)temp[1] / 8192.0 * 9.81;
+        data[2]=(float)temp[2] / 8192.0 * 9.81;
+        }
+    if (currentAcceleroRange == MPU9250_ACCELERO_RANGE_8G){
+        data[0]=(float)temp[0] / 4096.0 * 9.81;
+        data[1]=(float)temp[1] / 4096.0 * 9.81;
+        data[2]=(float)temp[2] / 4096.0 * 9.81;
+        }
+    if (currentAcceleroRange == MPU9250_ACCELERO_RANGE_16G){
+        data[0]=(float)temp[0] / 2048.0 * 9.81;
+        data[1]=(float)temp[1] / 2048.0 * 9.81;
+        data[2]=(float)temp[2] / 2048.0 * 9.81;
+        }
+    
+    #ifdef DOUBLE_ACCELERO
+        data[0]*=2;
+        data[1]*=2;   
+        data[2]*=2;
+    #endif   
+}
+//--------------------------------------------------
+//------------------Gyroscope-----------------------
+//--------------------------------------------------
+void MPU9250::setGyroRange( char range ) {
+    char temp;
+    currentGyroRange = range;
+    range = range & 0x03;
+    temp = this->read(MPU9250_GYRO_CONFIG_REG);
+    temp &= ~(3<<3);
+    temp = temp + range<<3;
+    this->write(MPU9250_GYRO_CONFIG_REG, temp);
+}
+
+int MPU9250::getGyroRawX( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU9250_GYRO_XOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+    
+int MPU9250::getGyroRawY( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU9250_GYRO_YOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+int MPU9250::getGyroRawZ( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU9250_GYRO_ZOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+void MPU9250::getGyroRaw( int *data ) {
+    char temp[6];
+    this->read(MPU9250_GYRO_XOUT_H_REG, temp, 6);
+    data[0] = (int)(short)((temp[0]<<8) + temp[1]);
+    data[1] = (int)(short)((temp[2]<<8) + temp[3]);
+    data[2] = (int)(short)((temp[4]<<8) + temp[5]);
+}
+
+void MPU9250::getGyro( float *data ) {
+    int temp[3];
+    this->getGyroRaw(temp);
+    if (currentGyroRange == MPU9250_GYRO_RANGE_250) {
+        data[0]=(float)temp[0] / 301.0;
+        data[1]=(float)temp[1] / 301.0;
+        data[2]=(float)temp[2] / 301.0;
+        }               //7505.5
+    if (currentGyroRange == MPU9250_GYRO_RANGE_500){
+        data[0]=(float)temp[0] / 3752.9;
+        data[1]=(float)temp[1] / 3752.9;
+        data[2]=(float)temp[2] / 3752.9;
+        }
+    if (currentGyroRange == MPU9250_GYRO_RANGE_1000){
+        data[0]=(float)temp[0] / 1879.3;;
+        data[1]=(float)temp[1] / 1879.3;
+        data[2]=(float)temp[2] / 1879.3;
+        }
+    if (currentGyroRange == MPU9250_GYRO_RANGE_2000){
+        data[0]=(float)temp[0] / 939.7;
+        data[1]=(float)temp[1] / 939.7;
+        data[2]=(float)temp[2] / 939.7;
+        }
+}
+//--------------------------------------------------
+//-------------------Temperature--------------------
+//--------------------------------------------------
+int MPU9250::getTempRaw( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU9250_TEMP_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+float MPU9250::getTemp( void ) {
+    float retval;
+    retval=(float)this->getTempRaw();
+    retval=(retval+521.0)/340.0+35.0;
+    return retval;
+}
+
+
+//--------------------------------------------------
+//------------------Magnetometer--------------------
+//--------------------------------------------------
+void MPU9250::read(char address, char subaddress, char *data, int length)
+{
+//    connection.write(MPU9250_ADDRESS * 2, &address, 1, true);
+//    connection.read(MPU9250_ADDRESS * 2, data, length);
+    connection.write(address, &subaddress, 1, true);
+    connection.read(address, data, length);
+}
+
+//void MPU9250::setMagnetoRange( char range ) {
+//    char temp;
+//    currentMagnetoRange = range;
+//    range = range & 0x03;
+//    temp = this->read(MPU9250_MAGNETO_CONFIG_REG);
+//    temp &= ~(3<<3);
+//    temp = temp + range<<3;
+//    this->write(MPU9250_MAGNETO_CONFIG_REG, temp);
+//}
+
+
+
+int MPU9250::getMagnetoRawX( void )
+{
+    short retval;
+    char data[2];
+    this->read(AK8963_ADDRESS, AK8963_XOUT_L, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+int MPU9250::getMagnetoRawY( void )
+{
+    short retval;
+    char data[2];
+    this->read(AK8963_ADDRESS, AK8963_YOUT_L, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+//
+int MPU9250::getMagnetoRawZ( void )
+{
+    short retval;
+    char data[2];
+    this->read(AK8963_ADDRESS, AK8963_ZOUT_L, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+void MPU9250::getMagnetoRaw( int *data ) {
+    char temp[6];
+    this->read(AK8963_ADDRESS, AK8963_XOUT_L, temp, 6);
+    data[0] = (int)(short)((temp[1]<<8) + temp[0]);
+    data[1] = (int)(short)((temp[3]<<8) + temp[2]);
+    data[2] = (int)(short)((temp[5]<<8) + temp[4]);
+}
+
+void MPU9250::getMagneto( float *data ) {
+    int temp[3];
+    this->getMagnetoRaw(temp);
+    data[0]=(float)temp[0] * .15f;
+    data[1]=(float)temp[1] * .15f;
+    data[2]=(float)temp[2] * .15f;
+}
+
+/**Additional function added by Montvydas Klumbys, which will allow easy offset, angle calculation and much more.
+ function for getting angles in degrees from accelerometer
+*/
+void MPU9250::getAcceleroAngle( float *data ) {
+    float temp[3];
+    this->getAccelero(temp);
+    
+    data[X_AXIS] = atan (temp[Y_AXIS]/sqrt(pow(temp[X_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading
+    data[Y_AXIS] = atan (-1*temp[X_AXIS]/sqrt(pow(temp[Y_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading
+    data[Z_AXIS] = atan (sqrt(pow(temp[X_AXIS], 2) + pow(temp[Y_AXIS], 2))/temp[Z_AXIS]) * RADIANS_TO_DEGREES; //This one is not used anywhere later on
+    
+//    data[Y_AXIS] = atan2 (temp[Y_AXIS],temp[Z_AXIS]) * RADIANS_TO_DEGREES;  //This spits out values between -180 to 180 (360 degrees)
+//    data[X_AXIS] = atan2 (-1*temp[X_AXIS], temp[Z_AXIS]) * RADIANS_TO_DEGREES;    //but it takes longer and system gets unstable when angles ~90 degrees
+}
+
+///function for getting offset values for the gyro & accelerometer
+void MPU9250::getOffset(float *accOffset, float *gyroOffset, int sampleSize){
+    float gyro[3];      
+    float accAngle[3];
+    
+    for (int i = 0; i < 3; i++) {
+        accOffset[i] = 0.0;     //initialise offsets to 0.0
+        gyroOffset[i] = 0.0;
+    }
+    
+    for (int i = 0; i < sampleSize; i++){
+        this->getGyro(gyro); //take real life measurements  
+        this->getAcceleroAngle (accAngle);
+        
+        for (int j = 0; j < 3; j++){
+            *(accOffset+j) += accAngle[j]/sampleSize;    //average measurements
+            *(gyroOffset+j) += gyro[j]/sampleSize;
+        }
+        wait (0.01);    //wait between each reading for accuracy 
+    } 
+}
+
+///function for computing angles for roll, pitch anf yaw
+void MPU9250::computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval){
+    float gyro[3];
+    float accAngle[3];
+    
+    this->getGyro(gyro);                //get gyro value in rad/s
+    this->getAcceleroAngle(accAngle);   //get angle from accelerometer
+    
+    for (int i = 0; i < 3; i++){
+        gyro[i] -= gyroOffset[i];       //substract offset values
+        accAngle[i] -= accOffset[i];    
+    }
+    
+    //apply filters on pitch and roll to get accurate angle values
+    angle[X_AXIS] = alpha * (angle[X_AXIS] + GYRO_SCALE*gyro[X_AXIS]*interval) + (1-alpha)*accAngle[X_AXIS]; 
+    angle[Y_AXIS] = alpha * (angle[Y_AXIS] + GYRO_SCALE*gyro[Y_AXIS]*interval) + (1-alpha)*accAngle[Y_AXIS];
+    
+    //calculate Yaw using just the gyroscope values - inaccurate
+    angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*interval;     
+}
+
+///function for setting a different Alpha value, which is used in complemetary filter calculations
+void MPU9250::setAlpha(float val){
+    alpha = val;   
+}
+
+///function for enabling interrupts on MPU9250 INT pin, when the data is ready to take
+void MPU9250::enableInt( void ){
+    char temp;
+    temp = this->read(MPU9250_RA_INT_ENABLE);
+    temp |= 0x01;
+    this->write(MPU9250_RA_INT_ENABLE, temp);
+}
+
+///function for disbling interrupts on MPU9250 INT pin, when the data is ready to take
+void MPU9250::disableInt ( void ){
+    char temp;
+    temp = this->read(MPU9250_RA_INT_ENABLE);
+    temp &= 0xFE;
+    this->write(MPU9250_RA_INT_ENABLE, temp);
+}
\ No newline at end of file

diff -r 000000000000 -r 2a7221ee9861 MPU9250/MPU9250.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250/MPU9250.h	Wed Mar 21 07:58:57 2018 +0000
@@ -0,0 +1,426 @@
+/** Daqn change log
+ * 2018/02/16 :  Replace words "MPU6050" with "MPU9250".
+ * 2018/02/21 :  
+ */
+
+/*Use #define MPU9250_ES before you include this file if you have an engineering sample (older EVBs will have them), to find out if you have one, check your accelerometer output. 
+If it is half of what you expected, and you still are on the correct planet, you got an engineering sample
+*/
+
+
+#ifndef MPU9250_H
+#define MPU9250_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+
+/**
+ * Defines
+ */
+#ifndef MPU9250_ADDRESS
+    #define MPU9250_ADDRESS             0x68 // address pin low (GND), default for InvenSense evaluation board
+#endif
+
+#ifdef MPU9250_ES
+        #define DOUBLE_ACCELERO
+#endif  
+
+/**
+ * Registers
+ */
+ #define MPU9250_CONFIG_REG         0x1A
+ #define MPU9250_GYRO_CONFIG_REG    0x1B
+ #define MPU9250_ACCELERO_CONFIG_REG    0x1C
+  
+ #define MPU9250_INT_PIN_CFG        0x37
+ 
+ #define MPU9250_ACCEL_XOUT_H_REG   0x3B
+ #define MPU9250_ACCEL_YOUT_H_REG   0x3D
+ #define MPU9250_ACCEL_ZOUT_H_REG   0x3F
+ 
+ #define MPU9250_TEMP_H_REG         0x41
+ 
+ #define MPU9250_GYRO_XOUT_H_REG    0x43
+ #define MPU9250_GYRO_YOUT_H_REG    0x45
+ #define MPU9250_GYRO_ZOUT_H_REG    0x47
+ 
+ #define MPU9250_MAG_XOUT_H_REG    0x04 
+ #define MPU9250_MAG_YOUT_H_REG    0x06
+ #define MPU9250_MAG_ZOUT_H_REG    0x08
+ 
+ 
+ 
+ #define MPU9250_PWR_MGMT_1_REG     0x6B
+ #define MPU9250_WHO_AM_I_REG       0x75
+ 
+                 
+ 
+ /**
+  * Definitions
+  */
+#define MPU9250_SLP_BIT             6
+#define MPU9250_BYPASS_BIT         1
+
+#define MPU9250_BW_256              0
+#define MPU9250_BW_188              1
+#define MPU9250_BW_98               2
+#define MPU9250_BW_42               3
+#define MPU9250_BW_20               4
+#define MPU9250_BW_10               5
+#define MPU9250_BW_5                6
+
+#define MPU9250_ACCELERO_RANGE_2G   0
+#define MPU9250_ACCELERO_RANGE_4G   1
+#define MPU9250_ACCELERO_RANGE_8G   2
+#define MPU9250_ACCELERO_RANGE_16G  3
+
+#define MPU9250_GYRO_RANGE_250      0
+#define MPU9250_GYRO_RANGE_500      1
+#define MPU9250_GYRO_RANGE_1000     2
+#define MPU9250_GYRO_RANGE_2000     3
+
+//interrupt address
+#define MPU9250_RA_INT_ENABLE 0x38  
+//define how the accelerometer is placed on surface
+#define X_AXIS 1
+#define Y_AXIS 2
+#define Z_AXIS 0
+//translation from radians to degrees
+#define RADIANS_TO_DEGREES 180/3.1415926536
+//constant used for the complementary filter, which ranges usually from 0.9 to 1.0
+#define ALPHA 0.96   //filter constant
+//scale the gyro
+#define GYRO_SCALE 2.7176 
+
+
+ /**
+  * Magnetometer
+  */
+#define AK8963_ADDRESS   0x0C<<1
+#define WHO_AM_I_AK8963  0x00 // should return 0x48
+#define INFO             0x01
+#define AK8963_ST1       0x02  // data ready status bit 0
+#define AK8963_XOUT_L    0x03  // data
+#define AK8963_XOUT_H    0x04
+#define AK8963_YOUT_L    0x05
+#define AK8963_YOUT_H    0x06
+#define AK8963_ZOUT_L    0x07
+#define AK8963_ZOUT_H    0x08
+#define AK8963_ST2       0x09  // Data overflow bit 3 and data read error status bit 2
+#define AK8963_CNTL      0x0A  // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
+#define AK8963_ASTC      0x0C  // Self test control
+#define AK8963_I2CDIS    0x0F  // I2C disable
+#define AK8963_ASAX      0x10  // Fuse ROM x-axis sensitivity adjustment value
+#define AK8963_ASAY      0x11  // Fuse ROM y-axis sensitivity adjustment value
+#define AK8963_ASAZ      0x12  // Fuse ROM z-axis sensitivity adjustment value
+
+
+/** MPU9250 IMU library.
+  *
+  * Example:
+  * @code
+  * Later, maybe
+  * @endcode
+  */
+class MPU9250
+{
+public:
+    /**
+    * Constructor.
+    *
+    * Sleep mode of MPU9250 is immediatly disabled
+    *
+    * @param sda - mbed pin to use for the SDA I2C line.
+    * @param scl - mbed pin to use for the SCL I2C line.
+    */
+    MPU9250(PinName sda, PinName scl);
+    
+    
+    /**
+    * Tests the I2C connection by reading the WHO_AM_I register. 
+    *
+    * @return True for a working connection, false for an error
+    */     
+    bool testConnection( void );
+    
+    /**
+    * Sets the bandwidth of the digital low-pass filter 
+    *
+    * Macros: MPU9250_BW_256 - MPU9250_BW_188 - MPU9250_BW_98 - MPU9250_BW_42 - MPU9250_BW_20 - MPU9250_BW_10 - MPU9250_BW_5
+    * Last number is the gyro's BW in Hz (accelero BW is virtually identical)
+    *
+    * @param BW - The three bits that set the bandwidth (use the predefined macros)
+    */     
+    void setBW( char BW );
+    
+    /**
+    * Sets the auxiliary I2C bus in bypass mode to read the sensors behind the MPU9250 (useful for eval board, otherwise just connect them to primary I2C bus) 
+    *
+    * @param state - Enables/disables the I2C bypass mode
+    */     
+    void setI2CBypass ( bool state );
+    
+    /**
+    * Sets the Accelero full-scale range
+    *
+    * Macros: MPU9250_ACCELERO_RANGE_2G - MPU9250_ACCELERO_RANGE_4G - MPU9250_ACCELERO_RANGE_8G - MPU9250_ACCELERO_RANGE_16G
+    *
+    * @param range - The two bits that set the full-scale range (use the predefined macros)
+    */
+    void setAcceleroRange(char range);
+    
+    /**
+    * Reads the accelero x-axis.
+    *
+    * @return 16-bit signed integer x-axis accelero data
+    */   
+    int getAcceleroRawX( void );
+    
+    /**
+    * Reads the accelero y-axis.
+    *
+    * @return 16-bit signed integer y-axis accelero data
+    */   
+    int getAcceleroRawY( void );
+    
+    /**
+    * Reads the accelero z-axis.
+    *
+    * @return 16-bit signed integer z-axis accelero data
+    */   
+    int getAcceleroRawZ( void );
+    
+    /**
+    * Reads all accelero data.
+    *
+    * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
+    */   
+    void getAcceleroRaw( int *data );
+    
+    /**
+    * Reads all accelero data, gives the acceleration in m/s2
+    *
+    * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
+    *
+    * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
+    */   
+    void getAccelero( float *data );
+    
+    /**
+    * Sets the Gyro full-scale range
+    *
+    * Macros: MPU9250_GYRO_RANGE_250 - MPU9250_GYRO_RANGE_500 - MPU9250_GYRO_RANGE_1000 - MPU9250_GYRO_RANGE_2000
+    *
+    * @param range - The two bits that set the full-scale range (use the predefined macros)
+    */
+    void setGyroRange(char range);
+    
+    /**
+    * Reads the gyro x-axis.
+    *
+    * @return 16-bit signed integer x-axis gyro data
+    */   
+    int getGyroRawX( void );
+    
+    /**
+    * Reads the gyro y-axis.
+    *
+    * @return 16-bit signed integer y-axis gyro data
+    */   
+    int getGyroRawY( void );
+    
+    /**
+    * Reads the gyro z-axis.
+    *
+    * @return 16-bit signed integer z-axis gyro data
+    */   
+    int getGyroRawZ( void );
+    
+    /**
+    * Reads all gyro data.
+    *
+    * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
+    */   
+    void getGyroRaw( int *data );  
+    
+    /**
+    * Reads all gyro data, gives the gyro in rad/s
+    *
+    * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
+    *
+    * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
+    */   
+    void getGyro( float *data);
+    
+    /**
+    * Reads temperature data.
+    *
+    * @return 16 bit signed integer with the raw temperature register value
+    */  
+    int getTempRaw( void );
+    
+    /**
+    * Returns current temperature
+    *
+    * @returns float with the current temperature
+    */  
+    float getTemp( void );
+    
+    /**
+    * Sets the sleep mode of the MPU9250 
+    *
+    * @param state - true for sleeping, false for wake up
+    */     
+    void setSleepMode( bool state );
+    
+    
+    /**
+    * Writes data to the device, could be private, but public is handy so you can transmit directly to the MPU. 
+    *
+    * @param adress - register address to write to
+    * @param data - data to write
+    */
+    void write( char address, char data);
+    
+    /**
+    * Read data from the device, could be private, but public is handy so you can transmit directly to the MPU. 
+    *
+    * @param adress - register address to write to
+    * @return - data from the register specified by RA
+    */
+    char read( char adress);
+    
+    /**
+    * Read multtiple regigsters from the device, more efficient than using multiple normal reads. 
+    *
+    * @param adress - register address to write to
+    * @param length - number of bytes to read
+    * @param data - pointer where the data needs to be written to 
+    */
+    void read( char adress, char *data, int length);
+    
+    /**
+    * function for calculating the angle from the accelerometer.
+    * it takes 3 values which correspond acceleration in X, Y and Z direction and calculates angles in degrees
+    * for pitch, roll and one more direction.. (NOT YAW!)
+    *
+    * @param data - angle calculated using only accelerometer
+    *
+    */ 
+    void getAcceleroAngle( float *data );
+    
+    
+    /**function which allows to produce the offset values for gyro and accelerometer.
+    * offset for gyro is simply a value, which needs to be substracted from original gyro rad/sec speed
+    * but offset for accelerometer is calculated in angles... later on might change that
+    * function simply takes the number of samples to be taken and calculated the average
+    * 
+    * @param accOffset - accelerometer offset in angle
+    * @param gyroOffset - gyroscope offset in rad/s
+    * @param sampleSize - number of samples to be taken for calculating offsets
+    *
+    */
+    void getOffset(float *accOffset, float *gyroOffset, int sampleSize);
+    
+    /**
+    * function for computing the angle, when accelerometer angle offset and gyro offset are both known.
+    * we also need to know how much time passed from previous calculation to now
+    * it produces the angle in degrees. However angles are produced from -90.0 to 90.0 degrees
+    * if anyone need smth different, they can update this library...
+    *
+    * @param angle - calculated accurate angle from complemetary filter
+    * @param accOffset - offset in angle for the accelerometer
+    * @param gyroOffset - offset in rad/s for the gyroscope
+    * @param interval - time before previous angle calculation and now
+    *
+    */
+    void computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval);
+    
+    ///function, which enables interrupts on MPU9250 INT pin
+    void enableInt( void );
+    
+    ///disables interrupts
+    void disableInt( void );
+    
+    /**function which sets the alpha value - constant for the complementary filter. default alpha = 0.97
+    *
+    * @param val - value the alpha (complementary filter constant) should be set to
+    *
+    */
+    void setAlpha(float val);
+    
+    ////////////////////////////////////////////////////////////////////////////
+    ////////////////////////////////////////////////////////////////////////////
+    ////////////////////////////////////////////////////////////////////////////
+    
+    /** Daqn added
+    * read bytes on specific address and subaddress.
+    *
+    * @param adress - register address to write to
+    * @param subadress - register address to write to
+    * @param length - number of bytes to read
+    * @param data - pointer where the data needs to be written to 
+    */
+    void read(char address, char subaddress, char *data, int length);
+    
+    /** Daqn added
+    * Sets the Magneto full-scale range
+    *
+    * Macros: MPU9250_MAGNETO_RANGE_250 - MPU9250_MAGNETO_RANGE_500 - MPU9250_MAGNETO_RANGE_1000 - MPU9250_MAGNETO_RANGE_2000
+    *
+    * @param range - The two bits that set the full-scale range (use the predefined macros)
+    */
+//    void setMagnetoRange(char range);
+    
+    /** Daqn added
+    * Reads the magnetometer x-axis.
+    *
+    * @return 16-bit signed integer x-axis magnetometer data
+    */   
+    int getMagnetoRawX( void );
+    
+    /** Daqn added
+    * Reads the magnetometer y-axis.
+    *
+    * @return 16-bit signed integer y-axis magnetometer data
+    */   
+    int getMagnetoRawY( void );
+    
+    /** Daqn added
+    * Reads the magnetometer z-axis.
+    *
+    * @return 16-bit signed integer z-axis magnetometer data
+    */   
+    int getMagnetoRawZ( void );
+    
+    /** Daqn added
+    * Reads all magnetometer data.
+    *
+    * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
+    */   
+    void getMagnetoRaw( int *data );  
+    
+    /** Daqn added
+    * Reads all magnetometer data, gives the magnetometer in [[[TBD]]]
+    *
+    * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
+    *
+    * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
+    */   
+    void getMagneto( float *data);
+    
+private:
+    
+    I2C connection;
+    char currentAcceleroRange;
+    char currentGyroRange;
+    float alpha;   
+
+};
+
+
+
+#endif

diff -r 000000000000 -r 2a7221ee9861 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 21 07:58:57 2018 +0000
@@ -0,0 +1,293 @@
+#include "mbed.h"
+#include "MPU9250.h"
+#include "math.h"
+#include "Eigen/Core.h"     // 線形代数のライブラリ
+#include "Eigen/Geometry.h" // 同上
+#define  PI 3.141592654     // 円周率
+
+Timer       timer;
+Ticker      ticker;
+DigitalOut  led1(LED1);
+DigitalOut  led2(LED2);
+DigitalOut  led3(LED3);
+DigitalOut  led4(LED4);
+//InterruptIn flt_pin(p30);
+Serial      pc (USBTX, USBRX);
+MPU9250     mpu(p28,p27);
+
+const float t_calibrate = 25.f;
+const float dt = 0.05;
+
+// 変数用意
+int   N = 0;                // 較正に用いるサンプル数
+
+float gyro[3];              // ジャイロセンサの出力
+float b_gyro[3] = {};       // ジャイロセンサのバイアス誤差．
+float acc[3];               // 加速度センサの出力
+// float acc_norm;             // 加速度ベクトルの2-ノルム（長さ）．
+float acc_normalized[3];    // 正規化した（長さ1の）加速度ベクトル．
+
+
+float pitch = 0.f;          // ピッチ角.
+float roll  = 0.f;          // バンク（ロール）角.
+float yaw;                  // 方位角（ヨー角）．
+Eigen::Vector4f q;          // 姿勢クォータニオン
+Eigen::Matrix4f Omg;
+Eigen::Vector4f q_dot;
+float omg[3];
+float q00_22, q11_33;
+
+void calibrate();
+void set_initial_quaternion(float roll, float pitch);
+void flt_pin_rise();
+void flt_pin_fall();
+void update();
+void update_correction();
+
+int main()
+{
+    mpu.setAcceleroRange(MPU9250_ACCELERO_RANGE_16G);
+    mpu.setGyroRange(MPU9250_GYRO_RANGE_2000);
+    
+    led1 = 1;
+    led2 = 1;
+    led3 = 1;
+//    pc.printf("=\r\n\n");
+//    pc.printf("Calibration (%3.1f s) will start in\r\n  5\r\n", t_calibrate);
+    wait(1.0);
+//    pc.printf("  4\r\n");
+    wait(1.0);
+//    pc.printf("  3\r\n");
+    wait(1.0);
+//    pc.printf("  2\r\n");
+    wait(1.0);
+//    pc.printf("  1\r\n\n");
+    wait(1.0);
+    led1 = 0;
+//    pc.printf("Calibrating...  ");
+    ticker.attach(&calibrate, 0.01);
+    timer.start();
+    while (timer.read() < t_calibrate);
+    ticker.detach();
+//    pc.printf("Done. (%d samples)\r\n\n", N);
+//    timer.stop();
+    led2 = 0;
+    
+    b_gyro[0] /= N;
+    b_gyro[1] /= N;
+    b_gyro[2] /= N;
+    roll  /= N;
+    pitch /= N;
+    
+//    printf("Initial Attitude\r\n");
+//    printf("  bank:%6.1f deg,   pitch:%6.1f deg\r\n\n",
+//              roll*180/PI ,     pitch*180/PI);
+    
+    set_initial_quaternion(roll, pitch);
+    
+    led3 = 0;
+//    pc.printf("waiting for launch...\r\n");
+//    flt_pin.mode(PullUp);
+//    flt_pin.fall(&flt_pin_fall);
+//    flt_pin.rise(&flt_pin_rise);
+    ticker.attach(&update_correction, dt);
+    led4 = 1;
+    while(1);
+}
+
+void calibrate()
+{
+    //////////////////////////////////////
+    //                                  //
+    // ジャイロセンサの較正・初期姿勢角の取得 //
+    //                                  //
+    //////////////////////////////////////
+    
+    // b_gyro にはジャイロセンサの出力の平均（バイアス誤差）を格納．
+    mpu.getGyro(gyro);
+    b_gyro[0] += gyro[0];
+    b_gyro[1] += gyro[1];
+    b_gyro[2] += gyro[2];
+    
+    // 重力加速度ベクトルを使って，姿勢角を算出
+    mpu.getAccelero(acc);
+//    acc_norm = sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
+
+//    acc_normalized[0] = acc[0] / acc_norm;
+//    acc_normalized[1] = acc[1] / acc_norm;
+//    acc_normalized[2] = acc[2] / acc_norm;
+    acc_normalized[2] = acc[2] / sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
+    
+    // N 回の姿勢角の平均をとって初期姿勢角とする
+    pitch += asin (acc_normalized[2]);
+    roll  += atan2(acc[1], -acc[0]);
+    
+    // count up
+    N++;
+}
+
+void set_initial_quaternion(float roll, float pitch)
+{
+    /////////////////////////////////////////
+    //                                     //
+    // 初期姿勢角から初期姿勢クォータニオンを計算 //
+    //                                     //
+    /////////////////////////////////////////
+    float c_roll2 = cos(roll/2);      // 繰り返しの計算を避けるための一時変数
+    float s_roll2 = sin(roll/2);      // 同上
+    
+    Eigen::Matrix4f qx0_otimes;
+    qx0_otimes << c_roll2,-s_roll2,    0   ,    0   ,
+                  s_roll2, c_roll2,    0   ,    0   ,
+                     0   ,    0   , c_roll2, s_roll2,
+                     0   ,    0   ,-s_roll2, c_roll2;
+    
+    Eigen::Vector4f q_y0(cos(pitch/2), 0.f, sin(pitch/2), 0.f);
+    
+    // 初期姿勢クォータニオン q
+    q = qx0_otimes * q_y0;
+}
+
+void flt_pin_rise()
+{
+    pc.printf("Launch!\r\n");
+    ticker.attach(&update, dt);
+}
+
+void flt_pin_fall()
+{
+    pc.printf("fall.\r\n");
+    ticker.detach();
+}
+
+ /**
+  * 観測値を用いて姿勢角を更新します．
+  * ジャイロセンサの出力を単純に積算します．
+  */
+void update()
+{
+    mpu.getGyro(gyro);
+    
+    omg[0] =  gyro[2] - b_gyro[2];
+    omg[1] = -gyro[1] + b_gyro[1];
+    omg[2] =  gyro[0] - b_gyro[0];
+    
+    // 姿勢クォータニオンの時間微分を計算
+    Omg << 0.f   ,-omg[0],-omg[1],-omg[2],
+           omg[0], 0.f   , omg[2],-omg[1],
+           omg[1],-omg[2], 0.f   , omg[0],
+           omg[2], omg[1],-omg[0], 0.f   ;
+//    q_dot = 0.5f * Omg * q;
+    
+    // 現在時刻の姿勢クォータニオン
+    q += Omg * q * 0.5f * dt;
+    q /= sqrtf(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
+    
+    // 姿勢クォータニオンからオイラー角への変換
+    q00_22 = (q[0] + q[2])*(q[0] - q[2]);
+    q11_33 = (q[1] + q[3])*(q[1] - q[3]);
+    roll  = atan2( 2*(q[2]*q[3]+q[0]*q[1]), q00_22 - q11_33);
+    pitch = asin (-2*(q[1]*q[3]-q[0]*q[2]));
+//    yaw   = atan2( 2*(q[1]*q[2]+q[0]*q[3]), q00_22 + q11_33);
+    
+//    pc.printf("%11.7f   ",timer.read());
+//    pc.printf("%6.1f %6.1f %6.1f\r\n",
+//               roll*180/PI,  pitch*180/PI,  yaw*180/PI);
+//    pc.printf("%6.1f %6.1f\r\n",
+//               roll*180/PI,  pitch*180/PI);
+}
+
+Eigen::Vector3f bB_tilde;
+Eigen::Vector3f bB_hat;
+Eigen::Vector3f bG(0.f, 0.f, -1.f);
+Eigen::Vector3f rot_vec;
+float rot_q[4] = {1.f, 0.f, 0.f, 0.f};
+Eigen::Matrix3f dcm;
+Eigen::Matrix4f rot_q_otimes;
+const float eps = 0.01;
+float q00, q01, q02, q03, q11, q12, q13, q22, q23, q33;
+float a_norm_div;
+float send_buff[4];
+char* send_bufc = (char*)send_buff;
+
+ /**
+  * 観測値を用いて姿勢角を更新します．
+  * ジャイロセンサの出力を積算した上で，加速度センサの出力を用いて補正します．
+  */
+void update_correction()
+{
+    mpu.getGyro(gyro);
+    
+    omg[0] =  gyro[2] - b_gyro[2];
+    omg[1] = -gyro[1] + b_gyro[1];
+    omg[2] =  gyro[0] - b_gyro[0];
+    
+    // 姿勢クォータニオンの時間微分を計算
+    Omg << 0.f   ,-omg[0],-omg[1],-omg[2],
+           omg[0], 0.f   , omg[2],-omg[1],
+           omg[1],-omg[2], 0.f   , omg[0],
+           omg[2], omg[1],-omg[0], 0.f   ;
+//    q_dot = 0.5f * Omg * q;
+    
+    // 現在時刻の姿勢クォータニオン
+    q += Omg * q * 0.5f * dt;
+    q /= sqrtf(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
+    
+    // 姿勢クォータニオンからオイラー角への変換
+    q00 = q[0]*q[0];
+    q01 = q[0]*q[1];
+    q02 = q[0]*q[2];
+    q03 = q[0]*q[3];
+    q11 = q[1]*q[1];
+    q12 = q[1]*q[2];
+    q13 = q[1]*q[3];
+    q22 = q[2]*q[2];
+    q23 = q[2]*q[3];
+    q33 = q[3]*q[3];
+    dcm <<  q00+q11-q22-q33,   2*(q12+q03)  ,   2*(q13-q02)  ,
+              2*(q12-q03)  , q00-q11+q22-q33,   2*(q23+q01)  ,
+              2*(q13+q02)  ,   2*(q23-q01)  , q00-q11-q22+q33;
+    bB_hat = dcm*bG;
+    mpu.getAccelero(acc);
+    a_norm_div = 1.f / sqrt(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2]);
+    bB_tilde << acc[2]*a_norm_div,-acc[1]*a_norm_div, acc[0]*a_norm_div;
+    rot_vec = bB_hat.cross(bB_hat - bB_tilde);
+    rot_q[1] = 0.5f*eps*rot_vec[0];
+    rot_q[2] = 0.5f*eps*rot_vec[1];
+    rot_q[3] = 0.5f*eps*rot_vec[2];
+    rot_q_otimes <<   1.f   ,-rot_q[1],-rot_q[2],-rot_q[3],
+                    rot_q[1],   1.f   , rot_q[3],-rot_q[2],
+                    rot_q[2],-rot_q[3],   1.f   , rot_q[1],
+                    rot_q[3], rot_q[2],-rot_q[1],   1.f   ;
+    q = rot_q_otimes * q;
+//    q00 = q[0]*q[0];    // Note that these values are based on
+//    q11 = q[1]*q[1];    // rotated quatenion (L253), so they are diferent from
+//    q22 = q[2]*q[2];    // the values calculated above (L228-237).
+//    q33 = q[3]*q[3];
+//    q /= sqrtf(q00 + q11 + q22 + q33);
+    
+    q /= sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
+    send_buff[0] = q[0];
+    send_buff[1] = q[1];
+    send_buff[2] = q[2];
+    send_buff[3] = q[3];
+//    send_buff[0] = 0.1f;
+//    send_buff[1] = 0.2f;
+//    send_buff[2] = 0.3f;
+//    send_buff[3] = 0.4f;
+//    pc.write(send_buf,4);
+    pc.putc('\0');
+    for (int i = 0; i < 16; i++)
+        pc.putc(send_bufc[i]);
+//    q00_22 = q00 - q22;
+//    q11_33 = q11 - q33;
+//    roll  = atan2( 2*(q[2]*q[3]+q[0]*q[1]), q00_22 - q11_33);
+//    pitch = asin (-2*(q[1]*q[3]-q[0]*q[2]));
+//    yaw   = atan2( 2*(q[1]*q[2]+q[0]*q[3]), q00_22 + q11_33);
+    
+//    pc.printf("%11.7f   ",timer.read());
+//    pc.printf("%6.1f %6.1f %6.1f\r\n",
+//               roll*180/PI,  pitch*180/PI,  yaw*180/PI);
+//    pc.printf("%6.1f %6.1f\r\n",
+//               roll*180/PI,  pitch*180/PI);
+}
\ No newline at end of file

diff -r 000000000000 -r 2a7221ee9861 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Mar 21 07:58:57 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/aa5281ff4a02
\ No newline at end of file