imu/gnss logger ver1.1
Dependencies: mbed MPU9250_SPI MG354PDH0 SDFileSystem
Diff: main.cpp
- Revision:
- 3:d564c0a27ba7
- Parent:
- 2:9216162a9e17
- Child:
- 4:d49256697f27
diff -r 9216162a9e17 -r d564c0a27ba7 main.cpp --- a/main.cpp Sat May 15 08:14:50 2021 +0000 +++ b/main.cpp Sun Aug 29 02:07:08 2021 +0000 @@ -10,11 +10,23 @@ //========================================================= //Port Setting Serial pc(USBTX, USBRX); // tx, rx -Serial epson_imu(p28, p27); // IMU通信用シリアルポート +Serial epson_imu(p9, p10); // IMU通信用シリアルポート SPI spi(p11, p12, p13); // mosi, miso, sclk -DigitalOut CS(p15); // NEO-7MのCSピン -DigitalIn log_switch(p16); // +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 @@ -22,6 +34,7 @@ //ファイルポインタ FILE *fp; FILE *im; +FILE *th; //========================================================= //IMUの変数 @@ -29,12 +42,32 @@ 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; @@ -316,11 +349,188 @@ if(logging_status==1){ fprintf(im,"%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]); - pc.printf("IL\r\n");//imu logging + //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 @@ -330,16 +540,29 @@ processGPS(); processGPS(); + + bem280_tempreture = BME_280_getTemperature(); + bem280_humidity = BME_280_getHumidity(); + bem280_pressure = BME_280_getPressure(); + + thermopile_voltage_1 = 3.3*(thermopile_input_1.read()); + thermopile_voltage_2 = 3.3*(thermopile_input_2.read()); + //計測時間の取得 measurement_time_g = processing_timer.read(); + //サーモパイルセンサと気圧、湿度、温度の記録 + fprintf(th,"%f,%f,%f,%f,%f,%f\r\n",measurement_time_g,bem280_tempreture,bem280_humidity,bem280_pressure,thermopile_voltage_1,thermopile_voltage_2); + + /* 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; @@ -351,9 +574,13 @@ } + /*--------------------------------------------*/ int main() { + CS=1; + BME_280_CS=1; + //power on wait 800ms form IMU wait(1.0); @@ -363,14 +590,28 @@ 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.frequency(5500000); + + 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"); @@ -396,20 +637,41 @@ processing_timer.start();//timer starts while(1) { - pc.printf("T2D\r\n"); + //ログスイッチの電圧出力の設定 + //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; } \ No newline at end of file