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

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

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