Keita Yoshioka / PhaseAccordanceMethod

位相一致法

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?

UserRevisionLine numberNew 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 }