ported the elm detection routine

README.md

@@ -3,7 +3,7 @@
 Computes a proxy for the elm energy from the IPSOLA shunt measurements.
 
 ## notes
-Binary data files in `data` (not tracked).  
+Binary data files in `data` (not tracked). Make sure everyone has read access.  
 This directory is mounted on the container in `/data`.  
 Project based on `anto/dma-sim`.  
 to ssh into the container `ssh -p 3399 user@<host_ip>`  

ap/include/dcs/rtdiag/elm_proxy/process.h

@@ -22,8 +22,11 @@ namespace dcs::rtdiag::elm_proxy {
 class Process final : public CxProcess {
  public:
   constexpr static const char* input_fname_usage = "p:input_fname";
-  constexpr static const char* input_floc_usage = "p:input_floc";
   constexpr static const char* block_length_usage = "p:block_length";
+  constexpr static const char* di_threshold_usage = "p:di_threshold";
+  constexpr static const char* debouncer_0_usage = "p:debouncer_0";
+  constexpr static const char* interval_usage = "p:interval";
+  constexpr static const char* max_elms_usage = "p:max_elms";
   constexpr static const char* cycle_number_usage = "i:cycle_number";
   constexpr static const char* output_usage = "o:output";
 
@@ -41,16 +44,25 @@ class Process final : public CxProcess {
  private:
   std::string config_url_;
   DcsParameter<std::string> input_fname_;
-  DcsParameter<std::string> input_floc_;
   DcsParameter<uint32_t> block_length_;
+  DcsParameter<double> di_threshold_;
+  DcsParameter<uint32_t> debouncer_0_;
+  DcsParameter<uint32_t> interval_;
+  DcsParameter<uint32_t> max_elms_;
   RequestedSynchronisedSignalUnit<uint32_t, CycleTagFamily> cycle_number_;
   ProvidedSignalUnit<uint64_t, CycleTagFamily> output_;
   ConfigInventory<CycleTagFamily> inventory_;
   void* rawData_;
+  double* pSmooth_;
+  double* pGradient_;
+  double* pLastSamples_;
   uint32_t nSamples_ = 0;
   uint32_t sampsPerCycle_ = 0;
   uint32_t cyclesPerBlock_ = 0;
   uint32_t nBlocks_ = 0;
+  double tbs_=0;
+  double t0_=0;
+  double t1_=0;
 };
 }  // namespace dcs::rtdiag::elm_proxy
 

ap/src/dcs/rtdiag/elm_proxy/process.cpp

@@ -17,19 +17,82 @@ bool make_dcs_rtdiag_elm_proxy_process(AppCtrlParams* params) {
   return ap->applBuild(params);
 }
 
+void* readDataFile(const char* floc, uint32_t* nSamples, double_t* tbs, double_t* t0, double_t* t1){
+  void* rawData;
+  uint32_t reads = 0;
+  FILE* fp = fopen( floc, "r");
+  if (fp != nullptr){
+    reads+=fread(nSamples,sizeof(uint32_t),1,fp);
+    reads+=fread(tbs,sizeof(double_t),1,fp);
+    reads+=fread(t0,sizeof(double_t),1,fp);
+    reads+=fread(t1,sizeof(double_t),1,fp);
+    rawData= malloc(*nSamples * sizeof(double_t));
+    reads+=fread(rawData,sizeof(double),*nSamples,fp);
+    fclose(fp);
+  }
+  if (reads<4){
+    return nullptr;
+  }
+  return rawData;
+}
+
+/*
+Implements the first derivative using the backward finite difference coefficients to the 2nd order
+https://en.wikipedia.org/wiki/Finite_difference_coefficient
+*/
+int gradient(double *dataIn, double *dataOut, size_t N, double *pastSamples){
+  double a0=1.5;
+  double a1=-2.;
+  double a2=0.5;
+  for (size_t i = 0; i < N; i++){
+    if (i == 0){
+      dataOut[i]=dataIn[i]*a0+pastSamples[1]*a1+pastSamples[0]*a2;
+    }else if (i == 1){
+      dataOut[i]=dataIn[i]*a0+dataIn[i-1]*a1+pastSamples[1]*a2;
+    }else{
+      dataOut[i]=dataIn[i]*a0+dataIn[i-1]*a1+dataIn[i-2]*a2;
+    }
+  }
+  return 0;
+}
+
+/*
+Applies the IIR discretization of a Butterworth LP filter of forst order
+to compute the filter parameters, in Python:
+b,a = scipy.signal.butter(order, 2*fc*tbs, analog=False)
+fc is the cuttoff frequency, tbs the time between samples (1/fs)
+*/
+void filter(double *x, double *y, size_t N, double last_x, double last_y, double b0, double b1, double a1){
+  //a[0]*y[n] = b[0]*x[n] + b[1]*x[n-1] + ... + b[M]*x[n-M]
+  //                      - a[1]*y[n-1] - ... - a[N]*y[n-N]
+  for (size_t i = 0; i < N; i++){
+    if (i == 0){
+      y[i]=x[i]*b0+last_x*b1-last_y*a1;
+    }else{
+      y[i]=x[i]*b0+x[i-1]*b1-y[i-1]*a1;
+    }
+  }
+}
+
 namespace dcs::rtdiag::elm_proxy {
 
 Process::Process(std::string config_url)
     : config_url_{std::move(config_url)},
       input_fname_{input_fname_usage},
-      input_floc_{input_floc_usage},
       block_length_(block_length_usage),
+      di_threshold_(di_threshold_usage),
+      debouncer_0_(debouncer_0_usage),
+      interval_(interval_usage),
+      max_elms_(max_elms_usage),
       cycle_number_(*this, cycle_number_usage, 1, 1),
       output_(*this, output_usage, 0),
       inventory_{*this, "Inventory"} {
   inventory_.add(&input_fname_);
-  inventory_.add(&input_floc_);
   inventory_.add(&block_length_);
+  inventory_.add(&di_threshold_);
+  inventory_.add(&debouncer_0_);
+  inventory_.add(&interval_);
+  inventory_.add(&max_elms_);
   inventory_.add(&cycle_number_);
   inventory_.add(&output_);
 }
@@ -39,6 +102,9 @@ ReturnState Process::doApplInit() { return ReturnState::ISOK; }
 ReturnState Process::doApplDeinit() {
   inventory_.reset();
   free(rawData_);
+  free(pSmooth_);
+  free(pGradient_);
+  free(pLastSamples_);
   return ReturnState::ISOK;
 }
 
@@ -91,30 +157,21 @@ ReturnState Process::doApplStart() {
   //output_.writeSample();
   wdStart();
   uint32_t rc = 0;
-  double_t tbs;
-  double_t t0;
-  double_t t1;
+
+  //READ DATA FROM FILE
+
   const char* fname = input_fname_.value().c_str();
-  report << INFO << "Opening file  " << input_fname_.value() << endl;
-  FILE* fp = fopen( fname, "r");
-  if (fp == NULL){
+  report << INFO << "Opening file  " << fname << endl;
+  rawData_=readDataFile(fname, &nSamples_, &tbs_, &t0_, &t1_);
+  if(!rawData_){
     report << ERR << "File reading failed" << endl;
-  }else{
-    rc+=fread(&nSamples_,sizeof(uint32_t),1,fp);
-    rc+=fread(&tbs,sizeof(double_t),1,fp);
-    rc+=fread(&t0,sizeof(double_t),1,fp);
-    rc+=fread(&t1,sizeof(double_t),1,fp);
-    rawData_= malloc(nSamples_*sizeof(double_t));
-    rc+=fread(rawData_,sizeof(double),nSamples_,fp);
-    fclose(fp);
+    return ReturnState::ISERROR;
   }
-  sampsPerCycle_= 1500 / (tbs*1e6);
+  sampsPerCycle_= (uint32_t) (1500. / (tbs_*1e6));
   cyclesPerBlock_ = block_length_.value() / sampsPerCycle_;
   nBlocks_ = (nSamples_ + block_length_.value() -1)/ block_length_.value(); //integer division with round up
   report << INFO << "nSamples: " << nSamples_ << endl;
-  report << INFO << "t0: " << t0 << " s" << endl;
-  report << INFO << "t1: " << t1 << " s" << endl;
-  report << INFO << "tbs: " << tbs*1e6 << " us" << endl;
+  report << INFO << "tbs_: " << tbs_*1e6 << " us" << endl;
   report << INFO << "samples per cycle: " << sampsPerCycle_ << endl;
   report << INFO << "cycles per block: " << cyclesPerBlock_ << endl;
   //Warnings for edge cases
@@ -132,8 +189,44 @@ ReturnState Process::doApplLoop() {
   namespace ch = std::chrono;
   uint32_t cycle = 1;
   uint32_t blockCount = 0;
+  uint32_t block_length = block_length_.value();
+  uint32_t debouncer_0 = debouncer_0_.value();
+  uint32_t interval = interval_.value();
+  uint32_t max_elms = max_elms_.value();
   double_t* block = (double_t*) rawData_;
   uint64_t lastBlock = 0;
+
+
+  //FILTER INIT
+  double_t past_x=0;
+  double_t past_y=0;
+  pSmooth_ = (double_t*) malloc(block_length * sizeof(double_t));
+  //1kHz
+  //IMPORTANT!!!!! multiplied all b coeficients by -1 for the filter to invert the signal!
+  double_t b0 = -0.01546629;
+  double_t b1 = -0.01546629;
+  double_t a1 = -0.96906742;
+
+  // GRADIENT INIT
+  double_t pastSamples[2] = {0,0};
+  pGradient_=(double_t*) malloc(block_length * sizeof(double_t));
+
+  //ELM DETECTION INIT
+  bool elmOn = false;
+  int32_t elmDebouncer = 0;
+  uint32_t tElm_start[max_elms];
+  double_t wElm[max_elms];
+  uint32_t elmLength[max_elms];
+  double_t elmInt = 0;
+  uint32_t elmCount = 0;
+  int32_t elmStart = 0;
+  double_t hold = 0;
+  pLastSamples_ = (double*) calloc(interval,sizeof(double));
+  double_t thr = di_threshold_.value()*tbs_; //in samples
+  int32_t nLastSamples = 0;
+  double_t* pWord2;
+
+  //MAIN DSC CYCLE LOOP
   while (cycle_number_.isSampling()) {
     if (!cycle_number_.getSampleAtTag(cycle)) {
       if (!cycle_number_.isSampling() || cycle_number_.isLast()) {
@@ -150,19 +243,92 @@ ReturnState Process::doApplLoop() {
         report << WARN << "Block pointer mismatch at cycle " << cycle << " block " << blockCount
         << " (got: 0x" << hex << output_.value() << " expected: 0x" << hex << *block << ")" << endl;
       }
+      //Process this block
+      /*
       report << INFO << "cycle " << cycle << " block " << blockCount << "\taddr 0x" << hex  << (uint64_t) block
       << " val[0]: " << block[0] << " val[-1]: " << block[block_length_.value()-1] << endl;
+      */
+      //Filter
+      filter(block, pSmooth_, block_length, past_x, past_y, b0, b1, a1);
+      past_x=block[block_length-1];
+      past_y=pSmooth_[block_length-1];
+
+      //Gradient
+      gradient(pSmooth_,pGradient_,block_length,pastSamples);
+      pastSamples[0]=pSmooth_[block_length-2];
+      pastSamples[1]=pSmooth_[block_length-1];
+
+      //ELM detection loop over the block
+      for (size_t j = 0; j < block_length; j++){
+        //start condition
+        if((pGradient_[j]>= thr) && (!elmOn)){
+          elmOn = true;
+          elmDebouncer = debouncer_0;
+          elmStart = (cycle-1)*sampsPerCycle_ + j - interval;
+          nLastSamples=interval > j ? interval-j : 0;
+          if(elmStart<0){ //handle this
+            elmStart=0;
+            nLastSamples=0;
+          }
+          elmInt=0;
+          if (nLastSamples==0){ //normal case
+            hold=pSmooth_[j-interval] > 0 ? pSmooth_[j-interval] : 0;
+            for (size_t k = j-interval; k <= j; k++){
+              elmInt+=pSmooth_[k];
+            }
+          }else{ //elm starts on the previous packet
+            hold=pLastSamples_[nLastSamples-1] > 0 ? pLastSamples_[nLastSamples-1] : 0;
+            for (size_t k = j; k < interval; k++){
+              elmInt+=pLastSamples_[k];
+            }
+            for (size_t k = 0; k <= j; k++){
+              elmInt+=pSmooth_[k];
+            }
+          }
+        }
+        //end condition
+        if( (pSmooth_[j] < hold) && elmOn){
+          elmOn = false;
+          if(elmDebouncer<=0){
+            //save the elm
+            tElm_start[elmCount]=elmStart;
+            elmLength[elmCount]= (cycle-1) * sampsPerCycle_ + j - elmStart;
+            wElm[elmCount]=elmInt*tbs_*2.45;
+            elmCount++;
+            if(elmCount>max_elms){
+              report << WARN << "ERROR: MAX_ELMS reached. Overwriting!" << endl;
+              elmCount=0;
+            }
+            /*
+            report << INFO << "ELM "<< elmCount << " - " << elmStart
+            << "  " << elmLength[elmCount-1] << " " << elmInt << endl;
+            */
+          }
+        }
+        elmDebouncer--;
+        elmInt+=pSmooth_[j];
+      }//ELM detection loop over the block
+
+      //save the last 'interval' samples of this word
+      pWord2 = pSmooth_ + block_length - interval;
+      memcpy((void*) pLastSamples_, (void*) pWord2, interval*sizeof(double_t));
+      report << INFO << "cycle " << cycle <<  "\t" << (cycle-1) * sampsPerCycle_
+      << "\tt=" <<  t0_ + (cycle-1) * sampsPerCycle_ * tbs_
+      << "s\tnELMs=" << elmCount << endl;
+
       output_.editValue() = (uint64_t) block;
+      //END condition
       if(blockCount == nBlocks_){
         output_.editState() = SampleProductionState::STOPPED;
       }
+      // iterate
       output_.publishSample(cycle);
       lastBlock=(uint64_t) block;
       block += block_length_.value();
     }
     ++cycle;
     wdStart(cycle);
-  }
+  } //MAIN DSC CYCLE LOOP
   wdStop();
   return ReturnState::ISOK;
 }

ap/test/config_elm_proxy.xml

@@ -2,12 +2,21 @@
     <USAGE name="p:input_fname" class="Parameter">
         <PARAMETER type="string" value="/data/data_Ipolsola_38320.bin"/>
     </USAGE>
-    <USAGE name="p:input_floc" class="Parameter">
-        <PARAMETER type="string" value="./"/>
-    </USAGE>
     <USAGE name="p:block_length" class="Parameter">
         <PARAMETER type="uint32_t" value="120000"/>
     </USAGE>
+    <USAGE name="p:di_threshold" class="Parameter">
+        <PARAMETER type="double" value="15000000"/>
+    </USAGE>
+    <USAGE name="p:debouncer_0" class="Parameter">
+        <PARAMETER type="uint32_t" value="50"/>
+    </USAGE>
+    <USAGE name="p:interval" class="Parameter">
+        <PARAMETER type="uint32_t" value="50"/>
+    </USAGE>
+    <USAGE name="p:max_elms" class="Parameter">
+        <PARAMETER type="uint32_t" value="1000"/>
+    </USAGE>
     <USAGE name="i:cycle_number" class="Input">
         <SIGNAL name="rts:Adm/Cycle/Number.val" type="uint32_t"/>
     </USAGE>