imu/gnss logger ver1.1
Dependencies: mbed MPU9250_SPI MG354PDH0 SDFileSystem
main.cpp
- Committer:
- Joeatsumi
- Date:
- 2021-11-16
- Revision:
- 5:38613e30475e
- Parent:
- 4:d49256697f27
File content as of revision 5:38613e30475e:
//========================================================= //GNSS logger with ublox-NEO7M //MPU board: mbed LPC1768 //GNSS module: ublox-NEO7M //2021/11/16 A.Toda //======================================================== #include "mbed.h" #include "SDFileSystem.h" #include "MG354PDH0.h" #include "MPU9250.h" //Include library //========================================================= //Port Setting Serial pc(USBTX, USBRX); // tx, rx Serial epson_imu(p9, p10); // IMU通信用シリアルポート SPI spi(p11, p12, p13); // mosi, miso, sclk DigitalOut myled_1(LED1); DigitalOut myled_2(LED2); DigitalOut myled_3(LED3); DigitalOut myled_4(LED4); DigitalOut CS(p14); // NEO-7MのCSピン DigitalOut BME_280_CS(p26); // BME280のCSピン DigitalOut log_low(p23); DigitalOut log_high(p22); DigitalIn log_switch(p15); AnalogIn thermopile_input_1(p20); AnalogIn thermopile_input_2(p19); MG354PDH0 imu(&epson_imu); // IMU SDFileSystem sd(p5, p6, p7, p8, "sd"); // mosi, miso, sclk, cs mpu9250_spi mpu9250(spi,p25); //define the mpu9250 object //ファイルポインタ FILE *fp; FILE *im; FILE *th; //========================================================= //IMUの変数 float gyro_val[3];//角速度の値 float acc_val[3];//加速度の値 //========================================================= //サーモパイルセンサの変数 float thermopile_voltage_1; float thermopile_voltage_2; //========================================================= //受信したメッセージから抽出したい情報 float latitude,longitude,height_float; //緯度、経度、高度 int gps_Fix; // GPSの測位状態この値が3ならば3D Fix状態である float velN_float,velE_float,velD_float; // NED座標系に置ける速度 //========================================================= //BME280の変数 const unsigned char BME280_SPI_MASK = 0x7F; uint16_t dig_T1; int16_t dig_T2, dig_T3; uint16_t dig_P1; int16_t dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9; uint16_t dig_H1, dig_H3; int16_t dig_H2, dig_H4, dig_H5, dig_H6; int32_t t_fine; float bem280_tempreture; float bem280_humidity; float bem280_pressure; //========================================================= //UBXデータを処理したかどうかのフラグ int flag_posllh,flag_velned; //========================================================= //処理時間計測の為のタイマー Timer processing_timer; //========================================================= //処理時間 int processed_time,processed_time_before,processed_time_after; float measurement_time_g; //========================================================= //Ticker Ticker timer1; // Ticker timer2; // //========================================================= //Logging variables 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{} gyro_val[0]=imu.read_angular_rate_x();//X軸周りの角速度の算出 gyro_val[1]=imu.read_angular_rate_y();//Y軸周りの角速度の算出 gyro_val[2]=imu.read_angular_rate_z();//Z軸周りの角速度の算出 acc_val[0]=imu.read_acceleration_x();//X軸の加速度の算出 acc_val[1]=imu.read_acceleration_y();//Y軸の加速度の算出 acc_val[2]=imu.read_acceleration_z();//Z軸の加速度の算出 thermopile_voltage_1 = 3.3*(thermopile_input_1.read());//サーモパイルセンサ_1出力の計測 thermopile_voltage_2 = 3.3*(thermopile_input_2.read());//サーモパイルセンサ_2出力の計測 mpu9250.AK8963_read_Magnetometer(); //計測時間の取得 measurement_time_g = processing_timer.read(); if(logging_status==1){ fprintf(im,"%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",measurement_time_g,gyro_val[0],gyro_val[1],gyro_val[2],acc_val[0],acc_val[1],acc_val[2],thermopile_voltage_1,thermopile_voltage_2,mpu9250.Magnetometer[0],mpu9250.Magnetometer[1],mpu9250.Magnetometer[2]); //pc.printf("IL\r\n");//imu logging }else if(logging_status==0){} } //==================BME_280の関数=============================== void BME_280_initialize() { char cmd[18]; BME_280_CS = 1; //spi.format(8, 0); // 8-bit, mode=0 //spi.frequency(1000000); // 1MHZ BME_280_CS = 0; spi.write(0xd0); // chip_id cmd[0] = spi.write(0); // read chip_id BME_280_CS = 1; BME_280_CS = 0; spi.write(0xf2 & BME280_SPI_MASK); // ctrl_hum spi.write(0x04); // Humidity oversampling x4 BME_280_CS = 1; BME_280_CS = 0; spi.write(0xf4 & BME280_SPI_MASK); // ctrl_meas spi.write((4<<5)|(4<<2)|3); // Temparature oversampling x4, Pressure oversampling x4, Normal mode BME_280_CS = 1; BME_280_CS = 0; spi.write(0xf5 & BME280_SPI_MASK); // config spi.write(0xa0); // Standby 1000ms, Filter off, 4-wire SPI interface BME_280_CS = 1; wait(1); BME_280_CS = 0; spi.write(0x88); // read dig_T regs for(int i = 0; i < 6; i++) cmd[i] = spi.write(0); BME_280_CS = 1; dig_T1 = (cmd[1] << 8) | cmd[0]; dig_T2 = (cmd[3] << 8) | cmd[2]; dig_T3 = (cmd[5] << 8) | cmd[4]; BME_280_CS = 0; spi.write(0x8e); // read dig_P regs for(int i = 0; i < 18; i++) cmd[i] = spi.write(0); BME_280_CS = 1; dig_P1 = (cmd[ 1] << 8) | cmd[ 0]; dig_P2 = (cmd[ 3] << 8) | cmd[ 2]; dig_P3 = (cmd[ 5] << 8) | cmd[ 4]; dig_P4 = (cmd[ 7] << 8) | cmd[ 6]; dig_P5 = (cmd[ 9] << 8) | cmd[ 8]; dig_P6 = (cmd[11] << 8) | cmd[10]; dig_P7 = (cmd[13] << 8) | cmd[12]; dig_P8 = (cmd[15] << 8) | cmd[14]; dig_P9 = (cmd[17] << 8) | cmd[16]; BME_280_CS = 0; spi.write(0xA1); // read dig_H1 reg cmd[0] = spi.write(0); BME_280_CS = 1; BME_280_CS = 0; spi.write(0xE1); // read dig_H regs for(int i = 0; i < 7; i++) cmd[1+i] = spi.write(0); BME_280_CS = 1; dig_H1 = cmd[0]; dig_H2 = (cmd[2] << 8) | cmd[1]; dig_H3 = cmd[3]; dig_H4 = (cmd[4] << 4) | (cmd[5] & 0x0f); dig_H5 = (cmd[6] << 4) | ((cmd[5]>>4) & 0x0f); dig_H6 = cmd[7]; } float BME_280_getTemperature() { uint32_t temp_raw; float tempf; char cmd[3]; BME_280_CS = 0; spi.write(0xfa); for(int i = 0; i < 3; i++) cmd[i] = spi.write(0); BME_280_CS = 1; temp_raw = (cmd[0] << 12) | (cmd[1] << 4) | (cmd[2] >> 4); int32_t temp; temp = (((((temp_raw >> 3) - (dig_T1 << 1))) * dig_T2) >> 11) + ((((((temp_raw >> 4) - dig_T1) * ((temp_raw >> 4) - dig_T1)) >> 12) * dig_T3) >> 14); t_fine = temp; temp = (temp * 5 + 128) >> 8; tempf = (float)temp; return (tempf/100.0f); } float BME_280_getPressure() { uint32_t press_raw; float pressf; char cmd[3]; BME_280_CS = 0; spi.write(0xf7); // press_msb for(int i = 0; i < 3; i++) cmd[i] = spi.write(0); BME_280_CS = 1; press_raw = (cmd[0] << 12) | (cmd[1] << 4) | (cmd[2] >> 4); int32_t var1, var2; uint32_t press; var1 = (t_fine >> 1) - 64000; var2 = (((var1 >> 2) * (var1 >> 2)) >> 11) * dig_P6; var2 = var2 + ((var1 * dig_P5) << 1); var2 = (var2 >> 2) + (dig_P4 << 16); var1 = (((dig_P3 * (((var1 >> 2)*(var1 >> 2)) >> 13)) >> 3) + ((dig_P2 * var1) >> 1)) >> 18; var1 = ((32768 + var1) * dig_P1) >> 15; if (var1 == 0) { return 0; } press = (((1048576 - press_raw) - (var2 >> 12))) * 3125; if(press < 0x80000000) { press = (press << 1) / var1; } else { press = (press / var1) * 2; } var1 = ((int32_t)dig_P9 * ((int32_t)(((press >> 3) * (press >> 3)) >> 13))) >> 12; var2 = (((int32_t)(press >> 2)) * (int32_t)dig_P8) >> 13; press = (press + ((var1 + var2 + dig_P7) >> 4)); pressf = (float)press; return (pressf/100.0f); } float BME_280_getHumidity() { uint32_t hum_raw; float humf; char cmd[2]; BME_280_CS = 0; spi.write(0xfd); // hum_msb for(int i = 0; i < 2; i++) cmd[i] = spi.write(0); BME_280_CS = 1; hum_raw = (cmd[0] << 8) | cmd[1]; int32_t v_x1; v_x1 = t_fine - 76800; v_x1 = (((((hum_raw << 14) -(((int32_t)dig_H4) << 20) - (((int32_t)dig_H5) * v_x1)) + ((int32_t)16384)) >> 15) * (((((((v_x1 * (int32_t)dig_H6) >> 10) * (((v_x1 * ((int32_t)dig_H3)) >> 11) + 32768)) >> 10) + 2097152) * (int32_t)dig_H2 + 8192) >> 14)); v_x1 = (v_x1 - (((((v_x1 >> 15) * (v_x1 >> 15)) >> 7) * (int32_t)dig_H1) >> 4)); v_x1 = (v_x1 < 0 ? 0 : v_x1); v_x1 = (v_x1 > 419430400 ? 419430400 : v_x1); humf = (float)(v_x1 >> 12); return (humf/1024.0f); } //========================================================== void ublox_logging() { //detach the rotary imu mesurement timer1.detach(); processGPS(); processGPS(); processGPS(); bem280_tempreture = BME_280_getTemperature(); bem280_humidity = BME_280_getHumidity(); bem280_pressure = BME_280_getPressure(); //計測時間の取得 measurement_time_g = processing_timer.read(); //気圧、湿度、温度の記録 fprintf(th,"%f,%f,%f,%f\r\n",measurement_time_g,bem280_tempreture,bem280_humidity,bem280_pressure); /* if(logging_status==1){ fprintf(fp, "%f,%f,%f,%f,%f,%f,%f\r\n",measurement_time_g,latitude,longitude,height_float,velN_float,velE_float,velD_float); }else if(logging_status==0){} */ //位置と速度情報を読み取った場合 if((flag_posllh==1)&&(flag_velned==1)){ fprintf(fp, "%f,%f,%f,%f,%f,%f,%f\r\n",measurement_time_g,latitude,longitude,height_float,velN_float,velE_float,velD_float); /*フラグを0に戻す*/ flag_posllh=0; flag_velned=0; }else{} //attach a timer for imu mesurement (400 Hz) timer1.attach(&imu_mesurement, imu_interval); } /*--------------------------------------------*/ int main() { mpu9250.init(1,BITS_DLPF_CFG_188HZ); mpu9250.AK8963_calib_Magnetometer(); CS=1; BME_280_CS=1; //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();//サンプリングモードへの移行 //ログスイッチの電圧出力の設定 log_low = 0; log_high = 1; //ロガーの動作状態を見るためのLED //初期状態ではロガーが記録中の表示をする myled_1 = 1; myled_2 = 0; //UART initialization pc.baud(460800); //460.8 kbps spi.format(8, 0); // data size: 8bit, mode0 spi.frequency(1000000); // 5.5MHz BME_280_initialize(); mkdir("/sd/mydir",0777);//SDファイル作成 fp = fopen("/sd/mydir/gps.csv", "a");//最初のSDopen時間かかるのでwhile外で行う im = fopen("/sd/mydir/imu.csv", "a"); th = fopen("/sd/mydir/thermopile.csv", "a"); if(fp == NULL) { error("Could not open file for write\n"); }else{} pc.printf("FO\r\n");//file open logging_status=1; wait(0.1); //フラグのリセット flag_posllh=0; flag_velned=0; //------------------------------------------- //Timer //------------------------------------------- //timer1: imu mesurement, 400 Hz timer1.attach(&imu_mesurement, imu_interval); //timer2: GNSS mesurement, 5 Hz timer2.attach(&ublox_logging, gnss_interval); processing_timer.start();//timer starts while(1) { //ログスイッチの電圧出力の設定 //log_low = 0; //log_high = 1; //pc.printf("T2D\r\n"); myled_1 = 0; myled_2 = 0; timer2.detach(); if(logging_status==0){ fclose(fp); fclose(im); fclose(th); pc.printf("FC\r\n"); timer2.detach(); timer1.detach(); break; }else if(logging_status==1){} //ロガーの動作状態を見るためのLED //初期状態ではロガーが記録中の表示をする myled_1 = 1; myled_2 = 0; timer2.attach(&ublox_logging, gnss_interval); wait(0.8); // }//while //ロガーの動作状態を見るためのLED //ロガーが記録を終了した表示をする myled_1 = 0; myled_2 = 1; }