close Warning: Can't use blame annotator: 
svn blame failed on trunk/lib/FlairFilter/src/X4X8Multiplex_impl.cpp: 200029 - Couldn't perform atomic initialization

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

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

lic
File size: 6.4 KB
RevLine 
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
30using std::string;
31using namespace flair::core;
32using namespace flair::gui;
33using namespace flair::filter;
34
35X4X8Multiplex_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
64X4X8Multiplex_impl::~X4X8Multiplex_impl(void)
65{
66}
67
68void 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
84void 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
111if(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
170float 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
182string 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}
Note: See TracBrowser for help on using the repository browser.