Changeset 256 in flair-src

Timestamp:

Jul 23, 2018, 5:03:47 PM (6 years ago)

Author:

Sanahuja Guillaume

Message:

add saturation to euler derivative

Location:

trunk

Files:

• ReleaseNotes (modified) (1 diff)
• lib/FlairFilter/src/EulerDerivative_impl.cpp (modified) (3 diffs)
• lib/FlairFilter/src/unexported/EulerDerivative_impl.h (modified) (1 diff)

trunk/ReleaseNotes

@@ -6 +6 @@
 - file_ui com is only created when sending logs from uav to gcs (reduce overall data exchange between the 2)
 - resolved https://devel.hds.utc.fr/projects/flair/ticket/37
+- added a saturation to EulerDerivative: saturate in case of numerical problems
 
 -----------------------------------------------------------

trunk/lib/FlairFilter/src/EulerDerivative_impl.cpp

@@ -60 +60 @@
  
   cvmatrix_descriptor *desc = new cvmatrix_descriptor(output->Rows(), output->Cols());
-  prev_value = new Matrix(self, desc, output->GetDataType().GetElementDataType(), name);
+  prev_input = new Matrix(self, desc, output->GetDataType().GetElementDataType(), name);
+  prev_output = new Matrix(self, desc, output->GetDataType().GetElementDataType(), name);
+  delete desc;
 
 
   // init UI
   GroupBox *reglages_groupbox = new GroupBox(position, name);
-  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto:",
-                        " s", 0, 1, 0.01);
+  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto:"," s", 0, 1, 0.01);
+  sat = new DoubleSpinBox(reglages_groupbox->NewRow(), "saturation, -1 to disable:",-1,100000,1,1,-1);
 }
 
@@ -87 +89 @@
     for (int i = 0; i < input->Rows(); i++) {
       for (int j = 0; j < input->Cols(); j++) {
-        prev_value->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
+        prev_input->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
       }
     }
@@ -101 +103 @@
       for (int i = 0; i < input->Rows(); i++) {
         for (int j = 0; j < input->Cols(); j++) {
-          output->SetValueNoMutex(
-              i, j, (input->ValueNoMutex(i, j) - prev_value->ValueNoMutex(i, j)) /
-                        delta_t);
-          prev_value->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
+          float result=(input->ValueNoMutex(i, j) - prev_input->ValueNoMutex(i, j)) / delta_t;
+          if(sat->Value()!=-1) {
+            if(result>sat->Value() && result>0) result=sat->Value();
+            if(result<-sat->Value() && result<0) result=-sat->Value();
+          }/* filter by acc
+          float acc=result-prev_output->ValueNoMutex(i, j)/delta_t;
+          if(acc>20 || acc<-20)  {
+            printf("%s %f\n",self->ObjectName().c_str(),acc);
+          } else {*/
+            output->SetValueNoMutex(i, j, result);
+          //}
+          prev_output->SetValueNoMutex(i, j, result);
+          prev_input->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
+          
         }
       }

trunk/lib/FlairFilter/src/unexported/EulerDerivative_impl.h

@@ -44 +44 @@
 
 private:
-  flair::gui::DoubleSpinBox *T;
+  flair::gui::DoubleSpinBox *T,*sat;
   bool first_update;
-  flair::core::Matrix *prev_value;
+  flair::core::Matrix *prev_input,*prev_output;
   flair::filter::EulerDerivative *self;
 };