source: flair-src/trunk/lib/FlairCore/src/GpsData.cpp@ 204

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

modifs gps
File size: 7.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: 2016/07/01
6// filename: GpsData.cpp
7//
8// author: Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: class defining gps datas
14//
15//
16/*********************************************************************/
17
18#include "GpsData.h"
19#include "Euler.h"
20#include "IODataElement.h"
21#include <math.h>
22#include <string.h>
23#include <stdlib.h>
24
25using std::string;
26
27namespace flair {
28namespace core {
29
30/*! \class GpsDataElement
31 */
32class GpsDataElement : public IODataElement {
33public:
34 GpsDataElement(const GpsData *inGpsdata, string name,
35 GpsData::PlotableData_t inPlotableData)
36 : IODataElement(inGpsdata, name) {
37 gpsdata = inGpsdata;
38 plotableData = inPlotableData;
39
40 switch (plotableData) {
41 case GpsData::Latitude:
42 case GpsData::Longitude:
43 size = 8;
44 break;
45 case GpsData::Altitude:
46 case GpsData::East:
47 case GpsData::North:
48 case GpsData::Up:
49 case GpsData::EastVelocity:
50 case GpsData::NorthVelocity:
51 case GpsData::Pdop:
52 case GpsData::Hdop:
53 case GpsData::Vdop:
54 size=4;
55 break;
56 case GpsData::NumberOfSatellites:
57 case GpsData::FixQuality:
58 size=1;
59 break;
60 }
61 }
62
63 ~GpsDataElement() {}
64
65 void CopyData(char *dst) const {
66 double latitude,longitude;
67 float altitude,east,north,up,eastVelocity,northVelocity,pDop,hDop,vDop;
68
69 gpsdata->GetLla(latitude,longitude,altitude);
70 uint8_t numberOfSatellites=gpsdata->GetNumberOfSatellites();
71 GpsData::FixQuality_t fixQuality=gpsdata->GetFixQuality();
72 gpsdata->GetEnu(east,north,up);
73 gpsdata->GetVelocity(eastVelocity,northVelocity);
74 gpsdata->GetDop(pDop,hDop,vDop);
75
76 switch (plotableData) {
77 case GpsData::Latitude:
78 memcpy(dst, &latitude, sizeof(latitude));
79 break;
80 case GpsData::Longitude:
81 memcpy(dst, &longitude, sizeof(longitude));
82 break;
83 case GpsData::Altitude:
84 memcpy(dst, &altitude, sizeof(altitude));
85 break;
86 case GpsData::NumberOfSatellites:
87 memcpy(dst, &numberOfSatellites, sizeof(numberOfSatellites));
88 break;
89 case GpsData::FixQuality:
90 memcpy(dst, &fixQuality, sizeof(fixQuality));
91 break;
92 case GpsData::East:
93 memcpy(dst, &east, sizeof(east));
94 break;
95 case GpsData::North:
96 memcpy(dst, &north, sizeof(north));
97 break;
98 case GpsData::Up:
99 memcpy(dst, &up, sizeof(up));
100 break;
101 case GpsData::EastVelocity:
102 memcpy(dst, &eastVelocity, sizeof(eastVelocity));
103 break;
104 case GpsData::NorthVelocity:
105 memcpy(dst, &northVelocity, sizeof(northVelocity));
106 break;
107 case GpsData::Pdop:
108 memcpy(dst, &pDop, sizeof(pDop));
109 break;
110 case GpsData::Hdop:
111 memcpy(dst, &hDop, sizeof(hDop));
112 break;
113 case GpsData::Vdop:
114 memcpy(dst, &vDop, sizeof(vDop));
115 break;
116 default:
117 gpsdata->Err("data type unavailable.\n");
118 break;
119 }
120 }
121
122 DataType const &GetDataType(void) const {
123 switch (plotableData) {
124 case GpsData::Latitude:
125 case GpsData::Longitude:
126 return doubleType;
127 break;
128 case GpsData::Altitude:
129 case GpsData::East:
130 case GpsData::North:
131 case GpsData::Up:
132 case GpsData::EastVelocity:
133 case GpsData::NorthVelocity:
134 case GpsData::Pdop:
135 case GpsData::Hdop:
136 case GpsData::Vdop:
137 return floatType;
138 break;
139 case GpsData::NumberOfSatellites:
140 case GpsData::FixQuality:
141 return UInt8Type;
142 break;
143 default:
144 return dummyType;
145 }
146 }
147
148private:
149 const GpsData *gpsdata;
150 GpsData::PlotableData_t plotableData;
151};
152
153GpsData::GpsData(const Object *parent, std::string name, int n)
154 : io_data(parent, name, n), dataType() {
155 if (n > 1)
156 Warn("n>1 not supported\n");
157
158 AppendLogDescription("latitude", doubleType);
159 AppendLogDescription("longitude", doubleType);
160 AppendLogDescription("altitude", floatType);
161 AppendLogDescription("nb_sat",UInt8Type);
162 AppendLogDescription("fix quality",UInt8Type);
163 AppendLogDescription("east", floatType);
164 AppendLogDescription("north", floatType);
165 AppendLogDescription("up", floatType);
166 AppendLogDescription("east velocity", floatType);
167 AppendLogDescription("north velocity", floatType);
168 AppendLogDescription("pdop", floatType);
169 AppendLogDescription("hdop", floatType);
170 AppendLogDescription("vdop", floatType);
171
172 numberOfSatellites=0;
173 fixQuality=FixQuality_t::Invalid;
174}
175
176GpsData::~GpsData() {}
177
178void GpsData::GetLla(double &outLatitude, double &outLongitude, float &outAltitude) const {
179 GetMutex();
180 outLatitude=latitude;
181 outLongitude=longitude;
182 outAltitude=altitude;
183 ReleaseMutex();
184}
185
186void GpsData::SetLla(double inLatitude,double inLongitude,float inAltitude) {
187 GetMutex();
188 latitude=inLatitude;
189 longitude=inLongitude;
190 altitude=inAltitude;
191 ReleaseMutex();
192}
193
194void GpsData::GetEnu(float &outEast, float &outNorth,float &outUp) const {
195 GetMutex();
196 outEast=east;
197 outNorth=north;
198 outUp=up;
199 ReleaseMutex();
200}
201
202void GpsData::SetEnu(float inEast, float inNorth,float inUp) {
203 GetMutex();
204 east=inEast;
205 north=inNorth;
206 up=inUp;
207 ReleaseMutex();
208}
209
210
211void GpsData::GetVelocity(float &outEastVelocity, float &outNorthVelocity) const {
212 GetMutex();
213 outEastVelocity=eastVelocity;
214 outNorthVelocity=northVelocity;
215 ReleaseMutex();
216}
217
218void GpsData::SetVelocity(float inEastVelocity, float inNorthVelocity) {
219 GetMutex();
220 eastVelocity=inEastVelocity;
221 northVelocity=inNorthVelocity;
222 ReleaseMutex();
223}
224
225uint8_t GpsData::GetNumberOfSatellites(void) const {
226 return numberOfSatellites;
227}
228
229void GpsData::SetNumberOfSatellites(uint8_t inNumberOfSatellites) {
230 numberOfSatellites=inNumberOfSatellites;
231}
232
233GpsData::FixQuality_t GpsData::GetFixQuality(void) const {
234 return fixQuality;
235}
236
237void GpsData::SetFixQuality(FixQuality_t inFixQuality) {
238 fixQuality=inFixQuality;
239}
240
241void GpsData::GetDop(float &inPdop, float &inHdop,float &inVdop) const {
242 inPdop=pDop;
243 inHdop=hDop;
244 inVdop=vDop;
245}
246
247void GpsData::SetDop(float inPdop, float inHdop,float inVdop) {
248 pDop=inPdop;
249 hDop=inHdop;
250 vDop=inVdop;
251}
252
253IODataElement *GpsData::Element(PlotableData_t data_type) const {
254 string name;
255 switch (data_type) {
256 case GpsData::Latitude:
257 name = "Latitude";
258 break;
259 case GpsData::Longitude:
260 name = "Longitude";
261 break;
262 case GpsData::Altitude:
263 name = "Altitude";
264 break;
265 case GpsData::NumberOfSatellites:
266 name = "NumberOfSatellites";
267 break;
268 case GpsData::FixQuality:
269 name = "FixQuality";
270 break;
271 case GpsData::East:
272 name = "East";
273 break;
274 case GpsData::North:
275 name = "North";
276 break;
277 case GpsData::Up:
278 name = "Up";
279 break;
280 case GpsData::EastVelocity:
281 name = "EastVelocity";
282 break;
283 case GpsData::NorthVelocity:
284 name = "NorthVelocity";
285 break;
286 case GpsData::Pdop:
287 name = "Pdop";
288 break;
289 case GpsData::Hdop:
290 name = "Hdop";
291 break;
292 case GpsData::Vdop:
293 name = "Vdop";
294 break;
295 }
296
297 return new GpsDataElement(this, name, data_type);
298}
299
300void GpsData::CopyDatas(char *dst) const {
301 GetMutex();
302
303 Queue(&dst, &latitude, sizeof(latitude));
304 Queue(&dst, &longitude, sizeof(longitude));
305 Queue(&dst, &altitude, sizeof(altitude));
306 Queue(&dst, &numberOfSatellites, sizeof(numberOfSatellites));
307 Queue(&dst, &fixQuality, sizeof(FixQuality_t));
308 Queue(&dst, &east, sizeof(east));
309 Queue(&dst, &north, sizeof(north));
310 Queue(&dst, &up, sizeof(up));
311 Queue(&dst, &eastVelocity, sizeof(eastVelocity));
312 Queue(&dst, &northVelocity, sizeof(northVelocity));
313 Queue(&dst, &pDop, sizeof(pDop));
314 Queue(&dst, &hDop, sizeof(hDop));
315 Queue(&dst, &vDop, sizeof(vDop));
316
317 ReleaseMutex();
318}
319
320void GpsData::Queue(char **dst, const void *src, size_t size) const {
321 memcpy(*dst, src, size);
322 *dst += size;
323}
324
325} // end namespace core
326} // end namespace flair
Note: See TracBrowser for help on using the repository browser.