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

Last change on this file since 3 was 3, checked in by Sanahuja Guillaume, 6 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.