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

Last change on this file since 3 was 3, checked in by Sanahuja Guillaume, 6 years ago

sensoractuator

File size: 17.4 KB
RevLine 
[3]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,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
64Gx3_25_imu_impl::~Gx3_25_imu_impl() {
65}
66
67void 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
172void Gx3_25_imu_impl::GetData(uint8_t* buf,ssize_t buf_size,Time *time) {
173    ssize_t read = 0;
174    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
200float 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
216void 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    }
257}
258
259void 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));
273    } 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));
280    } 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
289void 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
348void 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
397int 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
438void 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
484void 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    }
518}
519
520void 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
536bool 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}
Note: See TracBrowser for help on using the repository browser.