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: 2013/04/29
|
---|
6 | // filename: BlCtrlV2_x4_speed.cpp
|
---|
7 | //
|
---|
8 | // author: Guillaume Sanahuja
|
---|
9 | // Copyright Heudiasyc UMR UTC/CNRS 7253
|
---|
10 | //
|
---|
11 | // version: $Id: $
|
---|
12 | //
|
---|
13 | // purpose: objet integrant les moteurs i2c, controle en vitesse
|
---|
14 | //
|
---|
15 | //
|
---|
16 | /*********************************************************************/
|
---|
17 |
|
---|
18 | #include "BlCtrlV2_x4_speed.h"
|
---|
19 | #include "I2cPort.h"
|
---|
20 | #include <TabWidget.h>
|
---|
21 | #include <Tab.h>
|
---|
22 | #include <GroupBox.h>
|
---|
23 | #include <SpinBox.h>
|
---|
24 | #include <DoubleSpinBox.h>
|
---|
25 | #include <ComboBox.h>
|
---|
26 | #include <PushButton.h>
|
---|
27 | #include <cvmatrix.h>
|
---|
28 | #include <Mutex.h>
|
---|
29 | #include <FrameworkManager.h>
|
---|
30 | #include <DataPlot1D.h>
|
---|
31 | #include <math.h>
|
---|
32 | #include <string.h>
|
---|
33 |
|
---|
34 | #define TAU_US 1000
|
---|
35 |
|
---|
36 | using std::string;
|
---|
37 | using namespace flair::core;
|
---|
38 | using namespace flair::gui;
|
---|
39 |
|
---|
40 | namespace flair
|
---|
41 | {
|
---|
42 | namespace actuator
|
---|
43 | {
|
---|
44 | BlCtrlV2_x4_speed::BlCtrlV2_x4_speed(FrameworkManager* parent,string name,I2cPort* i2cport,uint8_t base_address,uint8_t priority) : Thread(parent,name,priority),IODevice(parent,name)
|
---|
45 | {
|
---|
46 | this->i2cport=i2cport;
|
---|
47 | slave_address=base_address;
|
---|
48 | tested_motor=-1;
|
---|
49 | enabled=false;
|
---|
50 | int_av_g=0;
|
---|
51 | int_av_d=0;
|
---|
52 | int_ar_g=0;
|
---|
53 | int_ar_d=0;
|
---|
54 |
|
---|
55 | //flight time
|
---|
56 | FILE *file;
|
---|
57 | file=fopen("/etc/flight_time","r");
|
---|
58 | if (file == NULL)
|
---|
59 | {
|
---|
60 | Printf("fichier d'info de vol vide\n");
|
---|
61 | time_sec=0;
|
---|
62 | }
|
---|
63 | else
|
---|
64 | {
|
---|
65 | char ligne[32];
|
---|
66 | fgets(ligne, 32, file);
|
---|
67 | time_sec=atoi(ligne);
|
---|
68 | Printf("temps de vol total: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
|
---|
69 | fclose(file);
|
---|
70 | }
|
---|
71 |
|
---|
72 | //station sol
|
---|
73 | main_tab=new Tab(parent->GetTabWidget(),name);
|
---|
74 | tab=new TabWidget(main_tab->NewRow(),name);
|
---|
75 | Tab *sensor_tab=new Tab(tab,"Reglages");
|
---|
76 | reglages_groupbox=new GroupBox(sensor_tab->NewRow(),name);
|
---|
77 | poles=new SpinBox(reglages_groupbox->NewRow(),"nb poles",0,255,1);
|
---|
78 | kp=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"kp",0.,255,0.001,4);
|
---|
79 | ki=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"ki",0.,255,0.001,4);
|
---|
80 | min=new SpinBox(reglages_groupbox->NewRow(),"min pwm",0.,2048,1);
|
---|
81 | max=new SpinBox(reglages_groupbox->LastRowLastCol(),"max pwm",0.,2048,1);
|
---|
82 | test=new SpinBox(reglages_groupbox->LastRowLastCol(),"test value",0.,2048,1);
|
---|
83 | start_value=new SpinBox(reglages_groupbox->NewRow(),"valeur demarrage",0,10000,10);
|
---|
84 | trim=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"pas decollage",0,1000,.1);
|
---|
85 |
|
---|
86 | av_g=new ComboBox(reglages_groupbox->NewRow(),"avant gauche");
|
---|
87 | av_g->AddItem("1");
|
---|
88 | av_g->AddItem("2");
|
---|
89 | av_g->AddItem("3");
|
---|
90 | av_g->AddItem("4");
|
---|
91 | button_avg=new PushButton(reglages_groupbox->LastRowLastCol(),"test avg");
|
---|
92 |
|
---|
93 | av_d=new ComboBox(reglages_groupbox->LastRowLastCol(),"avant droite:");
|
---|
94 | av_d->AddItem("1");
|
---|
95 | av_d->AddItem("2");
|
---|
96 | av_d->AddItem("3");
|
---|
97 | av_d->AddItem("4");
|
---|
98 | button_avd=new PushButton(reglages_groupbox->LastRowLastCol(),"test avd");
|
---|
99 |
|
---|
100 | ar_g=new ComboBox(reglages_groupbox->NewRow(),"arriere gauche:");
|
---|
101 | ar_g->AddItem("1");
|
---|
102 | ar_g->AddItem("2");
|
---|
103 | ar_g->AddItem("3");
|
---|
104 | ar_g->AddItem("4");
|
---|
105 | button_arg=new PushButton(reglages_groupbox->LastRowLastCol(),"test arg");
|
---|
106 |
|
---|
107 | ar_d=new ComboBox(reglages_groupbox->LastRowLastCol(),"arriere droite:");
|
---|
108 | ar_d->AddItem("1");
|
---|
109 | ar_d->AddItem("2");
|
---|
110 | ar_d->AddItem("3");
|
---|
111 | ar_d->AddItem("4");
|
---|
112 | button_ard=new PushButton(reglages_groupbox->LastRowLastCol(),"test ard");
|
---|
113 |
|
---|
114 | pas=new ComboBox(reglages_groupbox->NewRow(),"pas helice avant gauche:");
|
---|
115 | pas->AddItem("normal");
|
---|
116 | pas->AddItem("inverse");
|
---|
117 |
|
---|
118 | input=new cvmatrix((IODevice*)this,8,1,floatType);
|
---|
119 |
|
---|
120 | cvmatrix_descriptor* desc=new cvmatrix_descriptor(4,2);
|
---|
121 | desc->SetElementName(0,0,"avant gauche");
|
---|
122 | desc->SetElementName(1,0,"arriere droite");
|
---|
123 | desc->SetElementName(2,0,"avant droite");
|
---|
124 | desc->SetElementName(3,0,"arriere gauche");
|
---|
125 |
|
---|
126 | desc->SetElementName(0,1,"cons avant gauche");
|
---|
127 | desc->SetElementName(1,1,"cons arriere droite");
|
---|
128 | desc->SetElementName(2,1,"cons avant droite");
|
---|
129 | desc->SetElementName(3,1,"cons arriere gauche");
|
---|
130 | output=new cvmatrix((IODevice*)this,desc,floatType);
|
---|
131 |
|
---|
132 |
|
---|
133 |
|
---|
134 | /*
|
---|
135 |
|
---|
136 | //le 3ieme lu est la tension batteire
|
---|
137 | if(i2c_mutex!=NULL) i2c_mutex->GetMutex();
|
---|
138 | uint16_t pwm_moteur;
|
---|
139 | pwm_moteur=0;
|
---|
140 | ssize_t read;
|
---|
141 | uint8_t rx[8];
|
---|
142 | SetSlave(slave_address);
|
---|
143 |
|
---|
144 | for(int j=0;j<10;j++)
|
---|
145 | {
|
---|
146 |
|
---|
147 |
|
---|
148 | WriteValue(pwm_moteur);
|
---|
149 |
|
---|
150 |
|
---|
151 | read = rt_dev_read(i2c_fd, rx, sizeof(rx));
|
---|
152 |
|
---|
153 | if(read<0)
|
---|
154 | {
|
---|
155 | rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur rt_dev_read (%s)\n",IODevice::ObjectName().c_str(),strerror(-read));
|
---|
156 | }
|
---|
157 | else if (read != sizeof(rx))
|
---|
158 | {
|
---|
159 | rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur rt_dev_read %i/2\n",IODevice::ObjectName().c_str(),read);
|
---|
160 |
|
---|
161 | }
|
---|
162 | for(int i=0;i<sizeof(rx);i++) printf("%i ",rx[i]);
|
---|
163 |
|
---|
164 | printf("\n");
|
---|
165 |
|
---|
166 | }
|
---|
167 |
|
---|
168 | if(i2c_mutex!=NULL) i2c_mutex->ReleaseMutex();*/
|
---|
169 | }
|
---|
170 |
|
---|
171 | BlCtrlV2_x4_speed::~BlCtrlV2_x4_speed(void)
|
---|
172 | {
|
---|
173 | SafeStop();
|
---|
174 | Join();
|
---|
175 | delete main_tab;
|
---|
176 | }
|
---|
177 |
|
---|
178 | void BlCtrlV2_x4_speed::UseDefaultPlot(void)
|
---|
179 | {
|
---|
180 | Tab *plot_tab=new Tab(tab,"Mesures");
|
---|
181 | DataPlot1D *av_g_plot=new DataPlot1D(plot_tab->NewRow(),"avg",0,10000);
|
---|
182 | av_g_plot->AddCurve(output->Element(0,0));
|
---|
183 | av_g_plot->AddCurve(output->Element(0,1),DataPlot::Blue);
|
---|
184 | DataPlot1D *av_d_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"avd",0,10000);
|
---|
185 | av_d_plot->AddCurve(output->Element(2,0));
|
---|
186 | av_d_plot->AddCurve(output->Element(2,1),DataPlot::Blue);
|
---|
187 | DataPlot1D *ar_g_plot=new DataPlot1D(plot_tab->NewRow(),"arg",0,10000);
|
---|
188 | ar_g_plot->AddCurve(output->Element(3,0));
|
---|
189 | ar_g_plot->AddCurve(output->Element(3,1),DataPlot::Blue);
|
---|
190 | DataPlot1D *ar_d_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"ard",0,10000);
|
---|
191 | ar_d_plot->AddCurve(output->Element(1,0));
|
---|
192 | ar_d_plot->AddCurve(output->Element(1,1),DataPlot::Blue);
|
---|
193 | }
|
---|
194 |
|
---|
195 | float BlCtrlV2_x4_speed::TrimValue(void)
|
---|
196 | {
|
---|
197 | return (float)trim->Value();
|
---|
198 | }
|
---|
199 |
|
---|
200 | int BlCtrlV2_x4_speed::StartValue(void)
|
---|
201 | {
|
---|
202 | return start_value->Value();
|
---|
203 | }
|
---|
204 |
|
---|
205 | void BlCtrlV2_x4_speed::Run(void)
|
---|
206 | {
|
---|
207 | WarnUponSwitches(true);
|
---|
208 |
|
---|
209 | SetPeriodUS(TAU_US);
|
---|
210 |
|
---|
211 | while(!ToBeStopped())
|
---|
212 | {
|
---|
213 | WaitPeriod();
|
---|
214 |
|
---|
215 | Update();
|
---|
216 | }
|
---|
217 |
|
---|
218 | WarnUponSwitches(false);
|
---|
219 | }
|
---|
220 |
|
---|
221 | void BlCtrlV2_x4_speed::Update(void)
|
---|
222 | {
|
---|
223 | float u_roll,u_pitch,u_yaw,u_gaz;
|
---|
224 | float trim_roll,trim_pitch,trim_yaw;
|
---|
225 | float pwm[4];
|
---|
226 | uint16_t pwm_moteur[4];
|
---|
227 |
|
---|
228 |
|
---|
229 | //on prend une fois pour toute le mutex et on fait des accès directs
|
---|
230 | input->GetMutex();
|
---|
231 |
|
---|
232 | u_roll=input->ValueNoMutex(0,0);
|
---|
233 | u_pitch=input->ValueNoMutex(1,0);
|
---|
234 | u_yaw=input->ValueNoMutex(2,0);
|
---|
235 | u_gaz=input->ValueNoMutex(3,0)+input->ValueNoMutex(7,0)*input->ValueNoMutex(7,0);//ugaz+trim*trim
|
---|
236 | trim_roll=input->ValueNoMutex(4,0);
|
---|
237 | trim_pitch=input->ValueNoMutex(5,0);
|
---|
238 | trim_yaw=input->ValueNoMutex(6,0);
|
---|
239 |
|
---|
240 | input->ReleaseMutex();
|
---|
241 |
|
---|
242 | if(pas->CurrentIndex()==1)
|
---|
243 | {
|
---|
244 | trim_yaw=-trim_yaw;
|
---|
245 | u_yaw=-u_yaw;
|
---|
246 | }
|
---|
247 |
|
---|
248 | //rt_printf("%f %f %f %f\n",u_roll,u_pitch,u_yaw,u_gaz);
|
---|
249 | //if(u_gaz!=0) rt_printf("gaz: %f\n",u_gaz);
|
---|
250 |
|
---|
251 | //avant gauche
|
---|
252 | if(u_gaz+u_pitch+u_roll+u_yaw>0)
|
---|
253 | {
|
---|
254 | pwm[0]=trim_pitch+trim_roll+trim_yaw+sqrtf(u_gaz+u_pitch+u_roll+u_yaw);
|
---|
255 | }else
|
---|
256 | {
|
---|
257 | pwm[0]=trim_pitch+trim_roll+trim_yaw;
|
---|
258 | }
|
---|
259 |
|
---|
260 | //arriere gauche
|
---|
261 | if(u_gaz-u_pitch+u_roll-u_yaw>0)
|
---|
262 | {
|
---|
263 | pwm[3]=-trim_pitch+trim_roll-trim_yaw+sqrtf(u_gaz-u_pitch+u_roll-u_yaw);
|
---|
264 | }else
|
---|
265 | {
|
---|
266 | pwm[3]=-trim_pitch+trim_roll-trim_yaw;
|
---|
267 | }
|
---|
268 |
|
---|
269 | //arriere droit
|
---|
270 | if(u_gaz-u_pitch-u_roll+u_yaw>0)
|
---|
271 | {
|
---|
272 | pwm[1]=-trim_pitch-trim_roll+trim_yaw+sqrtf(u_gaz-u_pitch-u_roll+u_yaw);
|
---|
273 | }else
|
---|
274 | {
|
---|
275 | pwm[1]=-trim_pitch-trim_roll+trim_yaw;
|
---|
276 | }
|
---|
277 |
|
---|
278 | //avant droit
|
---|
279 | if(u_gaz+u_pitch-u_roll-u_yaw>0)
|
---|
280 | {
|
---|
281 | pwm[2]=trim_pitch-trim_roll-trim_yaw+sqrtf(u_gaz+u_pitch-u_roll-u_yaw);
|
---|
282 | }else
|
---|
283 | {
|
---|
284 | pwm[2]=trim_pitch-trim_roll-trim_yaw;
|
---|
285 | }
|
---|
286 |
|
---|
287 |
|
---|
288 | int_av_g+=ki->Value()*(pwm[0]-speed_av_g);
|
---|
289 | pwm[0]=kp->Value()*(pwm[0]-speed_av_g)+int_av_g;
|
---|
290 |
|
---|
291 | int_ar_g+=ki->Value()*(pwm[3]-speed_ar_g);
|
---|
292 | pwm[3]=kp->Value()*(pwm[3]-speed_ar_g)+int_ar_g;
|
---|
293 |
|
---|
294 | int_ar_d+=ki->Value()*(pwm[1]-speed_ar_d);
|
---|
295 | pwm[1]=kp->Value()*(pwm[1]-speed_ar_d)+int_ar_d;
|
---|
296 |
|
---|
297 | int_av_d+=ki->Value()*(pwm[2]-speed_av_d);
|
---|
298 | pwm[2]=kp->Value()*(pwm[2]-speed_av_d)+int_av_d;
|
---|
299 |
|
---|
300 | //rt_printf("%f\n",pwm[0]);
|
---|
301 | for(int i=0;i<4;i++) pwm_moteur[i]=SatPWM(pwm[i],min->Value(),max->Value());
|
---|
302 |
|
---|
303 | if(button_avg->Clicked()==true)
|
---|
304 | {
|
---|
305 | tested_motor=0;
|
---|
306 | StartTest();
|
---|
307 | }
|
---|
308 | if(button_avd->Clicked()==true)
|
---|
309 | {
|
---|
310 | tested_motor=2;
|
---|
311 | StartTest();
|
---|
312 | }
|
---|
313 | if(button_arg->Clicked()==true)
|
---|
314 | {
|
---|
315 | tested_motor=3;
|
---|
316 | StartTest();
|
---|
317 | }
|
---|
318 | if(button_ard->Clicked()==true)
|
---|
319 | {
|
---|
320 | tested_motor=1;
|
---|
321 | StartTest();
|
---|
322 | }
|
---|
323 |
|
---|
324 | if(tested_motor!=-1)
|
---|
325 | {
|
---|
326 | for(int i=0;i<4;i++)
|
---|
327 | {
|
---|
328 | pwm_moteur[i]=0;
|
---|
329 | }
|
---|
330 | pwm_moteur[tested_motor]=(uint16_t)test->Value();
|
---|
331 |
|
---|
332 | if(GetTime()>(start_time+2*1000000000)) StopTest();
|
---|
333 | }
|
---|
334 |
|
---|
335 |
|
---|
336 | i2cport->GetMutex();
|
---|
337 |
|
---|
338 | if(enabled==true)
|
---|
339 | {
|
---|
340 | i2cport->SetSlave(slave_address+av_g->CurrentIndex());
|
---|
341 | WriteValue(pwm_moteur[0]);
|
---|
342 |
|
---|
343 | i2cport->SetSlave(slave_address+av_d->CurrentIndex());
|
---|
344 | WriteValue(pwm_moteur[2]);
|
---|
345 |
|
---|
346 | i2cport->SetSlave(slave_address+ar_g->CurrentIndex());
|
---|
347 | WriteValue(pwm_moteur[3]);
|
---|
348 |
|
---|
349 | i2cport->SetSlave(slave_address+ar_d->CurrentIndex());
|
---|
350 | WriteValue(pwm_moteur[1]);
|
---|
351 |
|
---|
352 | }
|
---|
353 | else
|
---|
354 | {
|
---|
355 | for(int i=0;i<4;i++)
|
---|
356 | {
|
---|
357 | i2cport->SetSlave(slave_address+i);
|
---|
358 | WriteValue(0);
|
---|
359 | }
|
---|
360 | int_av_g=0;
|
---|
361 | int_av_d=0;
|
---|
362 | int_ar_g=0;
|
---|
363 | int_ar_d=0;
|
---|
364 | }
|
---|
365 |
|
---|
366 | i2cport->SetSlave(slave_address+av_g->CurrentIndex());
|
---|
367 | speed_av_g=GetSpeed();
|
---|
368 |
|
---|
369 | i2cport->SetSlave(slave_address+av_d->CurrentIndex());
|
---|
370 | speed_av_d=GetSpeed();
|
---|
371 |
|
---|
372 | i2cport->SetSlave(slave_address+ar_g->CurrentIndex());
|
---|
373 | speed_ar_g=GetSpeed();
|
---|
374 |
|
---|
375 | i2cport->SetSlave(slave_address+ar_d->CurrentIndex());
|
---|
376 | speed_ar_d=GetSpeed();
|
---|
377 |
|
---|
378 | i2cport->ReleaseMutex();
|
---|
379 |
|
---|
380 |
|
---|
381 | //on prend une fois pour toute le mutex et on fait des accès directs
|
---|
382 | output->GetMutex();
|
---|
383 | output->SetValueNoMutex(0,0,speed_av_g);
|
---|
384 | output->SetValueNoMutex(1,0,speed_ar_d);
|
---|
385 | output->SetValueNoMutex(2,0,speed_av_d);
|
---|
386 | output->SetValueNoMutex(3,0,speed_ar_g);
|
---|
387 | // rt_printf("%i %i %i %i\n",pwm_moteur[0],pwm_moteur[1],pwm_moteur[2],pwm_moteur[3]);
|
---|
388 | output->ReleaseMutex();
|
---|
389 |
|
---|
390 | output->SetDataTime(GetTime());
|
---|
391 | ProcessUpdate(output);
|
---|
392 |
|
---|
393 | }
|
---|
394 |
|
---|
395 | void BlCtrlV2_x4_speed::StartTest(void)
|
---|
396 | {
|
---|
397 | start_time=GetTime();
|
---|
398 | SetEnabled(true);
|
---|
399 | }
|
---|
400 |
|
---|
401 | void BlCtrlV2_x4_speed::StopTest(void)
|
---|
402 | {
|
---|
403 | SetEnabled(false);
|
---|
404 | tested_motor=-1;
|
---|
405 | }
|
---|
406 |
|
---|
407 | uint16_t BlCtrlV2_x4_speed::SatPWM(float vel_cons,uint16_t min,uint16_t max)
|
---|
408 | {
|
---|
409 | uint16_t sat_value=(uint16_t)vel_cons;
|
---|
410 |
|
---|
411 | if(vel_cons>((float)sat_value+0.5)) sat_value++;
|
---|
412 |
|
---|
413 | if(vel_cons<(float)min) sat_value=min;
|
---|
414 | if(vel_cons>(float)max) sat_value=max;
|
---|
415 |
|
---|
416 | return sat_value;
|
---|
417 | }
|
---|
418 |
|
---|
419 | void BlCtrlV2_x4_speed::LockUserInterface(void)
|
---|
420 | {
|
---|
421 | reglages_groupbox->setEnabled(false);
|
---|
422 | }
|
---|
423 |
|
---|
424 | void BlCtrlV2_x4_speed::UnlockUserInterface(void)
|
---|
425 | {
|
---|
426 | reglages_groupbox->setEnabled(true);
|
---|
427 | }
|
---|
428 |
|
---|
429 | void BlCtrlV2_x4_speed::SetEnabled(bool status)
|
---|
430 | {
|
---|
431 | enabled=status;
|
---|
432 | if(enabled==true)
|
---|
433 | {
|
---|
434 | LockUserInterface();
|
---|
435 |
|
---|
436 | flight_start_time = GetTime();
|
---|
437 | }
|
---|
438 | else
|
---|
439 | {
|
---|
440 | UnlockUserInterface();
|
---|
441 |
|
---|
442 | Time now= GetTime();
|
---|
443 | int t_sec;
|
---|
444 | FILE *file;
|
---|
445 | char ligne[32];
|
---|
446 |
|
---|
447 | t_sec=(now - flight_start_time)/1000000000;
|
---|
448 | time_sec+=t_sec;
|
---|
449 |
|
---|
450 | Printf("temps de vol: %is = %imin\n",t_sec,t_sec/60);
|
---|
451 | Printf("temps de vol total: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
|
---|
452 |
|
---|
453 | file=fopen("/etc/flight_time","w");
|
---|
454 | if (file == NULL)
|
---|
455 | {
|
---|
456 | Thread::Err("Erreur a l'ouverture du fichier d'info vol\n");
|
---|
457 | }
|
---|
458 | else
|
---|
459 | {
|
---|
460 | sprintf(ligne,"%i",time_sec);
|
---|
461 | fputs(ligne,file);
|
---|
462 | fclose(file);
|
---|
463 | }
|
---|
464 | }
|
---|
465 | }
|
---|
466 |
|
---|
467 | void BlCtrlV2_x4_speed::SetUroll(float value)
|
---|
468 | {
|
---|
469 | input->SetValue(0,0,value);
|
---|
470 | }
|
---|
471 |
|
---|
472 | void BlCtrlV2_x4_speed::SetUpitch(float value)
|
---|
473 | {
|
---|
474 | input->SetValue(1,0,value);
|
---|
475 | }
|
---|
476 |
|
---|
477 | void BlCtrlV2_x4_speed::SetUyaw(float value)
|
---|
478 | {
|
---|
479 | input->SetValue(2,0,value);
|
---|
480 | }
|
---|
481 |
|
---|
482 | void BlCtrlV2_x4_speed::SetUgaz(float value)
|
---|
483 | {
|
---|
484 | input->SetValue(3,0,value);
|
---|
485 | }
|
---|
486 |
|
---|
487 | void BlCtrlV2_x4_speed::SetRollTrim(float value)
|
---|
488 | {
|
---|
489 | input->SetValue(4,0,value);
|
---|
490 | }
|
---|
491 |
|
---|
492 | void BlCtrlV2_x4_speed::SetPitchTrim(float value)
|
---|
493 | {
|
---|
494 | input->SetValue(5,0,value);
|
---|
495 | }
|
---|
496 |
|
---|
497 | void BlCtrlV2_x4_speed::SetYawTrim(float value)
|
---|
498 | {
|
---|
499 | input->SetValue(6,0,value);
|
---|
500 | }
|
---|
501 |
|
---|
502 | void BlCtrlV2_x4_speed::SetGazTrim(float value)
|
---|
503 | {
|
---|
504 | input->SetValue(7,0,value);
|
---|
505 | }
|
---|
506 |
|
---|
507 | void BlCtrlV2_x4_speed::WriteValue(uint16_t value)
|
---|
508 | {
|
---|
509 | unsigned char tx[2];
|
---|
510 | ssize_t written;
|
---|
511 |
|
---|
512 | tx[0]=(unsigned char)(value>>3);//msb
|
---|
513 | tx[1]=16+8+(value&0x07);//16+8 pour recuperer la vitesse
|
---|
514 | written = i2cport->Write(tx, 2);
|
---|
515 | if(written<0)
|
---|
516 | {
|
---|
517 | Thread::Err("erreur rt_dev_write (%s)\n",strerror(-written));
|
---|
518 | }
|
---|
519 | else if (written != 2)
|
---|
520 | {
|
---|
521 | Thread::Err("erreur rt_dev_write %i/2\n",written);
|
---|
522 | }
|
---|
523 | }
|
---|
524 |
|
---|
525 | float BlCtrlV2_x4_speed::GetSpeed(void)
|
---|
526 | {
|
---|
527 | ssize_t read;
|
---|
528 | uint8_t value;
|
---|
529 | read = i2cport->Read(&value, 1);
|
---|
530 |
|
---|
531 | if(read<0)
|
---|
532 | {
|
---|
533 | Thread::Err("erreur rt_dev_read (%s)\n",strerror(-read));
|
---|
534 | }
|
---|
535 | else if (read != 1)
|
---|
536 | {
|
---|
537 | Thread::Err("erreur rt_dev_read %i/2\n",read);
|
---|
538 |
|
---|
539 | }
|
---|
540 |
|
---|
541 | return value*780./poles->Value();
|
---|
542 | }
|
---|
543 |
|
---|
544 | } // end namespace actuator
|
---|
545 | } // end namespace framewor
|
---|