Changeset 51 in flair-src for trunk/lib/FlairCore/src/GpsData.cpp


Ignore:
Timestamp:
Jul 26, 2016, 5:32:57 PM (5 years ago)
Author:
Sanahuja Guillaume
Message:

gps

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairCore/src/GpsData.cpp

    r45 r51  
    1818#include "GpsData.h"
    1919#include "Euler.h"
     20#include "IODataElement.h"
    2021#include <math.h>
    2122#include <string.h>
     
    3738    plotableData = inPlotableData;
    3839
    39     size = 0;
     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    }
    4058  }
    4159
     
    4361
    4462  void CopyData(char *dst) const {
    45 
    46 
    47   }
    48 
    49   FloatType const &GetDataType(void) const { return dataType; }
     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  }
    50131
    51132private:
    52133  const GpsData *gpsdata;
    53134  GpsData::PlotableData_t plotableData;
    54   FloatType dataType;
    55135};
    56136
     
    60140    Warn("n>1 not supported\n");
    61141
    62   AppendLogDescription("latitude", floatType);
    63   AppendLogDescription("longitude", floatType);
     142  AppendLogDescription("latitude", doubleType);
     143  AppendLogDescription("longitude", doubleType);
    64144  AppendLogDescription("altitude", floatType);
    65 
    66 
     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;
    67155}
    68156
    69157GpsData::~GpsData() {}
    70158
     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}
    71221
    72222IODataElement *GpsData::Element(PlotableData_t data_type) const {
    73 
    74   //return new GpsDataElement(this, name, data_type);
     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);
    75258}
    76259
     
    78261  GetMutex();
    79262
    80   //Queue(&dst, &rawAcc.x, sizeof(rawAcc.x));
     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));
    81273
    82274  ReleaseMutex();
Note: See TracChangeset for help on using the changeset viewer.