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;
}