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, 6 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.