hiroya taura / Mbed 2 deprecated LAURUS_program

MPU6050のサンプルプログラム２

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by LAURUS

main.cpp@8:602865d8fca3, 2015-06-12 (annotated)

Committer:
ojan
Date:
Fri Jun 12 15:28:05 2015 +0000
Revision:
8:602865d8fca3
Parent:
6:2b68f85a984a
Child:
9:6d4578dcc1ed
move servo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ojan 0:bc6f14fc60c7 1 #include "mbed.h"
ojan 0:bc6f14fc60c7 2 #include "MPU6050.h"
ojan 0:bc6f14fc60c7 3 #include "HMC5883L.h"
ojan 0:bc6f14fc60c7 4 #include "LPS25H.h"
ojan 1:6cd6d2760856 5 #include "GMS6_CR6.h"
ojan 0:bc6f14fc60c7 6 #include "ErrorLogger.h"
ojan 0:bc6f14fc60c7 7 #include "Vector.h"
ojan 3:5358a691a100 8 #include "Matrix.h"
ojan 3:5358a691a100 9 #include "Vector_Matrix_operator.h"
ojan 0:bc6f14fc60c7 10 #include "myConstants.h"
ojan 3:5358a691a100 11 #include "Log.h"
ojan 0:bc6f14fc60c7 12
ojan 0:bc6f14fc60c7 13 /********** private define **********/
ojan 0:bc6f14fc60c7 14 #define TRUE 1
ojan 0:bc6f14fc60c7 15 #define FALSE 0
ojan 8:602865d8fca3 16
ojan 8:602865d8fca3 17 const float dt = 0.1f; // 割り込み周期(s)
ojan 8:602865d8fca3 18 const float ServoMax = 0.0023f; // サーボの最大パルス長(s)
ojan 8:602865d8fca3 19 const float ServoMin = 0.0006f; // サーボの最小パルス長(s)
ojan 0:bc6f14fc60c7 20 /********** private macro **********/
ojan 1:6cd6d2760856 21
ojan 0:bc6f14fc60c7 22 /********** private typedef **********/
ojan 1:6cd6d2760856 23
ojan 0:bc6f14fc60c7 24 /********** private variables **********/
ojan 4:45dc5590abc0 25 DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力
ojan 4:45dc5590abc0 26 I2C i2c(PB_9, PB_8); // I2Cポート
ojan 1:6cd6d2760856 27 MPU6050 mpu(&i2c); // 加速度・角速度センサ
ojan 1:6cd6d2760856 28 HMC5883L hmc(&i2c); // 地磁気センサ
ojan 1:6cd6d2760856 29 LPS25H lps(&i2c); // 気圧センサ
ojan 8:602865d8fca3 30 Serial gps(PA_11, PA_12); // GPS通信用シリアルポート
ojan 1:6cd6d2760856 31 Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート
ojan 1:6cd6d2760856 32 GMS6_CR6 gms(&gps, &pc); // GPS
onaka 6:2b68f85a984a 33 Log logger(PB_5, PB_4, PB_3, PB_10, PA_9, PA_10, PC_1); // ロガー（microSD、XBee）
ojan 8:602865d8fca3 34 PwmOut servoL(PB_6), servoR(PC_7); // サーボ用PWM出力
ojan 8:602865d8fca3 35 AnalogIn rf(PC_0); // 照度センサ用アナログ入力
ojan 8:602865d8fca3 36 AnalogIn servoVcc(PA_0); // バッテリー電圧監視用アナログ入力(サーボ用)
ojan 8:602865d8fca3 37 AnalogIn logicVcc(PA_1); // バッテリー電圧監視用アナログ入力(ロジック用)
ojan 8:602865d8fca3 38 Ticker INT_timer; // 割り込みタイマー
ojan 0:bc6f14fc60c7 39
ojan 8:602865d8fca3 40 int lps_cnt = 0; // 気圧センサ読み取りカウント
ojan 8:602865d8fca3 41 uint8_t INT_flag = TRUE; // 割り込み可否フラグ
ojan 8:602865d8fca3 42 Vector raw_acc(3); // 加速度(m/s^2) 生
ojan 8:602865d8fca3 43 Vector raw_gyro(3); // 角速度(deg/s) 生
ojan 8:602865d8fca3 44 Vector raw_geomag(3); // 地磁気(?) 生
ojan 8:602865d8fca3 45 float raw_press; // 気圧(hPa) 生
ojan 8:602865d8fca3 46 Vector acc(3); // 加速度(m/s^2)
ojan 8:602865d8fca3 47 Vector gyro(3); // 角速度(deg/s)
ojan 8:602865d8fca3 48 Vector geomag(3); // 地磁気(?)
ojan 8:602865d8fca3 49 float press; // 気圧(hPa)
ojan 1:6cd6d2760856 50
ojan 8:602865d8fca3 51 Vector raw_g(3); // 重力ベクトル 生
ojan 8:602865d8fca3 52 Vector g(3); // 重力ベクトル
ojan 8:602865d8fca3 53 //Vector n(3); // 地磁気ベクトル
ojan 8:602865d8fca3 54
ojan 8:602865d8fca3 55 int pull_L = 0; // 左サーボの引っ張り量(mm：0~30)
ojan 8:602865d8fca3 56 int pull_R = 0; // 右サーボの引っ張り量(mm：0~30)
ojan 3:5358a691a100 57
ojan 3:5358a691a100 58 /* ----- Kalman Filter ----- */
ojan 3:5358a691a100 59 Vector pri_x(6);
ojan 3:5358a691a100 60 Matrix pri_P(6, 6);
ojan 3:5358a691a100 61 Vector post_x(6);
ojan 3:5358a691a100 62 Matrix post_P(6, 6);
ojan 3:5358a691a100 63 Matrix F(6, 6), H(3, 6);
ojan 3:5358a691a100 64 Matrix R(6, 6), Q(3, 3);
ojan 3:5358a691a100 65 Matrix I(6, 6);
ojan 3:5358a691a100 66 Matrix K(6, 3);
ojan 3:5358a691a100 67 Matrix S(3, 3), inv(3, 3);
ojan 3:5358a691a100 68 /* ----- ------------- ----- */
ojan 3:5358a691a100 69
ojan 3:5358a691a100 70 Timer timer;
ojan 3:5358a691a100 71
onaka 6:2b68f85a984a 72 char data[512] = {};
ojan 1:6cd6d2760856 73
ojan 0:bc6f14fc60c7 74 /********** private functions **********/
ojan 4:45dc5590abc0 75 void KalmanInit(); // カルマンフィルタ初期化
ojan 4:45dc5590abc0 76 void KalmanUpdate(); // カルマンフィルタ更新
ojan 0:bc6f14fc60c7 77 void INT_func(); // 割り込み用関数
ojan 3:5358a691a100 78 void toString(Matrix& m);
ojan 3:5358a691a100 79 void toString(Vector& v);
ojan 1:6cd6d2760856 80
ojan 0:bc6f14fc60c7 81 /********** main function **********/
ojan 0:bc6f14fc60c7 82
ojan 0:bc6f14fc60c7 83 int main() {
ojan 0:bc6f14fc60c7 84
ojan 0:bc6f14fc60c7 85 i2c.frequency(400000); // I2Cの通信速度を400kHzに設定
ojan 0:bc6f14fc60c7 86
ojan 0:bc6f14fc60c7 87 if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!"); // mpu6050初期化
ojan 0:bc6f14fc60c7 88 if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!"); // hmc5883l初期化
ojan 0:bc6f14fc60c7 89
ojan 4:45dc5590abc0 90 char* title = "log data\r\n"; // ログのタイトル行
onaka 6:2b68f85a984a 91 logger.initialize_sdlog(title); // ログファイル初期設定
ojan 3:5358a691a100 92
ojan 8:602865d8fca3 93 servoL.period(0.020f); // サーボの信号の周期は20ms
ojan 8:602865d8fca3 94 servoR.period(0.020f);
ojan 8:602865d8fca3 95
ojan 3:5358a691a100 96 KalmanInit();
ojan 3:5358a691a100 97
ojan 3:5358a691a100 98 INT_timer.attach(&INT_func, dt); // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
ojan 1:6cd6d2760856 99
ojan 1:6cd6d2760856 100 //重力ベクトルの初期化
ojan 4:45dc5590abc0 101 raw_g.SetComp(1, 0.0f);
ojan 4:45dc5590abc0 102 raw_g.SetComp(2, 0.0f);
ojan 4:45dc5590abc0 103 raw_g.SetComp(3, 1.0f);
ojan 0:bc6f14fc60c7 104
ojan 2:d2b60a1d0cd9 105 /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
ojan 0:bc6f14fc60c7 106 while(1) {
ojan 4:45dc5590abc0 107 timer.stop();
ojan 4:45dc5590abc0 108 timer.reset();
ojan 4:45dc5590abc0 109 timer.start();
ojan 3:5358a691a100 110 // 0.1秒おきにセンサーの出力をpcへ出力
ojan 4:45dc5590abc0 111 myled = 1; // LED is ON
ojan 4:45dc5590abc0 112 wait(0.05f); // 50 ms
ojan 4:45dc5590abc0 113 myled = 0; // LED is OFF
ojan 1:6cd6d2760856 114
ojan 8:602865d8fca3 115 pull_L = (pull_L+5)%30;
ojan 8:602865d8fca3 116 pull_R = (pull_R+5)%30;
ojan 8:602865d8fca3 117
ojan 0:bc6f14fc60c7 118 INT_flag = FALSE; // 割り込みによる変数書き換えを阻止
ojan 0:bc6f14fc60c7 119
ojan 5:182f6356bce1 120 float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
ojan 5:182f6356bce1 121 float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
ojan 5:182f6356bce1 122
ojan 5:182f6356bce1 123 sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%d\r\n",
ojan 5:182f6356bce1 124 g.GetComp(1), g.GetComp(2), g.GetComp(3),
ojan 4:45dc5590abc0 125 geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3),
ojan 5:182f6356bce1 126 press, gms.longitude, gms.latitude,
ojan 8:602865d8fca3 127 sv, lv, rf.read_u16());
onaka 6:2b68f85a984a 128 logger.write(data);
ojan 4:45dc5590abc0 129
ojan 0:bc6f14fc60c7 130 INT_flag = TRUE; // 割り込み許可
ojan 0:bc6f14fc60c7 131
ojan 4:45dc5590abc0 132 pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f);
ojan 4:45dc5590abc0 133
ojan 5:182f6356bce1 134 // 制御ルーチン
ojan 8:602865d8fca3 135 /*{
ojan 8:602865d8fca3 136 servoL.pulsewidth((ServoMax-ServoMin) * pull_L / 30.0f + ServoMin);
ojan 8:602865d8fca3 137 servoR.pulsewidth((ServoMax-ServoMin) * pull_R / 30.0f + ServoMin);
ojan 8:602865d8fca3 138
ojan 8:602865d8fca3 139
ojan 8:602865d8fca3 140
ojan 8:602865d8fca3 141 }*/
ojan 5:182f6356bce1 142
ojan 5:182f6356bce1 143
ojan 4:45dc5590abc0 144 // ループはきっかり1秒ごと
ojan 4:45dc5590abc0 145 while(timer.read_ms() < 1000);
ojan 4:45dc5590abc0 146
ojan 0:bc6f14fc60c7 147 }
ojan 2:d2b60a1d0cd9 148
ojan 2:d2b60a1d0cd9 149 /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */
ojan 0:bc6f14fc60c7 150 }
ojan 0:bc6f14fc60c7 151
ojan 3:5358a691a100 152 void KalmanInit() {
ojan 3:5358a691a100 153
ojan 3:5358a691a100 154 // 誤差共分散行列の値を決める（対角成分のみ）
ojan 3:5358a691a100 155 float alpha_R = 60.0f;
ojan 3:5358a691a100 156 float alpha_Q = 100.0f;
ojan 3:5358a691a100 157 R *= alpha_R;
ojan 3:5358a691a100 158 Q *= alpha_Q;
ojan 3:5358a691a100 159
ojan 3:5358a691a100 160 // 状態方程式のヤコビアンの初期値を代入（時間変化あり）
ojan 3:5358a691a100 161 float f[36] = {
ojan 4:45dc5590abc0 162 1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f,
ojan 4:45dc5590abc0 163 -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f,
ojan 4:45dc5590abc0 164 raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 165 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 166 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 3:5358a691a100 167 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
ojan 3:5358a691a100 168 };
ojan 3:5358a691a100 169
ojan 3:5358a691a100 170 F.SetComps(f);
ojan 3:5358a691a100 171
ojan 3:5358a691a100 172 // 観測方程式のヤコビアンの値を設定（時間変化無し）
ojan 3:5358a691a100 173 float h[18] = {
ojan 3:5358a691a100 174 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 175 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 3:5358a691a100 176 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f
ojan 3:5358a691a100 177 };
ojan 3:5358a691a100 178
ojan 3:5358a691a100 179 H.SetComps(h);
ojan 3:5358a691a100 180 }
ojan 3:5358a691a100 181
ojan 3:5358a691a100 182 void KalmanUpdate() {
ojan 3:5358a691a100 183 // ヤコビアンの更新
ojan 3:5358a691a100 184 float f[36] = {
ojan 4:45dc5590abc0 185 1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f,
ojan 4:45dc5590abc0 186 -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f,
ojan 4:45dc5590abc0 187 raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 188 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 189 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 3:5358a691a100 190 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
ojan 3:5358a691a100 191 };
ojan 3:5358a691a100 192
ojan 3:5358a691a100 193 F.SetComps(f);
ojan 3:5358a691a100 194
ojan 3:5358a691a100 195 // 事前推定値の更新
ojan 3:5358a691a100 196 pri_x = F * post_x;
ojan 3:5358a691a100 197 // 事前誤差分散行列の更新
ojan 3:5358a691a100 198 pri_P = F * post_P * F.Transpose() + R;
ojan 3:5358a691a100 199
ojan 3:5358a691a100 200 // カルマンゲインの計算
ojan 3:5358a691a100 201 S = Q + H * pri_P * H.Transpose();
ojan 3:5358a691a100 202 float det;
ojan 3:5358a691a100 203 if((det = S.Inverse(inv)) >= 0.0f) {
ojan 3:5358a691a100 204 pc.printf("E:%.3f\r\n", det);
ojan 3:5358a691a100 205 return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
ojan 3:5358a691a100 206 }
ojan 3:5358a691a100 207 K = pri_P * H.Transpose() * inv;
ojan 3:5358a691a100 208
ojan 3:5358a691a100 209 // 事後推定値の更新
ojan 4:45dc5590abc0 210 post_x = pri_x + K * (raw_geomag - H * pri_x);
ojan 3:5358a691a100 211 // 事後誤差分散行列の更新
ojan 3:5358a691a100 212 post_P = (I - K * H) * pri_P;
ojan 3:5358a691a100 213 }
ojan 3:5358a691a100 214
ojan 0:bc6f14fc60c7 215 void INT_func() {
ojan 4:45dc5590abc0 216 // センサーの値を更新
ojan 4:45dc5590abc0 217 mpu.read();
ojan 4:45dc5590abc0 218 hmc.read();
ojan 4:45dc5590abc0 219
ojan 4:45dc5590abc0 220 for(int i=0; i<3; i++) {
ojan 4:45dc5590abc0 221 raw_acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G);
ojan 4:45dc5590abc0 222 raw_gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
ojan 4:45dc5590abc0 223 raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
ojan 4:45dc5590abc0 224 }
ojan 0:bc6f14fc60c7 225
ojan 4:45dc5590abc0 226 Vector delta_g = Cross(raw_gyro, raw_g); // Δg = ω × g
ojan 4:45dc5590abc0 227 raw_g = 0.9f * (raw_g - delta_g * dt) + 0.1f * raw_acc.Normalize(); // 相補フィルタ
ojan 4:45dc5590abc0 228 raw_g = raw_g.Normalize();
ojan 4:45dc5590abc0 229
ojan 4:45dc5590abc0 230 KalmanUpdate();
ojan 0:bc6f14fc60c7 231
ojan 4:45dc5590abc0 232 // LPS25Hによる気圧の取得は10Hz
ojan 4:45dc5590abc0 233 lps_cnt = (lps_cnt+1)%10;
ojan 4:45dc5590abc0 234 if(lps_cnt == 0) {
ojan 4:45dc5590abc0 235 raw_press = (float)lps.pressure() * PRES_LSB_TO_HPA;
ojan 4:45dc5590abc0 236 }
ojan 4:45dc5590abc0 237 //pc.printf("%d(us)\r\n", timer.read_us());
ojan 4:45dc5590abc0 238
ojan 4:45dc5590abc0 239 if(INT_flag != FALSE) {
ojan 4:45dc5590abc0 240 g = raw_g;
ojan 4:45dc5590abc0 241 for(int i=0; i<3; i++) {
ojan 4:45dc5590abc0 242 geomag.SetComp(i+1, post_x.GetComp(i+1));
ojan 4:45dc5590abc0 243 }
ojan 4:45dc5590abc0 244 acc = raw_acc;
ojan 4:45dc5590abc0 245 gyro = raw_gyro;
ojan 4:45dc5590abc0 246 press = raw_press;
ojan 1:6cd6d2760856 247
ojan 0:bc6f14fc60c7 248 }
ojan 3:5358a691a100 249 }
ojan 3:5358a691a100 250
ojan 3:5358a691a100 251 void toString(Matrix& m) {
ojan 3:5358a691a100 252
ojan 3:5358a691a100 253 for(int i=0; i<m.GetRow(); i++) {
ojan 3:5358a691a100 254 for(int j=0; j<m.GetCol(); j++) {
ojan 3:5358a691a100 255 pc.printf("%.6f\t", m.GetComp(i+1, j+1));
ojan 3:5358a691a100 256 }
ojan 3:5358a691a100 257 pc.printf("\r\n");
ojan 3:5358a691a100 258 }
ojan 3:5358a691a100 259
ojan 3:5358a691a100 260 }
ojan 3:5358a691a100 261
ojan 3:5358a691a100 262 void toString(Vector& v) {
ojan 3:5358a691a100 263
ojan 3:5358a691a100 264 for(int i=0; i<v.GetDim(); i++) {
ojan 3:5358a691a100 265 pc.printf("%.6f\t", v.GetComp(i+1));
ojan 3:5358a691a100 266 }
ojan 3:5358a691a100 267 pc.printf("\r\n");
ojan 3:5358a691a100 268
ojan 0:bc6f14fc60c7 269 }