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

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

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