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 |
|
---|
25 | using std::string;
|
---|
26 |
|
---|
27 | namespace flair {
|
---|
28 | namespace core {
|
---|
29 |
|
---|
30 | /*! \class GpsDataElement
|
---|
31 | */
|
---|
32 | class GpsDataElement : public IODataElement {
|
---|
33 | public:
|
---|
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 |
|
---|
148 | private:
|
---|
149 | const GpsData *gpsdata;
|
---|
150 | GpsData::PlotableData_t plotableData;
|
---|
151 | };
|
---|
152 |
|
---|
153 | GpsData::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 |
|
---|
176 | GpsData::~GpsData() {}
|
---|
177 |
|
---|
178 | void GpsData::GetLla(double &outLatitude, double &outLongitude, float &outAltitude) const {
|
---|
179 | GetMutex();
|
---|
180 | outLatitude=latitude;
|
---|
181 | outLongitude=longitude;
|
---|
182 | outAltitude=altitude;
|
---|
183 | ReleaseMutex();
|
---|
184 | }
|
---|
185 |
|
---|
186 | void GpsData::SetLla(double inLatitude,double inLongitude,float inAltitude) {
|
---|
187 | GetMutex();
|
---|
188 | latitude=inLatitude;
|
---|
189 | longitude=inLongitude;
|
---|
190 | altitude=inAltitude;
|
---|
191 | ReleaseMutex();
|
---|
192 | }
|
---|
193 |
|
---|
194 | void GpsData::GetEnu(float &outEast, float &outNorth,float &outUp) const {
|
---|
195 | GetMutex();
|
---|
196 | outEast=east;
|
---|
197 | outNorth=north;
|
---|
198 | outUp=up;
|
---|
199 | ReleaseMutex();
|
---|
200 | }
|
---|
201 |
|
---|
202 | void GpsData::SetEnu(float inEast, float inNorth,float inUp) {
|
---|
203 | GetMutex();
|
---|
204 | east=inEast;
|
---|
205 | north=inNorth;
|
---|
206 | up=inUp;
|
---|
207 | ReleaseMutex();
|
---|
208 | }
|
---|
209 |
|
---|
210 |
|
---|
211 | void GpsData::GetVelocity(float &outEastVelocity, float &outNorthVelocity) const {
|
---|
212 | GetMutex();
|
---|
213 | outEastVelocity=eastVelocity;
|
---|
214 | outNorthVelocity=northVelocity;
|
---|
215 | ReleaseMutex();
|
---|
216 | }
|
---|
217 |
|
---|
218 | void GpsData::SetVelocity(float inEastVelocity, float inNorthVelocity) {
|
---|
219 | GetMutex();
|
---|
220 | eastVelocity=inEastVelocity;
|
---|
221 | northVelocity=inNorthVelocity;
|
---|
222 | ReleaseMutex();
|
---|
223 | }
|
---|
224 |
|
---|
225 | uint8_t GpsData::GetNumberOfSatellites(void) const {
|
---|
226 | return numberOfSatellites;
|
---|
227 | }
|
---|
228 |
|
---|
229 | void GpsData::SetNumberOfSatellites(uint8_t inNumberOfSatellites) {
|
---|
230 | numberOfSatellites=inNumberOfSatellites;
|
---|
231 | }
|
---|
232 |
|
---|
233 | GpsData::FixQuality_t GpsData::GetFixQuality(void) const {
|
---|
234 | return fixQuality;
|
---|
235 | }
|
---|
236 |
|
---|
237 | void GpsData::SetFixQuality(FixQuality_t inFixQuality) {
|
---|
238 | fixQuality=inFixQuality;
|
---|
239 | }
|
---|
240 |
|
---|
241 | void GpsData::GetDop(float &inPdop, float &inHdop,float &inVdop) const {
|
---|
242 | inPdop=pDop;
|
---|
243 | inHdop=hDop;
|
---|
244 | inVdop=vDop;
|
---|
245 | }
|
---|
246 |
|
---|
247 | void GpsData::SetDop(float inPdop, float inHdop,float inVdop) {
|
---|
248 | pDop=inPdop;
|
---|
249 | hDop=inHdop;
|
---|
250 | vDop=inVdop;
|
---|
251 | }
|
---|
252 |
|
---|
253 | IODataElement *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 |
|
---|
300 | void GpsData::RawRead(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 |
|
---|
320 | void 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
|
---|