GP22

uint8_t   Init =                0x70;
uint8_t   Power_On_Reset =      0x50;
uint8_t   Start_TOF =           0x01;
uint8_t   Start_Temp =          0x02;
uint8_t   Start_Cal_Resonator = 0x03;
uint8_t   Start_Cal_TDC =       0x04;
uint8_t   Start_TOF_Restart =   0x05;
uint8_t   Start_Temp_Restart =  0x06;

float     CLKHS_freq = 4.000;           // Clock frequency in MHz
float     CLKHS_freq_cal = 4.000;       // Calibrated Clock frequency in MHz
float     CLKHS_freq_corr_fact = 1.000; // Correction factor for Clock frequency
float     CLKHS_read;


float     Result_0_up;
float     Result_1_up;
float     Result_2_up;

float     Result_0_down;
float     Result_1_down;
float     Result_2_down;

float     average_Result_up;
float     average_Result_up2;
float     average_Result_down;

float     Time_of_flight_diff, Time_of_flight_sum;

float     PW1ST;
float     TOF_up;
float     TOF_down;

float     TOF_diff_avg=0, TOF_diff_sum=0,TOF_sum_avg=0, TOF_sum_sum=0;

float     TOF_diff_square_sum=0, Std_Dev_of_Diff=0;

uint16_t  no_of_avg = 10;
uint16_t  sum_counter = 1; //sum_counter starts with 1

uint8_t   Error_Bit = 0;
uint16_t  status_byte_UP, status_byte_DOWN;

float     distance = 0;
uint16_t  STAT_REG;


void GP22_TOF_Test2(void)
{
    unsigned int ticks;

    gp22_send_1byte(Power_On_Reset);      // Power on Reset to GP22
    Dly100us((void*)5);              // 500 us wait for GP22

    // Writing to the configuration registers

//      gp22_wr_config_reg(0x80, 0xE30BE800); // CR0 according Frontpanel Software Start options (START_CLKHS)=480us
    gp22_wr_config_reg(0x80, 0x430BE800); // CR0 0xE303E800 //Osc ON/OFF //(START_CLKHS)=Osc continously ON
    gp22_wr_config_reg(0x81, 0x21444000); // CR1 0x21444512
    gp22_wr_config_reg(0x82, 0xA0138800); // CR2 0x28138800
    gp22_wr_config_reg(0x83, 0xD0A24800); // CR3 En_AutoCalc_MB2
//      gp22_wr_config_reg(0x83, 0x50710100); // CR3 according Frontpanel Software
    gp22_wr_config_reg(0x84, 0x10004000); // CR4 0x10014A00
    //gp22_wr_config_reg(0x85, 0x40000000); // CR5 Fire_up
    //gp22_wr_config_reg(0x85, 0x20000000); // CR5 Fire_down
    gp22_wr_config_reg(0x86, 0xC0C06100); // CR6 0x40006000

//    Std_Dev_of_Diff = 0;
//    TOF_diff_sum = 0;
//    TOF_diff_square_sum = 0;
//    sum_counter = 1;
//    PW1ST = 0;

    //calibrate
    CLKHS_freq_cal = 4.000;
    gp22_send_1byte(Init);
    gp22_send_1byte(Start_Cal_Resonator);
    ticks = HAL_GetTick();
    while ((GET_DIO0() == GPIO_PIN_SET))// && (timeout_counter != 0))
    {
        if (HAL_GetTick() > (ticks + 200)) return;
    }
    CLKHS_read = read_n_bytes_2(4, 0xB0, 0x00, 16);
    CLKHS_freq_corr_fact = 61.035/CLKHS_read * CLKHS_freq;

    CLKHS_freq_cal *= CLKHS_freq_corr_fact;


    while (1)
    {
        //---------------- Rewriting Config reg 1 + 5  --------------------
        gp22_wr_config_reg(0x85, 0x40000000); // CR5 4=Fire_up
//      gp22_wr_config_reg(0x83, 0xD0710100);
        gp22_wr_config_reg(0x81, 0x21444000); // Config reg 1 0x21444512
        gp22_send_1byte(Init);
        gp22_send_1byte(Start_TOF_Restart);
        // minimum delay = 0.75ms
        Error_Bit = 0;
        // Wait for INT Slot_x
        ticks = HAL_GetTick();
        while ((GET_DIO0() == GPIO_PIN_SET))// && (timeout_counter != 0))
        {
            if (HAL_GetTick() > (ticks + 200)) return;
        }

        Error_Bit += gp22_status_count_error();

        Result_0_up = read_n_bytes_2(4, 0xB0, 0x00, 16)/(CLKHS_freq_cal*1000000.0);
        Result_1_up = read_n_bytes_2(4, 0xB0, 0x01, 16)/(CLKHS_freq_cal*1000000.0);
        Result_2_up = read_n_bytes_2(4, 0xB0, 0x02, 16)/(CLKHS_freq_cal*1000000.0);

        average_Result_up = read_n_bytes_2(4, 0xB0, 0x03, 16)/CLKHS_freq_cal*1000000.0;

        //---------------- Rewriting Config reg 1 + 5 --------------------
//        gp22_wr_config_reg(0x85, 0x20000000); // CR5 2=Fire_down
//        gp22_wr_config_reg(0x81, 0x21444000); // Config reg 1 0x21444512
        gp22_send_1byte(Init);
        // minimum delay = 0.65ms

        // Wait for INT Slot_x
        ticks = HAL_GetTick();
        while ((GET_DIO0() == GPIO_PIN_SET))// && (timeout_counter != 0))
        {
            if (HAL_GetTick() > (ticks + 200))
                return;
        }

        Error_Bit += gp22_status_count_error();
        Result_0_down = read_n_bytes_2(4, 0xB0, 0x00, 16)/(CLKHS_freq_cal*1000000.0);
        Result_1_down = read_n_bytes_2(4, 0xB0, 0x01, 16)/(CLKHS_freq_cal*1000000.0);
        Result_2_down = read_n_bytes_2(4, 0xB0, 0x02, 16)/(CLKHS_freq_cal*1000000.0);
        average_Result_down = read_n_bytes_2(4, 0xB0, 0x03, 16)/(CLKHS_freq_cal*1000000.0);
        char str3[256];
        memset(str3, 0, sizeof(str3));

        sprintf(str3, "\nRes0 %1.10\nRes1 %10.10\nRes2 %1.10 ",Result_0_down, Result_1_down, Result_2_down);
        USB_Transmit_Data((unsigned char *) &str3, strlen(str3));
//      char str3[256];
//      memset(str3, 0, sizeof(str3));
//
//      sprintf(str3, "\n CLKHS_freq_corr_fact = %1.6f; 61.035 /CLKHS_read= %1.6f;  * CLKHS_freq= %1.10f; \n CLKHS_freq_cal %1.6f",CLKHS_freq_corr_fact,CLKHS_read, CLKHS_freq ,CLKHS_freq_cal,Result_0_down, Result_1_down, Result_2_down);
//      USB_Transmit_Data((unsigned char *) &str3, strlen(str3));
        //----------------------------------------------------------------
        // Result after two measurements (first Result_down then Result_up)
      if (Error_Bit > 0) {
          char error_msg[256] = {0}; // Puffer initialisieren
          STAT_REG = GP22_read_2byte_status(); // Direkter Status-Lesevorgang

          // Fehlerbits sammeln
          char errors[128] = "";
          if (STAT_REG & 0x0200) strcat(errors, "Timeout_TDC|");
          if (STAT_REG & 0x0400) strcat(errors, "Timeout_Precounter|");
          if (STAT_REG & 0x0800) strcat(errors, "Error_open|");
          if (STAT_REG & 0x1000) strcat(errors, "Error_short|");
          if (STAT_REG & 0x2000) strcat(errors, "EEPROM_eq_CREG|");
          if (STAT_REG & 0x4000) strcat(errors, "EEPROM_DED|");
          if (STAT_REG & 0x8000) strcat(errors, "EEPROM_error|");

          // Letztes '|' entfernen
          if (strlen(errors) > 0) errors[strlen(errors)-1] = '\0';

          // Komplette Meldung generieren
          snprintf(error_msg, sizeof(error_msg),
              "\nTOF_Messung skipped!\n"
              "Fehler: %s\n"
              "CLK-Faktor: %.3f\n"
              "Status-Reg: 0x%04X",
              errors, CLKHS_freq_corr_fact, STAT_REG);

          USB_Transmit_Data((uint8_t*)error_msg, strlen(error_msg));
        }else
        {

        average_Result_up /= 3;
        average_Result_down /= 3;

        // Zeit in ns
        Result_0_up *= 1000;
        Result_1_up *= 1000;
        Result_2_up *= 1000;
        average_Result_up *= 1000;

        Result_0_down *= 1000;
        Result_1_down *= 1000;
        Result_2_down *= 1000;
        average_Result_down *= 1000;

        average_Result_up2= Result_0_up+Result_1_up+Result_2_up;
        TOF_up = average_Result_up;
        TOF_down = average_Result_down;
        PW1ST += read_n_bytes_2(1, 0xB0, 0x08, 7);


    //    printf("\n Result Up= %6.3f ns / Down= %6.3f ns",TOF_up, TOF_down);
        Time_of_flight_diff = TOF_up - TOF_down;
        //printf("\n Difference Up-Down = %6.3f ns", Time_of_flight_diff);

        Time_of_flight_sum = TOF_up + TOF_down;

        // to add up
        TOF_diff_sum += Time_of_flight_diff;
        TOF_diff_square_sum = TOF_diff_square_sum + (Time_of_flight_diff * Time_of_flight_diff);
        TOF_sum_sum += Time_of_flight_sum;


        char str1[256];
        sprintf(str1, "\nUP=%6.3f ns Down= %6.3f ns\n Sum        RES_3(Up+Down)= %6.3f ns\n Sum_avg= %2.6\n AVg upR3= %6.3f und AVG_UP3TEST = %6.3f",TOF_up, TOF_down, Time_of_flight_sum, TOF_sum_sum,average_Result_up, average_Result_up2);
        USB_Transmit_Data((unsigned char *) &str1, strlen(str1));


        sum_counter ++;

        if (sum_counter > no_of_avg)    // Output after no_of_avg measurements
        {
              TOF_diff_avg = TOF_diff_sum / no_of_avg;
              TOF_sum_avg = TOF_sum_sum / no_of_avg;
              //printf("\n\n Avg. value of difference = %6.3f ns", TOF_diff_avg);
              Std_Dev_of_Diff = sqrt( (TOF_diff_square_sum - (TOF_diff_square_sum/no_of_avg)) / (no_of_avg-1));
              //printf("\n Std.Dev. of Diff. = %6.1f ps", Std_Dev_of_Diff*1000);
              //printf("\n PW1ST = %1.2f ", PW1ST/(2*no_of_avg));
              distance_cm = (TOF_sum_avg * 1e-9f * 1480.0f) * 100.0f / 2.0f;

            char str[256];

            memset(str, 0, sizeof(str));

              //printf("\n Std.Dev. of Diff. = %6.1f ps", Std_Dev_of_Diff*1000);
              //printf("\n PW1ST = %1.2f ", PW1ST/(2*no_of_avg));

            sprintf(str, "Std.Dev. of Diff. = %6.1f ps\r\n Cal=  %1.3f \r\n TOF_UP=  %1.6f \r\n TOF_DOWN=  %1.6f \r\n\nDIstanz=%6.3f cm clock = %1.3f\n PW1ST = %1.2f\n Sum_avg = %0.6f, no_of_avg = %lu", Std_Dev_of_Diff * 1000, CLKHS_freq_corr_fact, TOF_up, TOF_down,distance_cm, CLKHS_freq_corr_fact, PW1ST/(2*no_of_avg), TOF_sum_sum, no_of_avg);

            USB_Transmit_Data((unsigned char *) &str, strlen(str));

              Std_Dev_of_Diff = 0;
              TOF_diff_sum = 0;
              TOF_diff_square_sum = 0;
              sum_counter = 1;
              PW1ST = 0;
              TOF_sum_avg = 0;
              distance_cm = 0;
              TOF_sum_sum = 0;

//              char str2[256];
//               sprintf(str2, "\nDIstanz=%6.3f cm clock = %1.3f\n", distance_cm, CLKHS_freq_corr_fact);
//               USB_Transmit_Data((unsigned char *) &str2, strlen(str2));
              break;
        }
        }

    } // End while TOF-RESTART
}


// ........................................... //

float Calculate_PT_Temperature(float pt, float resistance)
{
    //  (3.9083 * pt - ((3.9083 * pt)^2 + (2.31 * pt) * (pt - adc))^0.5) * 1000 / 1.155 / pt;

    // Konstanten für PT100 (IEC 60751, für Temperaturen zwischen 0°C und 100°C)
    double R0 = pt;//100.0;// Widerstand bei 0°C
    double A  = 3.9083e-3; // Koeffizient A
    double B  = -5.775e-7; // Koeffizient B

    // Quadratische Formel: T = (-A + sqrt(A^2 - 4*B*(1 - R/R0))) / (2*B)
    double temp = (-A + sqrt(A * A - 4 * B * (1 - resistance / R0))) / (2 * B);
    return (float)temp;
}

void GP22_Example(void)
{
    LED_GREEN_ON();

    Init_DIO0_as_Input(true);

    if (GET_DIO0() == GPIO_PIN_RESET)
    {

    }

//    while ((GET_DIO0() == GPIO_PIN_SET) && (timeout_counter != 0))
//    {
//      timeout_counter--;
//    }

    SPI2_ReInit();

    USB_Install_RX_Callback(USB_GP22_Example_RX);

    BOARD_DELAY(200);

    GP22_reset();

    if (GP22_testCommunication())
    {
        LED_BLUE_ON();

        // in "Time_of_flight" is the result
        GP22_TOF_Test();

        // only high 4 Byte
        // unsigned int ids = GP22_readNBytes(OPCODE_READ_ID, 7);
    }

    Task_Typedef test_task;

    InitTask(&test_task, 500);

    LED_GREEN_OFF();

    while (1)
    {
        if (IsTask(&test_task))
        {
            LED_GREEN_TOGGLE();

            // in "Time_of_flight" is the result
//          GP22_TOF_Test();
//
//          char str[256];
//          memset(str, 0, sizeof(str));
//
//          sprintf(str, "Tcold: %3.3f, Thot: %3.3f\r\n", Calculate_PT_Temperature(1000.0, (Rcold_by_Rref * 1000.0)), Calculate_PT_Temperature(1000.0, (Rhot_by_Rref * 1000.0)));
//
//          USB_Transmit_Data((unsigned char *) &str, strlen(str));

//          memset(str, 0, sizeof(str));
//
//          sprintf(str, "TOF: %f\r\n", Time_of_flight);
//
//          USB_Transmit_Data((unsigned char *) &str, strlen(str));

            GP22_TOF_Test2();

//          memset(str, 0, sizeof(str));
//
//            //printf("\n Std.Dev. of Diff. = %6.1f ps", Std_Dev_of_Diff*1000);
//            //printf("\n PW1ST = %1.2f ", PW1ST/(2*no_of_avg));
//
//          sprintf(str, "Std.Dev. of Diff. = %6.1f ps\r\n Cal=  %1.3f \r\n TOF_UP=  %1.6f \r\n TOF_DOWN=  %1.6f \r\n\nDIstanz=%6.3f cm clock = %1.3f\n", Std_Dev_of_Diff * 1000, CLKHS_freq_corr_fact, TOF_up, TOF_down,distance_cm, CLKHS_freq_corr_fact);
//
//          USB_Transmit_Data((unsigned char *) &str, strlen(str));


        }
    }
}