Changeset 51 in flair-src for trunk/lib


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

gps

Location:
trunk/lib
Files:
15 edited
2 moved

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

  • trunk/lib/FlairFilter/src/Ahrs.h

    r15 r51  
    1818
    1919namespace flair {
    20 namespace core {
    21 class Euler;
    22 class Vector3D;
    23 class ImuData;
    24 class Quaternion;
    25 class AhrsData;
    26 }
    27 namespace gui {
    28 class Tab;
    29 class DataPlot1D;
    30 }
    31 namespace sensor {
    32 class Imu;
    33 }
     20  namespace core {
     21    class Euler;
     22    class Vector3D;
     23    class ImuData;
     24    class Quaternion;
     25    class AhrsData;
     26  }
     27  namespace gui {
     28    class Tab;
     29    class DataPlot1D;
     30  }
     31  namespace sensor {
     32    class Imu;
     33  }
    3434}
    3535

  • trunk/lib/FlairSensorActuator/src/Imu.h

    r15 r51  
    1717
    1818namespace flair {
    19 namespace core {
    20 class ImuData;
    21 class OneAxisRotation;
    22 }
    23 namespace gui {
    24 class Tab;
    25 class TabWidget;
    26 class GroupBox;
    27 class Layout;
    28 class DataPlot1D;
    29 }
     19  namespace core {
     20    class ImuData;
     21    class OneAxisRotation;
     22  }
     23  namespace gui {
     24    class Tab;
     25    class TabWidget;
     26    class GroupBox;
     27    class Layout;
     28    class DataPlot1D;
     29  }
    3030}
    3131

  • trunk/lib/FlairSensorActuator/src/Mb800.cpp

    r42 r51  
    2828
    2929Mb800::Mb800(const FrameworkManager *parent, string name,
    30              SerialPort *serialport, Gps::NMEAFlags_t NMEAFlags,
     30             SerialPort *serialport, NmeaGps::NMEAFlags_t NMEAFlags,
    3131             uint8_t priority)
    32     : Gps(parent, name, NMEAFlags), Thread(parent, name, priority) {
     32    : NmeaGps(parent, name, NMEAFlags), Thread(parent, name, priority) {
    3333  this->serialport = serialport;
    3434  serialport->SetRxTimeout(100000000);

  • trunk/lib/FlairSensorActuator/src/Mb800.h

    r15 r51  
    1515
    1616#include <Thread.h>
    17 #include <Gps.h>
     17#include <NmeaGps.h>
    1818
    1919namespace flair {
    … …  
    3030* \brief Class for mb800 gps receiver
    3131*/
    32 class Mb800 : public core::Thread, public Gps {
     32class Mb800 : public core::Thread, public NmeaGps {
    3333public:
    3434  /*!
    … …  
    4444  */
    4545  Mb800(const core::FrameworkManager *parent, std::string name,
    46         core::SerialPort *serialport, Gps::NMEAFlags_t NMEAFlags,
     46        core::SerialPort *serialport, NmeaGps::NMEAFlags_t NMEAFlags,
    4747        uint8_t priority);
    4848

  • trunk/lib/FlairSensorActuator/src/NmeaGps.cpp

    r42 r51  
    44// %flair:license}
    55//  created:    2013/08/23
    6 //  filename:   Gps.cpp
     6//  filename:   NmeaGps.cpp
    77//
    88//  author:     Guillaume Sanahuja
    … …  
    1111//  version:    $Id: $
    1212//
    13 //  purpose:    objet integrant le recepteur gps mb800
     13//  purpose:    Base class for GPS using NMEA sentances
    1414//
    1515//
    1616/*********************************************************************/
    1717
    18 #include "Gps.h"
     18#include "NmeaGps.h"
    1919#include "geodesie.h"
    2020#include <Euler.h>
    21 #include <cvmatrix.h>
    2221#include <DataPlot1D.h>
    2322#include <Tab.h>
    … …  
    3130#include <Vector3D.h>
    3231#include <Label.h>
     32#include <GpsData.h>
    3333#include <string.h>
    3434
    … …  
    4141namespace sensor {
    4242
    43 Gps::Gps(const FrameworkManager *parent, string name, NMEAFlags_t NMEAFlags)
     43NmeaGps::NmeaGps(const FrameworkManager *parent, string name, NMEAFlags_t NMEAFlags)
    4444    : IODevice(parent, name) {
    4545  this->NMEAFlags = NMEAFlags;
    … …  
    5050
    5151  if ((NMEAFlags & GGA) == 0) {
    52     Err("Enable at least GGA sentence\n");
    53   }
    54 
    55   int index = 0;
    56   if ((NMEAFlags & GGA) != 0) {
    57     index += 3;
    58   }
    59   if ((NMEAFlags & VTG) != 0) {
    60     index += 2;
    61   }
    62   if ((NMEAFlags & GST) != 0) {
    63     index += 3;
    64   }
    65 
    66   cvmatrix_descriptor *desc = new cvmatrix_descriptor(index, 1);
    67   index = 0;
    68   if ((NMEAFlags & GGA) != 0) {
    69     desc->SetElementName(0, 0, "e");
    70     desc->SetElementName(1, 0, "n");
    71     desc->SetElementName(2, 0, "u");
    72     index += 3;
    73   }
    74   if ((NMEAFlags & VTG) != 0) {
    75     desc->SetElementName(index, 0, "ve");
    76     desc->SetElementName(index + 1, 0, "vn");
    77     index += 2;
    78   }
     52    Err("Enable at least the GGA sentence\n");
     53  }
     54
     55 /*
    7956  if ((NMEAFlags & GST) != 0) {
    8057    desc->SetElementName(index, 0, "dev_lat");
    … …  
    8360    index += 3;
    8461  }
    85   output = new cvmatrix((IODevice *)this, desc, floatType);
    86   AddDataToLog(output);
     62  */
    8763
    8864  // station sol
    … …  
    9773  position = new GeoCoordinate((IODevice *)this, "position", 0, 0, 0);
    9874
    99   fix = FixQuality_t::Invalid;
    100   nb_sat = 0;
    10175  take_ref = false;
    102   nb_sat_label->SetText("nb_sat: %i", nb_sat);
    103   fix_label->SetText("fix: %i", fix);
    104 }
    105 
    106 Gps::~Gps() {
     76
     77  gpsData = new GpsData(this);
     78  AddDataToLog(gpsData);
     79
     80  nb_sat_label->SetText("nb_sat: %i", gpsData->GetNumberOfSatellites());
     81  fix_label->SetText("fix: %i", gpsData->GetFixQuality());
     82}
     83
     84NmeaGps::~NmeaGps() {
    10785  nmea_parser_destroy(&parser);
    10886  delete main_tab;
    10987}
    11088
    111 void Gps::UseDefaultPlot(void) {
    112   int index = 0;
     89const GpsData *NmeaGps::GetDatas(void) const {
     90   return gpsData;
     91}
     92
     93void NmeaGps::GetDatas(core::GpsData **outGpsData) const {
     94  *outGpsData = gpsData;
     95}
     96
     97void NmeaGps::UseDefaultPlot(void) {
    11398  plot_tab = new Tab(tab, "Mesures");
    11499
    115100  if ((NMEAFlags & GGA) != 0) {
    116101    e_plot = new DataPlot1D(plot_tab->NewRow(), "e", -10, 10);
    117     e_plot->AddCurve(output->Element(index));
     102    e_plot->AddCurve(gpsData->Element(GpsData::East));
    118103    n_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "n", -10, 10);
    119     n_plot->AddCurve(output->Element(index + 1));
     104    n_plot->AddCurve(gpsData->Element(GpsData::North));
    120105    u_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "u", -10, 10);
    121     u_plot->AddCurve(output->Element(index + 2));
    122     index += 3;
     106    u_plot->AddCurve(gpsData->Element(GpsData::Up));
    123107  }
    124108  if ((NMEAFlags & VTG) != 0) {
    125109    ve_plot = new DataPlot1D(plot_tab->NewRow(), "ve", -10, 10);
    126     ve_plot->AddCurve(output->Element(index));
     110    ve_plot->AddCurve(gpsData->Element(GpsData::EastVelocity));
    127111    vn_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "vn", -10, 10);
    128     vn_plot->AddCurve(output->Element(index + 1));
    129     index += 2;
     112    vn_plot->AddCurve(gpsData->Element(GpsData::NorthVelocity));
    130113  }
    131114
    … …  
    135118}
    136119
    137 DataPlot1D *Gps::EPlot(void) const {
     120DataPlot1D *NmeaGps::EPlot(void) const {
    138121  if ((NMEAFlags & GGA) != 0) {
    139122    return e_plot;
    … …  
    144127}
    145128
    146 DataPlot1D *Gps::NPlot(void) const {
     129DataPlot1D *NmeaGps::NPlot(void) const {
    147130  if ((NMEAFlags & GGA) != 0) {
    148131    return n_plot;
    … …  
    153136}
    154137
    155 DataPlot1D *Gps::UPlot(void) const {
     138DataPlot1D *NmeaGps::UPlot(void) const {
    156139  if ((NMEAFlags & GGA) != 0) {
    157140    return u_plot;
    … …  
    162145}
    163146
    164 DataPlot1D *Gps::VEPlot(void) const {
     147DataPlot1D *NmeaGps::VEPlot(void) const {
    165148  if ((NMEAFlags & VTG) != 0) {
    166149    return ve_plot;
    … …  
    171154}
    172155
    173 DataPlot1D *Gps::VNPlot(void) const {
     156DataPlot1D *NmeaGps::VNPlot(void) const {
    174157  if ((NMEAFlags & VTG) != 0) {
    175158    return vn_plot;
    … …  
    180163}
    181164
    182 Layout *Gps::GetLayout(void) const { return sensor_tab; }
    183 
    184 Tab *Gps::GetPlotTab(void) const { return plot_tab; }
    185 
    186 TabWidget *Gps::GetTab(void) const { return tab; }
    187 
    188 uint16_t Gps::NbSat(void) const {
    189   output->GetMutex();
    190   uint16_t result = nb_sat;
    191   output->ReleaseMutex();
    192   return result;
    193 }
    194 
    195 Gps::FixQuality_t Gps::FixQuality(void) const {
    196   output->GetMutex();
    197   FixQuality_t result = fix;
    198   output->ReleaseMutex();
    199   return result;
    200 }
    201 
    202 void Gps::SetRef(void) { take_ref = true; }
    203 
    204 void Gps::GetENUPosition(Vector3D *point) {
    205   output->GetMutex();
    206   point->x = output->ValueNoMutex(0, 0);
    207   point->y = output->ValueNoMutex(1, 0);
    208   point->z = output->ValueNoMutex(2, 0);
    209   output->ReleaseMutex();
    210 }
    211 
    212 void Gps::parseFrame(const char *frame, int frame_size) {
     165Layout *NmeaGps::GetLayout(void) const { return sensor_tab; }
     166
     167Tab *NmeaGps::GetPlotTab(void) const { return plot_tab; }
     168
     169TabWidget *NmeaGps::GetTab(void) const { return tab; }
     170
     171void NmeaGps::SetRef(void) { take_ref = true; }
     172
     173void NmeaGps::GetEnu(Vector3D *point) {
     174  gpsData->GetMutex();
     175  gpsData->GetEnu(point->x,point->y,point->z);
     176  gpsData->ReleaseMutex();
     177}
     178
     179void NmeaGps::parseFrame(const char *frame, int frame_size) {
    213180
    214181  int result;
    … …  
    222189
    223190  if (result == 1) {
     191    nmea_info2pos(&info, &pos);
    224192    // Printf("%s\n",frame);
    225     // Printf("nb:%i fix:%i lat:%f long:%f alt:%f vel:%f
    226     // angle:%f\n",pack.satinuse,pack.sig,info.lat,info.lon,info.elv,info.speed*1000./3600.,info.direction);
    227     // Printf("lat:%f long:%f\n",pos.lat,pos.lon);
    228     output->GetMutex(); // on utilise le mutex de l'output pour fix et nb_sat
    229     if ((int)fix != pack.sig) {
    230       fix = (FixQuality_t)pack.sig;
    231       fix_label->SetText("fix: %i", fix);
    232     }
    233     if (nb_sat != pack.satinuse) {
    234       nb_sat = pack.satinuse;
    235       nb_sat_label->SetText("nb_sat: %i", nb_sat);
    236     }
    237     output->ReleaseMutex();
    238 
    239     nmea_info2pos(&info, &pos);
     193   //  Printf("nb:%i fix:%i lat:%f long:%f alt:%f vel:%f angle:%f\n",pack.satinuse,pack.sig,info.lat,info.lon,info.elv,info.speed*1000./3600.,info.direction);
     194//Printf("lat:%f long:%f\n",pos.lat,pos.lon);
     195
     196    gpsData->GetMutex(); // on utilise le mutex de gpsData pour fix et nb_sat
     197    if (gpsData->GetFixQuality() != (GpsData::FixQuality_t)pack.sig) {
     198      gpsData->SetFixQuality((GpsData::FixQuality_t)pack.sig);
     199      fix_label->SetText("fix: %i", pack.sig);
     200    }
     201    if (gpsData->GetNumberOfSatellites() != pack.satinuse) {
     202      gpsData->SetNumberOfSatellites(pack.satinuse) ;
     203      nb_sat_label->SetText("nb_sat: %i", pack.satinuse);
     204    }
     205    gpsData->ReleaseMutex();
     206
     207    gpsData->SetLla(Euler::ToDegree(pos.lat), Euler::ToDegree(pos.lon),info.elv);
    240208    position->SetCoordinates(Euler::ToDegree(pos.lat), Euler::ToDegree(pos.lon),
    241209                             info.elv);
    … …  
    257225      // Printf("lon:%f lat:%f elv:%f\n",pos.lon,pos.lat,info.elv);
    258226
    259       // on prend une fois pour toute le mutex et on fait des accès directs
    260       output->GetMutex();
    261       output->SetValueNoMutex(0, 0, e);
    262       output->SetValueNoMutex(1, 0, n);
    263       output->SetValueNoMutex(2, 0, u);
    264 
    265       int index = 3;
     227      gpsData->SetEnu(e,n,u);
     228
    266229      if ((NMEAFlags & VTG) != 0) {
    267         output->SetValueNoMutex(index, 0,
    268                                 info.speed * 1000. / 3600. *
    269                                     sin(Euler::ToRadian(info.direction)));
    270         output->SetValueNoMutex(index + 1, 0,
    271                                 info.speed * 1000. / 3600. *
    272                                     cos(Euler::ToRadian(info.direction)));
    273         index += 2;
    274       }
     230        gpsData->SetVelocity(info.speed * 1000. / 3600. * sin(Euler::ToRadian(info.direction)),
     231                                  info.speed * 1000. / 3600. * cos(Euler::ToRadian(info.direction)));
     232      }/*
    275233      if ((NMEAFlags & GST) != 0) {
    276234        // Thread::Printf("dev_lon:%f dev_lat:%f
    … …  
    280238        output->SetValueNoMutex(index + 2, 0, info.dev_elv);
    281239        index += 3;
    282       }
    283       output->ReleaseMutex();
    284 
    285       output->SetDataTime(GetTime());
    286       ProcessUpdate(output);
     240      }*/
     241
     242
     243      gpsData->SetDataTime(GetTime());
     244      ProcessUpdate(gpsData);
    287245    }
    288246  }

  • trunk/lib/FlairSensorActuator/src/NmeaGps.h

    r42 r51  
    44// %flair:license}
    55/*!
    6  * \file Gps.h
    7  * \brief Base class for GPS
     6 * \file NmeaGps.h
     7 * \brief Base class for GPS using NMEA sentances
    88 * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
    99 * \date 2013/08/23
    … …  
    1111 */
    1212
    13 #ifndef GPS_H
    14 #define GPS_H
     13#ifndef NMEAGPS_H
     14#define NMEAGPS_H
    1515
    1616#include <IODevice.h>
    … …  
    1818
    1919namespace flair {
    20 namespace core {
    21 class cvmatrix;
    22 class FrameworkManager;
    23 class GeoCoordinate;
    24 class Vector3D;
    25 }
    26 namespace gui {
    27 class Layout;
    28 class DataPlot1D;
    29 class Tab;
    30 class TabWidget;
    31 class PushButton;
    32 class Map;
    33 class Label;
    34 }
     20  namespace core {
     21    class FrameworkManager;
     22    class GeoCoordinate;
     23    class Vector3D;
     24    class GpsData;
     25  }
     26  namespace gui {
     27    class Layout;
     28    class DataPlot1D;
     29    class Tab;
     30    class TabWidget;
     31    class PushButton;
     32    class Map;
     33    class Label;
     34  }
    3535}
    3636
    3737namespace flair {
    3838namespace sensor {
    39 /*! \class Gps
     39
     40/*! \class NmeaGps
    4041*
    41 * \brief Base class for GPS
     42* \brief Base class for GPS using NMEA sentances
    4243*/
    43 class Gps : public core::IODevice {
     44class NmeaGps : public core::IODevice {
    4445public:
    45   /*!
    46   \enum FixQuality_t
    47   \brief Fix qualty indicators
    48   */
    49   enum class FixQuality_t {
    50     Invalid = 0,    /*!< invalid */
    51     Gps = 1,        /*!< Gps */
    52     DGps = 2,       /*!< Differential Gps */
    53     Pps = 3,        /*!< Pps */
    54     Rtk = 4,        /*!< RTK */
    55     RtkFloat = 5,   /*!< RTK float */
    56     Estimated = 6,  /*!< Estimated */
    57     Manual = 7,     /*!< Manual */
    58     Simulation = 8, /*!< Simulation */
    59   };
    60 
    6146  /*!
    6247  \enum NMEAFlags_t
    … …  
    7257  * \brief Constructor
    7358  *
    74   * Construct a Gps.
     59  * Construct a NmeaGps.
    7560  *
    7661  * \param parent parent
    … …  
    7863  * \param NMEAFlags NMEA sentances to enable
    7964  */
    80   Gps(const core::FrameworkManager *parent, std::string name,
     65  NmeaGps(const core::FrameworkManager *parent, std::string name,
    8166      NMEAFlags_t NMEAFlags);
    8267
    … …  
    8570  *
    8671  */
    87   ~Gps();
     72  ~NmeaGps();
     73
     74  /*!
     75  * \brief Get GPS datas
     76  *
     77  * \return GpsData
     78  */
     79  const core::GpsData *GetDatas(void) const;
    8880
    8981  /*!
    … …  
    148140  */
    149141  gui::Tab *GetPlotTab(void) const;
    150 
    151   /*!
    152   * \brief Number of used satellites
    153   *
    154   * \return number of used satellites
    155   */
    156   uint16_t NbSat(void) const;
    157 
    158   /*!
    159   * \brief Fix Quality
    160   *
    161   * \return fix quality
    162   */
    163   FixQuality_t FixQuality(void) const;
    164142
    165143  /*!
    … …  
    178156  * \param point to store position
    179157  */
    180   void GetENUPosition(core::Vector3D *point);
     158  void GetEnu(core::Vector3D *point);
    181159
    182160protected:
    … …  
    197175protected:
    198176  core::GeoCoordinate *position;
     177
     178  /*!
     179  * \brief Get GPS datas
     180  *
     181  * \param gpsData GPS datas
     182  */
     183  void GetDatas(core::GpsData **gpsData) const;
    199184
    200185private:
    … …  
    219204  gui::Map *map;
    220205  gui::Label *nb_sat_label, *fix_label;
    221   uint16_t nb_sat;
    222   FixQuality_t fix;
    223206  bool take_ref;
    224207  nmeaINFO info;
    … …  
    227210  nmeaPOS pos;
    228211  double lat_ref, long_ref, alt_ref;
    229 
    230   // matrix
    231   core::cvmatrix *output;
     212  core::GpsData* gpsData;
    232213};
    233214} // end namespace sensor
    234215} // end namespace framewor
    235 #endif // GPS_H
     216#endif // NMEAGPS_H

  • trunk/lib/FlairSensorActuator/src/Novatel.cpp

    r15 r51  
    2828
    2929Novatel::Novatel(const FrameworkManager *parent, string name,
    30                  SerialPort *serialport, Gps::NMEAFlags_t NMEAFlags,
     30                 SerialPort *serialport, NmeaGps::NMEAFlags_t NMEAFlags,
    3131                 uint8_t priority)
    32     : Gps(parent, name, NMEAFlags), Thread(parent, name, priority) {
     32    : NmeaGps(parent, name, NMEAFlags), Thread(parent, name, priority) {
    3333  this->serialport = serialport;
    3434}

  • trunk/lib/FlairSensorActuator/src/Novatel.h

    r15 r51  
    1515
    1616#include <Thread.h>
    17 #include <Gps.h>
     17#include <NmeaGps.h>
    1818
    1919namespace flair {
    … …  
    3030* \brief Class for Novatel gps receiver
    3131*/
    32 class Novatel : public core::Thread, public Gps {
     32class Novatel : public core::Thread, public NmeaGps {
    3333public:
    3434  /*!
    … …  
    4444  */
    4545  Novatel(const core::FrameworkManager *parent, std::string name,
    46           core::SerialPort *serialport, Gps::NMEAFlags_t NMEAFlags,
     46          core::SerialPort *serialport, NmeaGps::NMEAFlags_t NMEAFlags,
    4747          uint8_t priority);
    4848

  • trunk/lib/FlairSensorActuator/src/SimuGps.cpp

    r15 r51  
    2020#include <string.h>
    2121#include <GeoCoordinate.h>
     22#include <GpsData.h>
    2223
    2324using std::string;
    … …  
    2829
    2930SimuGps::SimuGps(const FrameworkManager *parent, string name,
    30                  Gps::NMEAFlags_t NMEAFlags, uint8_t priority)
    31     : Thread(parent, name, priority), Gps(parent, name, NMEAFlags) {}
     31                 NmeaGps::NMEAFlags_t NMEAFlags, uint8_t priority)
     32    : Thread(parent, name, priority), NmeaGps(parent, name, NMEAFlags) {}
    3233
    3334SimuGps::~SimuGps() {
    … …  
    3738
    3839void SimuGps::Run(void) {
    39   char response[200] = {0};
    40   int size, result;
    4140  // double lat=0;
     41  char buf[500];
     42  nmeaGPGGA gga;
     43  nmeaGPVTG vtg;
    4244  SetPeriodMS(500);
    4345  WarnUponSwitches(true);
    4446
     47  gga.sig=1;
     48  gga.satinuse=2;
     49  gga.lat=49.402313;
     50  gga.lon=2.795463;
     51
     52  vtg.spn=1;
     53  vtg.dir=0;
     54
    4555  while (!ToBeStopped()) {
    4656    WaitPeriod();
     57
     58    nmea_gen_GPGGA(buf,sizeof(buf),&gga);
     59    parseFrame(buf,sizeof(buf));
    4760    position->SetCoordinates(49.402313, 2.795463, 0);
    4861    //      lat+=.5;

  • trunk/lib/FlairSensorActuator/src/SimuGps.h

    r15 r51  
    1515
    1616#include <Thread.h>
    17 #include <Gps.h>
     17#include <NmeaGps.h>
    1818
    1919namespace flair {
    … …  
    2929* \brief Class for a simulation GPS
    3030*/
    31 class SimuGps : public core::Thread, public Gps {
     31class SimuGps : public core::Thread, public NmeaGps {
    3232public:
    3333  /*!
    … …  
    4242  */
    4343  SimuGps(const core::FrameworkManager *parent, std::string name,
    44           Gps::NMEAFlags_t NMEAFlags, uint8_t priority);
     44          NmeaGps::NMEAFlags_t NMEAFlags, uint8_t priority);
    4545
    4646  /*!

  • trunk/lib/FlairSensorActuator/src/Srf08.cpp

    r15 r51  
    9999
    100100  if (written < 0) {
    101     Thread::Err("erreur rt_dev_write (%s)\n", strerror(-written));
     101    Thread::Err("error write i2c (%s)\n", strerror(-written));
    102102  } else if (written != 2) {
    103     Thread::Err("erreur rt_dev_write %i/2\n", written);
     103    Thread::Err("error write i2c %i/2\n", written);
    104104  }
    105105}
    … …  
    120120    if (written < 0) {
    121121      i2cport->ReleaseMutex();
    122       Thread::Err("erreur rt_dev_write (%s)\n", strerror(-written));
     122      Thread::Err("error write i2c (%s)\n", strerror(-written));
    123123      nb_err++;
    124124      if (nb_err == 20) {
    125         Thread::Err("erreur rt_dev_write (%s), too many errors\n",
     125        Thread::Err("error write i2c (%s), too many errors\n",
    126126                    strerror(-written));
    127127        return;
    … …  
    134134      if (read < 0) {
    135135        if (read != -ETIMEDOUT)
    136           Thread::Err("erreur rt_dev_read (%s) %i\n", strerror(-written), read);
     136          Thread::Err("error read i2c (%s) %i\n", strerror(-read), read);
    137137        nb_err++;
    138138        if (nb_err == 20) {
    139           Thread::Err("erreur rt_dev_read (%s), too many errors\n",
     139          Thread::Err("error read i2c (%s), too many errors\n",
    140140                      strerror(-written));
    141141          return;
    … …  
    143143        SleepMS(1);
    144144      } else if (read != 2) {
    145         Thread::Err("erreur rt_dev_read %i/2\n", read);
     145        Thread::Err("error read i2c %i/2\n", read);
    146146        return;
    147147      } else if (read == 2) {
    … …  
    187187
    188188  if (written < 0) {
    189     Thread::Err("erreur rt_dev_write (%s)\n", strerror(-written));
     189    Thread::Err("error write i2c (%s)\n", strerror(-written));
    190190  } else if (written != 2) {
    191     Thread::Err("erreur rt_dev_write %i/2\n", written);
     191    Thread::Err("error write i2c %i/2\n", written);
    192192  }
    193193}
    … …  
    211211
    212212  if (written < 0) {
    213     Thread::Err("erreur write (%s)\n", strerror(-written));
     213    Thread::Err("error write i2c (%s)\n", strerror(-written));
    214214  } else if (written != 2) {
    215     Thread::Err("erreur write %i/2\n", written);
     215    Thread::Err("error write i2c %i/2\n", written);
    216216  }
    217217}

  • trunk/lib/FlairSensorActuator/src/Srf08.h

    r15 r51  
    4040  * \brief Constructor
    4141  *
    42   * Construct a SimuUs. Control part.
     42  * Construct a SRF08 sensor
    4343  *
    4444  * \param parent parent
Note: See TracChangeset for help on using the changeset viewer.