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