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

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

m

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}
225
226uint16_t ParrotNavBoard::Checksum(const uint8_t *data) const{
227 uint16_t checksum = 0;
228 for(int i = 2; i < 60-2; i += 2) {
229 checksum += data[i] + (data[i+1] << 8);
230 }
231 return checksum;
232}
233
234} // end namespace sensor
235} // end namespace flair
236
Note: See TracBrowser for help on using the repository browser.