Ignore:
Timestamp:
02/28/17 12:45:08 (5 years ago)
Author:
Sanahuja Guillaume
Message:

modif euler/lowpass

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairFilter/src/LowPassFilter_impl.cpp

    r15 r147  
    3535                                       string name,
    3636                                       const cvmatrix *init_value) {
    37   first_update = true;
    3837
    3938  if (init_value != NULL) {
    40     prev_value = (cvmatrix *)init_value;
     39    // init output matrix of same size as init
     40    cvmatrix_descriptor *desc =new cvmatrix_descriptor(init_value->Rows(), init_value->Cols());
     41
     42    for (int i = 0; i < init_value->Rows(); i++) {
     43      for (int j = 0; j < init_value->Cols(); j++) {
     44        desc->SetElementName(i, j, init_value->Name(i, j));
     45      }
     46    }
     47    output = new cvmatrix(self, desc,init_value->GetDataType().GetElementDataType(), name);
     48    for (int i = 0; i < init_value->Rows(); i++) {
     49      for (int j = 0; j < init_value->Cols(); j++) {
     50        output->SetValue(i, j, init_value->Value(i,j));
     51      }
     52    }
    4153  } else {
    4254    // if NULL, assume dimension 1, and init=0
    4355    cvmatrix_descriptor *desc = new cvmatrix_descriptor(1, 1);
    4456    desc->SetElementName(0, 0, "output");
    45     prev_value = new cvmatrix(self, desc, floatType, name);
    46     prev_value->SetValue(0, 0, 0);
     57    output = new cvmatrix(self, desc, floatType, name);
     58    output->SetValue(0, 0, 0);
    4759  }
    4860
     
    5365  freq = new DoubleSpinBox(reglages_groupbox->NewRow(), "cutoff frequency",
    5466                           " Hz", 0, 10000, 0.1, 2, 1);
    55 
    56   // init output matrix of same size as init
    57   cvmatrix_descriptor *desc =
    58       new cvmatrix_descriptor(prev_value->Rows(), prev_value->Cols());
    59 
    60   for (int i = 0; i < prev_value->Rows(); i++) {
    61     for (int j = 0; j < prev_value->Cols(); j++) {
    62       desc->SetElementName(i, j, prev_value->Name(i, j));
    63     }
    64   }
    65 
    66   output = new cvmatrix(self, desc,
    67                         prev_value->GetDataType().GetElementDataType(), name);
    68 
    69   output->SetValue(0, 0, 0);
     67                           
     68  previous_time=GetTime();
    7069}
    7170
     
    7473void LowPassFilter_impl::UpdateFrom(const io_data *data) {
    7574  float delta_t;
    76   float result;
    7775  cvmatrix *input = (cvmatrix *)data;
    7876
     
    8179  input->GetMutex();
    8280
    83   if (first_update == true) {
    84     for (int i = 0; i < input->Rows(); i++) {
    85       for (int j = 0; j < input->Cols(); j++) {
    86         output->SetValueNoMutex(i, j, prev_value->ValueNoMutex(i, j));
    87         prev_value->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
    88       }
    89     }
    90     first_update = false;
     81  if (T->Value() == 0) {
     82    delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
    9183  } else {
    92     if (T->Value() == 0) {
    93       delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
    94     } else {
    95       delta_t = T->Value();
    96     }
    97     for (int i = 0; i < input->Rows(); i++) {
    98       for (int j = 0; j < input->Cols(); j++) {
    99 
    100         if (freq->Value() != 0) {
    101           output->SetValueNoMutex(i, j, (1 - 2 * PI * freq->Value() * delta_t) *
    102                                                 prev_value->ValueNoMutex(i, j) +
    103                                             2 * PI * freq->Value() * delta_t *
    104                                                 input->ValueNoMutex(i, j));
    105         } else {
    106           output->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
    107         }
    108         prev_value->SetValueNoMutex(i, j, output->ValueNoMutex(i, j));
     84    delta_t = T->Value();
     85  }
     86  for (int i = 0; i < input->Rows(); i++) {
     87    for (int j = 0; j < input->Cols(); j++) {
     88      float cutoff=freq->Value();
     89      if (cutoff == 0 || freq->ValueChanged()) {
     90        output->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
     91      } else {
     92        output->SetValueNoMutex(i, j, (1 - 2 * PI * cutoff * delta_t) *
     93                                              output->ValueNoMutex(i, j) +
     94                                          2 * PI * cutoff * delta_t *
     95                                              input->ValueNoMutex(i, j));
    10996      }
    11097    }
    11198  }
     99 
    112100  input->ReleaseMutex();
    113101  output->ReleaseMutex();
Note: See TracChangeset for help on using the changeset viewer.