hiroya taura / Mbed 2 deprecated LAURUS_program

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

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by LAURUS

main.cpp@7:0ec343d29641, 2015-06-15 (annotated)

Committer:
onaka
Date:
Mon Jun 15 00:50:28 2015 +0000
Revision:
7:0ec343d29641
Parent:
6:2b68f85a984a
Child:
10:8ee11e412ad7
add config loader

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"
onaka 7:0ec343d29641 11 #include "SDFileSystem.h"
onaka 7:0ec343d29641 12 #include "BufferedSerial.h"
onaka 7:0ec343d29641 13 #include "ConfigFile.h"
ojan 0:bc6f14fc60c7 14
ojan 0:bc6f14fc60c7 15 /********** private define **********/
ojan 0:bc6f14fc60c7 16 #define TRUE 1
ojan 0:bc6f14fc60c7 17 #define FALSE 0
ojan 0:bc6f14fc60c7 18 /********** private macro **********/
ojan 1:6cd6d2760856 19
ojan 0:bc6f14fc60c7 20 /********** private typedef **********/
ojan 1:6cd6d2760856 21
ojan 0:bc6f14fc60c7 22 /********** private variables **********/
onaka 7:0ec343d29641 23 DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力
onaka 7:0ec343d29641 24 I2C i2c(PB_9, PB_8); // I2Cポート
onaka 7:0ec343d29641 25 MPU6050 mpu(&i2c); // 加速度・角速度センサ
onaka 7:0ec343d29641 26 HMC5883L hmc(&i2c); // 地磁気センサ
onaka 7:0ec343d29641 27 LPS25H lps(&i2c); // 気圧センサ
onaka 7:0ec343d29641 28 Serial gps(PA_11, PA_12); // GPS通信用シリアルポート
onaka 7:0ec343d29641 29 Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート
onaka 7:0ec343d29641 30 GMS6_CR6 gms(&gps, &pc); // GPS
onaka 7:0ec343d29641 31 Ticker INT_timer; // 割り込みタイマー
onaka 7:0ec343d29641 32 SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); // microSD
onaka 7:0ec343d29641 33 BufferedSerial xbee(PA_9, PA_10, PC_1); // Xbee
onaka 7:0ec343d29641 34 ConfigFile cfg; //ConfigFile
onaka 7:0ec343d29641 35 PwmOut servoL(PC_7), servoR(PB_6);
onaka 7:0ec343d29641 36 AnalogIn rf(PC_0);
onaka 7:0ec343d29641 37 AnalogIn servoVcc(PA_0);
onaka 7:0ec343d29641 38 AnalogIn logicVcc(PA_1);
ojan 5:182f6356bce1 39
ojan 0:bc6f14fc60c7 40
ojan 3:5358a691a100 41 const float dt = 0.1f; // 割り込み周期（s）
ojan 0:bc6f14fc60c7 42
ojan 0:bc6f14fc60c7 43 int lps_cnt = 0; // 気圧センサ読み取りカウント
ojan 3:5358a691a100 44 uint8_t INT_flag = TRUE; // 割り込み可否フラグ
ojan 4:45dc5590abc0 45 Vector raw_acc(3); // 加速度(m/s^2) 生
ojan 4:45dc5590abc0 46 Vector raw_gyro(3); // 角速度(deg/s) 生
ojan 4:45dc5590abc0 47 Vector raw_geomag(3); // 地磁気(?) 生
ojan 4:45dc5590abc0 48 float raw_press; // 気圧(hPa) 生
ojan 1:6cd6d2760856 49 Vector acc(3); // 加速度(m/s^2)
ojan 1:6cd6d2760856 50 Vector gyro(3); // 角速度(deg/s)
ojan 1:6cd6d2760856 51 Vector geomag(3); // 地磁気(?)
ojan 0:bc6f14fc60c7 52 float press; // 気圧(hPa)
ojan 1:6cd6d2760856 53
ojan 4:45dc5590abc0 54 Vector raw_g(3); // 重力ベクトル 生
ojan 4:45dc5590abc0 55 Vector g(3); // 重力ベクトル 生
ojan 3:5358a691a100 56 //Vector n(3); // 地磁気ベクトル
ojan 3:5358a691a100 57
onaka 7:0ec343d29641 58 /** config.txt **
onaka 7:0ec343d29641 59 * #から始めるのはコメント行
onaka 7:0ec343d29641 60 * #イコールの前後に空白を入れない
onaka 7:0ec343d29641 61 * target_x=111.222
onaka 7:0ec343d29641 62 * target_y=33.444
onaka 7:0ec343d29641 63 */
onaka 7:0ec343d29641 64 float target_x, target_y;
onaka 7:0ec343d29641 65
ojan 3:5358a691a100 66 /* ----- Kalman Filter ----- */
ojan 3:5358a691a100 67 Vector pri_x(6);
ojan 3:5358a691a100 68 Matrix pri_P(6, 6);
ojan 3:5358a691a100 69 Vector post_x(6);
ojan 3:5358a691a100 70 Matrix post_P(6, 6);
ojan 3:5358a691a100 71 Matrix F(6, 6), H(3, 6);
ojan 3:5358a691a100 72 Matrix R(6, 6), Q(3, 3);
ojan 3:5358a691a100 73 Matrix I(6, 6);
ojan 3:5358a691a100 74 Matrix K(6, 3);
ojan 3:5358a691a100 75 Matrix S(3, 3), inv(3, 3);
ojan 3:5358a691a100 76 /* ----- ------------- ----- */
ojan 3:5358a691a100 77
ojan 3:5358a691a100 78 Timer timer;
ojan 3:5358a691a100 79
onaka 6:2b68f85a984a 80 char data[512] = {};
ojan 1:6cd6d2760856 81
ojan 0:bc6f14fc60c7 82 /********** private functions **********/
onaka 7:0ec343d29641 83 void LoadConfig(); // config読み取り
onaka 7:0ec343d29641 84 int find_last(); // SDカード初期化用関数
ojan 4:45dc5590abc0 85 void KalmanInit(); // カルマンフィルタ初期化
ojan 4:45dc5590abc0 86 void KalmanUpdate(); // カルマンフィルタ更新
ojan 0:bc6f14fc60c7 87 void INT_func(); // 割り込み用関数
ojan 3:5358a691a100 88 void toString(Matrix& m);
ojan 3:5358a691a100 89 void toString(Vector& v);
ojan 1:6cd6d2760856 90
ojan 0:bc6f14fc60c7 91 /********** main function **********/
ojan 0:bc6f14fc60c7 92
ojan 0:bc6f14fc60c7 93 int main() {
ojan 0:bc6f14fc60c7 94
ojan 0:bc6f14fc60c7 95 i2c.frequency(400000); // I2Cの通信速度を400kHzに設定
ojan 0:bc6f14fc60c7 96
ojan 0:bc6f14fc60c7 97 if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!"); // mpu6050初期化
ojan 0:bc6f14fc60c7 98 if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!"); // hmc5883l初期化
ojan 0:bc6f14fc60c7 99
onaka 7:0ec343d29641 100 //Config読み取り
onaka 7:0ec343d29641 101 LoadConfig();
ojan 3:5358a691a100 102
onaka 7:0ec343d29641 103 //SDカード初期化
onaka 7:0ec343d29641 104 FILE *fp;
onaka 7:0ec343d29641 105 char filename[15];
onaka 7:0ec343d29641 106 int n = find_last();
onaka 7:0ec343d29641 107 if(n < 0){
onaka 7:0ec343d29641 108 pc.printf("Could not read a SD Card.\n");
onaka 7:0ec343d29641 109 return 0;
onaka 7:0ec343d29641 110 }
onaka 7:0ec343d29641 111 sprintf(filename, "/sd/log%03d.csv", n+1);
onaka 7:0ec343d29641 112 fp = fopen(filename, "w");
onaka 7:0ec343d29641 113 fprintf(fp, "log data\r\n");
onaka 7:0ec343d29641 114 xbee.printf("log data\r\n");
onaka 7:0ec343d29641 115
onaka 7:0ec343d29641 116 //カルマンフィルタ初期化
ojan 3:5358a691a100 117 KalmanInit();
ojan 3:5358a691a100 118
ojan 3:5358a691a100 119 INT_timer.attach(&INT_func, dt); // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
ojan 1:6cd6d2760856 120
ojan 1:6cd6d2760856 121 //重力ベクトルの初期化
ojan 4:45dc5590abc0 122 raw_g.SetComp(1, 0.0f);
ojan 4:45dc5590abc0 123 raw_g.SetComp(2, 0.0f);
ojan 4:45dc5590abc0 124 raw_g.SetComp(3, 1.0f);
ojan 0:bc6f14fc60c7 125
ojan 2:d2b60a1d0cd9 126 /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
ojan 0:bc6f14fc60c7 127 while(1) {
ojan 4:45dc5590abc0 128 timer.stop();
ojan 4:45dc5590abc0 129 timer.reset();
ojan 4:45dc5590abc0 130 timer.start();
ojan 3:5358a691a100 131 // 0.1秒おきにセンサーの出力をpcへ出力
ojan 4:45dc5590abc0 132 myled = 1; // LED is ON
ojan 4:45dc5590abc0 133 wait(0.05f); // 50 ms
ojan 4:45dc5590abc0 134 myled = 0; // LED is OFF
ojan 1:6cd6d2760856 135
ojan 0:bc6f14fc60c7 136 INT_flag = FALSE; // 割り込みによる変数書き換えを阻止
ojan 0:bc6f14fc60c7 137
ojan 5:182f6356bce1 138 float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
ojan 5:182f6356bce1 139 float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
ojan 5:182f6356bce1 140
ojan 5:182f6356bce1 141 sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%d\r\n",
ojan 5:182f6356bce1 142 g.GetComp(1), g.GetComp(2), g.GetComp(3),
ojan 4:45dc5590abc0 143 geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3),
ojan 5:182f6356bce1 144 press, gms.longitude, gms.latitude,
ojan 5:182f6356bce1 145 sv, lv, gms.Ns);
onaka 7:0ec343d29641 146 fprintf(fp, data);
onaka 7:0ec343d29641 147 xbee.printf(data);
ojan 4:45dc5590abc0 148
ojan 0:bc6f14fc60c7 149 INT_flag = TRUE; // 割り込み許可
ojan 0:bc6f14fc60c7 150
ojan 4:45dc5590abc0 151 pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f);
ojan 4:45dc5590abc0 152
ojan 5:182f6356bce1 153 // 制御ルーチン
ojan 5:182f6356bce1 154 {
ojan 5:182f6356bce1 155 }
ojan 5:182f6356bce1 156
ojan 5:182f6356bce1 157
ojan 4:45dc5590abc0 158 // ループはきっかり1秒ごと
ojan 4:45dc5590abc0 159 while(timer.read_ms() < 1000);
ojan 4:45dc5590abc0 160
ojan 0:bc6f14fc60c7 161 }
ojan 2:d2b60a1d0cd9 162
ojan 2:d2b60a1d0cd9 163 /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */
onaka 7:0ec343d29641 164 //fclose(fp);
onaka 7:0ec343d29641 165 }
onaka 7:0ec343d29641 166
onaka 7:0ec343d29641 167 void LoadConfig(){
onaka 7:0ec343d29641 168 char value[20];
onaka 7:0ec343d29641 169 //Read a configuration file from a mbed.
onaka 7:0ec343d29641 170 if (!cfg.read("/sd/config.txt")){
onaka 7:0ec343d29641 171 pc.printf("Config file does not exist\n");
onaka 7:0ec343d29641 172 }else{
onaka 7:0ec343d29641 173 //Get values
onaka 7:0ec343d29641 174 if (cfg.getValue("target_x", &value[0], sizeof(value))) target_x = atof(value);
onaka 7:0ec343d29641 175 else{
onaka 7:0ec343d29641 176 pc.printf("Failed to get value for target_x\n");
onaka 7:0ec343d29641 177 }
onaka 7:0ec343d29641 178 if (cfg.getValue("target_y", &value[0], sizeof(value))) target_y = atof(value);
onaka 7:0ec343d29641 179 else{
onaka 7:0ec343d29641 180 pc.printf("Failed to get value for target_y\n");
onaka 7:0ec343d29641 181 }
onaka 7:0ec343d29641 182 }
onaka 7:0ec343d29641 183 }
onaka 7:0ec343d29641 184
onaka 7:0ec343d29641 185 int find_last() {
onaka 7:0ec343d29641 186 int i, n = 0;
onaka 7:0ec343d29641 187 char c;
onaka 7:0ec343d29641 188 DIR *dp;
onaka 7:0ec343d29641 189 struct dirent *dirst;
onaka 7:0ec343d29641 190 dp = opendir("/sd/");
onaka 7:0ec343d29641 191 if (!dp){
onaka 7:0ec343d29641 192 pc.printf("Could not open directry.\n");
onaka 7:0ec343d29641 193 return -1;
onaka 7:0ec343d29641 194 }
onaka 7:0ec343d29641 195 while((dirst = readdir(dp)) != NULL) {
onaka 7:0ec343d29641 196 if(sscanf(dirst->d_name, "log%03d.csv%c", &i, &c) == 1 && i>n) {
onaka 7:0ec343d29641 197 n = i;
onaka 7:0ec343d29641 198 }
onaka 7:0ec343d29641 199 }
onaka 7:0ec343d29641 200 closedir(dp);
onaka 7:0ec343d29641 201 return n;
ojan 0:bc6f14fc60c7 202 }
ojan 0:bc6f14fc60c7 203
ojan 3:5358a691a100 204 void KalmanInit() {
ojan 3:5358a691a100 205
ojan 3:5358a691a100 206 // 誤差共分散行列の値を決める（対角成分のみ）
ojan 3:5358a691a100 207 float alpha_R = 60.0f;
ojan 3:5358a691a100 208 float alpha_Q = 100.0f;
ojan 3:5358a691a100 209 R *= alpha_R;
ojan 3:5358a691a100 210 Q *= alpha_Q;
ojan 3:5358a691a100 211
ojan 3:5358a691a100 212 // 状態方程式のヤコビアンの初期値を代入（時間変化あり）
ojan 3:5358a691a100 213 float f[36] = {
ojan 4:45dc5590abc0 214 1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f,
ojan 4:45dc5590abc0 215 -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f,
ojan 4:45dc5590abc0 216 raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 217 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 218 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 3:5358a691a100 219 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
ojan 3:5358a691a100 220 };
ojan 3:5358a691a100 221
ojan 3:5358a691a100 222 F.SetComps(f);
ojan 3:5358a691a100 223
ojan 3:5358a691a100 224 // 観測方程式のヤコビアンの値を設定（時間変化無し）
ojan 3:5358a691a100 225 float h[18] = {
ojan 3:5358a691a100 226 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 227 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 3:5358a691a100 228 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f
ojan 3:5358a691a100 229 };
ojan 3:5358a691a100 230
ojan 3:5358a691a100 231 H.SetComps(h);
ojan 3:5358a691a100 232 }
ojan 3:5358a691a100 233
ojan 3:5358a691a100 234 void KalmanUpdate() {
ojan 3:5358a691a100 235 // ヤコビアンの更新
ojan 3:5358a691a100 236 float f[36] = {
ojan 4:45dc5590abc0 237 1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f,
ojan 4:45dc5590abc0 238 -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f,
ojan 4:45dc5590abc0 239 raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 240 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 241 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 3:5358a691a100 242 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
ojan 3:5358a691a100 243 };
ojan 3:5358a691a100 244
ojan 3:5358a691a100 245 F.SetComps(f);
ojan 3:5358a691a100 246
ojan 3:5358a691a100 247 // 事前推定値の更新
ojan 3:5358a691a100 248 pri_x = F * post_x;
ojan 3:5358a691a100 249 // 事前誤差分散行列の更新
ojan 3:5358a691a100 250 pri_P = F * post_P * F.Transpose() + R;
ojan 3:5358a691a100 251
ojan 3:5358a691a100 252 // カルマンゲインの計算
ojan 3:5358a691a100 253 S = Q + H * pri_P * H.Transpose();
ojan 3:5358a691a100 254 float det;
ojan 3:5358a691a100 255 if((det = S.Inverse(inv)) >= 0.0f) {
ojan 3:5358a691a100 256 pc.printf("E:%.3f\r\n", det);
ojan 3:5358a691a100 257 return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
ojan 3:5358a691a100 258 }
ojan 3:5358a691a100 259 K = pri_P * H.Transpose() * inv;
ojan 3:5358a691a100 260
ojan 3:5358a691a100 261 // 事後推定値の更新
ojan 4:45dc5590abc0 262 post_x = pri_x + K * (raw_geomag - H * pri_x);
ojan 3:5358a691a100 263 // 事後誤差分散行列の更新
ojan 3:5358a691a100 264 post_P = (I - K * H) * pri_P;
ojan 3:5358a691a100 265 }
ojan 3:5358a691a100 266
ojan 0:bc6f14fc60c7 267 void INT_func() {
ojan 4:45dc5590abc0 268 // センサーの値を更新
ojan 4:45dc5590abc0 269 mpu.read();
ojan 4:45dc5590abc0 270 hmc.read();
ojan 4:45dc5590abc0 271
ojan 4:45dc5590abc0 272 for(int i=0; i<3; i++) {
ojan 4:45dc5590abc0 273 raw_acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G);
ojan 4:45dc5590abc0 274 raw_gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
ojan 4:45dc5590abc0 275 raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
ojan 4:45dc5590abc0 276 }
ojan 0:bc6f14fc60c7 277
ojan 4:45dc5590abc0 278 Vector delta_g = Cross(raw_gyro, raw_g); // Δg = ω × g
ojan 4:45dc5590abc0 279 raw_g = 0.9f * (raw_g - delta_g * dt) + 0.1f * raw_acc.Normalize(); // 相補フィルタ
ojan 4:45dc5590abc0 280 raw_g = raw_g.Normalize();
ojan 4:45dc5590abc0 281
ojan 4:45dc5590abc0 282 KalmanUpdate();
ojan 0:bc6f14fc60c7 283
ojan 4:45dc5590abc0 284 // LPS25Hによる気圧の取得は10Hz
ojan 4:45dc5590abc0 285 lps_cnt = (lps_cnt+1)%10;
ojan 4:45dc5590abc0 286 if(lps_cnt == 0) {
ojan 4:45dc5590abc0 287 raw_press = (float)lps.pressure() * PRES_LSB_TO_HPA;
ojan 4:45dc5590abc0 288 }
ojan 4:45dc5590abc0 289 //pc.printf("%d(us)\r\n", timer.read_us());
ojan 4:45dc5590abc0 290
ojan 4:45dc5590abc0 291 if(INT_flag != FALSE) {
ojan 4:45dc5590abc0 292 g = raw_g;
ojan 4:45dc5590abc0 293 for(int i=0; i<3; i++) {
ojan 4:45dc5590abc0 294 geomag.SetComp(i+1, post_x.GetComp(i+1));
ojan 4:45dc5590abc0 295 }
ojan 4:45dc5590abc0 296 acc = raw_acc;
ojan 4:45dc5590abc0 297 gyro = raw_gyro;
ojan 4:45dc5590abc0 298 press = raw_press;
ojan 1:6cd6d2760856 299
ojan 0:bc6f14fc60c7 300 }
ojan 3:5358a691a100 301 }
ojan 3:5358a691a100 302
ojan 3:5358a691a100 303 void toString(Matrix& m) {
ojan 3:5358a691a100 304
ojan 3:5358a691a100 305 for(int i=0; i<m.GetRow(); i++) {
ojan 3:5358a691a100 306 for(int j=0; j<m.GetCol(); j++) {
ojan 3:5358a691a100 307 pc.printf("%.6f\t", m.GetComp(i+1, j+1));
ojan 3:5358a691a100 308 }
ojan 3:5358a691a100 309 pc.printf("\r\n");
ojan 3:5358a691a100 310 }
ojan 3:5358a691a100 311
ojan 3:5358a691a100 312 }
ojan 3:5358a691a100 313
ojan 3:5358a691a100 314 void toString(Vector& v) {
ojan 3:5358a691a100 315
ojan 3:5358a691a100 316 for(int i=0; i<v.GetDim(); i++) {
ojan 3:5358a691a100 317 pc.printf("%.6f\t", v.GetComp(i+1));
ojan 3:5358a691a100 318 }
ojan 3:5358a691a100 319 pc.printf("\r\n");
ojan 3:5358a691a100 320
ojan 0:bc6f14fc60c7 321 }