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

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

Last change on this file since 173 was 173, checked in by Sanahuja Guillaume, 7 years ago

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