Changeset 51 in flair-src for trunk/lib/FlairCore/src


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

gps

Location:
trunk/lib/FlairCore/src
Files:
5 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairCore/src/FrameworkManager_impl.cpp

    r45 r51  
    618618void FrameworkManager_impl::AddDeviceToLog(IODevice *device) {
    619619  if (logger_defined == false) {
    620     Err("SetupLogger() was not called, not starting log\n");
     620    Warn("SetupLogger() was not called, not adding to log\n");
    621621    return;
    622622  }

  • 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();

  • trunk/lib/FlairCore/src/GpsData.h

    r45 r51  
    1414
    1515#include <io_data.h>
    16 #include <IODataElement.h>
    17 #include <Vector3D.h>
    1816
    1917namespace flair {
    2018namespace core {
     19
     20  class IODataElement;
     21  class Vector3D;
    2122
    2223/*! \class GpsData
    … …  
    3536    std::string GetDescription() const { return "gps data"; }
    3637    size_t GetSize() const {
    37       return 0;
     38      size_t size = 0;
     39      size += 2*doubleType.GetSize(); // Latitude, Longitude
     40      size += floatType.GetSize(); // Altitude
     41      size += UInt8Type.GetSize(); // NumberOfSatellites
     42      size += UInt8Type.GetSize(); // FixQuality_t
     43      size += 5*floatType.GetSize();//e,n,u,ve,vn
     44      return size;
    3845    }
    3946
    … …  
    4653  */
    4754  typedef enum {
    48 
     55    Latitude /*! latitude in degrees */,
     56    Longitude /*! longitude in degrees */,
     57    Altitude /*! altitude */,
     58    NumberOfSatellites /*! number of satellites */,
     59    FixQuality /*! fix quality */,
     60    East /*! east */,
     61    North /*! north */,
     62    Up /*! up */,
     63    EastVelocity /*! east velocity*/,
     64    NorthVelocity /*! north velocity*/,
    4965  } PlotableData_t;
     66
     67  /*!
     68  \enum FixQuality_t
     69  \brief Fix qualty indicators
     70  */
     71  enum class FixQuality_t : uint8_t {
     72    Invalid = 0,    /*!< invalid */
     73    Gps = 1,        /*!< Gps */
     74    DGps = 2,       /*!< Differential Gps */
     75    Pps = 3,        /*!< Pps */
     76    Rtk = 4,        /*!< RTK */
     77    RtkFloat = 5,   /*!< RTK float */
     78    Estimated = 6,  /*!< Estimated */
     79    Manual = 7,     /*!< Manual */
     80    Simulation = 8, /*!< Simulation */
     81  };
    5082
    5183  /*!
    … …  
    77109  IODataElement *Element(PlotableData_t data_type) const;
    78110
     111  /*!
     112  * \brief Get latitude, longitude and altitude
     113  *
     114  * This method is mutex protected.
     115  *
     116  * \param latitude latitude
     117  * \param longitude longitude
     118  * \param altitude altitude
     119  *
     120  */
     121  void GetLla(double &latitude, double &longitude,
     122                          float &altitude) const;
     123
     124  /*!
     125  * \brief Set latitude, longitude and altitude
     126  *
     127  * This method is mutex protected.
     128  *
     129  * \param latitude latitude
     130  * \param longitude longitude
     131  * \param altitude altitude
     132  *
     133  */
     134  void SetLla(double latitude,double longitude,
     135                          float altitude);
     136
     137
     138  /*!
     139  * \brief Get east, north and up
     140  *
     141  * This method is mutex protected.
     142  *
     143  * \param east east
     144  * \param north north
     145  * \param up up
     146  *
     147  */
     148  void GetEnu(float &east, float &north,
     149                          float &up) const;
     150
     151  /*!
     152  * \brief Set east, north and up
     153  *
     154  * This method is mutex protected.
     155  *
     156  * \param east east
     157  * \param north north
     158  * \param up up
     159  *
     160  */
     161  void SetEnu(float east, float north,
     162                          float up);
     163
     164  /*!
     165  * \brief Get east and north velocities
     166  *
     167  * This method is mutex protected.
     168  *
     169  * \param eastVelocity east velocity
     170  * \param northVelocity north velocity
     171  *
     172  */
     173  void GetVelocity(float &eastVelocity, float &northVelocity) const;
     174
     175  /*!
     176  * \brief Set east and north velocities
     177  *
     178  * This method is mutex protected.
     179  *
     180  * \param eastVelocity east velocity
     181  * \param northVelocity north velocity
     182  *
     183  */
     184  void SetVelocity(float eastVelocity, float northVelocity);
     185
     186  /*!
     187  * \brief Get number of satellites
     188  *
     189  * \return number of satellites
     190  *
     191  */
     192  uint8_t GetNumberOfSatellites(void) const;
     193
     194  /*!
     195  * \brief Set number of satellites
     196  *
     197  * \param numberOfSatellites number of satellites
     198  *
     199  */
     200  void SetNumberOfSatellites(uint8_t numberOfSatellites);
     201
     202  /*!
     203  * \brief Get fix quality
     204  *
     205  * \return fix quality
     206  *
     207  */
     208  FixQuality_t GetFixQuality(void) const;
     209
     210  /*!
     211  * \brief Set fix quality
     212  *
     213  * \param fixQuality fix quality
     214  *
     215  */
     216  void SetFixQuality(FixQuality_t fixQuality);
    79217
    80218  Type const &GetDataType() const { return dataType; }
    … …  
    91229  void CopyDatas(char *dst) const;
    92230
    93 
    94 
    95231  void Queue(char **dst, const void *src, size_t size) const;
    96232  Type dataType;
     233  double latitude,longitude;
     234  float altitude;
     235  uint8_t numberOfSatellites;
     236  FixQuality_t fixQuality;
     237  float east,north,up,eastVelocity,northVelocity;
    97238};
    98239

  • trunk/lib/FlairCore/src/io_data.cpp

    r15 r51  
    2626DummyType dummyType;
    2727FloatType floatType;
     28DoubleType doubleType;
    2829SignedIntegerType Int8Type(8);
    2930SignedIntegerType Int16Type(16);
     31UnsignedIntegerType UInt8Type(8);
     32UnsignedIntegerType UInt16Type(16);
    3033
    3134io_data::io_data(const Object *parent, string name, int n)

  • trunk/lib/FlairCore/src/io_data.h

    r15 r51  
    5858extern SignedIntegerType Int16Type;
    5959
     60class UnsignedIntegerType : public ScalarType {
     61public:
     62  UnsignedIntegerType(size_t sizeInBits) : ScalarType(sizeInBits / 8) {}
     63  std::string GetDescription() const {
     64    return "uint" + std::to_string(GetSize() * 8) + "_t";
     65  };
     66};
     67extern UnsignedIntegerType UInt8Type;
     68extern UnsignedIntegerType UInt16Type;
     69
    6070class FloatType : public ScalarType {
    6171public:
    … …  
    6474};
    6575extern FloatType floatType;
     76
     77class DoubleType : public ScalarType {
     78public:
     79  DoubleType() : ScalarType(8) {}
     80  std::string GetDescription() const { return "double"; };
     81};
     82extern DoubleType doubleType;
    6683
    6784/*! \class io_data
Note: See TracChangeset for help on using the changeset viewer.