[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:
|
---|
| 51 | size=4;
|
---|
| 52 | break;
|
---|
| 53 | case GpsData::NumberOfSatellites:
|
---|
| 54 | case GpsData::FixQuality:
|
---|
| 55 | size=1;
|
---|
| 56 | break;
|
---|
| 57 | }
|
---|
[42] | 58 | }
|
---|
| 59 |
|
---|
| 60 | ~GpsDataElement() {}
|
---|
| 61 |
|
---|
| 62 | void CopyData(char *dst) const {
|
---|
[51] | 63 | double latitude,longitude;
|
---|
| 64 | float altitude,east,north,up,eastVelocity,northVelocity;
|
---|
[42] | 65 |
|
---|
[51] | 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);
|
---|
[42] | 71 |
|
---|
[51] | 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 | }
|
---|
[42] | 107 | }
|
---|
| 108 |
|
---|
[51] | 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 | }
|
---|
[42] | 131 |
|
---|
| 132 | private:
|
---|
| 133 | const GpsData *gpsdata;
|
---|
| 134 | GpsData::PlotableData_t plotableData;
|
---|
| 135 | };
|
---|
| 136 |
|
---|
| 137 | GpsData::GpsData(const Object *parent, std::string name, int n)
|
---|
[45] | 138 | : io_data(parent, name, n), dataType() {
|
---|
[42] | 139 | if (n > 1)
|
---|
| 140 | Warn("n>1 not supported\n");
|
---|
| 141 |
|
---|
[51] | 142 | AppendLogDescription("latitude", doubleType);
|
---|
| 143 | AppendLogDescription("longitude", doubleType);
|
---|
[45] | 144 | AppendLogDescription("altitude", floatType);
|
---|
[51] | 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);
|
---|
[42] | 152 |
|
---|
[51] | 153 | numberOfSatellites=0;
|
---|
| 154 | fixQuality=FixQuality_t::Invalid;
|
---|
[42] | 155 | }
|
---|
| 156 |
|
---|
| 157 | GpsData::~GpsData() {}
|
---|
| 158 |
|
---|
[51] | 159 | void GpsData::GetLla(double &outLatitude, double &outLongitude, float &outAltitude) const {
|
---|
| 160 | GetMutex();
|
---|
| 161 | outLatitude=latitude;
|
---|
| 162 | outLongitude=longitude;
|
---|
| 163 | outAltitude=altitude;
|
---|
| 164 | ReleaseMutex();
|
---|
| 165 | }
|
---|
[42] | 166 |
|
---|
[51] | 167 | void GpsData::SetLla(double inLatitude,double inLongitude,float inAltitude) {
|
---|
| 168 | GetMutex();
|
---|
| 169 | latitude=inLatitude;
|
---|
| 170 | longitude=inLongitude;
|
---|
| 171 | altitude=inAltitude;
|
---|
| 172 | ReleaseMutex();
|
---|
| 173 | }
|
---|
| 174 |
|
---|
| 175 | void GpsData::GetEnu(float &outEast, float &outNorth,float &outUp) const {
|
---|
| 176 | GetMutex();
|
---|
| 177 | outEast=east;
|
---|
| 178 | outNorth=north;
|
---|
| 179 | outUp=up;
|
---|
| 180 | ReleaseMutex();
|
---|
| 181 | }
|
---|
| 182 |
|
---|
| 183 | void GpsData::SetEnu(float inEast, float inNorth,float inUp) {
|
---|
| 184 | GetMutex();
|
---|
| 185 | east=inEast;
|
---|
| 186 | north=inNorth;
|
---|
| 187 | up=inUp;
|
---|
| 188 | ReleaseMutex();
|
---|
| 189 | }
|
---|
| 190 |
|
---|
| 191 |
|
---|
| 192 | void GpsData::GetVelocity(float &outEastVelocity, float &outNorthVelocity) const {
|
---|
| 193 | GetMutex();
|
---|
| 194 | outEastVelocity=eastVelocity;
|
---|
| 195 | outNorthVelocity=northVelocity;
|
---|
| 196 | ReleaseMutex();
|
---|
| 197 | }
|
---|
| 198 |
|
---|
| 199 | void GpsData::SetVelocity(float inEastVelocity, float inNorthVelocity) {
|
---|
| 200 | GetMutex();
|
---|
| 201 | eastVelocity=inEastVelocity;
|
---|
| 202 | northVelocity=inNorthVelocity;
|
---|
| 203 | ReleaseMutex();
|
---|
| 204 | }
|
---|
| 205 |
|
---|
| 206 | uint8_t GpsData::GetNumberOfSatellites(void) const {
|
---|
| 207 | return numberOfSatellites;
|
---|
| 208 | }
|
---|
| 209 |
|
---|
| 210 | void GpsData::SetNumberOfSatellites(uint8_t inNumberOfSatellites) {
|
---|
| 211 | numberOfSatellites=inNumberOfSatellites;
|
---|
| 212 | }
|
---|
| 213 |
|
---|
| 214 | GpsData::FixQuality_t GpsData::GetFixQuality(void) const {
|
---|
| 215 | return fixQuality;
|
---|
| 216 | }
|
---|
| 217 |
|
---|
| 218 | void GpsData::SetFixQuality(FixQuality_t inFixQuality) {
|
---|
| 219 | fixQuality=inFixQuality;
|
---|
| 220 | }
|
---|
| 221 |
|
---|
[42] | 222 | IODataElement *GpsData::Element(PlotableData_t data_type) const {
|
---|
[51] | 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 | }
|
---|
[42] | 256 |
|
---|
[51] | 257 | return new GpsDataElement(this, name, data_type);
|
---|
[42] | 258 | }
|
---|
| 259 |
|
---|
| 260 | void GpsData::CopyDatas(char *dst) const {
|
---|
| 261 | GetMutex();
|
---|
| 262 |
|
---|
[51] | 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));
|
---|
[42] | 273 |
|
---|
| 274 | ReleaseMutex();
|
---|
| 275 | }
|
---|
| 276 |
|
---|
| 277 | void 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
|
---|