2021 ver of EPSON IMU
Dependencies: mbed QMC5883L SDFileSystem
main.cpp
- Committer:
- Joeatsumi
- Date:
- 2019-08-31
- Revision:
- 1:bf98282e69a4
- Parent:
- 0:3ba177faeee0
- Child:
- 2:a9f80f95aff9
File content as of revision 1:bf98282e69a4:
/*EPSONのセンサを試すために組んだ20190831。 地磁気センサの方位値を姿勢によって補正できる。 */ #include "mbed.h" #include "string.h" #include "QMC5883L.h"//地磁気センサのライブラリ #include "SDFileSystem.h"//sdカードを操作するためのライブラリ #define SCALE_FACTOR_GYRO 0.016 #define SCALE_FACTOR_ACC 0.20 #define GRAVITIONAL_ACCELERATION 9.80665 Serial pc(USBTX, USBRX); // tx, rx QMC5883L compass(p9,p10); Serial device(p13, p14); // tx, rx SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board DigitalIn switch_blue(p21);//sdカードへの書き込みを終了させる /*-----------------プロトタイプ宣言----------------*/ void pc_rx (); void dev_rx (); void call_window1(); void call_window0(); void GLOB_CMD_read(); void DIAG_STAT_read(); void UART_CTRL_write(); /*サンプリングモードへの移行*/ void move_to_sampling_mode(); /*IMUが動作可能かどうかの確認*/ void power_on_sequence1(); /*IMUが動作可能かどうかの確認*/ void power_on_sequence2(); /*X軸周りの角速度を算出*/ float read_angular_rate_x(); /*Y軸周りの角速度を算出*/ float read_angular_rate_y(); /*Z軸周りの角速度を算出*/ float read_angular_rate_z(); /*X軸の加速度を算出*/ float read_acceleration_x(); /*Y軸の加速度を算出*/ float read_acceleration_y(); /*Z軸の加速度を算出*/ float read_acceleration_z(); /*-----------------------------------------------*/ /*-------------タイマー関数---------------*/ Timer passed_time; /*---------------------------------------*/ /*-------------------各種の変数-----------------*/ float gyro_val[3];//角速度の値 float angle_val[3];//角度の値 float acc_val[3];//加速度の値 float acc_angle[2];//加速度から計算した角度の値 int16_t mag_val[3];//地磁気の値 float time_new,time_old; /*---------------------------------------------*/ int main() { pc.baud(460800); device.baud(460800); // pc.attach(pc_rx, Serial::RxIrq); //device.attach(dev_rx, Serial::RxIrq); /* power on wait 800ms */ wait(1.0); compass.init();//地磁気センサの起動 power_on_sequence1();//IMUが動作可能かどうかの確認 power_on_sequence2();//IMUが動作可能かどうかの確認 UART_CTRL_write();//IMUのボーレートを480600,手動モードへ移行 move_to_sampling_mode();//サンプリングモードへの移行 passed_time.start(); time_old=passed_time.read(); /*ジャイロで計算する初期角度の設定*/ angle_val[0]=0.0; angle_val[1]=0.0; angle_val[2]=0.0; /*----------------------------*/ /*sdカードにファイルを構成する そのファイルを書き込み状態にする*/ mkdir("/sd/IMU_logger", 0777); FILE *fp = fopen("/sd/IMU_logger/log.csv", "w"); if(fp == NULL) { error("Could not open file for write\n"); } /*-------------------------------------------------------*/ pc.printf("Writing start!\r\n"); while(1){ gyro_val[0]=read_angular_rate_x();//X軸周りの角速度の算出 gyro_val[1]=read_angular_rate_y();//Y軸周りの角速度の算出 gyro_val[2]=read_angular_rate_z();//Z軸周りの角速度の算出 acc_val[0]=read_acceleration_x();//X軸の加速度の算出 acc_val[1]=read_acceleration_y();//Y軸の加速度の算出 acc_val[2]=read_acceleration_z();//Z軸の加速度の算出 mag_val[0]=compass.getMagXvalue();//X軸の地磁気の算出 mag_val[1]=compass.getMagYvalue();//Y軸の地磁気の算出 mag_val[2]=compass.getMagZvalue();//Z軸の地磁気の算出 time_new=passed_time.read();//時間更新 angle_val[0]+=(time_new-time_old)*gyro_val[0];//X軸周りの角度の算出 angle_val[1]+=(time_new-time_old)*gyro_val[1];//Y軸周りの角度の算出 angle_val[2]+=(time_new-time_old)*gyro_val[2];//Z軸周りの角度の算出 //pc.printf("%f:%f:%f\r\n",gyro_val[0],gyro_val[1],gyro_val[2]); //pc.printf("%f:%f:%f\r\n",angle_val[0],angle_val[1],angle_val[2]); //pc.printf("%f:%f:%f\r\n",acc_val[0],acc_val[1],acc_val[2]); fprintf(fp,"%f,%f,%f,%f\r\n",time_new,angle_val[0],angle_val[1],angle_val[2]);//sdファイルに書き込み time_old=time_new; if(switch_blue==1){//青いタクトスイッチが押されている事が確認されたら fclose(fp);//sdカードに作ったファイルを閉じる(書き込みをやめる) pc.printf("Writing finish!"); break;//while文から離脱 }else{} wait(0.01); }//while 終わり }//main終わり /*------------------関数の宣言------------------*/ void pc_rx () { device.putc(pc.getc()); } void dev_rx () { pc.putc(device.getc()); } void call_window1(){ device.putc(0xFE); device.putc(0x01); device.putc(0x0d); } void call_window0(){ device.putc(0xFE); device.putc(0x00); device.putc(0x0d); } void GLOB_CMD_read(){//IMUが使えるかどうかの確認 char val[2]; short i; device.putc(0x0A); device.putc(0x00); device.putc(0x0d); while(device.getc()!=0x0A){ } val[0] = device.getc(); val[1] = device.getc(); i=(val[0]<<8)+val[1]; if(i==0){ pc.printf("IMU is ready.1\r\n"); }else{ pc.printf("IMU is not ready.1\r\n"); } } void DIAG_STAT_read(){ char val[2]; short i; device.putc(0x04); device.putc(0x00); device.putc(0x0d); while(device.getc()!=0x04){ } val[0] = device.getc(); val[1] = device.getc(); i=(val[0]<<8)+val[1]; if(i==0){ pc.printf("IMU is ready.2\r\n"); }else{ pc.printf("IMU is not ready.2\r\n"); } } void UART_CTRL_write(){//IMUのボーレートを480600,手動モードへ移行 call_window1(); device.putc(0x09); device.putc(0x00); device.putc(0x0d); device.putc(0x08); device.putc(0x00); device.putc(0x0d); } /*IMUが動作可能かどうかの確認*/ void power_on_sequence1(){ call_window1(); GLOB_CMD_read(); } /*IMUが動作可能かどうかの確認*/ void power_on_sequence2(){ call_window0(); DIAG_STAT_read(); } void move_to_sampling_mode(){ call_window0(); device.putc(0x83); device.putc(0x01); device.putc(0x0d); } float read_angular_rate_x(){ char val[4]; float gyro_x; call_window0(); device.putc(0x12);//register of gyro x (High) device.putc(0x00); device.putc(0x0d); while(device.getc()!=0x12){ } val[0] = device.getc(); val[1] = device.getc(); device.putc(0x14);//register of gyro x (Low) device.putc(0x00); device.putc(0x0d); while(device.getc()!=0x14){ } val[2] = device.getc(); val[3] = device.getc(); gyro_x=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3]; //SCALE_FACTOR 0.016 gyro_x*=(SCALE_FACTOR_GYRO/65536); return gyro_x; } float read_angular_rate_y(){ char val[4]; float gyro_y; call_window0(); device.putc(0x16);//register of gyro y (High) device.putc(0x00); device.putc(0x0d); while(device.getc()!=0x16){ } val[0] = device.getc(); val[1] = device.getc(); device.putc(0x18);//register of gyro y (Low) device.putc(0x00); device.putc(0x0d); while(device.getc()!=0x18){ } val[2] = device.getc(); val[3] = device.getc(); gyro_y=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3]; //SCALE_FACTOR 0.016 gyro_y*=(SCALE_FACTOR_GYRO/65536); return gyro_y; } float read_angular_rate_z(){ char val[4]; float gyro_z; call_window0(); device.putc(0x1A);//register of gyro z (High) device.putc(0x00); device.putc(0x0d); while(device.getc()!=0x1A){ } val[0] = device.getc(); val[1] = device.getc(); device.putc(0x1C);//register of gyro z (Low) device.putc(0x00); device.putc(0x0d); while(device.getc()!=0x1C){ } val[2] = device.getc(); val[3] = device.getc(); gyro_z=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3]; //SCALE_FACTOR 0.016 gyro_z*=(SCALE_FACTOR_GYRO/65536); return gyro_z; } float read_acceleration_x(){ char val[4]; float acceleration_x; call_window0(); device.putc(0x1E);//register of gyro z (High) device.putc(0x00); device.putc(0x0d); while(device.getc()!=0x1E){ } val[0] = device.getc(); val[1] = device.getc(); device.putc(0x20);//register of gyro z (Low) device.putc(0x00); device.putc(0x0d); while(device.getc()!=0x20){ } val[2] = device.getc(); val[3] = device.getc(); acceleration_x=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3]; //SCALE_FACTOR 0.016 acceleration_x*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0)); return acceleration_x; } float read_acceleration_y(){ char val[4]; float acceleration_y; call_window0(); device.putc(0x22);//register of gyro z (High) device.putc(0x00); device.putc(0x0d); while(device.getc()!=0x22){ } val[0] = device.getc(); val[1] = device.getc(); device.putc(0x24);//register of gyro z (Low) device.putc(0x00); device.putc(0x0d); while(device.getc()!=0x24){ } val[2] = device.getc(); val[3] = device.getc(); acceleration_y=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3]; acceleration_y*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0)); return acceleration_y; } float read_acceleration_z(){ char val[4]; float acceleration_z; call_window0(); device.putc(0x26);//register of gyro z (High) device.putc(0x00); device.putc(0x0d); while(device.getc()!=0x26){ } val[0] = device.getc(); val[1] = device.getc(); device.putc(0x28);//register of gyro z (Low) device.putc(0x00); device.putc(0x0d); while(device.getc()!=0x28){ } val[2] = device.getc(); val[3] = device.getc(); acceleration_z=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3]; acceleration_z*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0)); return acceleration_z; } /*-------------------------------------------*/