File content as of revision 4:6f167145f4b0:

//=========================================================
//IMU/GNSS logger with ublox-NEO7M 
//MPU board:  mbed LPC1768
//GNSS module: ublox-NEO7M
//2022/01/14  A.Toda
//========================================================
#include "mbed.h"
#include "MG354PDH0.h"
#include "SDFileSystem.h"
//=========================================================
//Port Setting
Serial pc(USBTX, USBRX); // tx, rx            // PCとの通信用シリアルポート
Serial   epson_imu(p9, p10);                  // IMU通信用シリアルポート
SPI spi(p11, p12, p13);  // mosi, miso, sclk  SPIを使用するセンサのバス
DigitalOut CS(p14);      // NEO-7MのCSピン

DigitalIn log_switch(p15); //記録終了用のスイッチ

AnalogIn thermopile_input_1(p20); //アナログ電圧の読み取り
AnalogIn thermopile_input_2(p19); //アナログ電圧の読み取り

DigitalOut myled_1(LED1);
DigitalOut myled_2(LED2);
DigitalOut myled_3(LED3);
DigitalOut myled_4(LED4);

//ライブラリとポートの対応づけ
SDFileSystem sd(p5, p6, p7, p8, "sd"); // mosi, miso, sclk, cs
MG354PDH0        imu(&epson_imu);                     // IMU

//ファイルポインタ
FILE *fp;
FILE *im;

//=========================================================
//IMUの変数
float IMU_BUFFER = 50;//慣性センサをsdに記録する前にためておくバッファの大きさ
float time_i[20];//慣性センサの計測時刻
float gyro_val[20][3];//角速度の値
float acc_val[20][3];//加速度の値

int imu_counter;//何回分の慣性センサ計測値がバッファにたまっている貯まっているかのカウント
//=========================================================
//受信したメッセージから抽出したい情報
float latitude,longitude,height_float;  //緯度、経度、高度
int gps_Fix; // GPSの測位状態この値が3ならば3D Fix状態である
float velN_float,velE_float,velD_float; // NED座標系に置ける速度

float time_g[20];//gnssの計測時刻
float gnss_val[20][6];//gnss受信機から得た値

int gnss_counter;//何回分の慣性センサ計測値がバッファにたまっている貯まっているかのカウント
//=========================================================
//UBXデータを処理したかどうかのフラグ
int flag_posllh,flag_velned;

//=========================================================
//処理時間計測の為のタイマー
Timer processing_timer;

//=========================================================
//処理時間
int processed_time,processed_time_before,processed_time_after,measurement_time_g,measurement_time_i;

//=========================================================
//Ticker
Ticker timer1; //
Ticker timer2; //

//=========================================================
//Logging variables

//Logging counter 100Hzの割り込みで何回目であるかを0から99でカウント
int logging_counter;

float imu_mesurement_freq = 100.0; //Hz
float gnss_mesurement_freq = 5.0;  //theta_update_freq;

float imu_interval = 1.0f/imu_mesurement_freq;   //sec
float gnss_interval = 1.0f/gnss_mesurement_freq; //sec

int logging_status;
//=========================================================
//Header char
const unsigned char UBX_HEADER[]        = { 0xB5, 0x62 };
const unsigned char NAV_POSLLH_HEADER[] = { 0x01, 0x02 };
const unsigned char NAV_STATUS_HEADER[] = { 0x01, 0x03 };

const unsigned char NAV_VELNED_HEADER[] = { 0x01, 0x12 };

enum _ubxMsgType {
  MT_NONE,
  MT_NAV_POSLLH,
  MT_NAV_STATUS,
  MT_NAV_VELNED
};

//=========================================================
//メッセージの構造体
struct NAV_POSLLH {
  unsigned char cls;
  unsigned char id;
  unsigned short len;
  unsigned long iTOW;
  long lon;
  long lat;
  long height;
  long hMSL;
  unsigned long hAcc;
  unsigned long vAcc;
};

struct NAV_STATUS {
  unsigned char cls;
  unsigned char id;
  unsigned short len;
  unsigned long iTOW;
  unsigned char gpsFix;
  char flags;
  char fixStat;
  char flags2;
  unsigned long ttff;
  unsigned long msss;
};

struct NAV_VELNED  {
  unsigned char cls;
  unsigned char id;
  unsigned short len;
  unsigned long iTOW;
  signed long velN;
  signed long velE;
  signed long velD;
  unsigned long speed;
  unsigned long gSpeed;
  signed long heading;
  unsigned long sAcc;
  unsigned long cAcc;
    
};

//=========================================================
//受信したメッセージを格納する為の共用体
union UBXMessage {
  NAV_VELNED navVelned;//payload size is 36bytes
  NAV_POSLLH navPosllh;//payload size is 28bytes
  NAV_STATUS navStatus;//payload size is 16bytes
};

UBXMessage ubxMessage;

// The last two bytes of the message is a checksum value, used to confirm that the received payload is valid.
// The procedure used to calculate this is given as pseudo-code in the uBlox manual.
void calcChecksum(unsigned char* CK, int msgSize) {
  memset(CK, 0, 2);
  for (int i = 0; i < msgSize; i++) {
    CK[0] += ((unsigned char*)(&ubxMessage))[i];
    CK[1] += CK[0];
  }
}

//=========================================================
// Compares the first two bytes of the ubxMessage struct with a specific message header.
// Returns true if the two bytes match.
bool compareMsgHeader(const unsigned char* msgHeader) {
  unsigned char* ptr = (unsigned char*)(&ubxMessage);
  return ptr[0] == msgHeader[0] && ptr[1] == msgHeader[1];
}

//=========================================================
// Reads in bytes from the GPS module and checks to see if a valid message has been constructed.
// Returns the type of the message found if successful, or MT_NONE if no message was found.
// After a successful return the contents of the ubxMessage union will be valid, for the 
// message type that was found. Note that further calls to this function can invalidate the
// message content, so you must use the obtained values before calling this function again.
void processGPS() {
  
  static int fpos = 0;
  static unsigned char checksum[2];
  
  static unsigned char currentMsgType = MT_NONE;
  static int payloadSize = sizeof(UBXMessage);
  
  CS = 0;  //SPIによる読み出しを開始
  
  processed_time_before = processing_timer.read_us();// captureing prossing time
  
  /*
  NEO-7Mに(0xFF)を送って、取得した情報を1byteずつ以下の
  for文で確認する。
  
  */
  for(int buff_counter=1;buff_counter<50;buff_counter++){  
    
    unsigned char c = spi.write(0xFF); 
    
    if ( fpos < 2 ) {
      // For the first two bytes we are simply looking for a match with the UBX header bytes (0xB5,0x62)
      if ( c == UBX_HEADER[fpos] )
        fpos++;
      else
        fpos = 0; // Reset to beginning state.
    }
    else {
      // If we come here then fpos >= 2, which means we have found a match with the UBX_HEADER
      // and we are now reading in the bytes that make up the payload.
      
      // Place the incoming byte into the ubxMessage struct. The position is fpos-2 because
      // the struct does not include the initial two-byte header (UBX_HEADER).
      if ( (fpos-2) < payloadSize )
        ((unsigned char*)(&ubxMessage))[fpos-2] = c;


      fpos++;
      
      if ( fpos == 4 ) {
        // We have just received the second byte of the message type header, 
        // so now we can check to see what kind of message it is.
        
        if ( compareMsgHeader(NAV_VELNED_HEADER) ) {
          currentMsgType = MT_NAV_VELNED;
          payloadSize = sizeof(NAV_VELNED);
          
        }
        else if ( compareMsgHeader(NAV_STATUS_HEADER) ) {
          currentMsgType = MT_NAV_STATUS;
          payloadSize = sizeof(NAV_STATUS);
        }
        
        else if ( compareMsgHeader(NAV_POSLLH_HEADER) ) {
          currentMsgType = MT_NAV_POSLLH;
          payloadSize = sizeof(NAV_POSLLH);
          
        }
        
        else {
          // unknown message type, bail
          fpos = 0;
          continue;
        }
      }

      if ( fpos == (payloadSize+2) ) {
        // All payload bytes have now been received, so we can calculate the 
        // expected checksum value to compare with the next two incoming bytes.
        calcChecksum(checksum, payloadSize);
      }
      else if ( fpos == (payloadSize+3) ) {
        // First byte after the payload, ie. first byte of the checksum.
        // Does it match the first byte of the checksum we calculated?
        if ( c != checksum[0] ) {
          // Checksum doesn't match, reset to beginning state and try again.
          fpos = 0; 
        }
      }
      else if ( fpos == (payloadSize+4) ) {
        // Second byte after the payload, ie. second byte of the checksum.
        // Does it match the second byte of the checksum we calculated?
        fpos = 0; // We will reset the state regardless of whether the checksum matches.
        if ( c == checksum[1] ) {
          // Checksum matches, we have a valid message.
          if(currentMsgType==MT_NAV_POSLLH){
             latitude=ubxMessage.navPosllh.lat/10000000.0f;
             longitude=ubxMessage.navPosllh.lon/10000000.0f;
             height_float=float(ubxMessage.navPosllh.height);
             
             //pc.printf("latitude=%f,longitude=%f,height=%f\r\n",latitude,longitude,height_float);
       
        
             flag_posllh=1;//位置情報を読み取った合図としてフラグを立てる
             //pc.printf("flag_posllh=%d\r\n",flag_posllh);
             
              }
          else if(currentMsgType==MT_NAV_VELNED){
              velN_float=float(ubxMessage.navVelned.velN);
              velE_float=float(ubxMessage.navVelned.velE);
              velD_float=float(ubxMessage.navVelned.velD);
              
              //pc.printf("velN=%f,velE=%f,velD=%f\r\n",velN_float,velE_float,velD_float);
              flag_velned=1;//速度情報を読み取った合図としてフラグを立てる
              //pc.printf("flag_velned=%d\r\n",flag_velned);
              
              }
          else if(currentMsgType==MT_NAV_STATUS){
              
              }
          else{}
          //return currentMsgType; 
        }
      }
      else if ( fpos > (payloadSize+4) ) {
        // We have now read more bytes than both the expected payload and checksum 
        // together, so something went wrong. Reset to beginning state and try again.
        fpos = 0;
      }
    }
  }
  
  CS = 1;  //SPIによる読み出しを終了させる
  

  //processGPS()の処理に必要な時間の計測
  //複数のメッセージを読み取る、つまりこの関数をメッセージの数だけwhile内で読み出すとき、
  //この関数の処理時間(processed_time)として保存されるのは
  //最後に呼び出されたprocessGPSの処理時間となる。
   processed_time_after = processing_timer.read_us();// captureing prossing time
   processed_time=processed_time_after-processed_time_before;
   
   /*processGPSの処理時間の表示*/
   //pc.printf("processed_time_after(us)=%d;",(processed_time_after));
   //pc.printf("processed_time(us)=%d\r\n",(processed_time));
   //pc.printf("%d,%d\r\n",processed_time_after,processed_time);
   
}

void imu_mesurement(){

    if(log_switch==1){
        logging_status=1;
    }else if(log_switch==0){
        logging_status=0;
    }else{}
    
}

void ublox_logging()
{
    
    //============慣性センサのロギング============
    time_i[imu_counter] = processing_timer.read_us();//慣性センサの計測時刻
    gyro_val[imu_counter][0]=imu.read_angular_rate_x();//X軸周りの角速度の算出
    gyro_val[imu_counter][1]=imu.read_angular_rate_y();//Y軸周りの角速度の算出
    gyro_val[imu_counter][2]=imu.read_angular_rate_z();//Z軸周りの角速度の算出
    
    acc_val[imu_counter][0]=imu.read_acceleration_x();//X軸の加速度の算出
    acc_val[imu_counter][1]=imu.read_acceleration_y();//Y軸の加速度の算出
    acc_val[imu_counter][2]=imu.read_acceleration_z();//Z軸の加速度の算出   
    
    imu_counter++;
    //=========================================
    
    if(((logging_counter+1)%20)==0){
        //detach the rotary imu mesurement
        
        processGPS();
        processGPS();
        processGPS();
        
        //位置と速度情報を読み取った場合
        if((flag_posllh==1)&&(flag_velned==1)){
            time_g[gnss_counter] = processing_timer.read_us();
            gnss_val[gnss_counter][0] = latitude;
            gnss_val[gnss_counter][1] = longitude;
            gnss_val[gnss_counter][2] = height_float;
            gnss_val[gnss_counter][3] = velN_float;
            gnss_val[gnss_counter][4] = velE_float;
            gnss_val[gnss_counter][5] = velD_float;
            
            pc.printf("U\r\n");
            
            /*フラグを0に戻す*/
            flag_posllh=0;
            flag_velned=0;
            
            gnss_counter++;
            
        }else{}//if((flag_posllh==1)&&(flag_velned==1)){   
    }else{}//if(((logging_counter+1)%20)==0){
    
    
    if((logging_counter+1)>=100){
        logging_counter = 0;
    }else{
        logging_counter++;
    }
    
    if(log_switch==1){
        logging_status=1;
    }else if(log_switch==0){
        logging_status=0;
        fclose(fp);
        fclose(im);
        pc.printf("FC\r\n");
        timer1.detach();
        
        //ロガーの動作状態を見るためのLED
        //ロガーが記録を終了した表示をする
        myled_1 = 0;
        myled_2 = 1;
        
    }else{}
    
    
    
}


/*--------------------------------------------*/
int main() {
    
    //power on wait 800ms form IMU
    wait(1.0);
    
    //IMU initialize
    imu.power_on_sequence1();//IMUが動作可能かどうかの確認
    imu.power_on_sequence2();//IMUが動作可能かどうかの確認
    imu.UART_CTRL_write();//IMUのボーレートを480600,手動モードへ移行
    imu.move_to_sampling_mode();//サンプリングモードへの移行
    
    //UART initialization
    pc.baud(460800); //115.2 kbps
    
    spi.frequency(1000000);
    
    mkdir("/sd/mydir",0777);//SDファイル作成
    fp = fopen("/sd/mydir/gps.csv", "a");//最初のSDopen時間かかるのでwhile外で行う
    im = fopen("/sd/mydir/imu.csv", "a");
    
    if(fp == NULL) {
       error("Could not open file for write\n");
       }else{}
    
    //ロガーの動作状態を見るためのLED
    //初期状態ではロガーが記録中の表示をする
    myled_1 = 1;
    myled_2 = 0;
    
    pc.printf("FO\r\n");//file open
    logging_status=1;
    
    wait(0.1);
    
    //フラグのリセット
    flag_posllh=0;
    flag_velned=0;
    
    imu_counter = 0;
    gnss_counter = 0;
    logging_counter = 0;
    
    //-------------------------------------------  
    //Timer
    //-------------------------------------------  
    //timer1: imu mesurement, 100 Hz
    timer1.attach(&ublox_logging, imu_interval);
    
    processing_timer.start();//timer starts
    
    while(1) {
        for (int i = 0; i < imu_counter; i++){
            fprintf(im,"%f,%f,%f,%f,%f,%f,%f\r\n",time_i[i],gyro_val[i][0],gyro_val[i][1],gyro_val[i][2],acc_val[i][0],acc_val[i][1],acc_val[i][2]);
        }
        imu_counter = 0;
        
        for (int i = 0; i < gnss_counter; i++){
            fprintf(fp,"%f,%f,%f,%f,%f,%f,%f\r\n",time_g[i],gnss_val[i][0],gnss_val[i][1],gnss_val[i][2],gnss_val[i][3],gnss_val[i][4],gnss_val[i][5]);
        }
        gnss_counter = 0;
        }//while
        
}