source: flair-src/trunk/lib/FlairSensorActuator/src/SimulatedGps.cpp @ 330

Last change on this file since 330 was 286, checked in by Sanahuja Guillaume, 3 years ago

draw vrpn axis in simulator

File size: 3.7 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5//  created:    2014/05/26
6//  filename:   SimulatedGps.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Class for a simulation GPS
14//
15//
16/*********************************************************************/
17#ifdef CORE2_64
18
19#include "SimulatedGps.h"
20#include <FrameworkManager.h>
21#include <GpsData.h>
22#include <SharedMem.h>
23#include <SpinBox.h>
24#include <DoubleSpinBox.h>
25#include <GroupBox.h>
26#include <Euler.h>
27#include <Matrix.h>
28#include <sstream>
29#include "geodesie.h"
30
31using std::string;
32using std::ostringstream;
33using namespace flair::core;
34using namespace flair::gui;
35using namespace Geodesie;
36
37namespace flair {
38namespace sensor {
39
40SimulatedGps::SimulatedGps(string name,
41                 NmeaGps::NMEAFlags_t NMEAFlags, uint32_t modelId,uint32_t deviceId,uint8_t priority)
42    : NmeaGps(name, NMEAFlags),Thread(getFrameworkManager(), name, priority) {
43
44  dataRate = new SpinBox(GetGroupBox()->NewRow(), "data rate", " Hz", 1, 500, 1, 200);
45  latitudeRef = new DoubleSpinBox(GetGroupBox()->NewRow(), "latitude ref", " deg", -90, 90, 1, 6,49.402313);
46  longitudeRef = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "longitude ref", " deg", -180, 180, 1, 6,2.795463);
47  altitudeRef= new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "altitude ref", " m", 0, 6000, 100, 1,0);
48  fixQuality = new SpinBox(GetGroupBox()->NewRow(), "fix quality", 1, 8, 1, 1);
49  numberOfSatellites = new SpinBox(GetGroupBox()->NewRow(), "number of satellites", 1, 15, 1, 5);
50  magneticDeclination= new DoubleSpinBox(GetGroupBox()->NewRow(), "magnetic declination", " deg", -180, 180, .1, 2,0);
51 
52  shmem = new SharedMem((Thread *)this, ShMemName(modelId, deviceId),
53                        sizeof(gps_states_t));
54                       
55  SetIsReady(true);
56}
57
58
59SimulatedGps::~SimulatedGps() {
60  SafeStop();
61  Join();
62}
63
64string SimulatedGps::ShMemName(uint32_t modelId,uint32_t deviceId) {
65  ostringstream dev_name;
66  dev_name << "simu" <<  modelId << "_gps_" << deviceId;
67  return dev_name.str().c_str();
68}
69
70void SimulatedGps::Run(void) {
71  gps_states_t state;
72  char buf[512];
73  nmeaGPGGA gga;
74  nmeaGPVTG vtg;
75  nmeaPOS pos;
76  nmeaINFO info;
77
78  vtg.dir_t='T';
79  vtg.dec_m='M';
80  vtg.spk_k='K';
81  vtg.spn_n='N';
82  gga.elv_units='M';
83  gga.diff_units='M';
84
85  SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
86
87  WarnUponSwitches(true);
88
89
90  while (!ToBeStopped()) {
91    WaitPeriod();
92
93    if (dataRate->ValueChanged() == true) {
94      SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
95    }
96
97    shmem->Read((char *)&state, sizeof(gps_states_t));
98
99    double x, y, z;
100    ENU_2_ECEF(state.e, state.n, state.u, x,y,z, Euler::ToRadian(longitudeRef->Value()), Euler::ToRadian(latitudeRef->Value()), altitudeRef->Value());
101    ECEF_2_Geographique( x, y, z,pos.lon, pos.lat, gga.elv);
102    nmea_pos2info(&pos,&info);
103
104    if(pos.lat>0) {
105      gga.ns='N';
106      gga.lat=info.lat;
107    } else {
108      gga.ns='S';
109      gga.lat=-info.lat;
110    }
111    if(pos.lon>0) {
112      gga.ew='E';
113      gga.lon=info.lon;
114    } else {
115      gga.ew='W';
116      gga.lon=-info.lon;
117    }
118
119    gga.sig=fixQuality->Value();
120    gga.satinuse=numberOfSatellites->Value();
121
122    int size=nmea_gen_GPGGA(buf,sizeof(buf),&gga);
123    parseFrame(buf,size);
124   
125    vtg.dir=90-Euler::ToDegree(atan2f(state.vn,state.ve));
126    vtg.spk=sqrtf(state.ve*state.ve+state.vn*state.vn)*3600./1000.;
127    size=nmea_gen_GPVTG(buf,sizeof(buf),&vtg);
128    parseFrame(buf,size);
129  }
130
131  WarnUponSwitches(false);
132}
133
134} // end namespace sensor
135} // end namespace flair
136
137#endif
Note: See TracBrowser for help on using the repository browser.