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

Last change on this file since 330 was 268, checked in by Sanahuja Guillaume, 3 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.