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

Last change on this file since 173 was 173, checked in by Sanahuja Guillaume, 5 years ago

modif imu

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