位相一致法
PhaseMethod.cpp
- Committer:
- k0050288
- Date:
- 2018-08-20
- Revision:
- 0:05d61debb1fe
File content as of revision 0:05d61debb1fe:
#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; }