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

Last change on this file since 136 was 103, checked in by Sanahuja Guillaume, 5 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.