[無人機]MPU-9255姿態計算

mega的I2c接腳位置不同
https://www.arduino.cc/en/Reference/Wire

磁力計
http://wukcsoft.blogspot.tw/2014/06/read-gy-80.html

校正方式
http://gogoprivateryan.blogspot.tw/2014/07/mpu-6050-google.html

[Ardunio]MPU9250

手邊的MPU9255是九加一軸感測器
基本上就是MPU9250加上溫度，
MPU9250是三軸加速度計加上三軸陀螺儀加上三軸羅盤
挺威的，

今天測試拿掉範例的中斷，直接存取Row data
看起來應該正常，但因為時間趕，得多測幾次看看有沒有bug。

MPU9250規格書
https://www.invensense.com/products/motion-tracking/9-axis/mpu-9250/

[無人機]姿態擷取問題

如官網上說的，擷取raw值很容易，剩下的很麻煩，
其實，歸功於許多開源的前輩，在ardunio上已經很容易取得raw值了，

至於如何估算姿態？
用MPU6050有個好處，裡面的DMP模組已經可以提供pitch,row,yaw角度了，
如果單純只做角度估測，DMP就夠用。

But....
四軸的控制，我們還有許多工作得處理，像是再加上其他的感測器如超音波，
就會造成時序的衝突，導致DMP或者超音波其中一個不work，

所以，拿掉DMP吧，如果你未來的內容不只是角度估測而已的話。

好家在，之前碩論做過，不過之前只用卡曼估算Pitch、Roll兩軸，很快就可以解決，
但是如果想加上Yaw軸，網路上多用四元數來解決，下面就紀錄一下這過程。

拿到MPU6050，有幾項工作得做，

一、確認raw值的scale範圍
在下面網頁有說明
http://www.i2cdevlib.com/forums/topic/4-understanding-raw-values-of-accelerometer-and-gyrometer/

在MPU6050.cpp初始化函示有提到，預設為+/-2g，+/-250degree/s
/** Power on and prepare for general usage.
 * This will activate the device and take it out of sleep mode (which must be done
 * after start-up). This function also sets both the accelerometer and the gyroscope
 * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets
 * the clock source to use the X Gyro for reference, which is slightly better than
 * the default internal clock source.
 */
void MPU6050::initialize() {
    setClockSource(MPU6050_CLOCK_PLL_XGYRO);
    setFullScaleGyroRange(MPU6050_GYRO_FS_250);
    setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
    setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
}

如果有必要修改，下面是修改方式

mpu.setFullScaleGyroRange(0);
//0 = +/- 250 degrees/sec | 1 = +/- 500 degrees/sec | 2 = +/- 1000 degrees/sec | 3 =  +/- 2000 degrees/sec
mpu.setFullScaleAccelRange(0);
 //0 = +/- 2g | 1 = +/- 4g | 2 = +/- 8g | 3 =  +/- 16g 

二、取得重力與角速度
在這個函式拿到xyz軸的加速度計重力值以及陀螺儀的角速度
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

三、四元數是啥?

幾個背景知識參考
四元數、四元數與旋轉
開源飛行器crazepony的軟體姿態解法，他的參考來源
對岸的參考資料有提到Z方向的需求要看應用
關於六軸與九軸的比較

一個簡單的範例，這個範例簡單好用，不過只有在運動時穩定，靜止時十分糟糕，
應該是沒有濾波的關係。


But....我發現我手邊有個九軸感測器，對於Yaw旋轉的效果會比六軸好

網路資源，網路資源二，資源三感覺資源二可以試試看。
如果還是會造成時序衝突，那就用之前碩論的卡曼，可能還比較好一點。

[無人機]關於Schedule-MPU6050 DMP Mode的中斷問題

加入超音波後，DMP常常會當掉，或者OVERFLOW

研究了超音波的讀取方式使用pulsein()
詳見官網文件如下連結
https://www.arduino.cc/en/Reference/PulseIn
發現這個會依照echo長度而變動的值，也就是echo高電位多久，就會被hold在這裡多久，


另外
下面連結
http://www.i2cdevlib.com/forums/topic/27-fifo-overflow/
有提到修改I2C的傳輸頻率，以及DMP資料的輸出速度


目前將I2C改成100KHz
DMP輸出改50Hz

發現沒有用，超音波很容易就會因為距離太遠而停止

[Ardunio]GY-521六軸姿態

這顆六軸姿態很普及，Arduino對這顆已經有現成資源
可以看下面文章

http://dory168.blogspot.tw/p/gy-521sensormpu-6050dmp-ic.html

http://ming-shian.blogspot.tw/2014/05/arduino21mpu6050row-data.html

http://playground.arduino.cc/Main/MPU-6050

http://www.i2cdevlib.com/usage

[Arduino]Day6-超音波感測器

下面範例是計算出距離後，如果距離大於100,就顯示其中一顆LED，沒有就另一顆亮

int ECHO_Pin=4;  // ultrasonic ECHO
int TRIG_Pin=5; // ultrasonic TRIG
int Near_LED = 9;
int Far_LED = 10;

void setup()
{
  Serial.begin(9600);
  pinMode(ECHO_Pin, INPUT);
  pinMode(TRIG_Pin, OUTPUT);
}
void loop()
{
  digitalWrite(TRIG_Pin, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_Pin, HIGH); // Pulse for 10 micro sec to trigger ultrasonic detection
  delayMicroseconds(10);
  digitalWrite(TRIG_Pin, LOW);
  int distance = pulseIn(ECHO_Pin, HIGH);  // Read receiver pulse time
  distance= distance/58;   // Transform pulse time to distance
  Serial.println(distance);   //Output distance

  if(distance > 100)
  {
    digitalWrite(Far_LED, HIGH);
    digitalWrite(Near_LED, LOW);
  }else{
    digitalWrite(Far_LED, LOW);
    digitalWrite(Near_LED, HIGH);
  }
  delay(50);
}



超音波模組運作原理
http://coopermaa2nd.blogspot.tw/2012/09/hc-sr04.html
http://mcudiy.blogspot.tw/2012/12/usm-ultrasonic-module-hc-sr04.html

datasheet
https://www.google.com.tw/url?sa=t&rct=j&q=&esrc=s&source=web&cd=3&cad=rja&uact=8&ved=0ahUKEwjPxZqsgIrOAhUDEpQKHfkICa4QFggnMAI&url=http%3A%2F%2Fwww.micropik.com%2FPDF%2FHCSR04.pdf&usg=AFQjCNGQka2FGj3tvAzU2HEDfgZvvCQpNw&sig2=axF9GWdLv_KJdj3alHP1aA


DataSheet裡的波形圖

新學到的

PulseIn:量測input腳位的高電位或低電位脈波長度

https://www.arduino.cc/en/Reference/PulseIn