Diff: main.cpp

Revision:

3:d564c0a27ba7

Parent:

2:9216162a9e17

Child:

4:d49256697f27

--- 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