File content as of revision 2:a9f80f95aff9:

/*
EPSONのセンサを試すために組んだ20210423。
*/

#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(p9, p10);  // tx, rx
Serial device(p28, p27);  // 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.printf("Baud rate setting\r\n");
        
   // 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;
    /*----------------------------*/
    /*
    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,",time_new);
        pc.printf("%f,%f,%f,",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ファイルに書き込み
        //pc.printf("%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(){
    pc.printf("call_window1\r\n");
    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が使えるかどうかの確認
    pc.printf("GLOB_CMD_read\r\n");
    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();
    
    pc.printf("%c",val[0]);
    pc.printf("%c",val[1]);
    
    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(){
    pc.printf("power on sequence 1\r\n");
    call_window1();
    GLOB_CMD_read();
    }

/*IMUが動作可能かどうかの確認*/
void power_on_sequence2(){
    pc.printf("power on sequence 2\r\n");
    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;
    
    }
    
/*-------------------------------------------*/