source: flair-src/trunk/lib/FlairSensorActuator/src/SimuGps.cpp@ 133

Last change on this file since 133 was 68, checked in by Sanahuja Guillaume, 8 years ago

gps vtg

File size: 4.3 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: SimuGps.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
18#include "SimuGps.h"
19#include <FrameworkManager.h>
20#include <GeoCoordinate.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 <cvmatrix.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
40SimuGps::SimuGps(const FrameworkManager *parent, string name,
41 NmeaGps::NMEAFlags_t NMEAFlags, uint32_t deviceId,uint8_t priority)
42 : NmeaGps(parent, name, NMEAFlags),Thread(parent, 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
51 ostringstream dev_name;
52 dev_name << "simu_gps_" << deviceId;
53 shmem = new SharedMem((Thread *)this, dev_name.str().c_str(),
54 sizeof(gps_states_t));
55}
56
57
58SimuGps::SimuGps(const IODevice *parent, string name, uint32_t deviceId)
59 : NmeaGps(parent, name), Thread(parent, name, 0) {
60 dataRate = NULL;
61
62 ostringstream dev_name;
63 dev_name << "simu_gps_" << deviceId;
64 shmem = new SharedMem((Thread *)this, dev_name.str().c_str(),
65 sizeof(gps_states_t));
66}
67
68SimuGps::~SimuGps() {
69 SafeStop();
70 Join();
71}
72
73void SimuGps::UpdateFrom(const io_data *data) {
74 if (data != NULL) {
75 cvmatrix *input = (cvmatrix *)data;
76 gps_states_t state;
77
78 input->GetMutex();
79 //simulator is ned, convert it to enu
80 //TODO: put simulator in enu?
81 state.e = input->ValueNoMutex(5, 0);//y simulator
82 state.n = input->ValueNoMutex(4, 0);//x simulator
83 state.u = -input->ValueNoMutex(6, 0);//z simulator
84 state.ve = input->ValueNoMutex(11, 0);//vy simulator
85 state.vn = input->ValueNoMutex(10, 0);//vx simulator
86 input->ReleaseMutex();
87
88 shmem->Write((char *)&state, sizeof(gps_states_t));
89 }
90}
91
92void SimuGps::Run(void) {
93 gps_states_t state;
94 char buf[512];
95 nmeaGPGGA gga;
96 nmeaGPVTG vtg;
97 nmeaPOS pos;
98 nmeaINFO info;
99
100 vtg.dir_t='T';
101 vtg.dec_m='M';
102 vtg.spk_k='K';
103 vtg.spn_n='N';
104
105 if (dataRate == NULL) {
106 Thread::Err("not applicable for simulation part.\n");
107 return;
108 }
109
110 SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
111
112 WarnUponSwitches(true);
113
114
115 while (!ToBeStopped()) {
116 WaitPeriod();
117
118 if (dataRate->ValueChanged() == true) {
119 SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
120 }
121
122 shmem->Read((char *)&state, sizeof(gps_states_t));
123
124 double x, y, z;
125 ENU_2_ECEF(state.e, state.n, state.u, x,y,z, Euler::ToRadian(longitudeRef->Value()), Euler::ToRadian(latitudeRef->Value()), altitudeRef->Value());
126 ECEF_2_Geographique( x, y, z,pos.lon, pos.lat, gga.elv);
127 nmea_pos2info(&pos,&info);
128
129 if(pos.lat>0) {
130 gga.ns='N';
131 gga.lat=info.lat;
132 } else {
133 gga.ns='S';
134 gga.lat=-info.lat;
135 }
136 if(pos.lon>0) {
137 gga.ew='E';
138 gga.lon=info.lon;
139 } else {
140 gga.ew='W';
141 gga.lon=-info.lon;
142 }
143
144 gga.sig=fixQuality->Value();
145 gga.satinuse=numberOfSatellites->Value();
146
147 nmea_gen_GPGGA(buf,sizeof(buf),&gga);
148 parseFrame(buf,sizeof(buf));
149
150 vtg.dir=90-Euler::ToDegree(atan2f(state.vn,state.ve));
151 vtg.spk=sqrtf(state.ve*state.ve+state.vn*state.vn)*3600./1000.;
152 nmea_gen_GPVTG(buf,sizeof(buf),&vtg);
153 parseFrame(buf,sizeof(buf));
154 }
155
156 WarnUponSwitches(false);
157}
158
159} // end namespace sensor
160} // end namespace framewor
Note: See TracBrowser for help on using the repository browser.