Revision 0:05d61debb1fe, committed 2018-08-20

Comitter:

k0050288

Date:

Mon Aug 20 02:28:15 2018 +0000

Commit message:

?????

Changed in this revision

diff -r 000000000000 -r 05d61debb1fe PhaseMethod.cpp
--- /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

diff -r 000000000000 -r 05d61debb1fe PhaseMethod.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PhaseMethod.h	Mon Aug 20 02:28:15 2018 +0000
@@ -0,0 +1,44 @@
+/*
+** 位相一致法の計算をする
+** 2018/07/04
+*/
+
+#ifndef _PHASEMETHOD_H
+#define _PHASEMETHOD_H
+
+#include "mbed.h"
+#include "Adc.h"
+#include "Thermometer.h"
+
+#define INT_WINDOW    1500                 // 積分窓:1ms分のデータ(AD変換が1.5MHzのため)
+#define REF_VALUE     2500.0               // 積分窓を開始する闘値
+#define M_PI          3.141592653589793    // 円周率
+#define SYNCHRO_DELAY 255.0 / 1000000.0    // Nucleo同士の同期遅れ 255µs (平均 (誤差最大5µs))
+#define SAMPLING 1.0 / (1.5 * 1000000.0)   // サンプリング 1.5MHz を秒に変換
+#define PERIOD   2.0 / 1000.0              // syncPatternの周期
+#define TX_SYNC  1.0 / 1000.0              // 送信側で生成したsyncPatternの中心の時間
+
+class PhaseMethod{
+private:
+    Adc* adc;
+    Thermometer* thermometer;
+
+    double sinc(double x);  // 標本化関数
+    
+public:
+    void init(Adc* adc, Thermometer* thermometer);
+    void selectSync();
+    void calculation();
+    
+    double epoch;
+    double TxTime;         // 伝播時間
+    double arriveTime;     // 到達時間
+    double distance;       // 距離
+    double I1;
+    double I2;
+    double Q1;
+    double Q2;
+    
+    int calAdcVal[INT_WINDOW];   // 位相一致で使う値を格納
+};
+#endif