source: flair-src/tags/0.3/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp@ 359

Last change on this file since 359 was 268, checked in by Sanahuja Guillaume, 6 years ago

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