Changeset 147 in flair-src for trunk


Ignore:
Timestamp:
Feb 28, 2017, 12:45:08 PM (8 years ago)
Author:
Sanahuja Guillaume
Message:

modif euler/lowpass

Location:
trunk
Files:
16 edited

Legend:

Unmodified
Added
Removed

  • trunk/ReleaseNotes

    r135 r147  
    11* Actual Version:
    22
     3- resolved https://devel.hds.utc.fr/software/flair/ticket/16
    34- added lib/FlairCore/src/Semaphore.cpp
    45- added fixed cameras in simulator, see $FLAIR_ROOT/flair-bin/models/indoor_fligh

  • trunk/demos/CircleFollower/uav/src/CircleFollower.cpp

    r139 r147  
    4646    getFrameworkManager()->AddDeviceToLog(uavVrpn);
    4747    uav->GetAhrs()->YawPlot()->AddCurve(uavVrpn->State()->Element(2),DataPlot::Green);
    48     vrpnclient->Start();
    4948                                                                                                                                 
    5049    startCircle=new PushButton(GetButtonsLayout()->NewRow(),"start_circle");
    … …  
    5655        targetVrpn=new MetaVrpnObject("target");
    5756    }
     57                vrpnclient->Start();
    5858
    5959    getFrameworkManager()->AddDeviceToLog(targetVrpn);

  • trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp

    r143 r147  
    5858
    5959        //optical flow stack
    60         //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
    61         opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
    62         opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlow,"vitesse du flux optique");
    63         //opticalFlowSpeed=vitesse de déplacement en pixel par seconde (moyenne sur tous les points et division par le delta T)
    64         cvmatrix* twoByOneSpeed=new cvmatrix((const Thread*)this,2,1,floatType);
    65         opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOneSpeed);
    66         cvmatrix* twoByOneAccelerationRaw=new cvmatrix((const Thread*)this,2,1,floatType);
    67         opticalFlowAccelerationRaw=new EulerDerivative(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"derivative",twoByOneAccelerationRaw);
    68         cvmatrix* twoByOneAcceleration=new cvmatrix((const Thread*)this,2,1,floatType);
    69         opticalFlowAcceleration=new LowPassFilter(opticalFlowAccelerationRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Acceleration lowPass",twoByOneAcceleration);
    70 
    71         getFrameworkManager()->AddDeviceToLog(opticalFlowSpeedRaw);
     60  //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
     61  opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
     62  opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlow,"vitesse du flux optique");
     63  //opticalFlowSpeed=vitesse de déplacement en pixels par seconde (moyenne sur tous les points et division par le delta T)
     64  cvmatrix* twoByOne=new cvmatrix((const Thread*)this,2,1,floatType);
     65  opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOne);
     66  opticalFlowAccelerationRaw=new EulerDerivative(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"derivative",twoByOne);
     67  opticalFlowAcceleration=new LowPassFilter(opticalFlowAccelerationRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Acceleration lowPass",twoByOne);
     68  delete twoByOne;
     69 
     70  getFrameworkManager()->AddDeviceToLog(opticalFlowSpeedRaw);
    7271
    7372        Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
    7473        DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x speed (px/s)",-250,250);
    7574        DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y speed (px/s)",-250,250);
    76         DataPlot1D* xAccelerationPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_acceleration",-250,250);
    77         DataPlot1D* yAccelerationPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_acceleration",-250,250);
     75  DataPlot1D* xAccelerationPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_acceleration",-250,250);
     76  DataPlot1D* yAccelerationPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_acceleration",-250,250);
    7877
    7978        xVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(0,0));
    … …  
    8180        yVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(1,0));
    8281        yVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(1,0),DataPlot::Blue);
    83         xAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(0,0));
    84         xAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(0,0),DataPlot::Blue);
    85         yAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(1,0));
    86         yAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(1,0),DataPlot::Blue);
     82  xAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(0,0));
     83  xAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(0,0),DataPlot::Blue);
     84  yAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(1,0));
     85  yAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(1,0),DataPlot::Blue);
    8786
    8887        u_x=new Pid(setupLawTab->At(1,0),"u_x");

  • trunk/lib/FlairCore/src/Box.cpp

    r15 r147  
    4949}
    5050
    51 void Box::SetValueChanged(void) { value_changed = true; }
     51void Box::SetValueChanged(void) {value_changed = true; }
    5252
    5353void Box::GetMutex(void) const { ((Layout *)Parent())->mutex->GetMutex(); }

  • trunk/lib/FlairCore/src/OneAxisRotation.cpp

    r144 r147  
    5757
    5858float OneAxisRotation::GetAngle() const {
    59   return pimpl_->GetAngle();
     59  return Euler::ToRadian(pimpl_->GetAngle());
    6060}
    6161

  • trunk/lib/FlairCore/src/OneAxisRotation.h

    r144 r147  
    8181  */
    8282  void ComputeRotation(core::RotationMatrix &matrix) const;
    83 
     83 
     84  /*!
     85  * \brief Get angle
     86  *
     87  * \return angle in radians
     88  */
    8489  float GetAngle() const;
    8590  int GetAxis() const;

  • trunk/lib/FlairCore/src/cvmatrix.h

    r15 r147  
    5555  * Construct an io_data representing a CvMat. \n
    5656  * It uses a cvmatrix_descriptor to get size and elements' names. \n
    57   * Names are used for graphs and logs.
     57  * Names are used for graphs and logs. \n
     58  * All values are initialized to 0.
    5859  *
    5960  * \param parent parent
    … …  
    7172  *
    7273  * Construct an io_data representing a CvMat. \n
    73   * Elements are unamed.
     74  * Elements are unamed. \n
     75  * All values are initialized to 0.
    7476  *
    7577  * \param parent parent

  • trunk/lib/FlairFilter/src/ButterworthLowPass_impl.cpp

    r15 r147  
    4747  f = new PoleFilter(order);
    4848
    49   if (T->Value() != 0)
     49  if (T->Value() != 0) {
    5050    f->setup(1. / T->Value(), cutoff->Value());
     51  }
     52 
    5153  f->reset();
    5254
    5355  first_update = true;
     56  this->order=order;
    5457}
    5558
    … …  
    5962  float result;
    6063  cvmatrix *input = (cvmatrix *)data;
    61 
    62   if (T->ValueChanged() || cutoff->ValueChanged()) {
     64  float delta_t;
     65 
     66  if (T->ValueChanged()) {
    6367    if (T->Value() != 0) {
    6468      f->setup(1. / T->Value(), cutoff->Value());
    65     } else {
    66       first_update = true;
     69      settingsChanged(input->Value(0, 0));
    6770    }
    6871  }
    6972
     73  if (T->Value() == 0) {
     74    delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
     75    f->setup(1. / delta_t, cutoff->Value());
     76  } else {
     77    delta_t=T->Value();
     78  }
     79 
    7080  // on prend une fois pour toute les mutex et on fait des accès directs
    7181  output->GetMutex();
    7282  input->GetMutex();
    7383
    74   if (T->Value() == 0) {
    75     float delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
     84  if (cutoff->ValueChanged()) {
    7685    f->setup(1. / delta_t, cutoff->Value());
     86    settingsChanged(input->ValueNoMutex(0, 0));
    7787  }
    7888
    … …  
    90100  previous_time = data->DataTime();
    91101}
     102
     103//ne gere pas les oscillations (s'arrete des qu'une valeure est bonne a 5%)
     104void ButterworthLowPass_impl::settingsChanged(float inputValue) {
     105  float result=f->filter(inputValue);
     106 
     107  while(result<inputValue*0.95 || result>inputValue*1.05) {
     108      result=f->filter(inputValue);
     109  }
     110}

  • trunk/lib/FlairFilter/src/EulerDerivative.h

    r15 r147  
    4545  * IODevice::ProcessUpdate is called. \n
    4646  * The optional init_value parameters allow to specify
    47   * the size of the input datas and its inital values.
     47  * the size of the input datas and output inital values.
    4848  * If unspecified, a 1*1 size is used, and values are
    4949  * initialized with 0.
    … …  
    5252  * \param position position to display settings
    5353  * \param name name
    54   * \param init_value initial value
     54  * \param init_value initial value, it is safe to destroy it after calling this constructor
    5555  */
    5656  EulerDerivative(const core::IODevice *parent,

  • trunk/lib/FlairFilter/src/EulerDerivative_impl.cpp

    r15 r147  
    3333                                           const cvmatrix *init_value) {
    3434  first_update = true;
     35 
     36  if (init_value != NULL) {
     37    // init output matrix of same size as init
     38    cvmatrix_descriptor *desc =new cvmatrix_descriptor(init_value->Rows(), init_value->Cols());
    3539
    36   if (init_value != NULL) {
    37     prev_value = (cvmatrix *)init_value;
     40    for (int i = 0; i < init_value->Rows(); i++) {
     41      for (int j = 0; j < init_value->Cols(); j++) {
     42        desc->SetElementName(i, j, init_value->Name(i, j));
     43      }
     44    }
     45    output = new cvmatrix(self, desc,init_value->GetDataType().GetElementDataType(), name);
     46    for (int i = 0; i < init_value->Rows(); i++) {
     47      for (int j = 0; j < init_value->Cols(); j++) {
     48        output->SetValue(i, j, init_value->Value(i,j));
     49      }
     50    }
    3851  } else {
    3952    // if NULL, assume dimension 1, and init=0
    4053    cvmatrix_descriptor *desc = new cvmatrix_descriptor(1, 1);
    4154    desc->SetElementName(0, 0, "output");
    42     prev_value = new cvmatrix(self, desc, floatType, name);
    43     prev_value->SetValue(0, 0, 0);
     55    output = new cvmatrix(self, desc, floatType, name);
     56    output->SetValue(0, 0, 0);
    4457  }
     58 
     59 
     60  cvmatrix_descriptor *desc = new cvmatrix_descriptor(output->Rows(), output->Cols());
     61  prev_value = new cvmatrix(self, desc, output->GetDataType().GetElementDataType(), name);
     62
    4563
    4664  // init UI
    … …  
    4866  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto:",
    4967                        " s", 0, 1, 0.01);
    50 
    51   // init output matrix of same size as init
    52   cvmatrix_descriptor *desc =
    53       new cvmatrix_descriptor(prev_value->Rows(), prev_value->Cols());
    54 
    55   for (int i = 0; i < prev_value->Rows(); i++) {
    56     for (int j = 0; j < prev_value->Cols(); j++) {
    57       desc->SetElementName(i, j, prev_value->Name(i, j));
    58     }
    59   }
    60 
    61   output = new cvmatrix(self, desc,
    62                         prev_value->GetDataType().GetElementDataType(), name);
    6368}
    6469
    … …  
    7681    for (int i = 0; i < input->Rows(); i++) {
    7782      for (int j = 0; j < input->Cols(); j++) {
    78         output->SetValueNoMutex(i, j, prev_value->ValueNoMutex(i, j));
    7983        prev_value->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
    8084      }

  • trunk/lib/FlairFilter/src/LowPassFilter.h

    r15 r147  
    5252  * \param position position to display settings
    5353  * \param name name
    54   * \param init_value initial value
     54  * \param init_value initial value, it is safe to destroy it after calling this constructor
    5555  */
    5656  LowPassFilter(const core::IODevice *parent,

  • 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();

  • trunk/lib/FlairFilter/src/unexported/ButterworthLowPass_impl.h

    r15 r147  
    128128
    129129private:
     130  void settingsChanged(float inputValue);
    130131  flair::gui::DoubleSpinBox *cutoff, *T;
    131132  PoleFilter *f;
    132133  bool first_update;
    133134  flair::core::Time previous_time;
     135  int order;
    134136};
    135137

  • trunk/lib/FlairFilter/src/unexported/LowPassFilter_impl.h

    r15 r147  
    4343private:
    4444  flair::core::Time previous_time;
    45   bool first_update;
    46   flair::core::cvmatrix *prev_value;
    4745  flair::gui::DoubleSpinBox *freq, *T;
    4846};

  • trunk/lib/FlairMeta/src/MetaVrpnObject.cpp

    r135 r147  
    6262  pbas = new LowPassFilter(this, GetVrpnClient()->GetLayout()->NewRow(),
    6363                           name + " Passe bas", prev_value);
     64  delete prev_value;
    6465
    6566  desc = new cvmatrix_descriptor(7, 1);
    … …  
    7475  euler = new EulerDerivative(pbas, GetVrpnClient()->GetLayout()->NewRow(),
    7576                              name + "_euler", prev_value);
     77  delete prev_value;
    7678
    7779  vx_opti_plot = new DataPlot1D(GetPlotTab()->NewRow(), "vx", -3, 3);

  • trunk/lib/FlairSimulator/src/SimuCameraGL.cpp

    r146 r147  
    191191
    192192  shmem->Write(buf, width * height * 3);
    193 Printf("écriture faite!\n");
    194193}
    195194
Note: See TracChangeset for help on using the changeset viewer.