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

gps

File:
1 moved

Legend:

Unmodified
Added
Removed
  • 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  }
Note: See TracChangeset for help on using the changeset viewer.