Diff: PhaseMethod.cpp

Revision:

0:05d61debb1fe

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PhaseMethod.cpp	Mon Aug 20 02:28:15 2018 +0000
@@ -0,0 +1,137 @@
+#include "PhaseMethod.h"
+
+void PhaseMethod::init(Adc* adc, Thermometer* thermometer)
+{
+    epoch      = 0;
+    arriveTime = 0;
+    distance   = 0;
+    I1 = 0;
+    I2 = 0;
+    Q1 = 0;
+    Q2 = 0;
+    memset(calAdcVal, 0, sizeof(calAdcVal));
+
+    this->adc          = adc;
+    this->thermometer = thermometer;
+}
+
+/* 闘値を決め，積分窓の位置を決める */
+void PhaseMethod::selectSync()
+{
+    int num    = 0;
+    int ave    = 0;
+
+    /* 積分窓の開始点を設定 */
+    // to do
+    // もし闘値に行かなければ倍率を変えてもう一度AD変換やり直し
+    for(int i = 0; i < ADC_TIMES; i++) {
+        if(adc->ADCVal[i] > REF_VALUE) {
+            num    = i;
+            break;
+        } else if(i == (ADC_TIMES - 1)) {
+            // to do
+            ;
+        }
+    }
+
+    /* 積分窓内の配列を格納 */
+    for(int i = 0; i < INT_WINDOW; i++) {
+        calAdcVal[i] = adc->ADCVal[num + i];
+        ave += calAdcVal[i];
+    }
+
+    /* 積分窓内の配列を0中心にする */
+    ave = ave / INT_WINDOW;
+    for(int i = 0; i < INT_WINDOW; i++) {
+        calAdcVal[i] = calAdcVal[i] - ave;
+    }
+
+    /* 闘値までにかかる時間(到達時間) */
+    TxTime = num * SAMPLING;
+}
+
+
+void PhaseMethod::calculation()
+{
+    double intTime = 0.001;       // 積分時間1ms(積分窓の幅：周期2msの為)
+    double f1 = 39750.0;          // 周波数 39.75 kHz
+    double f2 = 40250.0;          // 周波数 40.25 kHz
+    double w1 = 2.0 * M_PI * f1;    // 角加速度
+    double w2 = 2.0 * M_PI * f2;
+    long double sin1[INT_WINDOW] = { 0.0 };
+    long double sin2[INT_WINDOW] = { 0.0 };
+    long double cos1[INT_WINDOW] = { 0.0 };
+    long double cos2[INT_WINDOW] = { 0.0 };
+    double W1, W2, T1, T2, S1, S2, C1, C2, a1, a2;
+    double soundSpeed  = 331.5f + (0.61f * thermometer->temp);                  // 音速(温度計で温度を計測)[m/s]
+    double addtionTime = (SAMPLING * (INT_WINDOW / 2.0)) - TX_SYNC - SYNCHRO_DELAY ;  //  + 積分窓中心までの時間 - 生成したsyncPattenの中心時間 - 同期遅れ
+
+    /* 温度を計測 */
+    thermometer->read();
+
+    /* 積分窓で波形を切り取る */
+    selectSync();
+
+
+    /* 積分窓で切り取った波形にIQ検波 */
+    /*
+    for(int i = 0; i < INT_WINDOW; i++){
+        sin1[i] = sin(w1 * i * intTime / INT_WINDOW - w1 * intTime / 2.0);
+        sin2[i] = sin(w2 * i * intTime / INT_WINDOW - w2 * intTime / 2.0);
+        cos1[i] = cos(w1 * i * intTime / INT_WINDOW - w1 * intTime / 2.0);
+        cos2[i] = cos(w2 * i * intTime / INT_WINDOW - w2 * intTime / 2.0);
+        */
+    /* 実数部と虚数部に分ける */
+    /*
+    Q1 += calAdcVal[i] * sin1[i] / INT_WINDOW;
+    Q2 += calAdcVal[i] * sin2[i] / INT_WINDOW;
+    I1 += calAdcVal[i] * cos1[i] / INT_WINDOW;
+    I2 += calAdcVal[i] * cos2[i] / INT_WINDOW;
+    }
+    */
+
+    /* 方程式を解く */
+    /*
+    W1 = sinc(w1 * intTime * M_PI);
+    W2 = sinc(w2 * intTime * M_PI);
+    T1 = sinc((w1 - w2) * intTime / 2.0 * M_PI);
+    T2 = sinc((w1 + w2) * intTime / 2.0 * M_PI);
+    */
+    for(int i = 0; i < INT_WINDOW; i++) {
+        sin1[i] = sin(w1 * i * intTime / double(INT_WINDOW) - w1 * intTime / 2.0);
+        sin2[i] = sin(w2 * i * intTime / double(INT_WINDOW) - w2 * intTime / 2.0);
+        cos1[i] = cos(w1 * i * intTime / double(INT_WINDOW) - w1 * intTime / 2.0);
+        cos2[i] = cos(w2 * i * intTime / double(INT_WINDOW) - w2 * intTime / 2.0);
+
+        /* 実数部と虚数部に分ける */
+        Q1 += double(calAdcVal[i] * sin1[i]) / double(INT_WINDOW);
+        Q2 += double(calAdcVal[i] * sin2[i]) / double(INT_WINDOW);
+        I1 += double(calAdcVal[i] * cos1[i]) / double(INT_WINDOW);
+        I2 += double(calAdcVal[i] * cos2[i]) / double(INT_WINDOW);
+    }
+
+    W1 = sinc(w1 * intTime * M_PI);
+    W2 = sinc(w2 * intTime * M_PI);
+    T1 = sinc((w1 - w2) * intTime / 2.0 * M_PI);
+    T2 = sinc((w1 + w2) * intTime / 2.0 * M_PI);
+
+    S1 = (2.0 * I1 - 2.0 * I2 * (T1  + T2)  / (1.0 + W2))  / (1.0 + W1  - (T1  + T2)  * (T1  + T2)  / (1.0 + W2));
+    S2 = (2.0 * I1 - 2.0 * I2 * (1.0 + W1)  / (T1  + T2))  / (T1  + T2  - (1.0 + W1)  * (1.0 + W2)  / (T1  + T2));
+    C1 = (2.0 * Q1 - 2.0 * Q2 * (T1  - T2)  / (W2  - 1.0)) / (W1  + 1.0 + (T1  - T2)  * (T1  - T2)  / (1.0 - W2));
+    C2 = (2.0 * Q1 - 2.0 * Q2 * (W1  + 1.0) / (T1  - T2))  / (T2  - T1  + (W1  + 1.0) * (W2  - 1.0) / (T2  - T1));
+    a1 = sqrt(S1 * S1 + C1 * C1);
+    a2 = sqrt(S2 * S2 + C2 * C2);
+
+
+    /* 到達時間 */
+    epoch = - (asin((S1 * C2 - C1 * S2) / (a1 * a2))) / (w1 - w2);
+    arriveTime = epoch + TxTime + addtionTime;          // epoch + 伝播時間 + おまけ時間
+
+    /* 距離[mm] */
+    distance = soundSpeed * arriveTime * 1000.0;
+}
+
+double PhaseMethod::sinc(double x)
+{
+    return sin(x) / x;
+}
\ No newline at end of file