source: flair-src/trunk/lib/FlairSensorActuator/src/NmeaGps.cpp @ 67

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

corrections bugs checkpoint map

File size: 7.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:   NmeaGps.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Base class for GPS using NMEA sentances
14//
15//
16/*********************************************************************/
17
18#include "NmeaGps.h"
19#include "geodesie.h"
20#include <Euler.h>
21#include <DataPlot1D.h>
22#include <Tab.h>
23#include <TabWidget.h>
24#include <GridLayout.h>
25#include <GroupBox.h>
26#include <PushButton.h>
27#include <FrameworkManager.h>
28#include <Map.h>
29#include <GeoCoordinate.h>
30#include <Vector3D.h>
31#include <Label.h>
32#include <GpsData.h>
33#include <string.h>
34
35using std::string;
36using namespace Geodesie;
37using namespace flair::core;
38using namespace flair::gui;
39
40namespace flair {
41namespace sensor {
42
43NmeaGps::NmeaGps(const FrameworkManager *parent, string name, NMEAFlags_t NMEAFlags)
44    : IODevice(parent, name) {
45  this->NMEAFlags = NMEAFlags;
46
47  nmea_zero_INFO(&info);
48  nmea_parser_init(&parser);
49  altRef = 0;
50
51  if ((NMEAFlags & GGA) == 0) {
52    Err("Enable at least the GGA sentence\n");
53  }
54
55 /*
56  if ((NMEAFlags & GST) != 0) {
57    desc->SetElementName(index, 0, "dev_lat");
58    desc->SetElementName(index + 1, 0, "dev_lon");
59    desc->SetElementName(index + 2, 0, "dev_elv");
60    index += 3;
61  }
62  */
63
64  // station sol
65  mainTab = new Tab(parent->GetTabWidget(), name);
66  tab = new TabWidget(mainTab->NewRow(), name);
67  sensorTab = new Tab(tab, "Setup");
68  setupGroupbox = new GroupBox(sensorTab->NewRow(), name);
69  buttonRef = new PushButton(setupGroupbox->NewRow(), "set ref");
70  nbSatLabel = new Label(setupGroupbox->NewRow(), "nb_sat");
71  fixLabel = new Label(setupGroupbox->LastRowLastCol(), "fix");
72
73  position = new GeoCoordinate((IODevice *)this, "position", 0, 0, 0);
74
75  takeRef = false;
76
77  gpsData = new GpsData(this);
78  AddDataToLog(gpsData);
79
80  nbSatLabel->SetText("number of satellies: %i", gpsData->GetNumberOfSatellites());
81  fixLabel->SetText("fix quality: %i", gpsData->GetFixQuality());
82}
83
84NmeaGps::NmeaGps(const IODevice *parent, std::string name) : IODevice(parent, name) {
85  gpsData = new GpsData(this);
86  mainTab = NULL;
87  tab = NULL;
88  sensorTab = NULL;
89}
90
91NmeaGps::~NmeaGps() {
92  if (mainTab != NULL) {
93    nmea_parser_destroy(&parser);
94    delete mainTab;
95  }
96}
97
98GroupBox *NmeaGps::GetGroupBox(void) const { return setupGroupbox; }
99
100const GpsData *NmeaGps::GetDatas(void) const {
101   return gpsData;
102}
103
104void NmeaGps::GetDatas(core::GpsData **outGpsData) const {
105  *outGpsData = gpsData;
106}
107
108void NmeaGps::UseDefaultPlot(void) {
109  plotTab = new Tab(tab, "Mesures");
110
111  if ((NMEAFlags & GGA) != 0) {
112    ePlot = new DataPlot1D(plotTab->NewRow(), "e", -10, 10);
113    ePlot->AddCurve(gpsData->Element(GpsData::East));
114    nPlot = new DataPlot1D(plotTab->LastRowLastCol(), "n", -10, 10);
115    nPlot->AddCurve(gpsData->Element(GpsData::North));
116    uPlot = new DataPlot1D(plotTab->LastRowLastCol(), "u", -10, 10);
117    uPlot->AddCurve(gpsData->Element(GpsData::Up));
118  }
119  if ((NMEAFlags & VTG) != 0) {
120    vePlot = new DataPlot1D(plotTab->NewRow(), "ve", -10, 10);
121    vePlot->AddCurve(gpsData->Element(GpsData::EastVelocity));
122    vnPlot = new DataPlot1D(plotTab->LastRowLastCol(), "vn", -10, 10);
123    vnPlot->AddCurve(gpsData->Element(GpsData::NorthVelocity));
124  }
125
126  Tab *map_tab = new Tab(tab, "carte");
127  map = new Map(map_tab->NewRow(), "map");
128  map->AddPoint(position, "drone");
129}
130
131DataPlot1D *NmeaGps::EPlot(void) const {
132  if ((NMEAFlags & GGA) != 0) {
133    return ePlot;
134  } else {
135    Err("GGA sentence not requested\n");
136    return NULL;
137  }
138}
139
140DataPlot1D *NmeaGps::NPlot(void) const {
141  if ((NMEAFlags & GGA) != 0) {
142    return nPlot;
143  } else {
144    Err("GGA sentence not requested\n");
145    return NULL;
146  }
147}
148
149DataPlot1D *NmeaGps::UPlot(void) const {
150  if ((NMEAFlags & GGA) != 0) {
151    return uPlot;
152  } else {
153    Err("GGA sentence not requested\n");
154    return NULL;
155  }
156}
157
158DataPlot1D *NmeaGps::VEPlot(void) const {
159  if ((NMEAFlags & VTG) != 0) {
160    return vePlot;
161  } else {
162    Err("GGA sentence not requested\n");
163    return NULL;
164  }
165}
166
167DataPlot1D *NmeaGps::VNPlot(void) const {
168  if ((NMEAFlags & VTG) != 0) {
169    return vnPlot;
170  } else {
171    Err("GGA sentence not requested\n");
172    return NULL;
173  }
174}
175
176Layout *NmeaGps::GetLayout(void) const { return sensorTab; }
177
178Tab *NmeaGps::GetPlotTab(void) const { return plotTab; }
179
180TabWidget *NmeaGps::GetTab(void) const { return tab; }
181
182void NmeaGps::SetRef(void) { takeRef = true; }
183
184void NmeaGps::GetEnu(Vector3D *point) {
185  gpsData->GetMutex();
186  gpsData->GetEnu(point->x,point->y,point->z);
187  gpsData->ReleaseMutex();
188}
189
190void NmeaGps::parseFrame(const char *frame, int frame_size) {
191
192  int result;
193
194  result = nmea_parse(&parser, frame, frame_size, &info);
195  if (result != 1) {
196    Warn("unrecognized nmea sentence: %s\n",frame);
197    return;
198  }
199
200  result = nmea_parse_GPGGA(frame, frame_size, &pack);
201
202  if (result == 1) {
203    nmea_info2pos(&info, &pos);
204    // Printf("%s\n",frame);
205   //  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);
206//Printf("lat:%f long:%f\n",pos.lat,pos.lon);
207
208    gpsData->GetMutex(); // on utilise le mutex de gpsData pour fix et nb_sat
209    if (gpsData->GetFixQuality() != (GpsData::FixQuality_t)pack.sig) {
210      gpsData->SetFixQuality((GpsData::FixQuality_t)pack.sig);
211      fixLabel->SetText("fix: %i", pack.sig);
212    }
213    if (gpsData->GetNumberOfSatellites() != pack.satinuse) {
214      gpsData->SetNumberOfSatellites(pack.satinuse) ;
215      nbSatLabel->SetText("nb_sat: %i", pack.satinuse);
216    }
217    gpsData->ReleaseMutex();
218
219    gpsData->SetLla(Euler::ToDegree(pos.lat), Euler::ToDegree(pos.lon),info.elv);
220    position->SetCoordinates(Euler::ToDegree(pos.lat), Euler::ToDegree(pos.lon),
221                             info.elv);
222
223    if ((info.sig == 2 && altRef == 0) || buttonRef->Clicked() == true ||
224        takeRef == true) {
225      Printf("prise pos ref\n");
226      latRef = pos.lat;
227      longRef = pos.lon;
228      altRef = info.elv;
229      takeRef = false;
230    }
231    // if(alt_ref!=0)
232    {
233      double x, y, z;
234      double e, n, u;
235      Geographique_2_ECEF(pos.lon, pos.lat, info.elv, x, y, z);
236      ECEF_2_ENU(x, y, z, e, n, u, longRef, latRef, altRef);
237      // Printf("lon:%f lat:%f elv:%f\n",pos.lon,pos.lat,info.elv);
238
239      gpsData->SetEnu(e,n,u);
240
241      if ((NMEAFlags & VTG) != 0) {
242        gpsData->SetVelocity(info.speed * 1000. / 3600. * sin(Euler::ToRadian(info.direction)),
243                                  info.speed * 1000. / 3600. * cos(Euler::ToRadian(info.direction)));
244      }/*
245      if ((NMEAFlags & GST) != 0) {
246        // Thread::Printf("dev_lon:%f dev_lat:%f
247        // dev_elv:%f\n",info.dev_lat,info.dev_lon,info.dev_elv);
248        output->SetValueNoMutex(index, 0, info.dev_lat);
249        output->SetValueNoMutex(index + 1, 0, info.dev_lon);
250        output->SetValueNoMutex(index + 2, 0, info.dev_elv);
251        index += 3;
252      }*/
253
254
255      gpsData->SetDataTime(GetTime());
256      ProcessUpdate(gpsData);
257    }
258  }
259}
260
261} // end namespace sensor
262} // end namespace framewor
Note: See TracBrowser for help on using the repository browser.