source: flair-src/trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp@ 10

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

sensoractuator
File size: 17.4 KB
Line 
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: 2011/08/19
6// filename: Gx3_25_imu_impl.cpp
7//
8// author: Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: objet integrant la centrale 3dmgx3-25
14//
15//
16/*********************************************************************/
17
18#include "Gx3_25_imu_impl.h"
19#include <FrameworkManager.h>
20#include <SerialPort.h>
21#include <GroupBox.h>
22#include <SpinBox.h>
23#include <CheckBox.h>
24#include <PushButton.h>
25#include <Label.h>
26#include <ImuData.h>
27#include <AhrsData.h>
28#include <Euler.h>
29#include <Vector3D.h>
30#include <RotationMatrix.h>
31
32#include <math.h>
33#include <string.h>
34
35using std::string;
36using namespace flair::core;
37using namespace flair::gui;
38using namespace flair::sensor;
39
40Gx3_25_imu_impl::Gx3_25_imu_impl(Gx3_25_imu* self,string name,SerialPort *serialport,Gx3_25_imu::Command_t command,GroupBox* setupgroupbox) {
41 int err = 0;
42
43 this->self=self;
44 this->command=(uint8_t)command;
45 this->setupgroupbox=setupgroupbox;
46 this->serialport=serialport;
47
48 ahrsData=new AhrsData((Imu*)self);
49
50 //station sol
51 button_bias=new PushButton(setupgroupbox->NewRow(),"gyros bias");
52 data_rate=new SpinBox(setupgroupbox->NewRow(),"data rate (Hz):",1,500,1,200);
53 data_rate_label=new Label(setupgroupbox->LastRowLastCol(),"data_rate");
54 gyro_acc_size=new SpinBox(setupgroupbox->NewRow(),"gyro and acc filter win size:",1,32,1,15);
55 mag_size=new SpinBox(setupgroupbox->LastRowLastCol(),"mag filter win size:",1,32,1,17);
56 up_comp=new SpinBox(setupgroupbox->NewRow(),"up compensation (s):",1,1000,1,10);
57 north_comp=new SpinBox(setupgroupbox->LastRowLastCol(),"north compensation (s):",1,1000,1,10);
58 coning=new CheckBox(setupgroupbox->NewRow(),"enable Coning&Sculling:",true);
59 disable_magn=new CheckBox(setupgroupbox->LastRowLastCol(),"disable magnetometer:",true);
60 disable_north_comp=new CheckBox(setupgroupbox->NewRow(),"disable magnetic north compensation:",false);
61 disable_grav_comp=new CheckBox(setupgroupbox->NewRow(),"disable gravity compensation:",false);
62}
63
64Gx3_25_imu_impl::~Gx3_25_imu_impl() {
65}
66
67void Gx3_25_imu_impl::Run(void) {
68 Time imuTime;
69 ImuData* imuData;
70 self->GetDatas(&imuData);
71
72 self->WarnUponSwitches(true);
73
74 //reset IMU to be sure it is at 115200
75 Printf("Gx3_25_imu::Reset IMU at 921600\n");
76 serialport->SetBaudrate(921600);
77 DeviceReset();
78 self->Thread::SleepMS(100);
79 serialport->FlushInput();
80 serialport->SetBaudrate(115200);
81
82 SetBaudrate(921600);
83 Printf("Gx3_25_imu firmware number: %i\n",FirmwareNumber());
84 PrintModelInfo();
85 SamplingSettings();
86 GyrosBias();
87 //RealignUpNorth(true,true);
88
89 //on provoque les 9 ValueChanged
90 for(int i=0; i<9; i++) {
91 if(data_rate->ValueChanged()==true || disable_magn->ValueChanged()==true ||disable_north_comp->ValueChanged()==true ||disable_grav_comp->ValueChanged()==true ||coning->ValueChanged()==true
92 || gyro_acc_size->ValueChanged()==true || mag_size->ValueChanged()==true ||up_comp->ValueChanged()==true || north_comp->ValueChanged()==true) {
93 if(setupgroupbox->isEnabled()==true) SamplingSettings();
94 }
95 }
96
97 //periode a passer an argument (reglable)
98 //ou plutot laisser la periode geree par le centrale (polling)
99 //self->SetPeriodMS(2);
100 SetContinuousMode(command);
101
102 //_printf("firmware version: %i\n",get_firmware_number());
103
104 while(!self->ToBeStopped()) {
105 if(command==Gx3_25_imu::EulerAnglesAndAngularRates) {
106 uint8_t response[31] = {0};
107 uint8_t *buf=&response[1];
108 GetData(response,sizeof(response),&imuTime);
109
110 Euler eulerAngles;
111 eulerAngles.roll=Dequeue(&buf);
112 eulerAngles.pitch=Dequeue(&buf);
113 eulerAngles.yaw=Dequeue(&buf);
114
115 Vector3D filteredAngRates;
116 filteredAngRates.x=Dequeue(&buf);
117 filteredAngRates.y=Dequeue(&buf);
118 filteredAngRates.z=Dequeue(&buf);
119
120 ahrsData->SetQuaternionAndAngularRates(eulerAngles.ToQuaternion(),filteredAngRates);
121 } else if(command==Gx3_25_imu::AccelerationAngularRateAndOrientationMatrix) {
122 uint8_t response[67] = {0};
123 uint8_t *buf=&response[1];
124 GetData(response,sizeof(response),&imuTime);
125
126 Vector3D rawAcc;
127 rawAcc.x=9.80665*Dequeue(&buf);
128 rawAcc.y=9.80665*Dequeue(&buf);
129 rawAcc.z=9.80665*Dequeue(&buf);
130
131 Vector3D filteredAngRates;
132 filteredAngRates.x=Dequeue(&buf);
133 filteredAngRates.y=Dequeue(&buf);
134 filteredAngRates.z=Dequeue(&buf);
135
136 RotationMatrix matrix;
137 matrix(0,0)=Dequeue(&buf);
138 matrix(0,1)=Dequeue(&buf);
139 matrix(0,2)=Dequeue(&buf);
140 matrix(1,0)=Dequeue(&buf);
141 matrix(1,1)=Dequeue(&buf);
142 matrix(1,2)=Dequeue(&buf);
143 matrix(2,0)=Dequeue(&buf);
144 matrix(2,1)=Dequeue(&buf);
145 matrix(2,2)=Dequeue(&buf);
146
147 ahrsData->SetQuaternionAndAngularRates(matrix.ToEuler().ToQuaternion(),filteredAngRates);
148 imuData->SetRawAcc(rawAcc);
149 }
150
151 //change settings as soon as possible
152 //we assume that new imu datas will not come so fast
153 //so newt message from imu is an ack from the change settings
154 if(button_bias->Clicked()==true) GyrosBias();
155
156 if(data_rate->ValueChanged()==true || disable_magn->ValueChanged()==true ||disable_north_comp->ValueChanged()==true ||disable_grav_comp->ValueChanged()==true ||coning->ValueChanged()==true
157 || gyro_acc_size->ValueChanged()==true || mag_size->ValueChanged()==true ||up_comp->ValueChanged()==true || north_comp->ValueChanged()==true) {
158 if(setupgroupbox->isEnabled()==true) SamplingSettings();
159 }
160
161 imuData->SetDataTime(imuTime);
162 ahrsData->SetDataTime(imuTime);
163 self->UpdateImu();
164 self->ProcessUpdate(ahrsData);
165 }
166 SetContinuousMode(0);
167 SetBaudrate(115200);
168
169 self->WarnUponSwitches(false);
170}
171
172void Gx3_25_imu_impl::GetData(uint8_t* buf,ssize_t buf_size,Time *time) {
173 ssize_t read = 0;
174 ssize_t written = 0;
175 /*
176 written = serialport->Write(&command, sizeof(command));
177 if(written<0)
178 {
179 self->Thread::Err("Write error (%s)\n",strerror(-written));
180 }
181 else if (written != sizeof(command))
182 {
183 self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
184 }
185 */
186 read = serialport->Read(buf,buf_size);
187 *time=GetTime();
188 if(read<0) {
189 self->Thread::Err("Read error (%s)\n",strerror(-read));
190 } else if (read != buf_size) {
191 self->Thread::Err("Read error %i/%i\n",read,buf_size);
192 }
193
194 if(CalcChecksum(buf,buf_size)==false) {
195 self->Thread::Err("wrong checksum\n");
196 return;
197 }
198}
199
200float Gx3_25_imu_impl::Dequeue(uint8_t** buf) {
201 union float_4uint8 {
202 float f;
203 uint8_t b[4];
204 }
205 float_value;
206
207 float_value.b[0]=(*buf)[3];
208 float_value.b[1]=(*buf)[2];
209 float_value.b[2]=(*buf)[1];
210 float_value.b[3]=(*buf)[0];
211 (*buf)+=sizeof(float);
212
213 return float_value.f;
214}
215
216void Gx3_25_imu_impl::GyrosBias(void) {
217 if(setupgroupbox->isEnabled()==true) { //devrait toujours etre bon
218 uint8_t response[19] = {0};
219 uint8_t command[5];
220 ssize_t read = 0;
221 ssize_t written = 0;
222
223 Printf("Gx3_25_imu::gyros_bias: %s\n",self->IODevice::ObjectName().c_str());
224
225 command[0]=0xcd;//entete
226 command[1]=0xc1;//confirm
227 command[2]=0x29;//confirm
228 command[3]=0x27;//time MSB en us
229 command[4]=0x10;//time LSB en us
230
231 written = serialport->Write( &command, sizeof(command));
232 if(written<0) {
233 self->Thread::Err("Write error (%s)\n",strerror(-written));
234 } else if (written != sizeof(command)) {
235 self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
236 }
237
238 self->SleepUS(1.1*(command[3]*256+command[4]));//on fait un sleep un peu plus grand
239
240 read = serialport->Read(&response[0],sizeof(response));
241 if(read<0) {
242 self->Thread::Err("Read error (%s)\n",strerror(-read));
243 } else if (read != sizeof(response)) {
244 self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
245 }
246
247 if(CalcChecksum(response,sizeof(response))==false) {
248 self->Thread::Err("wrong checksum\n");
249 //return -1;
250 }
251
252 Printf("Gx3_25_imu::gyros_bias: %s ok\n",self->IODevice::ObjectName().c_str());
253
254 } else {
255 self->Thread::Err("error locked\n");
256 }
257}
258
259void Gx3_25_imu_impl::SetContinuousMode(uint8_t continuous_command) {
260 uint8_t response[8] = {0};
261 uint8_t command[4];
262 ssize_t read = 0;
263 ssize_t written = 0;
264
265 command[0]=0xc4;//entete
266 command[1]=0xc1;//confirm
267 command[2]=0x29;//confirm
268 command[3]=continuous_command;
269
270 written = serialport->Write(command,sizeof(command));
271 if(written<0) {
272 self->Thread::Err("Write error (%s)\n",strerror(-written));
273 } else if (written != sizeof(command)) {
274 self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
275 }
276
277 read = serialport->Read(response,sizeof(response));
278 if(read<0) {
279 self->Thread::Err("Read error (%s)\n",strerror(-read));
280 } else if (read != sizeof(response)) {
281 self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
282 }
283
284 if(CalcChecksum(response,sizeof(response))==false) {
285 self->Thread::Err("wrong checksum\n",self->IODevice::ObjectName().c_str());
286 }
287}
288
289void Gx3_25_imu_impl::SamplingSettings(void) {
290 uint8_t response[19] = {0};
291 uint8_t command[20];
292 uint8_t result;
293 ssize_t read = 0;
294 ssize_t written = 0;
295
296 uint16_t rate=1000/data_rate->Value();
297
298 command[0]=0xdb;//entete
299 command[1]=0xa8;//confirm
300 command[2]=0xb9;//confirm
301 command[3]=1;//change values
302 command[4]=(rate>>8)&0xff;//data rate MSB
303 command[5]=rate&0xff;//data rate LSB
304 result=0;
305 if(disable_magn->IsChecked()==true) result|=0x01;
306 if(disable_north_comp->IsChecked()==true) result|=0x04;
307 if(disable_grav_comp->IsChecked()==true) result|=0x08;
308
309 command[6]=result;
310 result=0x01;//Calculate orientation
311 if(coning->IsChecked()==true) result|=0x02;
312
313 command[7]=result;
314 command[8]=gyro_acc_size->Value();//gyro acc filter window
315 command[9]=mag_size->Value();//mag filter window
316 command[10]=(up_comp->Value()>>8)&0xff;//up comp MSB
317 command[11]=up_comp->Value()&0xff;//up comp LSB
318 command[12]=(north_comp->Value()>>8)&0xff;//north comp MSB
319 command[13]=north_comp->Value()&0xff;//north comp LSB
320 command[14]=0;//reserved
321 command[15]=0;//reserved
322 command[16]=0;//reserved
323 command[17]=0;//reserved
324 command[18]=0;//reserved
325 command[19]=0;//reserved
326
327 written = serialport->Write(&command, sizeof(command));
328 if(written<0) {
329 self->Thread::Err("Write error (%s)\n",strerror(-written));
330 } else if (written != sizeof(command)) {
331 self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
332 }
333
334 read = serialport->Read(&response[0],sizeof(response));
335 if(read<0) {
336 self->Thread::Err("Read error (%s)\n",strerror(-read));
337 } else if (read != sizeof(response)) {
338 self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
339 }
340
341 if(CalcChecksum(response,sizeof(response))==false) {
342 self->Thread::Err("wrong checksum\n",self->IODevice::ObjectName().c_str());
343 } else {
344 data_rate_label->SetText("real: %.2fHz",1000./rate);
345 }
346}
347
348void Gx3_25_imu_impl::SetBaudrate(int value) {
349 uint8_t response[10] = {0};
350 uint8_t command[11];
351 ssize_t read = 0;
352 ssize_t written = 0;
353
354 union int32_4uint8 {
355 int32_t i;
356 uint8_t b[4];
357 }
358 baudrate_value;
359
360 baudrate_value.i=value;
361 Printf("Gx3_25_imu::SetBaudrate: %s ->%i\n",self->IODevice::ObjectName().c_str(),baudrate_value.i);
362
363 command[0]=0xd9;//entete
364 command[1]=0xc3;//confirm
365 command[2]=0x55;//confirm
366 command[3]=1;//primary uart
367 command[4]=1;//chgt temporaire
368 command[5]=baudrate_value.b[3];
369 command[6]=baudrate_value.b[2];
370 command[7]=baudrate_value.b[1];
371 command[8]=baudrate_value.b[0];
372 command[9]=2;//uart enabled
373 command[10]=0;//reserved
374
375 written = serialport->Write(&command, sizeof(command));
376 if(written<0) {
377 self->Thread::Err("Write error (%s)\n",strerror(-written));
378 } else if (written != sizeof(command)) {
379 self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
380 }
381
382 read = serialport->Read(&response[0],sizeof(response));
383 if(read<0) {
384 self->Thread::Err("Read error (%s)\n",strerror(-read));
385 } else if (read != sizeof(response)) {
386 self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
387 }
388
389 if(CalcChecksum(response,sizeof(response))==false) {
390 self->Thread::Err("wrong checksum\n");
391 return ;
392 }
393
394 serialport->SetBaudrate(value);
395}
396
397int Gx3_25_imu_impl::FirmwareNumber(void) {
398 uint8_t response[7] = {0};
399 uint8_t command;
400 ssize_t read = 0;
401 ssize_t written = 0;
402 union int32_4uint8 {
403 int32_t i;
404 uint8_t b[4];
405 }
406 value;
407
408 command=0xe9;//entete
409
410 written = serialport->Write(&command, sizeof(command));
411 if(written<0) {
412 self->Thread::Err("Write error (%s)\n",strerror(-written));
413 } else if (written != sizeof(command)) {
414 self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
415 }
416
417 read = serialport->Read(&response[0],sizeof(response));
418 if(read<0) {
419 self->Thread::Err("Read error (%s)\n",strerror(-read));
420 } else if (read != sizeof(response)) {
421 self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
422 }
423
424 if(CalcChecksum(response,sizeof(response))==false) {
425 self->Thread::Err("wrong checksum\n");
426 return -1;
427 }
428
429 value.b[3]=response[1];
430 value.b[2]=response[2];
431 value.b[1]=response[3];
432 value.b[0]=response[4];
433
434 return value.i;
435
436}
437
438void Gx3_25_imu_impl::PrintModelInfo(void) {
439 uint8_t response[20] = {0};
440 uint8_t command[2];
441 ssize_t read = 0;
442 ssize_t written = 0;
443
444 for(int i=0; i<3; i++) {
445 command[0]=0xea;//entete
446 command[1]=i;//entete
447
448 written = serialport->Write(&command, sizeof(command));
449 if(written<0) {
450 self->Thread::Err("Write error (%s)\n",strerror(-written));
451 } else if (written != sizeof(command)) {
452 self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
453 }
454
455 read = serialport->Read(&response[0],sizeof(response));
456 if(read<0) {
457 self->Thread::Err("Read error (%s)\n",strerror(-read));
458 } else if (read != sizeof(response)) {
459 self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
460 }
461
462 if(CalcChecksum(response,sizeof(response))==false) {
463 self->Thread::Err("wrong checksum\n");
464 //return -1;
465 }
466
467 char* msg=(char*)(&response[2]);
468 msg[16]=0;
469 switch(i) {
470 case 0:
471 Printf("Gx3_25_imu model number: %s\n",msg);
472 break;
473 case 1:
474 Printf("Gx3_25_imu serial number: %s\n",msg);
475 break;
476 case 2:
477 Printf("Gx3_25_imu model name: %s\n",msg);
478 break;
479 }
480 }
481
482}
483
484void Gx3_25_imu_impl::RealignUpNorth(bool realign_up,bool realign_north) {
485 uint8_t response[7] = {0};
486 uint8_t command[10];
487 ssize_t read = 0;
488 ssize_t written = 0;
489
490 command[0]=0xdd;//entete
491 command[1]=0x54;//confirm
492 command[2]=0x4c;//confirm
493 command[3]=0;//send reply
494 command[4]=255;//up realign
495 command[5]=1;//north realign
496 command[6]=0;//reserved
497 command[7]=0;//reserved
498 command[8]=0;//reserved
499 command[9]=0;//reserved
500
501 written = serialport->Write(&command, sizeof(command));
502 if(written<0) {
503 self->Thread::Err("Write error (%s)\n",strerror(-written));
504 } else if (written != sizeof(command)) {
505 self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
506 }
507
508 read = serialport->Read(&response[0],sizeof(response));
509 if(read<0) {
510 self->Thread::Err("Read error(%s)\n",strerror(-read));
511 } else if (read != sizeof(response)) {
512 self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
513 }
514
515 if(CalcChecksum(response,sizeof(response))==false) {
516 self->Thread::Err("wrong checksum\n");
517 }
518}
519
520void Gx3_25_imu_impl::DeviceReset(void) {
521 uint8_t command[3];
522 ssize_t written = 0;
523
524 command[0]=0xfe;//entete
525 command[1]=0x9e;//confirm
526 command[2]=0x3a;//confirm
527
528 written = serialport->Write(&command, sizeof(command));
529 if(written<0) {
530 self->Thread::Err("Write error (%s)\n",strerror(-written));
531 } else if (written != sizeof(command)) {
532 self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
533 }
534}
535
536bool Gx3_25_imu_impl::CalcChecksum(uint8_t *buf,int size) {
537 uint16_t tChksum,tResponseChksum;
538
539 tChksum = 0;
540 for (int i = 0; i < size - 2; i++) tChksum += buf[i];
541 // Extract the big-endian checksum from reply
542 tResponseChksum = 0;
543 tResponseChksum = buf[size - 2] << 8;
544 tResponseChksum += buf[size - 1];
545
546 if(tChksum!=tResponseChksum)
547 return false;
548 else
549 return true;
550}
Note: See TracBrowser for help on using the repository browser.