Changeset 15 in flair-src for trunk/lib/FlairFilter/src/PidThrust_impl.cpp
- Timestamp:
- Apr 8, 2016, 3:40:57 PM (9 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairFilter/src/PidThrust_impl.cpp
r10 r15 28 28 using namespace flair::filter; 29 29 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; 30 PidThrust_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; 35 36 36 //init matrix37 self->input=new cvmatrix(self,2,1,floatType,name);37 // init matrix 38 self->input = new cvmatrix(self, 2, 1, floatType, name); 38 39 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); 46 47 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); 56 61 } 57 62 58 PidThrust_impl::~PidThrust_impl(void) { 63 PidThrust_impl::~PidThrust_impl(void) {} 64 65 void 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); 59 72 } 60 73 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); 74 void 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(); 69 119 } 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 else82 {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 multiplex112 //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.