source: flair-src/trunk/lib/FlairSensorActuator/src/NmeaGps.cpp @ 167

Last change on this file since 167 was 167, checked in by Sanahuja Guillaume, 5 years ago

modifs pour template vectors

File size: 6.9 KB
RevLine 
[3]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[3]4// %flair:license}
5//  created:    2013/08/23
[51]6//  filename:   NmeaGps.cpp
[3]7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
[51]13//  purpose:    Base class for GPS using NMEA sentances
[3]14//
15//
16/*********************************************************************/
17
[51]18#include "NmeaGps.h"
[3]19#include "geodesie.h"
20#include <Euler.h>
21#include <DataPlot1D.h>
22#include <Tab.h>
23#include <TabWidget.h>
24#include <GridLayout.h>
25#include <GroupBox.h>
26#include <PushButton.h>
27#include <FrameworkManager.h>
28#include <Map.h>
29#include <GeoCoordinate.h>
30#include <Vector3D.h>
31#include <Label.h>
[51]32#include <GpsData.h>
[3]33#include <string.h>
34
35using std::string;
36using namespace Geodesie;
37using namespace flair::core;
38using namespace flair::gui;
39
[15]40namespace flair {
41namespace sensor {
[3]42
[137]43NmeaGps::NmeaGps(string name, NMEAFlags_t NMEAFlags)
44    : IODevice(getFrameworkManager(), name) {
[15]45  this->NMEAFlags = NMEAFlags;
[3]46
[15]47  nmea_zero_INFO(&info);
48  nmea_parser_init(&parser);
[55]49  altRef = 0;
[3]50
[15]51  if ((NMEAFlags & GGA) == 0) {
[51]52    Err("Enable at least the GGA sentence\n");
[15]53  }
[3]54
[15]55  // station sol
[137]56  mainTab = new Tab(getFrameworkManager()->GetTabWidget(), name);
[55]57  tab = new TabWidget(mainTab->NewRow(), name);
58  sensorTab = new Tab(tab, "Setup");
59  setupGroupbox = new GroupBox(sensorTab->NewRow(), name);
60  buttonRef = new PushButton(setupGroupbox->NewRow(), "set ref");
61  nbSatLabel = new Label(setupGroupbox->NewRow(), "nb_sat");
62  fixLabel = new Label(setupGroupbox->LastRowLastCol(), "fix");
[3]63
[15]64  position = new GeoCoordinate((IODevice *)this, "position", 0, 0, 0);
[3]65
[55]66  takeRef = false;
[51]67
68  gpsData = new GpsData(this);
69  AddDataToLog(gpsData);
70
[55]71  nbSatLabel->SetText("number of satellies: %i", gpsData->GetNumberOfSatellites());
72  fixLabel->SetText("fix quality: %i", gpsData->GetFixQuality());
[3]73}
74
[55]75NmeaGps::NmeaGps(const IODevice *parent, std::string name) : IODevice(parent, name) {
76  gpsData = new GpsData(this);
77  mainTab = NULL;
78  tab = NULL;
79  sensorTab = NULL;
80}
81
[51]82NmeaGps::~NmeaGps() {
[55]83  if (mainTab != NULL) {
84    nmea_parser_destroy(&parser);
85    delete mainTab;
86  }
[3]87}
88
[55]89GroupBox *NmeaGps::GetGroupBox(void) const { return setupGroupbox; }
90
[51]91const GpsData *NmeaGps::GetDatas(void) const {
92   return gpsData;
93}
94
95void NmeaGps::GetDatas(core::GpsData **outGpsData) const {
96  *outGpsData = gpsData;
97}
98
99void NmeaGps::UseDefaultPlot(void) {
[55]100  plotTab = new Tab(tab, "Mesures");
[3]101
[15]102  if ((NMEAFlags & GGA) != 0) {
[55]103    ePlot = new DataPlot1D(plotTab->NewRow(), "e", -10, 10);
104    ePlot->AddCurve(gpsData->Element(GpsData::East));
105    nPlot = new DataPlot1D(plotTab->LastRowLastCol(), "n", -10, 10);
106    nPlot->AddCurve(gpsData->Element(GpsData::North));
107    uPlot = new DataPlot1D(plotTab->LastRowLastCol(), "u", -10, 10);
108    uPlot->AddCurve(gpsData->Element(GpsData::Up));
[15]109  }
110  if ((NMEAFlags & VTG) != 0) {
[55]111    vePlot = new DataPlot1D(plotTab->NewRow(), "ve", -10, 10);
112    vePlot->AddCurve(gpsData->Element(GpsData::EastVelocity));
113    vnPlot = new DataPlot1D(plotTab->LastRowLastCol(), "vn", -10, 10);
114    vnPlot->AddCurve(gpsData->Element(GpsData::NorthVelocity));
[15]115  }
[3]116
[15]117  Tab *map_tab = new Tab(tab, "carte");
118  map = new Map(map_tab->NewRow(), "map");
119  map->AddPoint(position, "drone");
[3]120}
121
[51]122DataPlot1D *NmeaGps::EPlot(void) const {
[15]123  if ((NMEAFlags & GGA) != 0) {
[55]124    return ePlot;
[15]125  } else {
126    Err("GGA sentence not requested\n");
127    return NULL;
128  }
[3]129}
130
[51]131DataPlot1D *NmeaGps::NPlot(void) const {
[15]132  if ((NMEAFlags & GGA) != 0) {
[55]133    return nPlot;
[15]134  } else {
135    Err("GGA sentence not requested\n");
136    return NULL;
137  }
[3]138}
139
[51]140DataPlot1D *NmeaGps::UPlot(void) const {
[15]141  if ((NMEAFlags & GGA) != 0) {
[55]142    return uPlot;
[15]143  } else {
144    Err("GGA sentence not requested\n");
145    return NULL;
146  }
[3]147}
148
[51]149DataPlot1D *NmeaGps::VEPlot(void) const {
[15]150  if ((NMEAFlags & VTG) != 0) {
[55]151    return vePlot;
[15]152  } else {
153    Err("GGA sentence not requested\n");
154    return NULL;
155  }
[3]156}
157
[51]158DataPlot1D *NmeaGps::VNPlot(void) const {
[15]159  if ((NMEAFlags & VTG) != 0) {
[55]160    return vnPlot;
[15]161  } else {
162    Err("GGA sentence not requested\n");
163    return NULL;
164  }
[3]165}
166
[55]167Layout *NmeaGps::GetLayout(void) const { return sensorTab; }
[3]168
[55]169Tab *NmeaGps::GetPlotTab(void) const { return plotTab; }
[3]170
[51]171TabWidget *NmeaGps::GetTab(void) const { return tab; }
[3]172
[55]173void NmeaGps::SetRef(void) { takeRef = true; }
[3]174
[167]175void NmeaGps::GetEnu(Vector3Df *point) {
[51]176  gpsData->GetMutex();
177  gpsData->GetEnu(point->x,point->y,point->z);
178  gpsData->ReleaseMutex();
[3]179}
180
[51]181void NmeaGps::parseFrame(const char *frame, int frame_size) {
[3]182
[15]183  int result;
[3]184
[15]185  result = nmea_parse(&parser, frame, frame_size, &info);
186  if (result != 1) {
[42]187    Warn("unrecognized nmea sentence: %s\n",frame);
[67]188    return;
[15]189  }
190
191  result = nmea_parse_GPGGA(frame, frame_size, &pack);
192
193  if (result == 1) {
[51]194    nmea_info2pos(&info, &pos);
[15]195    // Printf("%s\n",frame);
[51]196   //  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);
197//Printf("lat:%f long:%f\n",pos.lat,pos.lon);
198
199    gpsData->GetMutex(); // on utilise le mutex de gpsData pour fix et nb_sat
200    if (gpsData->GetFixQuality() != (GpsData::FixQuality_t)pack.sig) {
201      gpsData->SetFixQuality((GpsData::FixQuality_t)pack.sig);
[55]202      fixLabel->SetText("fix: %i", pack.sig);
[3]203    }
[51]204    if (gpsData->GetNumberOfSatellites() != pack.satinuse) {
205      gpsData->SetNumberOfSatellites(pack.satinuse) ;
[55]206      nbSatLabel->SetText("nb_sat: %i", pack.satinuse);
[15]207    }
[51]208    gpsData->ReleaseMutex();
[3]209
[51]210    gpsData->SetLla(Euler::ToDegree(pos.lat), Euler::ToDegree(pos.lon),info.elv);
[15]211    position->SetCoordinates(Euler::ToDegree(pos.lat), Euler::ToDegree(pos.lon),
212                             info.elv);
[3]213
[55]214    if ((info.sig == 2 && altRef == 0) || buttonRef->Clicked() == true ||
215        takeRef == true) {
[15]216      Printf("prise pos ref\n");
[55]217      latRef = pos.lat;
218      longRef = pos.lon;
219      altRef = info.elv;
220      takeRef = false;
[15]221    }
222    // if(alt_ref!=0)
[3]223    {
[15]224      double x, y, z;
225      double e, n, u;
226      Geographique_2_ECEF(pos.lon, pos.lat, info.elv, x, y, z);
[55]227      ECEF_2_ENU(x, y, z, e, n, u, longRef, latRef, altRef);
[15]228      // Printf("lon:%f lat:%f elv:%f\n",pos.lon,pos.lat,info.elv);
[3]229
[51]230      gpsData->SetEnu(e,n,u);
[3]231
[15]232      if ((NMEAFlags & VTG) != 0) {
[51]233        gpsData->SetVelocity(info.speed * 1000. / 3600. * sin(Euler::ToRadian(info.direction)),
234                                  info.speed * 1000. / 3600. * cos(Euler::ToRadian(info.direction)));
235      }/*
[15]236      if ((NMEAFlags & GST) != 0) {
237        // Thread::Printf("dev_lon:%f dev_lat:%f
238        // dev_elv:%f\n",info.dev_lat,info.dev_lon,info.dev_elv);
239        output->SetValueNoMutex(index, 0, info.dev_lat);
240        output->SetValueNoMutex(index + 1, 0, info.dev_lon);
241        output->SetValueNoMutex(index + 2, 0, info.dev_elv);
242        index += 3;
[51]243      }*/
[3]244
[51]245
246      gpsData->SetDataTime(GetTime());
247      ProcessUpdate(gpsData);
[3]248    }
[15]249  }
[3]250}
251
252} // end namespace sensor
253} // end namespace framewor
Note: See TracBrowser for help on using the repository browser.