Ignore:
Timestamp:
04/08/16 15:40:57 (8 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp

    r3 r15  
    3838using namespace flair::sensor;
    3939
    40 Gx3_25_imu_impl::Gx3_25_imu_impl(Gx3_25_imu* self,string name,SerialPort *serialport,Gx3_25_imu::Command_t command,GroupBox* setupgroupbox) {
    41     int err = 0;
    42 
    43     this->self=self;
    44     this->command=(uint8_t)command;
    45     this->setupgroupbox=setupgroupbox;
    46     this->serialport=serialport;
    47 
    48     ahrsData=new AhrsData((Imu*)self);
    49 
    50     //station sol
    51     button_bias=new PushButton(setupgroupbox->NewRow(),"gyros bias");
    52     data_rate=new SpinBox(setupgroupbox->NewRow(),"data rate (Hz):",1,500,1,200);
    53     data_rate_label=new Label(setupgroupbox->LastRowLastCol(),"data_rate");
    54     gyro_acc_size=new SpinBox(setupgroupbox->NewRow(),"gyro and acc filter win size:",1,32,1,15);
    55     mag_size=new SpinBox(setupgroupbox->LastRowLastCol(),"mag filter win size:",1,32,1,17);
    56     up_comp=new SpinBox(setupgroupbox->NewRow(),"up compensation (s):",1,1000,1,10);
    57     north_comp=new SpinBox(setupgroupbox->LastRowLastCol(),"north compensation (s):",1,1000,1,10);
    58     coning=new CheckBox(setupgroupbox->NewRow(),"enable Coning&Sculling:",true);
    59     disable_magn=new CheckBox(setupgroupbox->LastRowLastCol(),"disable magnetometer:",true);
    60     disable_north_comp=new CheckBox(setupgroupbox->NewRow(),"disable magnetic north compensation:",false);
    61     disable_grav_comp=new CheckBox(setupgroupbox->NewRow(),"disable gravity compensation:",false);
    62 }
    63 
    64 Gx3_25_imu_impl::~Gx3_25_imu_impl() {
    65 }
     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() {}
    6677
    6778void Gx3_25_imu_impl::Run(void) {
    68     Time imuTime;
    69     ImuData* imuData;
    70     self->GetDatas(&imuData);
    71 
    72     self->WarnUponSwitches(true);
    73 
    74     //reset IMU to be sure it is at 115200
    75     Printf("Gx3_25_imu::Reset IMU at 921600\n");
    76     serialport->SetBaudrate(921600);
    77     DeviceReset();
    78     self->Thread::SleepMS(100);
    79     serialport->FlushInput();
    80     serialport->SetBaudrate(115200);
    81 
    82     SetBaudrate(921600);
    83     Printf("Gx3_25_imu firmware number: %i\n",FirmwareNumber());
    84     PrintModelInfo();
    85     SamplingSettings();
    86     GyrosBias();
    87     //RealignUpNorth(true,true);
    88 
    89     //on provoque les 9 ValueChanged
    90     for(int i=0; i<9; i++) {
    91         if(data_rate->ValueChanged()==true || disable_magn->ValueChanged()==true ||disable_north_comp->ValueChanged()==true ||disable_grav_comp->ValueChanged()==true ||coning->ValueChanged()==true
    92                 || gyro_acc_size->ValueChanged()==true || mag_size->ValueChanged()==true ||up_comp->ValueChanged()==true || north_comp->ValueChanged()==true) {
    93             if(setupgroupbox->isEnabled()==true) SamplingSettings();
    94         }
    95     }
    96 
    97     //periode a passer an argument (reglable)
    98     //ou plutot laisser la periode geree par le centrale (polling)
    99     //self->SetPeriodMS(2);
    100     SetContinuousMode(command);
    101 
    102     //_printf("firmware version: %i\n",get_firmware_number());
    103 
    104     while(!self->ToBeStopped()) {
    105         if(command==Gx3_25_imu::EulerAnglesAndAngularRates) {
    106             uint8_t response[31] = {0};
    107             uint8_t *buf=&response[1];
    108             GetData(response,sizeof(response),&imuTime);
    109 
    110             Euler eulerAngles;
    111             eulerAngles.roll=Dequeue(&buf);
    112             eulerAngles.pitch=Dequeue(&buf);
    113             eulerAngles.yaw=Dequeue(&buf);
    114 
    115             Vector3D filteredAngRates;
    116             filteredAngRates.x=Dequeue(&buf);
    117             filteredAngRates.y=Dequeue(&buf);
    118             filteredAngRates.z=Dequeue(&buf);
    119 
    120             ahrsData->SetQuaternionAndAngularRates(eulerAngles.ToQuaternion(),filteredAngRates);
    121         } else if(command==Gx3_25_imu::AccelerationAngularRateAndOrientationMatrix) {
    122             uint8_t response[67] = {0};
    123             uint8_t *buf=&response[1];
    124             GetData(response,sizeof(response),&imuTime);
    125 
    126             Vector3D rawAcc;
    127             rawAcc.x=9.80665*Dequeue(&buf);
    128             rawAcc.y=9.80665*Dequeue(&buf);
    129             rawAcc.z=9.80665*Dequeue(&buf);
    130 
    131             Vector3D filteredAngRates;
    132             filteredAngRates.x=Dequeue(&buf);
    133             filteredAngRates.y=Dequeue(&buf);
    134             filteredAngRates.z=Dequeue(&buf);
    135 
    136             RotationMatrix matrix;
    137             matrix(0,0)=Dequeue(&buf);
    138             matrix(0,1)=Dequeue(&buf);
    139             matrix(0,2)=Dequeue(&buf);
    140             matrix(1,0)=Dequeue(&buf);
    141             matrix(1,1)=Dequeue(&buf);
    142             matrix(1,2)=Dequeue(&buf);
    143             matrix(2,0)=Dequeue(&buf);
    144             matrix(2,1)=Dequeue(&buf);
    145             matrix(2,2)=Dequeue(&buf);
    146 
    147             ahrsData->SetQuaternionAndAngularRates(matrix.ToEuler().ToQuaternion(),filteredAngRates);
    148             imuData->SetRawAcc(rawAcc);
    149         }
    150 
    151         //change settings as soon as possible
    152         //we assume that new imu datas will not come so fast
    153         //so newt message from imu is an ack from the change settings
    154         if(button_bias->Clicked()==true) GyrosBias();
    155 
    156         if(data_rate->ValueChanged()==true || disable_magn->ValueChanged()==true ||disable_north_comp->ValueChanged()==true ||disable_grav_comp->ValueChanged()==true ||coning->ValueChanged()==true
    157                 || gyro_acc_size->ValueChanged()==true || mag_size->ValueChanged()==true ||up_comp->ValueChanged()==true || north_comp->ValueChanged()==true) {
    158             if(setupgroupbox->isEnabled()==true) SamplingSettings();
    159         }
    160 
    161         imuData->SetDataTime(imuTime);
    162         ahrsData->SetDataTime(imuTime);
    163         self->UpdateImu();
    164         self->ProcessUpdate(ahrsData);
    165     }
    166     SetContinuousMode(0);
    167     SetBaudrate(115200);
    168 
    169     self->WarnUponSwitches(false);
    170 }
    171 
    172 void Gx3_25_imu_impl::GetData(uint8_t* buf,ssize_t buf_size,Time *time) {
     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.ToEuler().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];
    173248    ssize_t read = 0;
    174249    ssize_t written = 0;
    175     /*
    176         written = serialport->Write(&command, sizeof(command));
    177         if(written<0)
    178         {
    179             self->Thread::Err("Write error (%s)\n",strerror(-written));
    180         }
    181         else if (written != sizeof(command))
    182         {
    183             self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
    184         }
    185     */
    186     read = serialport->Read(buf,buf_size);
    187     *time=GetTime();
    188     if(read<0) {
    189         self->Thread::Err("Read error (%s)\n",strerror(-read));
    190     } else if (read != buf_size) {
    191         self->Thread::Err("Read error %i/%i\n",read,buf_size);
    192     }
    193 
    194     if(CalcChecksum(buf,buf_size)==false) {
    195         self->Thread::Err("wrong checksum\n");
    196         return;
    197     }
    198 }
    199 
    200 float Gx3_25_imu_impl::Dequeue(uint8_t** buf) {
    201     union float_4uint8 {
    202         float f;
    203         uint8_t b[4];
    204     }
    205     float_value;
    206 
    207     float_value.b[0]=(*buf)[3];
    208     float_value.b[1]=(*buf)[2];
    209     float_value.b[2]=(*buf)[1];
    210     float_value.b[3]=(*buf)[0];
    211     (*buf)+=sizeof(float);
    212 
    213     return float_value.f;
    214 }
    215 
    216 void Gx3_25_imu_impl::GyrosBias(void) {
    217     if(setupgroupbox->isEnabled()==true) { //devrait toujours etre bon
    218         uint8_t response[19] = {0};
    219         uint8_t command[5];
    220         ssize_t read = 0;
    221         ssize_t written = 0;
    222 
    223         Printf("Gx3_25_imu::gyros_bias: %s\n",self->IODevice::ObjectName().c_str());
    224 
    225         command[0]=0xcd;//entete
    226         command[1]=0xc1;//confirm
    227         command[2]=0x29;//confirm
    228         command[3]=0x27;//time MSB en us
    229         command[4]=0x10;//time LSB en us
    230 
    231         written = serialport->Write( &command, sizeof(command));
    232         if(written<0) {
    233             self->Thread::Err("Write error (%s)\n",strerror(-written));
    234         } else if (written != sizeof(command)) {
    235             self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
    236         }
    237 
    238         self->SleepUS(1.1*(command[3]*256+command[4]));//on fait un sleep un peu plus grand
    239 
    240         read = serialport->Read(&response[0],sizeof(response));
    241         if(read<0) {
    242             self->Thread::Err("Read error (%s)\n",strerror(-read));
    243         } else if (read != sizeof(response)) {
    244             self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
    245         }
    246 
    247         if(CalcChecksum(response,sizeof(response))==false) {
    248             self->Thread::Err("wrong checksum\n");
    249             //return -1;
    250         }
    251 
    252         Printf("Gx3_25_imu::gyros_bias: %s ok\n",self->IODevice::ObjectName().c_str());
    253 
    254     } else {
    255         self->Thread::Err("error locked\n");
    256     }
     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  }
    257288}
    258289
    259290void Gx3_25_imu_impl::SetContinuousMode(uint8_t continuous_command) {
    260     uint8_t response[8] = {0};
    261     uint8_t command[4];
    262     ssize_t read = 0;
    263     ssize_t written = 0;
    264 
    265     command[0]=0xc4;//entete
    266     command[1]=0xc1;//confirm
    267     command[2]=0x29;//confirm
    268     command[3]=continuous_command;
    269 
    270     written = serialport->Write(command,sizeof(command));
    271     if(written<0) {
    272         self->Thread::Err("Write error (%s)\n",strerror(-written));
     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));
    273484    } else if (written != sizeof(command)) {
    274         self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
    275     }
    276 
    277     read = serialport->Read(response,sizeof(response));
    278     if(read<0) {
    279         self->Thread::Err("Read error (%s)\n",strerror(-read));
     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));
    280491    } else if (read != sizeof(response)) {
    281         self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
    282     }
    283 
    284     if(CalcChecksum(response,sizeof(response))==false) {
    285         self->Thread::Err("wrong checksum\n",self->IODevice::ObjectName().c_str());
    286     }
    287 }
    288 
    289 void Gx3_25_imu_impl::SamplingSettings(void) {
    290     uint8_t response[19] = {0};
    291     uint8_t command[20];
    292     uint8_t result;
    293     ssize_t read = 0;
    294     ssize_t written = 0;
    295 
    296     uint16_t rate=1000/data_rate->Value();
    297 
    298     command[0]=0xdb;//entete
    299     command[1]=0xa8;//confirm
    300     command[2]=0xb9;//confirm
    301     command[3]=1;//change values
    302     command[4]=(rate>>8)&0xff;//data rate MSB
    303     command[5]=rate&0xff;//data rate LSB
    304     result=0;
    305     if(disable_magn->IsChecked()==true) result|=0x01;
    306     if(disable_north_comp->IsChecked()==true) result|=0x04;
    307     if(disable_grav_comp->IsChecked()==true) result|=0x08;
    308 
    309     command[6]=result;
    310     result=0x01;//Calculate orientation
    311     if(coning->IsChecked()==true) result|=0x02;
    312 
    313     command[7]=result;
    314     command[8]=gyro_acc_size->Value();//gyro acc filter window
    315     command[9]=mag_size->Value();//mag filter window
    316     command[10]=(up_comp->Value()>>8)&0xff;//up comp MSB
    317     command[11]=up_comp->Value()&0xff;//up comp LSB
    318     command[12]=(north_comp->Value()>>8)&0xff;//north comp MSB
    319     command[13]=north_comp->Value()&0xff;//north comp LSB
    320     command[14]=0;//reserved
    321     command[15]=0;//reserved
    322     command[16]=0;//reserved
    323     command[17]=0;//reserved
    324     command[18]=0;//reserved
    325     command[19]=0;//reserved
    326 
    327     written = serialport->Write(&command, sizeof(command));
    328     if(written<0) {
    329         self->Thread::Err("Write error (%s)\n",strerror(-written));
    330     } else if (written != sizeof(command)) {
    331         self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
    332     }
    333 
    334     read = serialport->Read(&response[0],sizeof(response));
    335     if(read<0) {
    336         self->Thread::Err("Read error (%s)\n",strerror(-read));
    337     } else if (read != sizeof(response)) {
    338         self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
    339     }
    340 
    341     if(CalcChecksum(response,sizeof(response))==false) {
    342         self->Thread::Err("wrong checksum\n",self->IODevice::ObjectName().c_str());
    343     } else {
    344         data_rate_label->SetText("real: %.2fHz",1000./rate);
    345     }
    346 }
    347 
    348 void Gx3_25_imu_impl::SetBaudrate(int value) {
    349     uint8_t response[10] = {0};
    350     uint8_t command[11];
    351     ssize_t read = 0;
    352     ssize_t written = 0;
    353 
    354     union int32_4uint8 {
    355         int32_t i;
    356         uint8_t b[4];
    357     }
    358     baudrate_value;
    359 
    360     baudrate_value.i=value;
    361     Printf("Gx3_25_imu::SetBaudrate: %s ->%i\n",self->IODevice::ObjectName().c_str(),baudrate_value.i);
    362 
    363     command[0]=0xd9;//entete
    364     command[1]=0xc3;//confirm
    365     command[2]=0x55;//confirm
    366     command[3]=1;//primary uart
    367     command[4]=1;//chgt temporaire
    368     command[5]=baudrate_value.b[3];
    369     command[6]=baudrate_value.b[2];
    370     command[7]=baudrate_value.b[1];
    371     command[8]=baudrate_value.b[0];
    372     command[9]=2;//uart enabled
    373     command[10]=0;//reserved
    374 
    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     }
    381 
    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     }
    388 
    389     if(CalcChecksum(response,sizeof(response))==false) {
    390         self->Thread::Err("wrong checksum\n");
    391         return ;
    392     }
    393 
    394     serialport->SetBaudrate(value);
    395 }
    396 
    397 int Gx3_25_imu_impl::FirmwareNumber(void) {
    398     uint8_t response[7] = {0};
    399     uint8_t command;
    400     ssize_t read = 0;
    401     ssize_t written = 0;
    402     union int32_4uint8 {
    403         int32_t i;
    404         uint8_t b[4];
    405     }
    406     value;
    407 
    408     command=0xe9;//entete
    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 -1;
    427     }
    428 
    429     value.b[3]=response[1];
    430     value.b[2]=response[2];
    431     value.b[1]=response[3];
    432     value.b[0]=response[4];
    433 
    434     return value.i;
    435 
    436 }
    437 
    438 void Gx3_25_imu_impl::PrintModelInfo(void) {
    439     uint8_t response[20] = {0};
    440     uint8_t command[2];
    441     ssize_t read = 0;
    442     ssize_t written = 0;
    443 
    444     for(int i=0; i<3; i++) {
    445         command[0]=0xea;//entete
    446         command[1]=i;//entete
    447 
    448         written = serialport->Write(&command, sizeof(command));
    449         if(written<0) {
    450             self->Thread::Err("Write error (%s)\n",strerror(-written));
    451         } else if (written != sizeof(command)) {
    452             self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
    453         }
    454 
    455         read = serialport->Read(&response[0],sizeof(response));
    456         if(read<0) {
    457             self->Thread::Err("Read error (%s)\n",strerror(-read));
    458         } else if (read != sizeof(response)) {
    459             self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
    460         }
    461 
    462         if(CalcChecksum(response,sizeof(response))==false) {
    463             self->Thread::Err("wrong checksum\n");
    464             //return -1;
    465         }
    466 
    467         char* msg=(char*)(&response[2]);
    468         msg[16]=0;
    469         switch(i) {
    470         case 0:
    471             Printf("Gx3_25_imu model number: %s\n",msg);
    472             break;
    473         case 1:
    474             Printf("Gx3_25_imu serial number: %s\n",msg);
    475             break;
    476         case 2:
    477             Printf("Gx3_25_imu model name: %s\n",msg);
    478             break;
    479         }
    480     }
    481 
    482 }
    483 
    484 void Gx3_25_imu_impl::RealignUpNorth(bool realign_up,bool realign_north) {
    485     uint8_t response[7] = {0};
    486     uint8_t command[10];
    487     ssize_t read = 0;
    488     ssize_t written = 0;
    489 
    490     command[0]=0xdd;//entete
    491     command[1]=0x54;//confirm
    492     command[2]=0x4c;//confirm
    493     command[3]=0;//send reply
    494     command[4]=255;//up realign
    495     command[5]=1;//north realign
    496     command[6]=0;//reserved
    497     command[7]=0;//reserved
    498     command[8]=0;//reserved
    499     command[9]=0;//reserved
    500 
    501     written = serialport->Write(&command, sizeof(command));
    502     if(written<0) {
    503         self->Thread::Err("Write error (%s)\n",strerror(-written));
    504     } else if (written != sizeof(command)) {
    505         self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
    506     }
    507 
    508     read = serialport->Read(&response[0],sizeof(response));
    509     if(read<0) {
    510         self->Thread::Err("Read error(%s)\n",strerror(-read));
    511     } else if (read != sizeof(response)) {
    512         self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
    513     }
    514 
    515     if(CalcChecksum(response,sizeof(response))==false) {
    516         self->Thread::Err("wrong checksum\n");
    517     }
     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  }
    518550}
    519551
    520552void Gx3_25_imu_impl::DeviceReset(void) {
    521     uint8_t command[3];
    522     ssize_t written = 0;
    523 
    524     command[0]=0xfe;//entete
    525     command[1]=0x9e;//confirm
    526     command[2]=0x3a;//confirm
    527 
    528     written = serialport->Write(&command, sizeof(command));
    529     if(written<0) {
    530         self->Thread::Err("Write error (%s)\n",strerror(-written));
    531     } else if (written != sizeof(command)) {
    532         self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
    533     }
    534 }
    535 
    536 bool Gx3_25_imu_impl::CalcChecksum(uint8_t *buf,int size) {
    537     uint16_t tChksum,tResponseChksum;
    538 
    539     tChksum = 0;
    540     for (int i = 0; i < size - 2; i++) tChksum += buf[i];
    541     // Extract the big-endian checksum from reply
    542     tResponseChksum = 0;
    543     tResponseChksum = buf[size - 2] << 8;
    544     tResponseChksum += buf[size - 1];
    545 
    546     if(tChksum!=tResponseChksum)
    547         return false;
    548     else
    549         return true;
    550 }
     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 TracChangeset for help on using the changeset viewer.