PhaseMethod.cpp

00001 #include "PhaseMethod.h"
00002 
00003 void PhaseMethod::init(Adc* adc, Thermometer* thermometer)
00004 {
00005     epoch      = 0;
00006     arriveTime = 0;
00007     distance   = 0;
00008     I1 = 0;
00009     I2 = 0;
00010     Q1 = 0;
00011     Q2 = 0;
00012     memset(calAdcVal, 0, sizeof(calAdcVal));
00013 
00014     this->adc          = adc;
00015     this->thermometer = thermometer;
00016 }
00017 
00018 /* 闘値を決め，積分窓の位置を決める */
00019 void PhaseMethod::selectSync()
00020 {
00021     int num    = 0;
00022     int ave    = 0;
00023 
00024     /* 積分窓の開始点を設定 */
00025     // to do
00026     // もし闘値に行かなければ倍率を変えてもう一度AD変換やり直し
00027     for(int i = 0; i < ADC_TIMES; i++) {
00028         if(adc->ADCVal[i] > REF_VALUE) {
00029             num    = i;
00030             break;
00031         } else if(i == (ADC_TIMES - 1)) {
00032             // to do
00033             ;
00034         }
00035     }
00036 
00037     /* 積分窓内の配列を格納 */
00038     for(int i = 0; i < INT_WINDOW; i++) {
00039         calAdcVal[i] = adc->ADCVal[num + i];
00040         ave += calAdcVal[i];
00041     }
00042 
00043     /* 積分窓内の配列を0中心にする */
00044     ave = ave / INT_WINDOW;
00045     for(int i = 0; i < INT_WINDOW; i++) {
00046         calAdcVal[i] = calAdcVal[i] - ave;
00047     }
00048 
00049     /* 闘値までにかかる時間(到達時間) */
00050     TxTime = num * SAMPLING;
00051 }
00052 
00053 
00054 void PhaseMethod::calculation()
00055 {
00056     double intTime = 0.001;       // 積分時間1ms(積分窓の幅：周期2msの為)
00057     double f1 = 39750.0;          // 周波数 39.75 kHz
00058     double f2 = 40250.0;          // 周波数 40.25 kHz
00059     double w1 = 2.0 * M_PI * f1;    // 角加速度
00060     double w2 = 2.0 * M_PI * f2;
00061     long double sin1[INT_WINDOW] = { 0.0 };
00062     long double sin2[INT_WINDOW] = { 0.0 };
00063     long double cos1[INT_WINDOW] = { 0.0 };
00064     long double cos2[INT_WINDOW] = { 0.0 };
00065     double W1, W2, T1, T2, S1, S2, C1, C2, a1, a2;
00066     double soundSpeed  = 331.5f + (0.61f * thermometer->temp);                  // 音速(温度計で温度を計測)[m/s]
00067     double addtionTime = (SAMPLING * (INT_WINDOW / 2.0)) - TX_SYNC - SYNCHRO_DELAY ;  //  + 積分窓中心までの時間 - 生成したsyncPattenの中心時間 - 同期遅れ
00068 
00069     /* 温度を計測 */
00070     thermometer->read();
00071 
00072     /* 積分窓で波形を切り取る */
00073     selectSync();
00074 
00075 
00076     /* 積分窓で切り取った波形にIQ検波 */
00077     /*
00078     for(int i = 0; i < INT_WINDOW; i++){
00079         sin1[i] = sin(w1 * i * intTime / INT_WINDOW - w1 * intTime / 2.0);
00080         sin2[i] = sin(w2 * i * intTime / INT_WINDOW - w2 * intTime / 2.0);
00081         cos1[i] = cos(w1 * i * intTime / INT_WINDOW - w1 * intTime / 2.0);
00082         cos2[i] = cos(w2 * i * intTime / INT_WINDOW - w2 * intTime / 2.0);
00083         */
00084     /* 実数部と虚数部に分ける */
00085     /*
00086     Q1 += calAdcVal[i] * sin1[i] / INT_WINDOW;
00087     Q2 += calAdcVal[i] * sin2[i] / INT_WINDOW;
00088     I1 += calAdcVal[i] * cos1[i] / INT_WINDOW;
00089     I2 += calAdcVal[i] * cos2[i] / INT_WINDOW;
00090     }
00091     */
00092 
00093     /* 方程式を解く */
00094     /*
00095     W1 = sinc(w1 * intTime * M_PI);
00096     W2 = sinc(w2 * intTime * M_PI);
00097     T1 = sinc((w1 - w2) * intTime / 2.0 * M_PI);
00098     T2 = sinc((w1 + w2) * intTime / 2.0 * M_PI);
00099     */
00100     for(int i = 0; i < INT_WINDOW; i++) {
00101         sin1[i] = sin(w1 * i * intTime / double(INT_WINDOW) - w1 * intTime / 2.0);
00102         sin2[i] = sin(w2 * i * intTime / double(INT_WINDOW) - w2 * intTime / 2.0);
00103         cos1[i] = cos(w1 * i * intTime / double(INT_WINDOW) - w1 * intTime / 2.0);
00104         cos2[i] = cos(w2 * i * intTime / double(INT_WINDOW) - w2 * intTime / 2.0);
00105 
00106         /* 実数部と虚数部に分ける */
00107         Q1 += double(calAdcVal[i] * sin1[i]) / double(INT_WINDOW);
00108         Q2 += double(calAdcVal[i] * sin2[i]) / double(INT_WINDOW);
00109         I1 += double(calAdcVal[i] * cos1[i]) / double(INT_WINDOW);
00110         I2 += double(calAdcVal[i] * cos2[i]) / double(INT_WINDOW);
00111     }
00112 
00113     W1 = sinc(w1 * intTime * M_PI);
00114     W2 = sinc(w2 * intTime * M_PI);
00115     T1 = sinc((w1 - w2) * intTime / 2.0 * M_PI);
00116     T2 = sinc((w1 + w2) * intTime / 2.0 * M_PI);
00117 
00118     S1 = (2.0 * I1 - 2.0 * I2 * (T1  + T2)  / (1.0 + W2))  / (1.0 + W1  - (T1  + T2)  * (T1  + T2)  / (1.0 + W2));
00119     S2 = (2.0 * I1 - 2.0 * I2 * (1.0 + W1)  / (T1  + T2))  / (T1  + T2  - (1.0 + W1)  * (1.0 + W2)  / (T1  + T2));
00120     C1 = (2.0 * Q1 - 2.0 * Q2 * (T1  - T2)  / (W2  - 1.0)) / (W1  + 1.0 + (T1  - T2)  * (T1  - T2)  / (1.0 - W2));
00121     C2 = (2.0 * Q1 - 2.0 * Q2 * (W1  + 1.0) / (T1  - T2))  / (T2  - T1  + (W1  + 1.0) * (W2  - 1.0) / (T2  - T1));
00122     a1 = sqrt(S1 * S1 + C1 * C1);
00123     a2 = sqrt(S2 * S2 + C2 * C2);
00124 
00125 
00126     /* 到達時間 */
00127     epoch = - (asin((S1 * C2 - C1 * S2) / (a1 * a2))) / (w1 - w2);
00128     arriveTime = epoch + TxTime + addtionTime;          // epoch + 伝播時間 + おまけ時間
00129 
00130     /* 距離[mm] */
00131     distance = soundSpeed * arriveTime * 1000.0;
00132 }
00133 
00134 double PhaseMethod::sinc(double x)
00135 {
00136     return sin(x) / x;
00137 }