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

Last change on this file was 318, checked in by Sanahuja Guillaume, 2 years ago
File size: 3.8 KB
RevLine 
[10]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[10]4// %flair:license}
[7]5//  created:    2014/11/07
6//  filename:   PidThrust_impl.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Class defining a PID
14//
15//
16/*********************************************************************/
17#include "PidThrust_impl.h"
18#include "PidThrust.h"
[214]19#include <Matrix.h>
[7]20#include <Layout.h>
21#include <GroupBox.h>
22#include <DoubleSpinBox.h>
23#include <DataPlot1D.h>
24
25using std::string;
26using namespace flair::core;
27using namespace flair::gui;
28using namespace flair::filter;
29
[15]30PidThrust_impl::PidThrust_impl(PidThrust *self, const LayoutPosition *position,
31                               string name) {
32  i = 0;
33  offset_g = 0;
34  first_update = true;
35  this->self = self;
[7]36
[15]37  // init matrix
[214]38  self->input = new Matrix(self, 2, 1, floatType, name);
[7]39
[318]40  MatrixDescriptor *desc = new MatrixDescriptor(5, 1);
[15]41  desc->SetElementName(0, 0, "p");
42  desc->SetElementName(1, 0, "i");
43  desc->SetElementName(2, 0, "d");
44  desc->SetElementName(3, 0, "p+i+d");
45  desc->SetElementName(4, 0, "p+i+d+offset");
[214]46  state = new Matrix(self, desc, floatType, name);
[148]47  delete desc;
[7]48
[15]49  GroupBox *reglages_groupbox = new GroupBox(position, name);
50  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto", " s",
51                        0, 1, 0.01);
52  kp = new DoubleSpinBox(reglages_groupbox->NewRow(), "kp:", 0, 90000000, 0.01);
53  ki = new DoubleSpinBox(reglages_groupbox->NewRow(), "ki:", 0, 90000000, 0.01);
54  sati = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "sat i:", 0, 1,
55                           0.01);
56  kd = new DoubleSpinBox(reglages_groupbox->NewRow(), "kd:", 0, 90000000, 0.01);
57  offset = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "offset g:",
58                             0, 1, 0.01);
59  sat = new DoubleSpinBox(reglages_groupbox->NewRow(), "sat:", 0, 1, 0.1);
60  pas_offset = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
61                                 "offset step:", 0, 1, .0001, 4);
[7]62}
63
[15]64PidThrust_impl::~PidThrust_impl(void) {}
[7]65
[15]66void PidThrust_impl::UseDefaultPlot(const LayoutPosition *position) {
67  DataPlot1D *plot = new DataPlot1D(position, self->ObjectName(), -1, 1);
68  plot->AddCurve(state->Element(0));
69  plot->AddCurve(state->Element(1), DataPlot::Green);
70  plot->AddCurve(state->Element(2), DataPlot::Blue);
71  plot->AddCurve(state->Element(3), DataPlot::Black);
72  plot->AddCurve(state->Element(4), DataPlot::Yellow);
[7]73}
74
[15]75void PidThrust_impl::UpdateFrom(const io_data *data) {
76  float p, d, total;
77  float delta_t;
[214]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  }
[7]84
[15]85  if (T->Value() == 0) {
[223]86    delta_t = (float)(data->DataDeltaTime()) / 1000000000.;
[15]87  } else {
88    delta_t = T->Value();
89  }
90  if (first_update == true) {
91    delta_t = 0;
92    first_update = false;
93  }
[7]94
[15]95  input->GetMutex();
96  p = kp->Value() * input->ValueNoMutex(0, 0);
97  i += ki->Value() * input->ValueNoMutex(0, 0) * delta_t;
98  if (i > sati->Value())
99    i = sati->Value();
100  if (i < -sati->Value())
101    i = -sati->Value();
102  d = kd->Value() * input->ValueNoMutex(1, 0);
103  input->ReleaseMutex();
[7]104
[15]105  total = p + i + d;
106  if (total > sat->Value())
107    total = sat->Value();
108  if (total < -sat->Value())
109    total = -sat->Value();
[7]110
[15]111  state->GetMutex();
112  state->SetValueNoMutex(0, 0, p);
113  state->SetValueNoMutex(1, 0, i);
114  state->SetValueNoMutex(2, 0, d);
115  state->SetValueNoMutex(3, 0, total);
116  state->SetValueNoMutex(4, 0, total - offset_g * offset_g);
117  state->ReleaseMutex();
[7]118
[15]119  //-offset_g, car on met -u_z dans le multiplex
120  // a revoir!
121  self->output->SetValue(0, 0, total - offset_g * offset_g);
122  self->output->SetDataTime(data->DataTime());
[7]123}
Note: See TracBrowser for help on using the repository browser.