main.cpp

00001 /*EPSONのセンサを試すために組んだ20190831。
00002 地磁気センサの方位値を姿勢によって補正できる。
00003 */
00004 
00005 #include "mbed.h"
00006 #include "string.h"
00007 
00008 #include "QMC5883L.h"//地磁気センサのライブラリ
00009 #include "SDFileSystem.h"//sdカードを操作するためのライブラリ
00010 
00011 #define SCALE_FACTOR_GYRO 0.016
00012 #define SCALE_FACTOR_ACC 0.20
00013 #define GRAVITIONAL_ACCELERATION 9.80665
00014 
00015 Serial pc(USBTX, USBRX); // tx, rx
00016 QMC5883L compass(p9,p10);
00017 Serial device(p13, p14);  // tx, rx
00018 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
00019 DigitalIn switch_blue(p21);//sdカードへの書き込みを終了させる
00020 
00021 /*-----------------プロトタイプ宣言----------------*/
00022 void pc_rx ();
00023 void dev_rx ();
00024 void call_window1();
00025 void call_window0();
00026 void GLOB_CMD_read();
00027 void DIAG_STAT_read();
00028 void UART_CTRL_write();
00029 /*サンプリングモードへの移行*/
00030 void move_to_sampling_mode();
00031 /*IMUが動作可能かどうかの確認*/
00032 void power_on_sequence1();
00033 /*IMUが動作可能かどうかの確認*/
00034 void power_on_sequence2();
00035 
00036 /*X軸周りの角速度を算出*/
00037 float read_angular_rate_x();
00038 /*Y軸周りの角速度を算出*/
00039 float read_angular_rate_y();
00040 /*Z軸周りの角速度を算出*/
00041 float read_angular_rate_z();
00042 
00043 /*X軸の加速度を算出*/
00044 float read_acceleration_x();
00045 /*Y軸の加速度を算出*/
00046 float read_acceleration_y();
00047 /*Z軸の加速度を算出*/
00048 float read_acceleration_z();
00049 /*-----------------------------------------------*/
00050 /*-------------タイマー関数---------------*/
00051 Timer passed_time;
00052 /*---------------------------------------*/
00053 /*-------------------各種の変数-----------------*/
00054 float gyro_val[3];//角速度の値
00055 float angle_val[3];//角度の値
00056 
00057 float acc_val[3];//加速度の値
00058 float acc_angle[2];//加速度から計算した角度の値
00059 
00060 int16_t mag_val[3];//地磁気の値
00061 
00062 float time_new,time_old;
00063 /*---------------------------------------------*/
00064 
00065 
00066 int main() {
00067     
00068     pc.baud(460800);
00069     device.baud(460800);
00070     
00071    // pc.attach(pc_rx, Serial::RxIrq);
00072    //device.attach(dev_rx, Serial::RxIrq);
00073     
00074     /*
00075     power on
00076     wait 800ms
00077     */
00078     wait(1.0);
00079     
00080     compass.init();//地磁気センサの起動
00081     
00082     power_on_sequence1();//IMUが動作可能かどうかの確認
00083     power_on_sequence2();//IMUが動作可能かどうかの確認
00084     UART_CTRL_write();//IMUのボーレートを480600,手動モードへ移行
00085     move_to_sampling_mode();//サンプリングモードへの移行
00086     
00087     passed_time.start();
00088     time_old=passed_time.read();
00089     
00090     /*ジャイロで計算する初期角度の設定*/
00091     angle_val[0]=0.0;
00092     angle_val[1]=0.0;
00093     angle_val[2]=0.0;
00094     /*----------------------------*/
00095     
00096     /*sdカードにファイルを構成する そのファイルを書き込み状態にする*/
00097     mkdir("/sd/IMU_logger", 0777);
00098     
00099     FILE *fp = fopen("/sd/IMU_logger/log.csv", "w");
00100         
00101     if(fp == NULL) {
00102         error("Could not open file for write\n");
00103     }
00104     /*-------------------------------------------------------*/
00105     
00106     pc.printf("Writing start!\r\n");
00107     
00108     while(1){
00109         
00110         gyro_val[0]=read_angular_rate_x();//X軸周りの角速度の算出
00111         gyro_val[1]=read_angular_rate_y();//Y軸周りの角速度の算出
00112         gyro_val[2]=read_angular_rate_z();//Z軸周りの角速度の算出
00113         
00114         acc_val[0]=read_acceleration_x();//X軸の加速度の算出
00115         acc_val[1]=read_acceleration_y();//Y軸の加速度の算出
00116         acc_val[2]=read_acceleration_z();//Z軸の加速度の算出
00117         
00118         mag_val[0]=compass.getMagXvalue();//X軸の地磁気の算出
00119         mag_val[1]=compass.getMagYvalue();//Y軸の地磁気の算出
00120         mag_val[2]=compass.getMagZvalue();//Z軸の地磁気の算出
00121         
00122         time_new=passed_time.read();//時間更新
00123         
00124         angle_val[0]+=(time_new-time_old)*gyro_val[0];//X軸周りの角度の算出
00125         angle_val[1]+=(time_new-time_old)*gyro_val[1];//Y軸周りの角度の算出
00126         angle_val[2]+=(time_new-time_old)*gyro_val[2];//Z軸周りの角度の算出
00127         
00128         //pc.printf("%f:%f:%f\r\n",gyro_val[0],gyro_val[1],gyro_val[2]);
00129         //pc.printf("%f:%f:%f\r\n",angle_val[0],angle_val[1],angle_val[2]);        
00130         //pc.printf("%f:%f:%f\r\n",acc_val[0],acc_val[1],acc_val[2]);        
00131         
00132         fprintf(fp,"%f,%f,%f,%f\r\n",time_new,angle_val[0],angle_val[1],angle_val[2]);//sdファイルに書き込み
00133         
00134         time_old=time_new;
00135         
00136         if(switch_blue==1){//青いタクトスイッチが押されている事が確認されたら
00137                    
00138             fclose(fp);//sdカードに作ったファイルを閉じる(書き込みをやめる)
00139             pc.printf("Writing finish!");
00140             break;//while文から離脱
00141                     
00142         }else{}
00143         
00144         wait(0.01);
00145         
00146         }//while 終わり
00147 }//main終わり
00148 
00149 /*------------------関数の宣言------------------*/
00150 void pc_rx () {
00151     device.putc(pc.getc());
00152 }
00153  
00154 void dev_rx () {
00155     pc.putc(device.getc());
00156 }
00157 
00158 void call_window1(){
00159     device.putc(0xFE);
00160     device.putc(0x01);
00161     device.putc(0x0d);
00162  }
00163 
00164 void call_window0(){
00165     device.putc(0xFE);
00166     device.putc(0x00);
00167     device.putc(0x0d);
00168  }
00169   
00170 void GLOB_CMD_read(){//IMUが使えるかどうかの確認
00171     
00172     char val[2];
00173     short i;
00174     
00175     device.putc(0x0A);
00176     device.putc(0x00);
00177     device.putc(0x0d);
00178       
00179     while(device.getc()!=0x0A){
00180       }
00181     val[0] = device.getc();
00182     val[1] = device.getc();
00183     
00184     i=(val[0]<<8)+val[1];
00185     if(i==0){
00186         pc.printf("IMU is ready.1\r\n");
00187         }else{
00188             pc.printf("IMU is not ready.1\r\n");
00189             }
00190             
00191     }
00192 
00193 void DIAG_STAT_read(){
00194     
00195     char val[2];
00196     short i;
00197     
00198     device.putc(0x04);
00199     device.putc(0x00);
00200     device.putc(0x0d);    
00201     
00202       
00203     while(device.getc()!=0x04){
00204       }
00205     val[0] = device.getc();
00206     val[1] = device.getc();
00207     
00208     i=(val[0]<<8)+val[1];
00209     if(i==0){
00210         pc.printf("IMU is ready.2\r\n");
00211         }else{
00212             pc.printf("IMU is not ready.2\r\n");
00213             }
00214     }
00215 
00216 void UART_CTRL_write(){//IMUのボーレートを480600,手動モードへ移行
00217     
00218     call_window1();
00219     
00220     device.putc(0x09);
00221     device.putc(0x00);
00222     device.putc(0x0d);
00223     
00224     device.putc(0x08);
00225     device.putc(0x00);
00226     device.putc(0x0d);
00227         
00228     }
00229         
00230 /*IMUが動作可能かどうかの確認*/
00231 void power_on_sequence1(){
00232     call_window1();
00233     GLOB_CMD_read();
00234     }
00235 
00236 /*IMUが動作可能かどうかの確認*/
00237 void power_on_sequence2(){
00238     call_window0();
00239     DIAG_STAT_read();
00240     }
00241 
00242 void move_to_sampling_mode(){
00243     call_window0();
00244     
00245     device.putc(0x83);
00246     device.putc(0x01);
00247     device.putc(0x0d);    
00248     }
00249     
00250 float read_angular_rate_x(){
00251     
00252     char val[4];
00253     float gyro_x;
00254     
00255     call_window0();
00256     
00257     device.putc(0x12);//register of gyro x (High)
00258     device.putc(0x00);
00259     device.putc(0x0d);    
00260     
00261     while(device.getc()!=0x12){
00262       }
00263     val[0] = device.getc();
00264     val[1] = device.getc();
00265 
00266     device.putc(0x14);//register of gyro x (Low)
00267     device.putc(0x00);
00268     device.putc(0x0d);    
00269     
00270     while(device.getc()!=0x14){
00271       }
00272     val[2] = device.getc();
00273     val[3] = device.getc();
00274     
00275     gyro_x=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
00276     //SCALE_FACTOR 0.016
00277     gyro_x*=(SCALE_FACTOR_GYRO/65536);
00278     
00279     return gyro_x;
00280     
00281     }
00282     
00283 float read_angular_rate_y(){
00284     
00285     char val[4];
00286     float gyro_y;
00287     
00288     call_window0();
00289     
00290     device.putc(0x16);//register of gyro y (High)
00291     device.putc(0x00);
00292     device.putc(0x0d);    
00293     
00294     while(device.getc()!=0x16){
00295       }
00296     val[0] = device.getc();
00297     val[1] = device.getc();
00298 
00299     device.putc(0x18);//register of gyro y (Low)
00300     device.putc(0x00);
00301     device.putc(0x0d);    
00302     
00303     while(device.getc()!=0x18){
00304       }
00305     val[2] = device.getc();
00306     val[3] = device.getc();
00307     
00308     gyro_y=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
00309     //SCALE_FACTOR 0.016
00310     gyro_y*=(SCALE_FACTOR_GYRO/65536);
00311     
00312     return gyro_y;
00313     
00314     }
00315     
00316 float read_angular_rate_z(){
00317     
00318     char val[4];
00319     float gyro_z;
00320     
00321     call_window0();
00322     
00323     device.putc(0x1A);//register of gyro z (High)
00324     device.putc(0x00);
00325     device.putc(0x0d);    
00326     
00327     while(device.getc()!=0x1A){
00328       }
00329     val[0] = device.getc();
00330     val[1] = device.getc();
00331 
00332     device.putc(0x1C);//register of gyro z (Low)
00333     device.putc(0x00);
00334     device.putc(0x0d);    
00335     
00336     while(device.getc()!=0x1C){
00337       }
00338     val[2] = device.getc();
00339     val[3] = device.getc();
00340     
00341     gyro_z=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
00342     //SCALE_FACTOR 0.016
00343     gyro_z*=(SCALE_FACTOR_GYRO/65536);
00344     
00345     return gyro_z;
00346     
00347     }
00348     
00349 float read_acceleration_x(){
00350     
00351     char val[4];
00352     float acceleration_x;
00353     
00354     call_window0();
00355     
00356     device.putc(0x1E);//register of gyro z (High)
00357     device.putc(0x00);
00358     device.putc(0x0d);    
00359     
00360     while(device.getc()!=0x1E){
00361       }
00362     val[0] = device.getc();
00363     val[1] = device.getc();
00364 
00365     device.putc(0x20);//register of gyro z (Low)
00366     device.putc(0x00);
00367     device.putc(0x0d);    
00368     
00369     while(device.getc()!=0x20){
00370       }
00371     val[2] = device.getc();
00372     val[3] = device.getc();
00373     
00374     acceleration_x=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
00375     //SCALE_FACTOR 0.016
00376     acceleration_x*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
00377     
00378     return acceleration_x;
00379     
00380     }
00381     
00382 float read_acceleration_y(){
00383     
00384     char val[4];
00385     float acceleration_y;
00386     
00387     call_window0();
00388     
00389     device.putc(0x22);//register of gyro z (High)
00390     device.putc(0x00);
00391     device.putc(0x0d);    
00392     
00393     while(device.getc()!=0x22){
00394       }
00395     val[0] = device.getc();
00396     val[1] = device.getc();
00397 
00398     device.putc(0x24);//register of gyro z (Low)
00399     device.putc(0x00);
00400     device.putc(0x0d);    
00401     
00402     while(device.getc()!=0x24){
00403       }
00404     val[2] = device.getc();
00405     val[3] = device.getc();
00406     
00407     acceleration_y=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
00408     acceleration_y*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
00409     
00410     return acceleration_y;
00411     
00412     }
00413 
00414 float read_acceleration_z(){
00415     
00416     char val[4];
00417     float acceleration_z;
00418     
00419     call_window0();
00420     
00421     device.putc(0x26);//register of gyro z (High)
00422     device.putc(0x00);
00423     device.putc(0x0d);    
00424     
00425     while(device.getc()!=0x26){
00426       }
00427     val[0] = device.getc();
00428     val[1] = device.getc();
00429 
00430     device.putc(0x28);//register of gyro z (Low)
00431     device.putc(0x00);
00432     device.putc(0x0d);    
00433     
00434     while(device.getc()!=0x28){
00435       }
00436     val[2] = device.getc();
00437     val[3] = device.getc();
00438     
00439     acceleration_z=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
00440     acceleration_z*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
00441     
00442     return acceleration_z;
00443     
00444     }
00445     
00446 /*-------------------------------------------*/