comp-cap: Move |filtered_duration = 0|
Posted by Anonymous on Wed 23rd Oct 2019 12:30
raw | new post
modification of post by Anonymous (view diff)

diff --git a/ptxdist/local_src/ecu01-comp-cap/mmc_imp.c b/ptxdist/local_src/ecu01-comp-cap/mmc_imp.c
index 42d913d..187afe4 100644
--- a/ptxdist/local_src/ecu01-comp-cap/mmc_imp.c
+++ b/ptxdist/local_src/ecu01-comp-cap/mmc_imp.c
@@ -171,221 +171,222 @@ int qa_mmc_calc( struct qa_mmc_dev *dev )
    read_unlock_irqrestore(&cc_phaseL->lock, flags);

    // copy right values
    flags = 0;
    read_lock_irqsave(&cc_phaseR->lock, flags);
    for (i=0;i < MAX_VALUE_CNT_QA; i++) {
       t_phase_R_tmp[i] =  cc_phaseR->value_aul[i]-i_overflow;
    }
    read_unlock_irqrestore(&cc_phaseR->lock, flags);

    // calculate durations from tmp arrays

    for (i=0;i < MAX_VALUE_CNT_QA-1 ;i++) {
       t_periode_duartion[i] = t_periode_tmp[i]-t_periode_tmp[i+1];
       t_phase_L_duartion[i] = t_phase_L_tmp[i]-t_phase_L_tmp[i+1];
       t_phase_R_duartion[i] = t_phase_R_tmp[i]-t_phase_R_tmp[i+1];
    }

    // Clean all ISR buffers if 16 old values exist and if the newest is older than 1 second

    if(t_periode_tmp[16] != 0){  // There are at least 16 ref values

       // read current time and correct it by i_overflow, too
       get_cnt_value(dev->cc_dev_Ref, &cnt_value);
       timer_now = cnt_value - i_overflow;

       // test if newest value in rev value is older than 1 second
       if ( ( timer_now - t_periode_tmp[0] ) > TIMER_SPEED) {

           // Clear ref values
          flags = 0;
          read_lock_irqsave(&cc_phaseRef->lock, flags);
          for (i=0;i < MAX_VALUE_CNT_QA; i++) {
             cc_phaseRef->value_aul[i] =  0;
          }
          read_unlock_irqrestore(&cc_phaseRef->lock, flags);

          // Clear left values
          flags = 0;
          read_lock_irqsave(&cc_phaseL->lock, flags);
          for (i=0;i < MAX_VALUE_CNT_QA; i++) {
             cc_phaseL->value_aul[i] =  0;
          }
          read_unlock_irqrestore(&cc_phaseL->lock, flags);

          // Clear right values
          flags = 0;
          read_lock_irqsave(&cc_phaseR->lock, flags);
          for (i=0;i < MAX_VALUE_CNT_QA; i++) {
            cc_phaseR->value_aul[i] =  0;
          }
          read_unlock_irqrestore(&cc_phaseR->lock, flags);

       }
    }

    if(t_periode_tmp[16] != 0){  // There are at least 16 ref values

       // be carefull not to chgen the order of that sequence. l_periode_duartion is used for left and right, too
       // at the end l_periode_duartion must have the value of the ref signal!

       // mean value of right values
       l_periode_duartion = 0;
       for (i=0;i < 12 ;i++) {
          l_periode_duartion += t_phase_R_duartion[i];
       }
       l_periode_duartion /= 12;
       // if duration is 0 there were no signals captured. This is an error
       if(l_periode_duartion == 0) i_mmc_error |= 0x4;

       // mean value of left values
       l_periode_duartion = 0;
       for (i=0;i < 12 ;i++) {
         l_periode_duartion += t_phase_L_duartion[i];
       }
       l_periode_duartion /= 12;
       // if duration is 0 there were no signals captured. This is an error
       if(l_periode_duartion == 0) i_mmc_error |= 0x2;

       // mean value of ref values
       l_periode_duartion = 0;
       for (i=0;i < 12 ;i++) {
         l_periode_duartion += t_periode_duartion[i];
       }
       l_periode_duartion /= 12;
       // if duration is 0 there were no signals captured.  This is an error
       if(l_periode_duartion == 0) i_mmc_error |= 0x1;
       // if duration is longer than 100 ms the frequencey of pulses is less than 10 Hz.  This is an error
       if(l_periode_duartion > MAX_MMC_PERIODE_DELAY) i_mmc_error |= 0x8;

+      filtered_duration = 0;
+
       // Filter to eliminate values much longer than average or much shorter than everage.
       // To prevent side effects activate filter only if L and R signal are not present
       // Interrupt from these signals should be preventet by disabling them if the bridge from Pin C3 to C4
       // is installed for FAG Sensors

       if ( 0x6 == (i_mmc_error & 0x6) ) {

         // There are 2 cases for shorter period than before
         // 1. Normal: The periode decereased because the speed raised.
         //            In this case all values in the array change their values and are close together.
         //            The average over all 12 values will be fine.
         // 2. Error: We got one or more additional interrupts.
         //           in this case we get one periode with is at less than half in value and on period
         //           which is more than half in value. But the sum of both is nearly the correct period.
         // Therefore we compare the values in the array to 0,8 * ul_periode_tmp which is the last average
         // If the additional interrupt is close to the real signal pulse, one of both values will pass the filter.
         // but if the additional interrupts is somewhere in the middle or if the is more than one additonal interrupt,
         // all additional periods are too short to pass the filter.

         const unsigned int l_periode_duartion_min = ul_periode_tmp*8/10;

         // There are 2 cases for longer periods than before
         // 1. Normal: The periode increased because the speed reduced.
         //            In this case all values in the array change their values and are close together.
         //            The average over all 12 values will be fine.
         // 2. Error: We lost one or more interrupts. We get periods with are at least double of correct value.
         //           If only 1 interrupts is lost the mean value over all changes is wrong by an factor of only 13 / 12 = 1,08.
         //           Using this new mean value as reference seems not practical.
         // If nothing passes the filter, the total average over all 12 values is used.
         // This average might leed to a speed too low if we lose interrupts. But in the next cycle this average
         // is rather close to the new values in the array.
         // Therefore we compare the values in the array to 1,5 * ul_periode_tmp which is the last average

         const unsigned int l_periode_duartion_max = ul_periode_tmp*15/10;

         unsigned int j   = 0;
         unsigned int rej = 0;

         ++filter_cnt;

-        filtered_duration = 0;

         for(i=0; i< 12; i++) {
           if((t_periode_duartion[i] > l_periode_duartion_min) && (t_periode_duartion[i] < l_periode_duartion_max)) {
             filtered_duration += t_periode_duartion[i];
                ++j;
           } else {
                //printk("CC filter (%0x) rejected: %x, mean: %x\n", filter_cnt, t_periode_duartion[i], l_periode_duartion);
            rej += t_periode_duartion[i];
          }
     }

        if(j != 0) {     // at least one value passed the filter
                filtered_duration /= j;
        }

        // sum up 1 for each active filtering
        if(j!=12) {
                ++filter_rej;
                rej /= (12-j);
                //printk("CC filter (%0x) rejected %d values: filtered: %x mean: %x (min: %x, max: %x) last: %x, rej_mean: %x\n", filter_cnt, 12-j, filtered_duration, l_periode_duartion, l_periode_duartion_min, l_periode_duartion_max, ul_periode_tmp, rej);
        }
       }

       // Wenn das Filter einen Wert berechnet und nicht alles verwirft, dann wird der gefilterte Wert benutzt.
       // Ansonsten wird der Mittelwert ?ber alle Werte verwendet.

       dev->ioc.durationRef = filtered_duration ? filtered_duration : l_periode_duartion;

       // Falls kein Fehler anliegt, berechne die Phasenverschiebungen
       if(!(i_mmc_error & 0x9) ) {
          // Falls PhaseL anliegt, berechne Phasenverschiebung left
          if(!(i_mmc_error & 0x2) ) {
             dev->ioc.phaseL = GetPhaseShift(t_periode_tmp, t_phase_L_tmp, MAX_VALUE_CNT_QA, l_periode_duartion);
          }
          // Falls Phasenverschiebung 0 ist, liegt keine PhaseL an => Fehler
          if (!(dev->ioc.phaseL))
             i_mmc_error |= 0x2;

          // Falls PhaseR anliegt, berechne Phasenverschiebung right
          if(!(i_mmc_error & 0x4) ) {
             dev->ioc.phaseR = GetPhaseShift(t_periode_tmp, t_phase_R_tmp, MAX_VALUE_CNT_QA, l_periode_duartion);
          }
          // Falls Phasenverschiebung 0 ist, liegt keine PhaseR an => Fehler
          if (!(dev->ioc.phaseR))
             i_mmc_error |= 0x4;
       }

    }
    else {
       i_mmc_error |= 0x8;   // There are less than 16 ref values
    }

    if( i_mmc_error == 0x7)  // Was bedeutet das?
       i_mmc_error = 0xf;

    if( i_mmc_error &  0x1)
       i_mmc_error |= 0x8;  // Was bedeutet das?

    // if Left is missing, stick to old value
    if(dev->ioc.phaseL)
       ul_phase_L_tmp = dev->ioc.phaseL;
    else
       dev->ioc.phaseL = ul_phase_L_tmp;

    // if Right is missing, stick to old value
    if(dev->ioc.phaseR)
       ul_phase_R_tmp = dev->ioc.phaseR;
    else
       dev->ioc.phaseR = ul_phase_R_tmp;

    // if ref is missing, stick to old value
    if(dev->ioc.durationRef)
       ul_periode_tmp = dev->ioc.durationRef;
    else
       dev->ioc.durationRef = ul_periode_tmp;

    dev->ioc.mmcError = i_mmc_error;

    get_counter(dev->cc_dev_Ref, &(dev->ioc.RefCnt));
    dev->ioc.RefError = filter_rej;
    struct cc_stat value;
    get_isr_error_value(dev->cc_dev_Ref, &(value));

    dev->ioc.IsrTolerCnt = value.isr_toler_cnt;
    dev->ioc.IsrMaxDelay = value.isr_max_delay;
    dev->ioc.IsrAvgDelay = value.isr_avg_delay;
    dev->ioc.IsrDelayCov = value.isr_delay_cov;

    //printk(iode_duartion[i] > l_periode_duartion_min) && (t_periode_duartion[i] < l_periode_duartion_max)) {