Ignore:
Timestamp:
Apr 8, 2016, 3:40:57 PM (9 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

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

    r10 r15  
    2828using namespace flair::filter;
    2929
    30 PidThrust_impl::PidThrust_impl(PidThrust* self,const LayoutPosition* position,string name) {
    31     i=0;
    32     offset_g=0;
    33         first_update=true;
    34         this->self=self;
     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;
    3536
    36     //init matrix
    37     self->input=new cvmatrix(self,2,1,floatType,name);
     37  // init matrix
     38  self->input = new cvmatrix(self, 2, 1, floatType, name);
    3839
    39     cvmatrix_descriptor* desc=new cvmatrix_descriptor(5,1);
    40     desc->SetElementName(0,0,"p");
    41         desc->SetElementName(1,0,"i");
    42         desc->SetElementName(2,0,"d");
    43         desc->SetElementName(3,0,"p+i+d");
    44         desc->SetElementName(4,0,"p+i+d+offset");
    45     state=new cvmatrix(self,desc,floatType,name);
     40  cvmatrix_descriptor *desc = new cvmatrix_descriptor(5, 1);
     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");
     46  state = new cvmatrix(self, desc, floatType, name);
    4647
    47     GroupBox* reglages_groupbox=new GroupBox(position,name);
    48             T=new DoubleSpinBox(reglages_groupbox->NewRow(),"period, 0 for auto"," s",0,1,0.01);
    49             kp=new DoubleSpinBox(reglages_groupbox->NewRow(),"kp:",0,90000000,0.01);
    50             ki=new DoubleSpinBox(reglages_groupbox->NewRow(),"ki:",0,90000000,0.01);
    51             sati=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"sat i:",0,1,0.01);
    52             kd=new DoubleSpinBox(reglages_groupbox->NewRow(),"kd:",0,90000000,0.01);
    53             offset=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"offset g:",0,1,0.01);
    54             sat=new DoubleSpinBox(reglages_groupbox->NewRow(),"sat:",0,1,0.1);
    55             pas_offset=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"offset step:",0,1,.0001,4);
     48  GroupBox *reglages_groupbox = new GroupBox(position, name);
     49  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto", " s",
     50                        0, 1, 0.01);
     51  kp = new DoubleSpinBox(reglages_groupbox->NewRow(), "kp:", 0, 90000000, 0.01);
     52  ki = new DoubleSpinBox(reglages_groupbox->NewRow(), "ki:", 0, 90000000, 0.01);
     53  sati = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "sat i:", 0, 1,
     54                           0.01);
     55  kd = new DoubleSpinBox(reglages_groupbox->NewRow(), "kd:", 0, 90000000, 0.01);
     56  offset = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "offset g:",
     57                             0, 1, 0.01);
     58  sat = new DoubleSpinBox(reglages_groupbox->NewRow(), "sat:", 0, 1, 0.1);
     59  pas_offset = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
     60                                 "offset step:", 0, 1, .0001, 4);
    5661}
    5762
    58 PidThrust_impl::~PidThrust_impl(void) {
     63PidThrust_impl::~PidThrust_impl(void) {}
     64
     65void PidThrust_impl::UseDefaultPlot(const LayoutPosition *position) {
     66  DataPlot1D *plot = new DataPlot1D(position, self->ObjectName(), -1, 1);
     67  plot->AddCurve(state->Element(0));
     68  plot->AddCurve(state->Element(1), DataPlot::Green);
     69  plot->AddCurve(state->Element(2), DataPlot::Blue);
     70  plot->AddCurve(state->Element(3), DataPlot::Black);
     71  plot->AddCurve(state->Element(4), DataPlot::Yellow);
    5972}
    6073
    61 void PidThrust_impl::UseDefaultPlot(const LayoutPosition* position)
    62 {
    63     DataPlot1D *plot=new DataPlot1D(position,self->ObjectName(),-1,1);
    64             plot->AddCurve(state->Element(0));
    65             plot->AddCurve(state->Element(1),DataPlot::Green);
    66             plot->AddCurve(state->Element(2),DataPlot::Blue);
    67             plot->AddCurve(state->Element(3),DataPlot::Black);
    68             plot->AddCurve(state->Element(4),DataPlot::Yellow);
     74void PidThrust_impl::UpdateFrom(const io_data *data) {
     75  float p, d, total;
     76  float delta_t;
     77  cvmatrix *input = (cvmatrix *)data;
     78
     79  if (T->Value() == 0) {
     80    delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
     81  } else {
     82    delta_t = T->Value();
     83  }
     84  if (first_update == true) {
     85    delta_t = 0;
     86    first_update = false;
     87  }
     88
     89  input->GetMutex();
     90  p = kp->Value() * input->ValueNoMutex(0, 0);
     91  i += ki->Value() * input->ValueNoMutex(0, 0) * delta_t;
     92  if (i > sati->Value())
     93    i = sati->Value();
     94  if (i < -sati->Value())
     95    i = -sati->Value();
     96  d = kd->Value() * input->ValueNoMutex(1, 0);
     97  input->ReleaseMutex();
     98
     99  total = p + i + d;
     100  if (total > sat->Value())
     101    total = sat->Value();
     102  if (total < -sat->Value())
     103    total = -sat->Value();
     104
     105  state->GetMutex();
     106  state->SetValueNoMutex(0, 0, p);
     107  state->SetValueNoMutex(1, 0, i);
     108  state->SetValueNoMutex(2, 0, d);
     109  state->SetValueNoMutex(3, 0, total);
     110  state->SetValueNoMutex(4, 0, total - offset_g * offset_g);
     111  state->ReleaseMutex();
     112
     113  //-offset_g, car on met -u_z dans le multiplex
     114  // a revoir!
     115  self->output->SetValue(0, 0, total - offset_g * offset_g);
     116  self->output->SetDataTime(data->DataTime());
     117
     118  previous_time = data->DataTime();
    69119}
    70 
    71 void PidThrust_impl::UpdateFrom(const io_data *data)
    72 {
    73     float p,d,total;
    74     float delta_t;
    75     cvmatrix *input=(cvmatrix*)data;
    76 
    77     if(T->Value()==0)
    78     {
    79         delta_t=(float)(data->DataTime()-previous_time)/1000000000.;
    80     }
    81     else
    82     {
    83         delta_t=T->Value();
    84     }
    85     if(first_update==true)
    86     {
    87         delta_t=0;
    88         first_update=false;
    89     }
    90 
    91     input->GetMutex();
    92     p=kp->Value()*input->ValueNoMutex(0,0);
    93     i+=ki->Value()*input->ValueNoMutex(0,0)*delta_t;
    94     if(i>sati->Value()) i=sati->Value();
    95     if(i<-sati->Value()) i=-sati->Value();
    96     d=kd->Value()*input->ValueNoMutex(1,0);
    97     input->ReleaseMutex();
    98 
    99     total=p+i+d;
    100     if(total>sat->Value()) total=sat->Value();
    101     if(total<-sat->Value()) total=-sat->Value();
    102 
    103     state->GetMutex();
    104     state->SetValueNoMutex(0,0,p);
    105     state->SetValueNoMutex(1,0,i);
    106     state->SetValueNoMutex(2,0,d);
    107     state->SetValueNoMutex(3,0,total);
    108     state->SetValueNoMutex(4,0,total-offset_g*offset_g);
    109     state->ReleaseMutex();
    110 
    111     //-offset_g, car on met -u_z dans le multiplex
    112     //a revoir!
    113     self->output->SetValue(0,0,total-offset_g*offset_g);
    114     self->output->SetDataTime(data->DataTime());
    115 
    116     previous_time=data->DataTime();
    117 }
Note: See TracChangeset for help on using the changeset viewer.