source: flair-src/trunk/lib/FlairFilter/src/X4X8Multiplex_impl.cpp @ 7

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

filter and meta

File size: 6.3 KB
Line 
1//  created:    2014/04/10
2//  filename:   X4X8Multiplex_impl.cpp
3//
4//  author:     Guillaume Sanahuja
5//              Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7//  version:    $Id: $
8//
9//  purpose:    Class defining X4 and X8 multiplexing
10//
11//
12/*********************************************************************/
13
14#include "X4X8Multiplex_impl.h"
15#include "X4X8Multiplex.h"
16#include <cvmatrix.h>
17#include <Tab.h>
18#include <GridLayout.h>
19#include <GroupBox.h>
20#include <ComboBox.h>
21#include <DataPlot1D.h>
22#include <math.h>
23
24#define MAX_MOTORS 8
25
26using std::string;
27using namespace flair::core;
28using namespace flair::gui;
29using namespace flair::filter;
30
31X4X8Multiplex_impl::X4X8Multiplex_impl(flair::filter::X4X8Multiplex* self,int nb_mot)
32{
33    this->nb_mot=nb_mot;
34    this->self=self;
35
36    if(nb_mot==4)
37    {
38        GroupBox *groupbox=new GroupBox(self->GetLayout()->NewRow(),"x4 multiplex");
39        pas=new ComboBox(groupbox->NewRow(),"front left blade pitch:");
40    }
41    else
42    {
43        GroupBox *groupbox=new GroupBox(self->GetLayout()->NewRow(),"x8 multiplex");
44        pas=new ComboBox(groupbox->NewRow(),"top front left blade pitch:");
45    }
46    pas->AddItem("normal");
47    pas->AddItem("inverted");
48
49    cvmatrix_descriptor* desc=new cvmatrix_descriptor(nb_mot,1);
50    for(int i=0;i<nb_mot;i++)
51    {
52        desc->SetElementName(i,0,MotorName(i));
53    }
54
55    output=new cvmatrix(self,desc,floatType);
56
57    self->AddDataToLog(output);
58}
59
60X4X8Multiplex_impl::~X4X8Multiplex_impl(void)
61{
62}
63
64void X4X8Multiplex_impl::UseDefaultPlot(void)
65{
66    Tab *plot_tab=new Tab(self->GetTabWidget(),"Values");
67    plots[0]=new DataPlot1D(plot_tab->NewRow(),"front left",0,1);
68    plots[1]=new DataPlot1D(plot_tab->LastRowLastCol(),"front right",0,1);
69    plots[2]=new DataPlot1D(plot_tab->NewRow(),"rear left",0,1);
70    plots[3]=new DataPlot1D(plot_tab->LastRowLastCol(),"rear right",0,1);
71
72    for(int i=0;i<4;i++) plots[i]->AddCurve(output->Element(i));
73
74    if(nb_mot==8)
75    {
76        for(int i=0;i<4;i++) plots[i]->AddCurve(output->Element(i+4),DataPlot::Blue);
77    }
78}
79
80void X4X8Multiplex_impl::UpdateFrom(const io_data *data)
81{
82    float u_roll,u_pitch,u_yaw,u_thrust;
83    float trim_roll,trim_pitch,trim_yaw;
84    float value[MAX_MOTORS];
85
86    cvmatrix* input=(cvmatrix*)data;
87
88    //on prend une fois pour toute le mutex et on fait des accès directs
89    input->GetMutex();
90
91    u_roll=input->ValueNoMutex(0,0);
92    u_pitch=input->ValueNoMutex(1,0);
93    u_yaw=input->ValueNoMutex(2,0);
94    u_thrust=input->ValueNoMutex(3,0);
95    trim_roll=input->ValueNoMutex(4,0);
96    trim_pitch=input->ValueNoMutex(5,0);
97    trim_yaw=input->ValueNoMutex(6,0);
98
99    input->ReleaseMutex();
100
101    if(pas->CurrentIndex()==1)
102    {
103        trim_yaw=-trim_yaw;
104        u_yaw=-u_yaw;
105    }
106
107if(nb_mot==2)
108{
109    //(top) front left
110    value[self->MultiplexValue(X4X8Multiplex::TopFrontLeft)]=Set(trim_pitch+trim_roll+trim_yaw,
111                 u_thrust+u_pitch+u_roll+u_yaw);
112
113    //(top) front right
114    value[self->MultiplexValue(X4X8Multiplex::TopFrontRight)]=Set(trim_pitch-trim_roll-trim_yaw,
115                 u_thrust+u_pitch-u_roll-u_yaw);
116}
117    if(nb_mot==4 || nb_mot==8)
118    {
119        //(top) front left
120        value[self->MultiplexValue(X4X8Multiplex::TopFrontLeft)]=Set(trim_pitch+trim_roll+trim_yaw,
121                     u_thrust+u_pitch+u_roll+u_yaw);
122
123        //(top) front right
124        value[self->MultiplexValue(X4X8Multiplex::TopFrontRight)]=Set(trim_pitch-trim_roll-trim_yaw,
125                     u_thrust+u_pitch-u_roll-u_yaw);
126
127        //(top) rear left
128        value[self->MultiplexValue(X4X8Multiplex::TopRearLeft)]=Set(-trim_pitch+trim_roll-trim_yaw,
129                     u_thrust-u_pitch+u_roll-u_yaw);
130
131        //(top) rear right
132        value[self->MultiplexValue(X4X8Multiplex::TopRearRight)]=Set(-trim_pitch-trim_roll+trim_yaw,
133                     u_thrust-u_pitch-u_roll+u_yaw);
134    }
135
136    if(nb_mot==8)
137    {
138        //bottom front left
139        value[self->MultiplexValue(X4X8Multiplex::BottomFrontLeft)]=Set(trim_pitch+trim_roll-trim_yaw,
140                     u_thrust+u_pitch+u_roll-u_yaw);
141
142        //bottom front right
143        value[self->MultiplexValue(X4X8Multiplex::BottomFrontRight)]=Set(trim_pitch-trim_roll+trim_yaw,
144                     u_thrust+u_pitch-u_roll+u_yaw);
145
146        //bottom rear left
147        value[self->MultiplexValue(X4X8Multiplex::BottomRearLeft)]=Set(-trim_pitch+trim_roll+trim_yaw,
148                     u_thrust-u_pitch+u_roll+u_yaw);
149
150        //bottom rear right
151        value[self->MultiplexValue(X4X8Multiplex::BottomRearRight)]=Set(-trim_pitch-trim_roll-trim_yaw,
152                     u_thrust-u_pitch-u_roll-u_yaw);
153
154    }
155
156    //on prend une fois pour toute le mutex et on fait des accès directs
157    output->GetMutex();
158    for(int i=0;i<nb_mot;i++) output->SetValueNoMutex(i,0,value[i]);
159    output->ReleaseMutex();
160
161    output->SetDataTime(data->DataTime());
162
163    self->ProcessUpdate(output);
164}
165
166float X4X8Multiplex_impl::Set(float trim,float u)
167{
168    float value=trim;
169
170    if(u>0)
171    {
172        value+=sqrtf(u);
173    }
174
175    return value;
176}
177
178string X4X8Multiplex_impl::MotorName(int index)
179{
180    switch(nb_mot)
181    {
182        case 4:
183        {
184            switch(index)
185            {
186                case 0:
187                    return "front left";
188                case 1:
189                    return "front rigth";
190                case 2:
191                    return "rear left";
192                case 3:
193                    return "rear rigth";
194                default:
195                    return "unammed motor";
196            }
197        }
198        case 8:
199        {
200            switch(index)
201            {
202                case 0:
203                    return "top front left";
204                case 1:
205                    return "top front rigth";
206                case 2:
207                    return "top rear left";
208                case 3:
209                    return "top rear rigth";
210                case 4:
211                    return "bottom front left";
212                case 5:
213                    return "bottom front rigth";
214                case 6:
215                    return "bottom rear left";
216                case 7:
217                    return "bottom rear rigth";
218                default:
219                    return "unammed motor";
220            }
221        }
222        default:
223        {
224            return "unammed motor";
225        }
226    }
227}
Note: See TracBrowser for help on using the repository browser.