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

Last change on this file since 224 was 224, checked in by Sanahuja Guillaume, 4 years ago

maj for armv5te

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