source: flair-src/trunk/lib/FlairSensorActuator/src/Gps.cpp@ 13

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

sensoractuator
File size: 8.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/08/23
6// filename: Gps.cpp
7//
8// author: Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: objet integrant le recepteur gps mb800
14//
15//
16/*********************************************************************/
17
18#include "Gps.h"
19#include "geodesie.h"
20#include <Euler.h>
21#include <cvmatrix.h>
22#include <DataPlot1D.h>
23#include <Tab.h>
24#include <TabWidget.h>
25#include <GridLayout.h>
26#include <GroupBox.h>
27#include <PushButton.h>
28#include <FrameworkManager.h>
29#include <Map.h>
30#include <GeoCoordinate.h>
31#include <Vector3D.h>
32#include <Label.h>
33#include <string.h>
34
35using std::string;
36using namespace Geodesie;
37using namespace flair::core;
38using namespace flair::gui;
39
40namespace flair
41{
42namespace sensor
43{
44
45Gps::Gps(const FrameworkManager* parent,string name,NMEAFlags_t NMEAFlags) : IODevice(parent,name)
46{
47 this->NMEAFlags=NMEAFlags;
48
49 nmea_zero_INFO(&info);
50 nmea_parser_init(&parser);
51 alt_ref=0;
52
53
54 if((NMEAFlags&GGA)==0)
55 {
56 Err("Enable at least GGA sentence\n");
57 }
58
59 int index=0;
60 if((NMEAFlags&GGA)!=0)
61 {
62 index+=3;
63 }
64 if((NMEAFlags&VTG)!=0)
65 {
66 index+=2;
67 }
68 if((NMEAFlags&GST)!=0)
69 {
70 index+=3;
71 }
72
73 cvmatrix_descriptor* desc=new cvmatrix_descriptor(index,1);
74 index=0;
75 if((NMEAFlags&GGA)!=0)
76 {
77 desc->SetElementName(0,0,"e");
78 desc->SetElementName(1,0,"n");
79 desc->SetElementName(2,0,"u");
80 index+=3;
81 }
82 if((NMEAFlags&VTG)!=0)
83 {
84 desc->SetElementName(index,0,"ve");
85 desc->SetElementName(index+1,0,"vn");
86 index+=2;
87 }
88 if((NMEAFlags&GST)!=0)
89 {
90 desc->SetElementName(index,0,"dev_lat");
91 desc->SetElementName(index+1,0,"dev_lon");
92 desc->SetElementName(index+2,0,"dev_elv");
93 index+=3;
94 }
95 output=new cvmatrix((IODevice*)this,desc,floatType);
96 AddDataToLog(output);
97
98 //station sol
99 main_tab=new Tab(parent->GetTabWidget(),name);
100 tab=new TabWidget(main_tab->NewRow(),name);
101 sensor_tab=new Tab(tab,"Reglages");
102 GroupBox* reglages_groupbox=new GroupBox(sensor_tab->NewRow(),name);
103 button_ref=new PushButton(reglages_groupbox->NewRow(),"set ref");
104 nb_sat_label=new Label(reglages_groupbox->NewRow(),"nb_sat");
105 fix_label=new Label(reglages_groupbox->LastRowLastCol(),"fix");
106
107 position=new GeoCoordinate((IODevice*)this,"position",0,0,0);
108
109 fix=FixQuality_t::Invalid;
110 nb_sat=0;
111 take_ref=false;
112 nb_sat_label->SetText("nb_sat: %i",nb_sat);
113 fix_label->SetText("fix: %i",fix);
114}
115
116Gps::~Gps()
117{
118 nmea_parser_destroy(&parser);
119 delete main_tab;
120}
121
122void Gps::UseDefaultPlot(void)
123{
124 int index=0;
125 plot_tab=new Tab(tab,"Mesures");
126
127 if((NMEAFlags&GGA)!=0)
128 {
129 e_plot=new DataPlot1D(plot_tab->NewRow(),"e",-10,10);
130 e_plot->AddCurve(output->Element(index));
131 n_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"n",-10,10);
132 n_plot->AddCurve(output->Element(index+1));
133 u_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"u",-10,10);
134 u_plot->AddCurve(output->Element(index+2));
135 index+=3;
136 }
137 if((NMEAFlags&VTG)!=0)
138 {
139 ve_plot=new DataPlot1D(plot_tab->NewRow(),"ve",-10,10);
140 ve_plot->AddCurve(output->Element(index));
141 vn_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"vn",-10,10);
142 vn_plot->AddCurve(output->Element(index+1));
143 index+=2;
144 }
145
146 Tab* map_tab=new Tab(tab,"carte");
147 map=new Map(map_tab->NewRow(),"map");
148 map->AddPoint(position,"drone");
149}
150
151DataPlot1D* Gps::EPlot(void) const
152{
153 if((NMEAFlags&GGA)!=0)
154 {
155 return e_plot;
156 }
157 else
158 {
159 Err("GGA sentence not requested\n");
160 return NULL;
161 }
162}
163
164DataPlot1D* Gps::NPlot(void) const
165{
166 if((NMEAFlags&GGA)!=0)
167 {
168 return n_plot;
169 }
170 else
171 {
172 Err("GGA sentence not requested\n");
173 return NULL;
174 }
175}
176
177DataPlot1D* Gps::UPlot(void) const
178{
179 if((NMEAFlags&GGA)!=0)
180 {
181 return u_plot;
182 }
183 else
184 {
185 Err("GGA sentence not requested\n");
186 return NULL;
187 }
188}
189
190DataPlot1D* Gps::VEPlot(void) const
191{
192 if((NMEAFlags&VTG)!=0)
193 {
194 return ve_plot;
195 }
196 else
197 {
198 Err("GGA sentence not requested\n");
199 return NULL;
200 }
201}
202
203DataPlot1D* Gps::VNPlot(void) const
204{
205 if((NMEAFlags&VTG)!=0)
206 {
207 return vn_plot;
208 }
209 else
210 {
211 Err("GGA sentence not requested\n");
212 return NULL;
213 }
214}
215
216Layout* Gps::GetLayout(void) const
217{
218 return sensor_tab;
219}
220
221Tab* Gps::GetPlotTab(void) const
222{
223 return plot_tab;
224}
225
226TabWidget* Gps::GetTab(void) const
227{
228 return tab;
229}
230
231uint16_t Gps::NbSat(void) const
232{
233 output->GetMutex();
234 uint16_t result=nb_sat;
235 output->ReleaseMutex();
236 return result;
237}
238
239Gps::FixQuality_t Gps::FixQuality(void) const
240{
241 output->GetMutex();
242 FixQuality_t result=fix;
243 output->ReleaseMutex();
244 return result;
245}
246
247void Gps::SetRef(void)
248{
249 take_ref=true;
250}
251
252void Gps::GetENUPosition(Vector3D *point)
253{
254 output->GetMutex();
255 point->x=output->ValueNoMutex(0,0);
256 point->y=output->ValueNoMutex(1,0);
257 point->z=output->ValueNoMutex(2,0);
258 output->ReleaseMutex();
259}
260
261void Gps::parseFrame(const char *frame, int frame_size){
262
263 int result;
264
265 result=nmea_parse(&parser, frame, frame_size, &info);
266 if(result!=1)
267 {
268 Warn("unrecognized nmea sentence\n");
269 Warn("%s\n",frame);
270 }
271
272 result=nmea_parse_GPGGA(frame, frame_size, &pack);
273
274 if(result==1)
275 {
276 //Printf("%s\n",frame);
277 //Printf("nb:%i fix:%i lat:%f long:%f alt:%f vel:%f angle:%f\n",pack.satinuse,pack.sig,info.lat,info.lon,info.elv,info.speed*1000./3600.,info.direction);
278 //Printf("lat:%f long:%f\n",pos.lat,pos.lon);
279 output->GetMutex();//on utilise le mutex de l'output pour fix et nb_sat
280 if((int)fix!=pack.sig)
281 {
282 fix=(FixQuality_t)pack.sig;
283 fix_label->SetText("fix: %i",fix);
284 }
285 if(nb_sat!=pack.satinuse)
286 {
287 nb_sat=pack.satinuse;
288 nb_sat_label->SetText("nb_sat: %i",nb_sat);
289 }
290 output->ReleaseMutex();
291
292
293 nmea_info2pos(&info,&pos);
294 position->SetCoordinates(Euler::ToDegree(pos.lat),Euler::ToDegree(pos.lon),info.elv);
295
296 if((info.sig==2 && alt_ref==0) || button_ref->Clicked()==true || take_ref==true)
297 {
298 Printf("prise pos ref\n");
299 lat_ref=pos.lat;
300 long_ref=pos.lon;
301 alt_ref=info.elv;
302 take_ref=false;
303 }
304 //if(alt_ref!=0)
305 {
306 double x,y,z;
307 double e,n,u;
308 Geographique_2_ECEF(pos.lon,pos.lat,info.elv,x,y,z);
309 ECEF_2_ENU(x,y,z,e, n, u,long_ref,lat_ref,alt_ref);
310 //Printf("lon:%f lat:%f elv:%f\n",pos.lon,pos.lat,info.elv);
311
312 //on prend une fois pour toute le mutex et on fait des accès directs
313 output->GetMutex();
314 output->SetValueNoMutex( 0, 0,e);
315 output->SetValueNoMutex( 1, 0,n);
316 output->SetValueNoMutex( 2, 0,u);
317
318 int index=3;
319 if((NMEAFlags&VTG)!=0)
320 {
321 output->SetValueNoMutex( index, 0,info.speed*1000./3600.*sin(Euler::ToRadian(info.direction)));
322 output->SetValueNoMutex( index+1, 0,info.speed*1000./3600.*cos(Euler::ToRadian(info.direction)));
323 index+=2;
324 }
325 if((NMEAFlags&GST)!=0)
326 {
327 //Thread::Printf("dev_lon:%f dev_lat:%f dev_elv:%f\n",info.dev_lat,info.dev_lon,info.dev_elv);
328 output->SetValueNoMutex( index, 0,info.dev_lat);
329 output->SetValueNoMutex( index+1, 0,info.dev_lon);
330 output->SetValueNoMutex( index+2, 0,info.dev_elv);
331 index+=3;
332 }
333 output->ReleaseMutex();
334
335 output->SetDataTime(GetTime());
336 ProcessUpdate(output);
337 }
338 }
339}
340
341} // end namespace sensor
342} // end namespace framewor
Note: See TracBrowser for help on using the repository browser.