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

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

modifs pour template vectors

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