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

Last change on this file since 51 was 51, checked in by Sanahuja Guillaume, 7 years ago

gps

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