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

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

passage quaternoin 3dmgx3

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