source: flair-src/trunk/lib/FlairFilter/src/EulerDerivative_impl.cpp

Last change on this file was 318, checked in by Sanahuja Guillaume, 2 years ago
File size: 4.1 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5//  created:    2011/05/01
6//  filename:   EulerDerivative.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    objet permettant le calcul d'une derivee d'Euler
14//
15//
16/*********************************************************************/
17
18#include "EulerDerivative.h"
19#include "EulerDerivative_impl.h"
20#include <Matrix.h>
21#include <Layout.h>
22#include <GroupBox.h>
23#include <DoubleSpinBox.h>
24
25using std::string;
26using namespace flair::core;
27using namespace flair::gui;
28using namespace flair::filter;
29
30EulerDerivative_impl::EulerDerivative_impl(EulerDerivative *self,
31                                           const LayoutPosition *position,
32                                           string name,
33                                           const Matrix *init_value) {
34  this->self = self;
35  first_update = true;
36 
37  if (init_value != NULL) {
38    // init output matrix of same size as init
39    MatrixDescriptor *desc =new MatrixDescriptor(init_value->Rows(), init_value->Cols());
40
41    for (int i = 0; i < init_value->Rows(); i++) {
42      for (int j = 0; j < init_value->Cols(); j++) {
43        desc->SetElementName(i, j, init_value->Name(i, j));
44      }
45    }
46    output = new Matrix(self, desc,init_value->GetDataType().GetElementDataType(), name);
47    delete desc;
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    }
53  } else {
54    // if NULL, assume dimension 1, and init=0
55    MatrixDescriptor *desc = new MatrixDescriptor(1, 1);
56    desc->SetElementName(0, 0, "output");
57    output = new Matrix(self, desc, floatType, name);
58    delete desc;
59  }
60 
61 
62  MatrixDescriptor *desc = new MatrixDescriptor(output->Rows(), output->Cols());
63  prev_input = new Matrix(self, desc, output->GetDataType().GetElementDataType(), name);
64  prev_output = new Matrix(self, desc, output->GetDataType().GetElementDataType(), name);
65  delete desc;
66
67
68  // init UI
69  GroupBox *reglages_groupbox = new GroupBox(position, name);
70  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto:"," s", 0, 1, 0.01);
71  sat = new DoubleSpinBox(reglages_groupbox->NewRow(), "saturation, -1 to disable:",-1,100000,1,1,-1);
72}
73
74EulerDerivative_impl::~EulerDerivative_impl() {}
75
76void EulerDerivative_impl::UpdateFrom(const io_data *data) {
77  float delta_t;
78  const Matrix* input = dynamic_cast<const Matrix*>(data);
79 
80  if (!input) {
81      self->Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());
82      return;
83  }
84
85  // on prend une fois pour toute les mutex et on fait des accès directs
86  output->GetMutex();
87  input->GetMutex();
88
89  if (first_update == true) {
90    for (int i = 0; i < input->Rows(); i++) {
91      for (int j = 0; j < input->Cols(); j++) {
92        prev_input->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
93      }
94    }
95    first_update = false;
96  } else {
97    if (T->Value() == 0) {
98      delta_t = (float)(data->DataDeltaTime()) / 1000000000.;
99    } else {
100      delta_t = T->Value();
101    }
102
103    if(delta_t!=0) {
104      for (int i = 0; i < input->Rows(); i++) {
105        for (int j = 0; j < input->Cols(); j++) {
106          float result=(input->ValueNoMutex(i, j) - prev_input->ValueNoMutex(i, j)) / delta_t;
107          if(sat->Value()!=-1) {
108            if(result>sat->Value() && result>0) result=sat->Value();
109            if(result<-sat->Value() && result<0) result=-sat->Value();
110          }/* filter by acc
111          float acc=result-prev_output->ValueNoMutex(i, j)/delta_t;
112          if(acc>20 || acc<-20)  {
113            printf("%s %f\n",self->ObjectName().c_str(),acc);
114          } else {*/
115            output->SetValueNoMutex(i, j, result);
116          //}
117          prev_output->SetValueNoMutex(i, j, result);
118          prev_input->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
119         
120        }
121      }
122    }
123  }
124
125  input->ReleaseMutex();
126  output->ReleaseMutex();
127
128  output->SetDataTime(data->DataTime());
129}
Note: See TracBrowser for help on using the repository browser.