[42] | 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"
|
---|
[51] | 20 | #include "IODataElement.h"
|
---|
[42] | 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 |
|
---|
[51] | 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:
|
---|
[180] | 51 | case GpsData::Pdop:
|
---|
| 52 | case GpsData::Hdop:
|
---|
| 53 | case GpsData::Vdop:
|
---|
[51] | 54 | size=4;
|
---|
| 55 | break;
|
---|
| 56 | case GpsData::NumberOfSatellites:
|
---|
| 57 | case GpsData::FixQuality:
|
---|
| 58 | size=1;
|
---|
| 59 | break;
|
---|
| 60 | }
|
---|
[42] | 61 | }
|
---|
| 62 |
|
---|
| 63 | ~GpsDataElement() {}
|
---|
| 64 |
|
---|
| 65 | void CopyData(char *dst) const {
|
---|
[51] | 66 | double latitude,longitude;
|
---|
[180] | 67 | float altitude,east,north,up,eastVelocity,northVelocity,pDop,hDop,vDop;
|
---|
[42] | 68 |
|
---|
[51] | 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);
|
---|
[180] | 74 | gpsdata->GetDop(pDop,hDop,vDop);
|
---|
[42] | 75 |
|
---|
[51] | 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;
|
---|
[180] | 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;
|
---|
[51] | 116 | default:
|
---|
| 117 | gpsdata->Err("data type unavailable.\n");
|
---|
| 118 | break;
|
---|
| 119 | }
|
---|
[42] | 120 | }
|
---|
| 121 |
|
---|
[51] | 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:
|
---|
[180] | 134 | case GpsData::Pdop:
|
---|
| 135 | case GpsData::Hdop:
|
---|
| 136 | case GpsData::Vdop:
|
---|
[51] | 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 | }
|
---|
[42] | 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)
|
---|
[45] | 154 | : io_data(parent, name, n), dataType() {
|
---|
[42] | 155 | if (n > 1)
|
---|
| 156 | Warn("n>1 not supported\n");
|
---|
| 157 |
|
---|
[51] | 158 | AppendLogDescription("latitude", doubleType);
|
---|
| 159 | AppendLogDescription("longitude", doubleType);
|
---|
[45] | 160 | AppendLogDescription("altitude", floatType);
|
---|
[51] | 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);
|
---|
[180] | 168 | AppendLogDescription("pdop", floatType);
|
---|
| 169 | AppendLogDescription("hdop", floatType);
|
---|
| 170 | AppendLogDescription("vdop", floatType);
|
---|
[42] | 171 |
|
---|
[51] | 172 | numberOfSatellites=0;
|
---|
| 173 | fixQuality=FixQuality_t::Invalid;
|
---|
[42] | 174 | }
|
---|
| 175 |
|
---|
| 176 | GpsData::~GpsData() {}
|
---|
| 177 |
|
---|
[51] | 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 | }
|
---|
[42] | 185 |
|
---|
[51] | 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 |
|
---|
[180] | 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 |
|
---|
[42] | 253 | IODataElement *GpsData::Element(PlotableData_t data_type) const {
|
---|
[51] | 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;
|
---|
[180] | 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;
|
---|
[51] | 295 | }
|
---|
[42] | 296 |
|
---|
[51] | 297 | return new GpsDataElement(this, name, data_type);
|
---|
[42] | 298 | }
|
---|
| 299 |
|
---|
[252] | 300 | void GpsData::RawRead(char *dst) const {
|
---|
[42] | 301 | GetMutex();
|
---|
| 302 |
|
---|
[51] | 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));
|
---|
[180] | 313 | Queue(&dst, &pDop, sizeof(pDop));
|
---|
| 314 | Queue(&dst, &hDop, sizeof(hDop));
|
---|
| 315 | Queue(&dst, &vDop, sizeof(vDop));
|
---|
[42] | 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
|
---|