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

Last change on this file since 124 was 51, checked in by Sanahuja Guillaume, 5 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.