hiroya taura
MPU6050のサンプルプログラム２
Dependencies: ConfigFile SDFileSystem mbed
Fork of
LAURUS_program
by 
LAURUS
Diff: main.cpp
- Revision:
- 10:8ee11e412ad7
- Parent:
- 7:0ec343d29641
- Parent:
- 9:6d4578dcc1ed
- Child:
- 11:083c8c9a5b84
--- a/main.cpp Mon Jun 15 00:50:28 2015 +0000
+++ b/main.cpp Mon Jun 15 13:29:36 2015 +0000
@@ -15,45 +15,70 @@
/********** private define **********/
#define TRUE 1
#define FALSE 0
+
+#define x 1
+#define y 2
+#define z 3
+
+const float dt = 0.1f; // 割り込み周期(s)
+const float ServoMax = 0.0023f; // サーボの最大パルス長(s)
+const float ServoMin = 0.0006f; // サーボの最小パルス長(s)
+const float PullMax = 25.0f; // 引っ張れる紐の最大量(mm)
/********** private macro **********/
/********** private typedef **********/
/********** private variables **********/
-DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力
-I2C i2c(PB_9, PB_8); // I2Cポート
-MPU6050 mpu(&i2c); // 加速度・角速度センサ
-HMC5883L hmc(&i2c); // 地磁気センサ
-LPS25H lps(&i2c); // 気圧センサ
-Serial gps(PA_11, PA_12); // GPS通信用シリアルポート
-Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート
-GMS6_CR6 gms(&gps, &pc); // GPS
-Ticker INT_timer; // 割り込みタイマー
+DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力
+I2C i2c(PB_9, PB_8); // I2Cポート
+MPU6050 mpu(&i2c); // 加速度・角速度センサ
+HMC5883L hmc(&i2c); // 地磁気センサ
+LPS25H lps(&i2c); // 気圧センサ
+Serial gps(PA_11, PA_12); // GPS通信用シリアルポート
+Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート
+GMS6_CR6 gms(&gps, &pc); // GPS
SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); // microSD
BufferedSerial xbee(PA_9, PA_10, PC_1); // Xbee
ConfigFile cfg; //ConfigFile
-PwmOut servoL(PC_7), servoR(PB_6);
-AnalogIn rf(PC_0);
-AnalogIn servoVcc(PA_0);
-AnalogIn logicVcc(PA_1);
+PwmOut servoL(PB_6), servoR(PC_7); // サーボ用PWM出力
+AnalogIn optSensor(PC_0); // 照度センサ用アナログ入力
+AnalogIn servoVcc(PA_0); // バッテリー電圧監視用アナログ入力(サーボ用)
+AnalogIn logicVcc(PA_1); // バッテリー電圧監視用アナログ入力(ロジック用)
+Ticker INT_timer; // 割り込みタイマー
-
-const float dt = 0.1f; // 割り込み周期（s）
+int lps_cnt = 0; // 気圧センサ読み取りカウント
+uint8_t INT_flag = TRUE; // 割り込み可否フラグ
+Vector raw_acc(3); // 加速度(m/s^2) 生
+Vector raw_gyro(3); // 角速度(deg/s) 生
+Vector raw_geomag(3); // 地磁気(?) 生
+float raw_press; // 気圧(hPa) 生
+Vector acc(3); // 加速度(m/s^2)
+Vector gyro(3); // 角速度(deg/s)
+Vector geomag(3); // 地磁気(?)
+float press; // 気圧(hPa)
-int lps_cnt = 0; // 気圧センサ読み取りカウント
-uint8_t INT_flag = TRUE; // 割り込み可否フラグ
-Vector raw_acc(3); // 加速度(m/s^2) 生
-Vector raw_gyro(3); // 角速度(deg/s) 生
-Vector raw_geomag(3); // 地磁気(?) 生
-float raw_press; // 気圧(hPa) 生
-Vector acc(3); // 加速度(m/s^2)
-Vector gyro(3); // 角速度(deg/s)
-Vector geomag(3); // 地磁気(?)
-float press; // 気圧(hPa)
+Vector raw_g(3); // 重力ベクトル 生
+Vector g(3); // 重力ベクトル
+Vector p(2); // 位置情報(経度, 緯度)
+Vector pre_p(2); // 過去の位置情報(経度, 緯度)
+int UTC_t = 0; // UTC時刻
+int pre_UTC_t = 0; // 前のUTC時刻
+//Vector n(3); // 地磁気ベクトル
-Vector raw_g(3); // 重力ベクトル 生
-Vector g(3); // 重力ベクトル 生
-//Vector n(3); // 地磁気ベクトル
+Vector b_f(3); // 機体座標に固定された、機体前方向きのベクトル（x軸）
+Vector b_u(3); // 機体座標に固定された、機体上方向きのベクトル（z軸）
+Vector b_l(3); // 機体座標に固定された、機体左方向きのベクトル（y軸）
+
+Vector r_f(3); // 参照座標に固定された、北向きのベクトル（X軸）
+Vector r_u(3); // 参照座標に固定された、上向きのベクトル（Z軸）
+Vector r_l(3); // 参照座標に固定された、西向きのベクトル（Y軸）
+
+int pull_L = 0; // 左サーボの引っ張り量(mm：0~PullMax)
+int pull_R = 0; // 右サーボの引っ張り量(mm：0~PullMax)
+
+float yaw = 0.0f; // 本体のヨー角（deg）
+float pitch = 0.0f; // 本体のピッチ角（deg）
+float roll = 0.0f; // 本体のロール角（deg）
/** config.txt **
* #から始めるのはコメント行
@@ -80,11 +105,12 @@
char data[512] = {};
/********** private functions **********/
-void LoadConfig(); // config読み取り
-int find_last(); // SDカード初期化用関数
-void KalmanInit(); // カルマンフィルタ初期化
-void KalmanUpdate(); // カルマンフィルタ更新
-void INT_func(); // 割り込み用関数
+void LoadConfig(); // config読み取り
+int find_last(); // SDカード初期化用関数
+void KalmanInit(); // カルマンフィルタ初期化
+void KalmanUpdate(); // カルマンフィルタ更新
+float distance(Vector p1, Vector p2); // 緯度・経度の差から2点間の距離を算出(m)
+void INT_func(); // 割り込み用関数
void toString(Matrix& m);
void toString(Vector& v);
@@ -113,6 +139,9 @@
fprintf(fp, "log data\r\n");
xbee.printf("log data\r\n");
+ servoL.period(0.020f); // サーボの信号の周期は20ms
+ servoR.period(0.020f);
+
//カルマンフィルタ初期化
KalmanInit();
@@ -123,6 +152,15 @@
raw_g.SetComp(2, 0.0f);
raw_g.SetComp(3, 1.0f);
+ // 機体に固定されたベクトルの初期化
+ b_f.SetComp(1, 0.0f);
+ b_f.SetComp(2, 0.0f);
+ b_f.SetComp(3, -1.0f);
+ b_u.SetComp(1, 1.0f);
+ b_u.SetComp(2, 0.0f);
+ b_u.SetComp(3, 0.0f);
+ b_l = Cross(b_u, b_f);
+
/* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
while(1) {
timer.stop();
@@ -130,11 +168,44 @@
timer.start();
// 0.1秒おきにセンサーの出力をpcへ出力
myled = 1; // LED is ON
- wait(0.05f); // 50 ms
- myled = 0; // LED is OFF
INT_flag = FALSE; // 割り込みによる変数書き換えを阻止
+ // データ処理
+ {
+ gms.read(); // GPSデータ取得
+ UTC_t = (int)gms.time;
+
+ // 参照座標系の基底を求める
+ r_u = g;
+ r_f = geomag.GetPerpCompTo(g).Normalize();
+ r_l = Cross(r_u, r_f);
+
+ // 回転行列を経由してオイラー角を求める
+ // オイラー角はヨー・ピッチ・ロールの順に回転したものとする
+ // 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。
+
+ float R_11 = r_f * b_f; // 回転行列の(1,1)成分
+ float R_12 = r_f * b_l; // 回転行列の(1,2)成分
+ float R_13 = r_f * b_u; // 回転行列の(1,3)成分
+ float R_23 = r_l * b_u; // 回転行列の(2,3)成分
+ float R_33 = r_u * b_u; // 回転行列の(3,3)成分
+
+ yaw = atan2(R_11, -R_12) * RAD_TO_DEG;
+ roll = atan2(R_33, -R_23) * RAD_TO_DEG;
+ pitch = atan2(R_11/cos(yaw), R_13) * RAD_TO_DEG;
+
+ if(UTC_t - pre_UTC_t >= 1) { // GPSデータが更新されていたら
+ p.SetComp(1, gms.longitude * DEG_TO_RAD);
+ p.SetComp(2, gms.latitude * DEG_TO_RAD);
+ } else { // 更新されていなかったら
+
+ }
+
+ pre_p = p;
+ pre_UTC_t = UTC_t;
+ }
+
float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
@@ -142,21 +213,29 @@
g.GetComp(1), g.GetComp(2), g.GetComp(3),
geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3),
press, gms.longitude, gms.latitude,
- sv, lv, gms.Ns);
+ sv, lv, optSensor.read_u16());
fprintf(fp, data);
xbee.printf(data);
INT_flag = TRUE; // 割り込み許可
- pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f);
-
// 制御ルーチン
{
+ //pull_L = (pull_L+5)%30;
+ //pull_R = (pull_R+5)%30;
+ pull_L = 0;
+ pull_R = 30;
+ servoL.pulsewidth((ServoMax - ServoMin) * pull_L / PullMax + ServoMin);
+ servoR.pulsewidth((ServoMax - ServoMin) * pull_R / PullMax + ServoMin);
+
}
+ myled = 0; // LED is OFF
- // ループはきっかり1秒ごと
- while(timer.read_ms() < 1000);
+ // ループはきっかり0.2秒ごと
+ while(timer.read_ms() < 200);
+
+ pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f);
}
@@ -264,6 +343,22 @@
post_P = (I - K * H) * pri_P;
}
+float distance(Vector p1, Vector p2) {
+ if(p1.GetDim() != p2.GetDim()) return 0.0f;
+
+ float mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f;
+ float s_mu_y = sin(mu_y);
+ float w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y);
+ float m = GPS_A * (1 - GPS_SQ_E) / (w * w * w);
+ float n = GPS_A / w;
+ float d1 = m * (p1.GetComp(2) - p2.GetComp(2));
+ float d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1));
+
+ return sqrt(d1 * d1 + d2 * d2);
+}
+
+/* -------------------- 割り込み関数 -------------------- */
+
void INT_func() {
// センサーの値を更新
mpu.read();
@@ -300,6 +395,8 @@
}
}
+/* -------------------- デバッグ用関数 -------------------- */
+
void toString(Matrix& m) {
for(int i=0; i<m.GetRow(); i++) {