source: flair-src/branches/sanscv/lib/FlairCore/src/GpsData.cpp @ 326

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

removing opencv dependency

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::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
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.