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

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

gps
File size: 6.8 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 size=4;
52 break;
53 case GpsData::NumberOfSatellites:
54 case GpsData::FixQuality:
55 size=1;
56 break;
57 }
58 }
59
60 ~GpsDataElement() {}
61
62 void CopyData(char *dst) const {
63 double latitude,longitude;
64 float altitude,east,north,up,eastVelocity,northVelocity;
65
66 gpsdata->GetLla(latitude,longitude,altitude);
67 uint8_t numberOfSatellites=gpsdata->GetNumberOfSatellites();
68 GpsData::FixQuality_t fixQuality=gpsdata->GetFixQuality();
69 gpsdata->GetEnu(east,north,up);
70 gpsdata->GetVelocity(eastVelocity,northVelocity);
71
72 switch (plotableData) {
73 case GpsData::Latitude:
74 memcpy(dst, &latitude, sizeof(latitude));
75 break;
76 case GpsData::Longitude:
77 memcpy(dst, &longitude, sizeof(longitude));
78 break;
79 case GpsData::Altitude:
80 memcpy(dst, &altitude, sizeof(altitude));
81 break;
82 case GpsData::NumberOfSatellites:
83 memcpy(dst, &numberOfSatellites, sizeof(numberOfSatellites));
84 break;
85 case GpsData::FixQuality:
86 memcpy(dst, &fixQuality, sizeof(fixQuality));
87 break;
88 case GpsData::East:
89 memcpy(dst, &east, sizeof(east));
90 break;
91 case GpsData::North:
92 memcpy(dst, &north, sizeof(north));
93 break;
94 case GpsData::Up:
95 memcpy(dst, &up, sizeof(up));
96 break;
97 case GpsData::EastVelocity:
98 memcpy(dst, &eastVelocity, sizeof(eastVelocity));
99 break;
100 case GpsData::NorthVelocity:
101 memcpy(dst, &northVelocity, sizeof(northVelocity));
102 break;
103 default:
104 gpsdata->Err("data type unavailable.\n");
105 break;
106 }
107 }
108
109 DataType const &GetDataType(void) const {
110 switch (plotableData) {
111 case GpsData::Latitude:
112 case GpsData::Longitude:
113 return doubleType;
114 break;
115 case GpsData::Altitude:
116 case GpsData::East:
117 case GpsData::North:
118 case GpsData::Up:
119 case GpsData::EastVelocity:
120 case GpsData::NorthVelocity:
121 return floatType;
122 break;
123 case GpsData::NumberOfSatellites:
124 case GpsData::FixQuality:
125 return UInt8Type;
126 break;
127 default:
128 return dummyType;
129 }
130 }
131
132private:
133 const GpsData *gpsdata;
134 GpsData::PlotableData_t plotableData;
135};
136
137GpsData::GpsData(const Object *parent, std::string name, int n)
138 : io_data(parent, name, n), dataType() {
139 if (n > 1)
140 Warn("n>1 not supported\n");
141
142 AppendLogDescription("latitude", doubleType);
143 AppendLogDescription("longitude", doubleType);
144 AppendLogDescription("altitude", floatType);
145 AppendLogDescription("nb_sat",UInt8Type);
146 AppendLogDescription("fix quality",UInt8Type);
147 AppendLogDescription("east", floatType);
148 AppendLogDescription("north", floatType);
149 AppendLogDescription("up", floatType);
150 AppendLogDescription("east velocity", floatType);
151 AppendLogDescription("north velocity", floatType);
152
153 numberOfSatellites=0;
154 fixQuality=FixQuality_t::Invalid;
155}
156
157GpsData::~GpsData() {}
158
159void GpsData::GetLla(double &outLatitude, double &outLongitude, float &outAltitude) const {
160 GetMutex();
161 outLatitude=latitude;
162 outLongitude=longitude;
163 outAltitude=altitude;
164 ReleaseMutex();
165}
166
167void GpsData::SetLla(double inLatitude,double inLongitude,float inAltitude) {
168 GetMutex();
169 latitude=inLatitude;
170 longitude=inLongitude;
171 altitude=inAltitude;
172 ReleaseMutex();
173}
174
175void GpsData::GetEnu(float &outEast, float &outNorth,float &outUp) const {
176 GetMutex();
177 outEast=east;
178 outNorth=north;
179 outUp=up;
180 ReleaseMutex();
181}
182
183void GpsData::SetEnu(float inEast, float inNorth,float inUp) {
184 GetMutex();
185 east=inEast;
186 north=inNorth;
187 up=inUp;
188 ReleaseMutex();
189}
190
191
192void GpsData::GetVelocity(float &outEastVelocity, float &outNorthVelocity) const {
193 GetMutex();
194 outEastVelocity=eastVelocity;
195 outNorthVelocity=northVelocity;
196 ReleaseMutex();
197}
198
199void GpsData::SetVelocity(float inEastVelocity, float inNorthVelocity) {
200 GetMutex();
201 eastVelocity=inEastVelocity;
202 northVelocity=inNorthVelocity;
203 ReleaseMutex();
204}
205
206uint8_t GpsData::GetNumberOfSatellites(void) const {
207 return numberOfSatellites;
208}
209
210void GpsData::SetNumberOfSatellites(uint8_t inNumberOfSatellites) {
211 numberOfSatellites=inNumberOfSatellites;
212}
213
214GpsData::FixQuality_t GpsData::GetFixQuality(void) const {
215 return fixQuality;
216}
217
218void GpsData::SetFixQuality(FixQuality_t inFixQuality) {
219 fixQuality=inFixQuality;
220}
221
222IODataElement *GpsData::Element(PlotableData_t data_type) const {
223 string name;
224 switch (data_type) {
225 case GpsData::Latitude:
226 name = "Latitude";
227 break;
228 case GpsData::Longitude:
229 name = "Longitude";
230 break;
231 case GpsData::Altitude:
232 name = "Altitude";
233 break;
234 case GpsData::NumberOfSatellites:
235 name = "NumberOfSatellites";
236 break;
237 case GpsData::FixQuality:
238 name = "FixQuality";
239 break;
240 case GpsData::East:
241 name = "East";
242 break;
243 case GpsData::North:
244 name = "North";
245 break;
246 case GpsData::Up:
247 name = "Up";
248 break;
249 case GpsData::EastVelocity:
250 name = "EastVelocity";
251 break;
252 case GpsData::NorthVelocity:
253 name = "NorthVelocity";
254 break;
255 }
256
257 return new GpsDataElement(this, name, data_type);
258}
259
260void GpsData::CopyDatas(char *dst) const {
261 GetMutex();
262
263 Queue(&dst, &latitude, sizeof(latitude));
264 Queue(&dst, &longitude, sizeof(longitude));
265 Queue(&dst, &altitude, sizeof(altitude));
266 Queue(&dst, &numberOfSatellites, sizeof(numberOfSatellites));
267 Queue(&dst, &fixQuality, sizeof(FixQuality_t));
268 Queue(&dst, &east, sizeof(east));
269 Queue(&dst, &north, sizeof(north));
270 Queue(&dst, &up, sizeof(up));
271 Queue(&dst, &eastVelocity, sizeof(eastVelocity));
272 Queue(&dst, &northVelocity, sizeof(northVelocity));
273
274 ReleaseMutex();
275}
276
277void GpsData::Queue(char **dst, const void *src, size_t size) const {
278 memcpy(*dst, src, size);
279 *dst += size;
280}
281
282} // end namespace core
283} // end namespace flair
Note: See TracBrowser for help on using the repository browser.