source: flair-src/tags/0.2.0/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp@ 267

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

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