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

Last change on this file since 340 was 340, checked in by Sanahuja Guillaume, 2 years ago

add servos

File size: 6.9 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(string name, NMEAFlags_t NMEAFlags)
44 : IODevice(getFrameworkManager(), 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 // station sol
56 mainTab = new Tab(getFrameworkManager()->GetTabWidget(), name);
57 tab = new TabWidget(mainTab->NewRow(), name);
58 sensorTab = new Tab(tab, "Setup");
59 setupGroupbox = new GroupBox(sensorTab->NewRow(), name);
60 buttonRef = new PushButton(setupGroupbox->NewRow(), "set ref");
61 nbSatLabel = new Label(setupGroupbox->NewRow(), "nb_sat");
62 fixLabel = new Label(setupGroupbox->LastRowLastCol(), "fix");
63
64 position = new GeoCoordinate((IODevice *)this, "position", 0, 0, 0);
65
66 takeRef = false;
67
68 gpsData = new GpsData(this);
69 AddDataToLog(gpsData);
70
71 nbSatLabel->SetText("number of satellies: %i", gpsData->GetNumberOfSatellites());
72 fixLabel->SetText("fix quality: %i", gpsData->GetFixQuality());
73}
74
75NmeaGps::~NmeaGps() {
76 nmea_parser_destroy(&parser);
77 delete mainTab;
78}
79
80GroupBox *NmeaGps::GetGroupBox(void) const { return setupGroupbox; }
81
82const GpsData *NmeaGps::GetDatas(void) const {
83 return gpsData;
84}
85
86void NmeaGps::GetDatas(core::GpsData **outGpsData) const {
87 *outGpsData = gpsData;
88}
89
90void NmeaGps::UseDefaultPlot(void) {
91 plotTab = new Tab(tab, "Mesures");
92
93 if ((NMEAFlags & GGA) != 0) {
94 ePlot = new DataPlot1D(plotTab->NewRow(), "e", -10, 10);
95 ePlot->AddCurve(gpsData->Element(GpsData::East));
96 nPlot = new DataPlot1D(plotTab->LastRowLastCol(), "n", -10, 10);
97 nPlot->AddCurve(gpsData->Element(GpsData::North));
98 uPlot = new DataPlot1D(plotTab->LastRowLastCol(), "u", -10, 10);
99 uPlot->AddCurve(gpsData->Element(GpsData::Up));
100 }
101 if ((NMEAFlags & VTG) != 0) {
102 vePlot = new DataPlot1D(plotTab->NewRow(), "ve", -10, 10);
103 vePlot->AddCurve(gpsData->Element(GpsData::EastVelocity));
104 vnPlot = new DataPlot1D(plotTab->LastRowLastCol(), "vn", -10, 10);
105 vnPlot->AddCurve(gpsData->Element(GpsData::NorthVelocity));
106 }
107
108 Tab *map_tab = new Tab(tab, "carte");
109 map = new Map(map_tab->NewRow(), "map");
110 map->AddPoint(position, "drone");
111}
112
113DataPlot1D *NmeaGps::EPlot(void) const {
114 if ((NMEAFlags & GGA) != 0) {
115 return ePlot;
116 } else {
117 Err("GGA sentence not requested\n");
118 return NULL;
119 }
120}
121
122DataPlot1D *NmeaGps::NPlot(void) const {
123 if ((NMEAFlags & GGA) != 0) {
124 return nPlot;
125 } else {
126 Err("GGA sentence not requested\n");
127 return NULL;
128 }
129}
130
131DataPlot1D *NmeaGps::UPlot(void) const {
132 if ((NMEAFlags & GGA) != 0) {
133 return uPlot;
134 } else {
135 Err("GGA sentence not requested\n");
136 return NULL;
137 }
138}
139
140DataPlot1D *NmeaGps::VEPlot(void) const {
141 if ((NMEAFlags & VTG) != 0) {
142 return vePlot;
143 } else {
144 Err("GGA sentence not requested\n");
145 return NULL;
146 }
147}
148
149DataPlot1D *NmeaGps::VNPlot(void) const {
150 if ((NMEAFlags & VTG) != 0) {
151 return vnPlot;
152 } else {
153 Err("GGA sentence not requested\n");
154 return NULL;
155 }
156}
157
158Layout *NmeaGps::GetLayout(void) const { return sensorTab; }
159
160Tab *NmeaGps::GetPlotTab(void) const { return plotTab; }
161
162TabWidget *NmeaGps::GetTab(void) const { return tab; }
163
164void NmeaGps::SetRef(void) { takeRef = true; }
165
166void NmeaGps::parseFrame(const char *frame, int frame_size) {
167 //avoid unrecognized nmea sentence (when no satelites
168 if(strncmp(frame,"$GNGGA,,",8)==0) return;
169 if(strncmp(frame,"$GNVTG,,",8)==0) return;
170 if(strncmp(frame,"$GNGST,,",8)==0) return;
171 if(strstr(frame,",,,,,")!=NULL) return;
172
173 int result;
174 result = nmea_parse(&parser, frame, frame_size, &info);
175 if (result != 1) {
176 Warn("unrecognized nmea sentence: %s\n",frame);
177 return;
178 }
179
180 result = nmea_parse_GPGGA(frame, frame_size, &pack);
181
182 if (result == 1) {
183 nmea_info2pos(&info, &pos);
184 // 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);
185//Printf("lat:%f long:%f\n",pos.lat,pos.lon);
186
187 gpsData->GetMutex(); // on utilise le mutex de gpsData pour fix et nb_sat
188 if (gpsData->GetFixQuality() != (GpsData::FixQuality_t)pack.sig) {
189 gpsData->SetFixQuality((GpsData::FixQuality_t)pack.sig);
190 fixLabel->SetText("fix: %i", pack.sig);
191 }
192 if (gpsData->GetNumberOfSatellites() != pack.satinuse) {
193 gpsData->SetNumberOfSatellites(pack.satinuse) ;
194 nbSatLabel->SetText("nb_sat: %i", pack.satinuse);
195 }
196 gpsData->ReleaseMutex();
197
198 gpsData->SetLla(Euler::ToDegree(pos.lat), Euler::ToDegree(pos.lon),info.elv);
199 position->SetCoordinates(Euler::ToDegree(pos.lat), Euler::ToDegree(pos.lon),
200 info.elv);
201
202 if ((info.sig == 2 && altRef == 0) || buttonRef->Clicked() == true ||
203 takeRef == true) {
204 Printf("prise pos ref\n");
205 latRef = pos.lat;
206 longRef = pos.lon;
207 altRef = info.elv;
208 takeRef = false;
209 }
210 // if(alt_ref!=0)
211 {
212 double x, y, z;
213 double e, n, u;
214 Geographique_2_ECEF(pos.lon, pos.lat, info.elv, x, y, z);
215 ECEF_2_ENU(x, y, z, e, n, u, longRef, latRef, altRef);
216 // Printf("lon:%f lat:%f elv:%f\n",pos.lon,pos.lat,info.elv);
217
218 gpsData->SetEnu(e,n,u);
219
220 if ((NMEAFlags & VTG) != 0) {
221 gpsData->SetVelocity(info.speed * 1000. / 3600. * sin(Euler::ToRadian(info.direction)),
222 info.speed * 1000. / 3600. * cos(Euler::ToRadian(info.direction)));
223 }
224 if ((NMEAFlags & GSA) != 0) {
225 gpsData->SetDop(info.PDOP,info.HDOP,info.VDOP);
226 }
227 /*
228 if ((NMEAFlags & GST) != 0) {
229 // Thread::Printf("dev_lon:%f dev_lat:%f
230 // dev_elv:%f\n",info.dev_lat,info.dev_lon,info.dev_elv);
231 output->SetValueNoMutex(index, 0, info.dev_lat);
232 output->SetValueNoMutex(index + 1, 0, info.dev_lon);
233 output->SetValueNoMutex(index + 2, 0, info.dev_elv);
234 index += 3;
235 }*/
236
237
238 gpsData->SetDataTime(GetTime());
239 ProcessUpdate(gpsData);
240 }
241 }
242}
243
244} // end namespace sensor
245} // end namespace flair
Note: See TracBrowser for help on using the repository browser.