位相一致法
PhaseMethod.cpp@0:05d61debb1fe, 2018-08-20 (annotated)
- Committer:
- k0050288
- Date:
- Mon Aug 20 02:28:15 2018 +0000
- Revision:
- 0:05d61debb1fe
?????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
k0050288 | 0:05d61debb1fe | 1 | #include "PhaseMethod.h" |
k0050288 | 0:05d61debb1fe | 2 | |
k0050288 | 0:05d61debb1fe | 3 | void PhaseMethod::init(Adc* adc, Thermometer* thermometer) |
k0050288 | 0:05d61debb1fe | 4 | { |
k0050288 | 0:05d61debb1fe | 5 | epoch = 0; |
k0050288 | 0:05d61debb1fe | 6 | arriveTime = 0; |
k0050288 | 0:05d61debb1fe | 7 | distance = 0; |
k0050288 | 0:05d61debb1fe | 8 | I1 = 0; |
k0050288 | 0:05d61debb1fe | 9 | I2 = 0; |
k0050288 | 0:05d61debb1fe | 10 | Q1 = 0; |
k0050288 | 0:05d61debb1fe | 11 | Q2 = 0; |
k0050288 | 0:05d61debb1fe | 12 | memset(calAdcVal, 0, sizeof(calAdcVal)); |
k0050288 | 0:05d61debb1fe | 13 | |
k0050288 | 0:05d61debb1fe | 14 | this->adc = adc; |
k0050288 | 0:05d61debb1fe | 15 | this->thermometer = thermometer; |
k0050288 | 0:05d61debb1fe | 16 | } |
k0050288 | 0:05d61debb1fe | 17 | |
k0050288 | 0:05d61debb1fe | 18 | /* 闘値を決め,積分窓の位置を決める */ |
k0050288 | 0:05d61debb1fe | 19 | void PhaseMethod::selectSync() |
k0050288 | 0:05d61debb1fe | 20 | { |
k0050288 | 0:05d61debb1fe | 21 | int num = 0; |
k0050288 | 0:05d61debb1fe | 22 | int ave = 0; |
k0050288 | 0:05d61debb1fe | 23 | |
k0050288 | 0:05d61debb1fe | 24 | /* 積分窓の開始点を設定 */ |
k0050288 | 0:05d61debb1fe | 25 | // to do |
k0050288 | 0:05d61debb1fe | 26 | // もし闘値に行かなければ倍率を変えてもう一度AD変換やり直し |
k0050288 | 0:05d61debb1fe | 27 | for(int i = 0; i < ADC_TIMES; i++) { |
k0050288 | 0:05d61debb1fe | 28 | if(adc->ADCVal[i] > REF_VALUE) { |
k0050288 | 0:05d61debb1fe | 29 | num = i; |
k0050288 | 0:05d61debb1fe | 30 | break; |
k0050288 | 0:05d61debb1fe | 31 | } else if(i == (ADC_TIMES - 1)) { |
k0050288 | 0:05d61debb1fe | 32 | // to do |
k0050288 | 0:05d61debb1fe | 33 | ; |
k0050288 | 0:05d61debb1fe | 34 | } |
k0050288 | 0:05d61debb1fe | 35 | } |
k0050288 | 0:05d61debb1fe | 36 | |
k0050288 | 0:05d61debb1fe | 37 | /* 積分窓内の配列を格納 */ |
k0050288 | 0:05d61debb1fe | 38 | for(int i = 0; i < INT_WINDOW; i++) { |
k0050288 | 0:05d61debb1fe | 39 | calAdcVal[i] = adc->ADCVal[num + i]; |
k0050288 | 0:05d61debb1fe | 40 | ave += calAdcVal[i]; |
k0050288 | 0:05d61debb1fe | 41 | } |
k0050288 | 0:05d61debb1fe | 42 | |
k0050288 | 0:05d61debb1fe | 43 | /* 積分窓内の配列を0中心にする */ |
k0050288 | 0:05d61debb1fe | 44 | ave = ave / INT_WINDOW; |
k0050288 | 0:05d61debb1fe | 45 | for(int i = 0; i < INT_WINDOW; i++) { |
k0050288 | 0:05d61debb1fe | 46 | calAdcVal[i] = calAdcVal[i] - ave; |
k0050288 | 0:05d61debb1fe | 47 | } |
k0050288 | 0:05d61debb1fe | 48 | |
k0050288 | 0:05d61debb1fe | 49 | /* 闘値までにかかる時間(到達時間) */ |
k0050288 | 0:05d61debb1fe | 50 | TxTime = num * SAMPLING; |
k0050288 | 0:05d61debb1fe | 51 | } |
k0050288 | 0:05d61debb1fe | 52 | |
k0050288 | 0:05d61debb1fe | 53 | |
k0050288 | 0:05d61debb1fe | 54 | void PhaseMethod::calculation() |
k0050288 | 0:05d61debb1fe | 55 | { |
k0050288 | 0:05d61debb1fe | 56 | double intTime = 0.001; // 積分時間1ms(積分窓の幅:周期2msの為) |
k0050288 | 0:05d61debb1fe | 57 | double f1 = 39750.0; // 周波数 39.75 kHz |
k0050288 | 0:05d61debb1fe | 58 | double f2 = 40250.0; // 周波数 40.25 kHz |
k0050288 | 0:05d61debb1fe | 59 | double w1 = 2.0 * M_PI * f1; // 角加速度 |
k0050288 | 0:05d61debb1fe | 60 | double w2 = 2.0 * M_PI * f2; |
k0050288 | 0:05d61debb1fe | 61 | long double sin1[INT_WINDOW] = { 0.0 }; |
k0050288 | 0:05d61debb1fe | 62 | long double sin2[INT_WINDOW] = { 0.0 }; |
k0050288 | 0:05d61debb1fe | 63 | long double cos1[INT_WINDOW] = { 0.0 }; |
k0050288 | 0:05d61debb1fe | 64 | long double cos2[INT_WINDOW] = { 0.0 }; |
k0050288 | 0:05d61debb1fe | 65 | double W1, W2, T1, T2, S1, S2, C1, C2, a1, a2; |
k0050288 | 0:05d61debb1fe | 66 | double soundSpeed = 331.5f + (0.61f * thermometer->temp); // 音速(温度計で温度を計測)[m/s] |
k0050288 | 0:05d61debb1fe | 67 | double addtionTime = (SAMPLING * (INT_WINDOW / 2.0)) - TX_SYNC - SYNCHRO_DELAY ; // + 積分窓中心までの時間 - 生成したsyncPattenの中心時間 - 同期遅れ |
k0050288 | 0:05d61debb1fe | 68 | |
k0050288 | 0:05d61debb1fe | 69 | /* 温度を計測 */ |
k0050288 | 0:05d61debb1fe | 70 | thermometer->read(); |
k0050288 | 0:05d61debb1fe | 71 | |
k0050288 | 0:05d61debb1fe | 72 | /* 積分窓で波形を切り取る */ |
k0050288 | 0:05d61debb1fe | 73 | selectSync(); |
k0050288 | 0:05d61debb1fe | 74 | |
k0050288 | 0:05d61debb1fe | 75 | |
k0050288 | 0:05d61debb1fe | 76 | /* 積分窓で切り取った波形にIQ検波 */ |
k0050288 | 0:05d61debb1fe | 77 | /* |
k0050288 | 0:05d61debb1fe | 78 | for(int i = 0; i < INT_WINDOW; i++){ |
k0050288 | 0:05d61debb1fe | 79 | sin1[i] = sin(w1 * i * intTime / INT_WINDOW - w1 * intTime / 2.0); |
k0050288 | 0:05d61debb1fe | 80 | sin2[i] = sin(w2 * i * intTime / INT_WINDOW - w2 * intTime / 2.0); |
k0050288 | 0:05d61debb1fe | 81 | cos1[i] = cos(w1 * i * intTime / INT_WINDOW - w1 * intTime / 2.0); |
k0050288 | 0:05d61debb1fe | 82 | cos2[i] = cos(w2 * i * intTime / INT_WINDOW - w2 * intTime / 2.0); |
k0050288 | 0:05d61debb1fe | 83 | */ |
k0050288 | 0:05d61debb1fe | 84 | /* 実数部と虚数部に分ける */ |
k0050288 | 0:05d61debb1fe | 85 | /* |
k0050288 | 0:05d61debb1fe | 86 | Q1 += calAdcVal[i] * sin1[i] / INT_WINDOW; |
k0050288 | 0:05d61debb1fe | 87 | Q2 += calAdcVal[i] * sin2[i] / INT_WINDOW; |
k0050288 | 0:05d61debb1fe | 88 | I1 += calAdcVal[i] * cos1[i] / INT_WINDOW; |
k0050288 | 0:05d61debb1fe | 89 | I2 += calAdcVal[i] * cos2[i] / INT_WINDOW; |
k0050288 | 0:05d61debb1fe | 90 | } |
k0050288 | 0:05d61debb1fe | 91 | */ |
k0050288 | 0:05d61debb1fe | 92 | |
k0050288 | 0:05d61debb1fe | 93 | /* 方程式を解く */ |
k0050288 | 0:05d61debb1fe | 94 | /* |
k0050288 | 0:05d61debb1fe | 95 | W1 = sinc(w1 * intTime * M_PI); |
k0050288 | 0:05d61debb1fe | 96 | W2 = sinc(w2 * intTime * M_PI); |
k0050288 | 0:05d61debb1fe | 97 | T1 = sinc((w1 - w2) * intTime / 2.0 * M_PI); |
k0050288 | 0:05d61debb1fe | 98 | T2 = sinc((w1 + w2) * intTime / 2.0 * M_PI); |
k0050288 | 0:05d61debb1fe | 99 | */ |
k0050288 | 0:05d61debb1fe | 100 | for(int i = 0; i < INT_WINDOW; i++) { |
k0050288 | 0:05d61debb1fe | 101 | sin1[i] = sin(w1 * i * intTime / double(INT_WINDOW) - w1 * intTime / 2.0); |
k0050288 | 0:05d61debb1fe | 102 | sin2[i] = sin(w2 * i * intTime / double(INT_WINDOW) - w2 * intTime / 2.0); |
k0050288 | 0:05d61debb1fe | 103 | cos1[i] = cos(w1 * i * intTime / double(INT_WINDOW) - w1 * intTime / 2.0); |
k0050288 | 0:05d61debb1fe | 104 | cos2[i] = cos(w2 * i * intTime / double(INT_WINDOW) - w2 * intTime / 2.0); |
k0050288 | 0:05d61debb1fe | 105 | |
k0050288 | 0:05d61debb1fe | 106 | /* 実数部と虚数部に分ける */ |
k0050288 | 0:05d61debb1fe | 107 | Q1 += double(calAdcVal[i] * sin1[i]) / double(INT_WINDOW); |
k0050288 | 0:05d61debb1fe | 108 | Q2 += double(calAdcVal[i] * sin2[i]) / double(INT_WINDOW); |
k0050288 | 0:05d61debb1fe | 109 | I1 += double(calAdcVal[i] * cos1[i]) / double(INT_WINDOW); |
k0050288 | 0:05d61debb1fe | 110 | I2 += double(calAdcVal[i] * cos2[i]) / double(INT_WINDOW); |
k0050288 | 0:05d61debb1fe | 111 | } |
k0050288 | 0:05d61debb1fe | 112 | |
k0050288 | 0:05d61debb1fe | 113 | W1 = sinc(w1 * intTime * M_PI); |
k0050288 | 0:05d61debb1fe | 114 | W2 = sinc(w2 * intTime * M_PI); |
k0050288 | 0:05d61debb1fe | 115 | T1 = sinc((w1 - w2) * intTime / 2.0 * M_PI); |
k0050288 | 0:05d61debb1fe | 116 | T2 = sinc((w1 + w2) * intTime / 2.0 * M_PI); |
k0050288 | 0:05d61debb1fe | 117 | |
k0050288 | 0:05d61debb1fe | 118 | S1 = (2.0 * I1 - 2.0 * I2 * (T1 + T2) / (1.0 + W2)) / (1.0 + W1 - (T1 + T2) * (T1 + T2) / (1.0 + W2)); |
k0050288 | 0:05d61debb1fe | 119 | S2 = (2.0 * I1 - 2.0 * I2 * (1.0 + W1) / (T1 + T2)) / (T1 + T2 - (1.0 + W1) * (1.0 + W2) / (T1 + T2)); |
k0050288 | 0:05d61debb1fe | 120 | C1 = (2.0 * Q1 - 2.0 * Q2 * (T1 - T2) / (W2 - 1.0)) / (W1 + 1.0 + (T1 - T2) * (T1 - T2) / (1.0 - W2)); |
k0050288 | 0:05d61debb1fe | 121 | C2 = (2.0 * Q1 - 2.0 * Q2 * (W1 + 1.0) / (T1 - T2)) / (T2 - T1 + (W1 + 1.0) * (W2 - 1.0) / (T2 - T1)); |
k0050288 | 0:05d61debb1fe | 122 | a1 = sqrt(S1 * S1 + C1 * C1); |
k0050288 | 0:05d61debb1fe | 123 | a2 = sqrt(S2 * S2 + C2 * C2); |
k0050288 | 0:05d61debb1fe | 124 | |
k0050288 | 0:05d61debb1fe | 125 | |
k0050288 | 0:05d61debb1fe | 126 | /* 到達時間 */ |
k0050288 | 0:05d61debb1fe | 127 | epoch = - (asin((S1 * C2 - C1 * S2) / (a1 * a2))) / (w1 - w2); |
k0050288 | 0:05d61debb1fe | 128 | arriveTime = epoch + TxTime + addtionTime; // epoch + 伝播時間 + おまけ時間 |
k0050288 | 0:05d61debb1fe | 129 | |
k0050288 | 0:05d61debb1fe | 130 | /* 距離[mm] */ |
k0050288 | 0:05d61debb1fe | 131 | distance = soundSpeed * arriveTime * 1000.0; |
k0050288 | 0:05d61debb1fe | 132 | } |
k0050288 | 0:05d61debb1fe | 133 | |
k0050288 | 0:05d61debb1fe | 134 | double PhaseMethod::sinc(double x) |
k0050288 | 0:05d61debb1fe | 135 | { |
k0050288 | 0:05d61debb1fe | 136 | return sin(x) / x; |
k0050288 | 0:05d61debb1fe | 137 | } |