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

Last change on this file since 10 was 10, checked in by Sanahuja Guillaume, 7 years ago

lic

File size: 3.6 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: 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"
19#include <cvmatrix.h>
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
30PidThrust_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;
35
36 //init matrix
37 self->input=new cvmatrix(self,2,1,floatType,name);
38
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);
46
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);
56}
57
58PidThrust_impl::~PidThrust_impl(void) {
59}
60
61void 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);
69}
70
71void 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 TracBrowser for help on using the repository browser.