source: flair-src/trunk/lib/FlairSensorActuator/src/ParrotNavBoard.cpp @ 4

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

sensoractuator

File size: 6.1 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:    2013/12/19
6//  filename:   ParrotNavBoard.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    objet integrant les capteurs ardrone2
14//
15//
16/*********************************************************************/
17
18#include "ParrotNavBoard.h"
19#include <FrameworkManager.h>
20#include <cvmatrix.h>
21#include <Imu.h>
22#include <ImuData.h>
23#include <UsRangeFinder.h>
24#include <SerialPort.h>
25
26#define G 9.81
27
28using std::string;
29using namespace flair::core;
30using namespace flair::sensor;
31
32namespace {
33
34//from /data/config.ini
35const float gyros_gains[3] = { 1.0679195e-03 ,-1.0669589e-03 ,-1.0683396e-03 };
36const float acc_gains[3] = { 2.0327346e+00 ,-1.9626701e+00 ,-1.9494846e+00 };
37
38class ParrotImu : public Imu {
39public:
40    ParrotImu(const FrameworkManager* parent,string name) :Imu(parent,name) {
41        //AddDataToLog(imudata);
42    }
43
44    ~ParrotImu() {}
45
46    void SetDatas(const Vector3D &rawAcc,const Vector3D &rawMag,const Vector3D &rawGyr,Time time) {
47        ImuData* imuData;
48        GetDatas(&imuData);
49        imuData->SetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
50        imuData->SetDataTime(time);
51        UpdateImu();
52        ProcessUpdate(imuData);
53    }
54
55private:
56    void UpdateFrom(const io_data *data){};
57};
58
59class ParrotUs : public UsRangeFinder {
60public:
61    ParrotUs(const FrameworkManager* parent,string name) :UsRangeFinder(parent,name) {}
62
63    ~ParrotUs() {}
64
65    void SetData(float value,Time time) {
66        output->SetValue(0,0,value);
67        output->SetDataTime(time);
68        ProcessUpdate(output);
69    }
70
71private:
72    void UpdateFrom(const io_data *data){};
73};
74
75}
76
77namespace flair { namespace sensor {
78
79ParrotNavBoard::ParrotNavBoard(const FrameworkManager* parent,string name,SerialPort* serialport,uint8_t priority) : Thread(parent,name,priority) {
80    this->serialport=serialport;
81
82    us=new ParrotUs(parent,"parrot-us");
83    imu=new ParrotImu(parent,"parrot-imu");
84
85    serialport->SetBaudrate(460800);
86}
87
88ParrotNavBoard::~ParrotNavBoard() {
89    SafeStop();
90    Join();
91
92    delete imu;
93    delete us;
94}
95
96Imu* ParrotNavBoard::GetImu(void) const {
97    return imu;
98}
99
100UsRangeFinder* ParrotNavBoard::GetUsRangeFinder(void) const {
101    return us;
102}
103
104void ParrotNavBoard::UseDefaultPlot(void) {
105    us->UseDefaultPlot();
106    imu->UseDefaultPlot();
107}
108
109void ParrotNavBoard::Run(void) {
110    uint8_t data[60];
111    nav_struct* nav=(nav_struct*)data;
112    ssize_t lu=0;
113
114    // stop acquisition
115    uint8_t cmd=0x02;
116    serialport->Write(&cmd, 1);
117
118    SleepMS(20);
119    serialport->FlushInput();
120
121    // start acquisition
122    cmd=0x01;
123    serialport->Write(&cmd, 1);
124
125    while(!ToBeStopped()) {
126        lu+=serialport->Read(data+lu,sizeof(data)-lu);
127        if(lu!=sizeof(data)) continue;
128
129        if(data[0]==0x3a) {
130            Time time=GetTime();
131
132            if(nav->checksum == Checksum(data)) {
133                computeDatas(nav,time);
134                lu=0;
135            } else {
136                Warn("Wrong checksum: got %x, expected %x, time %lld\n",nav->checksum,Checksum(data),time);
137                for(int i=0;i<lu;i++) Printf("%x ",data[i]);
138                Printf("\n");
139                int i;
140                for(i=1;i<lu;i++) {//skeep first
141                    if(data[i]==0x3a) break;
142                }
143                printf("ok en %i\n",i);
144                int k=0;
145                for(int j=i;j<lu;j++) {
146                        data[k]=data[j];
147                        k++;
148                }
149                lu=k;
150                for(int i=0;i<lu;i++) Printf("%x ",data[i]);
151                Printf("\n");
152            }
153        } else {
154            Warn("Wrong header: got %x, expected 0x3a, time %lld\n",data[0],GetTime());
155            for(int i=0;i<lu;i++) Printf("%x ",data[i]);
156            Printf("\n");
157            int i;
158            for(i=1;i<lu;i++) {//skeep first
159                if(data[i]==0x3a) break;
160            }
161            printf("ok en %i\n",i);
162            int k=0;
163            for(int j=i;j<lu;j++) {
164                    data[k]=data[j];
165                    k++;
166            }
167            lu=k;
168            for(int i=0;i<lu;i++) Printf("%x ",data[i]);
169            Printf("\n");
170        }
171    }
172}
173
174void ParrotNavBoard::computeDatas(nav_struct* nav,Time time) {
175    static float accs_offset[3]={0,0,0};
176    static float gyrs_offset[3]={0,0,0};
177    static int cpt=0;
178
179    float ax,ay,az;
180    float gx,gy,gz;
181    float mx,my,mz;
182
183    if(cpt<2000) { //init offsets
184        for(int i=0;i<3;i++) accs_offset[i]+=nav->acc[i];
185        for(int i=0;i<3;i++) gyrs_offset[i]+=nav->gyro[i];
186        cpt++;
187        ax=0;
188        ay=0;
189        az=-G;
190        gx=0;
191        gy=0;
192        gz=0;
193        mx=0;
194        my=0;
195        mz=0;
196    } else {
197        //ax=-G*(nav->acc[0]-accs_offset[0]/cpt)/512.;
198        //ay=G*(nav->acc[1]-accs_offset[1]/cpt)/512.;
199        //az=G*(-(nav->acc[2]-accs_offset[2]/cpt)/512.-1);
200        ax=acc_gains[0]*(nav->acc[0]-accs_offset[0]/cpt)/100.;//gains from parrot are in cm.s-2
201        ay=acc_gains[1]*(nav->acc[1]-accs_offset[1]/cpt)/100.;
202        az=acc_gains[2]*((nav->acc[2]-accs_offset[2]/cpt)/100.)-G;
203
204        gx=(nav->gyro[0]-gyrs_offset[0]/cpt)*gyros_gains[0];
205        gy=(nav->gyro[1]-gyrs_offset[1]/cpt)*gyros_gains[1];
206        gz=(nav->gyro[2]-gyrs_offset[2]/cpt)*gyros_gains[2];
207
208        mx=nav->mag[0];
209        my=nav->mag[1];
210        mz=nav->mag[2];
211    }
212
213    ((ParrotImu*)imu)->SetDatas(Vector3D(ax,ay,az),
214                                Vector3D(mx,my,mz),
215                                Vector3D(gx,gy,gz),
216                                time
217                                );
218
219
220     if(((nav->us_echo)&0x8000)==0x8000) {
221        float h  = (float)(((nav->us_echo)&0x7fff)) * 0.0340/100.;
222        ((ParrotUs*)us)->SetData(h,time);
223    }
224}
225uint16_t ParrotNavBoard::Checksum(const uint8_t *data) const{
226    uint16_t checksum = 0;
227    for(int i = 2; i < 60-2; i += 2) {
228        checksum += data[i] + (data[i+1] << 8);
229    }
230    return checksum;
231}
232
233} // end namespace sensor
234} // end namespace flair
235
Note: See TracBrowser for help on using the repository browser.