Linkit 7697 + IMU 10DOF v2.0 研讀紀錄(一)
Linkit 7697 + IMU 10DOF v2.0 研讀紀錄(二)：I2Cdev::readBites()

處理完MPU9250 connection failed的問題以後，接下來再回到感測器本身。根據最小添加的原則，這次我們只增加三軸的部分。在IMU_10DOF_V2_Test的範例程式碼中，三軸資料使用getAccel_Data(void)這支副程式取得。

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;
}

這裡用到一些變數，看過以後需要練習寫下變數的功能，如下：

int16_t ax, ay, az; //儲存三軸輸出，16位元
int16_t gx, gy, gz;
int16_t   mx, my, mz;

float Axyz[3];  //正規化輸出，使單位為g。因為前面初始化設定的時候使用+-2G作為最大輸出，
                //所以輸出範圍就是4g，輸出刻度有2^16 = 65536，因此Axyz[0] = ax / 65536 / 4
                //範例程式碼直接用Axyz[0] = ax / 16384，這會害人，如果使用者自行更改最大輸出範圍時。

這裡int16_t gx, gy, gz;
int16_t mx, my, mz;是因為呼叫MPU9250::getMotion9()必須要，所以一起附上。

上面附的程式碼裡面有變數的說明，這是看過程式碼以及規格文件以後能夠判斷出來的，這裡提醒我們一件事，範例程式碼終究只是範例程式碼，只負責展示功能但並不代表正確與有效率。

接著往下看，這邊會呼叫MPU9250::getMotion9()，

/** Get raw 9-axis motion sensor readings (accel/gyro/compass).
 * FUNCTION NOT FULLY IMPLEMENTED YET.
 * @param ax 16-bit signed integer container for accelerometer X-axis value
 * @param ay 16-bit signed integer container for accelerometer Y-axis value
 * @param az 16-bit signed integer container for accelerometer Z-axis value
 * @param gx 16-bit signed integer container for gyroscope X-axis value
 * @param gy 16-bit signed integer container for gyroscope Y-axis value
 * @param gz 16-bit signed integer container for gyroscope Z-axis value
 * @param mx 16-bit signed integer container for magnetometer X-axis value
 * @param my 16-bit signed integer container for magnetometer Y-axis value
 * @param mz 16-bit signed integer container for magnetometer Z-axis value
 * @see getMotion6()
 * @see getAcceleration()
 * @see getRotation()
 * @see MPU9250_RA_ACCEL_XOUT_H
 */
void MPU9250::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz) {
    
	//get accel and gyro
	getMotion6(ax, ay, az, gx, gy, gz);
	
	//read mag
	I2Cdev::writeByte(devAddr, MPU9250_RA_INT_PIN_CFG, 0x02); //set i2c bypass enable pin to true to access magnetometer
	delay(10);
	I2Cdev::writeByte(MPU9150_RA_MAG_ADDRESS, 0x0A, 0x01); //enable the magnetometer
	delay(10);
	I2Cdev::readBytes(MPU9150_RA_MAG_ADDRESS, MPU9150_RA_MAG_XOUT_L, 6, buffer);
	*mx = (((int16_t)buffer[1]) << 8) | buffer[0];
    *my = (((int16_t)buffer[3]) << 8) | buffer[2];
    *mz = (((int16_t)buffer[5]) << 8) | buffer[4];		
}

註解裡面就很清楚寫了：”FUNCTION NOT FULLY IMPLEMENTED YET.”，如果直接用這個function來實作系統功能，那心臟要夠大才行。

大略瀏覽一下它的功能，它使用getMotion6()去讀取六個參數，然後啟動磁力感測(magnetometer)，然後才去讀取磁力感測的數值。很顯然，讀取磁力感測那一段並沒有完整，我們先刪掉它。

然後看getMotion6()的內容，同時也可以在MPU9250.c裡面找到很多讀取三軸資料的函式，其基本寫法都是去呼叫I2Cdev::readBytes()。大概有這些：

void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz);
void getAcceleration(int16_t* x, int16_t* y, int16_t* z);
int16_t getAccelerationX();
int16_t getAccelerationY();
int16_t getAccelerationZ();

結構都類似getMotion6()

void MPU9250::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) {
    I2Cdev::readBytes(devAddr, MPU9250_RA_ACCEL_XOUT_H, 14, buffer);
    *ax = (((int16_t)buffer[0]) << 8) | buffer[1];
    *ay = (((int16_t)buffer[2]) << 8) | buffer[3];
    *az = (((int16_t)buffer[4]) << 8) | buffer[5];
    *gx = (((int16_t)buffer[8]) << 8) | buffer[9];
    *gy = (((int16_t)buffer[10]) << 8) | buffer[11];
    *gz = (((int16_t)buffer[12]) << 8) | buffer[13];
}

在getAcceleration()這個副程式的註解裡有足夠的說明讓我們使用三軸加速感測器的資料，列出來備忘與說明

/** Get 3-axis accelerometer readings.
* These registers store the most recent accelerometer measurements.
* Accelerometer measurements are written to these registers at the Sample Rate
* as defined in Register 25.
*
* The accelerometer measurement registers, along with the temperature
* measurement registers, gyroscope measurement registers, and external sensor
* data registers, are composed of two sets of registers: an internal register
* set and a user-facing read register set.
*
* The data within the accelerometer sensors’ internal register set is always
* updated at the Sample Rate. Meanwhile, the user-facing read register set
* duplicates the internal register set’s data values whenever the serial
* interface is idle. This guarantees that a burst read of sensor registers will
* read measurements from the same sampling instant. Note that if burst reads
* are not used, the user is responsible for ensuring a set of single byte reads
* correspond to a single sampling instant by checking the Data Ready interrupt.
*
* Each 16-bit accelerometer measurement has a full scale defined in ACCEL_FS
* (Register 28). For each full scale setting, the accelerometers’ sensitivity
* per LSB in ACCEL_xOUT is shown in the table below:
*
* <pre>
* AFS_SEL | Full Scale Range | LSB Sensitivity
* ——–+——————+—————-
* 0 | +/- 2g | 8192 LSB/mg
* 1 | +/- 4g | 4096 LSB/mg
* 2 | +/- 8g | 2048 LSB/mg
* 3 | +/- 16g | 1024 LSB/mg
* </pre>
*
* @param x 16-bit signed integer container for X-axis acceleration
* @param y 16-bit signed integer container for Y-axis acceleration
* @param z 16-bit signed integer container for Z-axis acceleration
* @see MPU9250_RA_GYRO_XOUT_H

接下來我們測試一下getMotion6()和getAcceleration()在執行速度上的差別。

程式碼還是寫在setup()裡面，測過第一遍以後，如果需要更多測試則可以寫在loop()裡面。

I2C (0x68) reading 14 bytes from 0x3B...
ARDUINO version = 10805
F1 A4 1C F8 35 50 F 0 FF 6A 0 5E FF E9. Done (14 read).
Acceleration(g) of X,Y,Z:
-0.22,0.45,0.83
use millis: 31
I2C (0x68) reading 6 bytes from 0x3B...
ARDUINO version = 10805
F1 80 1D 10 35 B0. Done (6 read).
Acceleration(g) of X,Y,Z:
-0.23,0.45,0.84
use millis: 26

把debug mode拿掉再測一次

Acceleration(g) of X,Y,Z:
-0.06,0.54,0.78
use millis: 0
Acceleration(g) of X,Y,Z:
0.00,0.46,0.82
use millis: 0

所以讀寫一次I2C bus的時間在 1ms以內。如果用400kHz的I2C線路速率來算，一次交換大概讀取20個bytes來算，每次的存取時間大約是0.4ms左右。

最後三軸的硬體上的更新速率(sample rate)應該是1kHz，這個數字是從文獻上看來的，還未從晶片上確認。我們在軟體上的讀取速率不應該超過1kHz。

最後整合一下在我們專案上可以使用的讀取三軸和陀螺儀資料的程式碼

int16_t ax, ay, az; //儲存三軸輸出，16位元
int16_t gx, gy, gz;

float Axyz[3];  //正規化輸出，使單位為g。因為前面初始化設定的時候使用+-2G作為最大輸出，
                //所以輸出範圍就是4g，輸出刻度有2^16 = 65536，因此Axyz[0] = ax / 65536 / 4
                //範例程式碼直接用Axyz[0] = ax / 16384，這會害人，如果使用者自行更改最大輸出範圍時。 
float Gxyz[3];

void get_Acc_Gyro_Data(void)
{
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    Axyz[0] = (double) ax / 16384;
    Axyz[1] = (double) ay / 16384;
    Axyz[2] = (double) az / 16384;
    Gxyz[0] = (double) gx * 250 / 32768;
    Gxyz[1] = (double) gy * 250 / 32768;
    Gxyz[2] = (double) gz * 250 / 32768;
}