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

Last change on this file since 214 was 214, checked in by Sanahuja Guillaume, 6 years ago

matrix

File size: 4.6 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: 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>
[51]21#include <GpsData.h>
[55]22#include <SharedMem.h>
23#include <SpinBox.h>
24#include <DoubleSpinBox.h>
25#include <GroupBox.h>
26#include <Euler.h>
[214]27#include <Matrix.h>
[55]28#include <sstream>
29#include "geodesie.h"
[3]30
31using std::string;
[55]32using std::ostringstream;
[3]33using namespace flair::core;
[55]34using namespace flair::gui;
35using namespace Geodesie;
[3]36
[15]37namespace flair {
38namespace sensor {
[3]39
[137]40SimuGps::SimuGps(string name,
[158]41 NmeaGps::NMEAFlags_t NMEAFlags, uint32_t modelId,uint32_t deviceId,uint8_t priority)
[137]42 : NmeaGps(name, NMEAFlags),Thread(getFrameworkManager(), name, priority) {
[3]43
[55]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);
[206]50 magneticDeclination= new DoubleSpinBox(GetGroupBox()->NewRow(), "magnetic declination", " deg", -180, 180, .1, 2,0);
51
[158]52 shmem = new SharedMem((Thread *)this, ShMemName(modelId, deviceId),
[55]53 sizeof(gps_states_t));
[157]54
55 SetIsReady(true);
[55]56}
57
58
[158]59SimuGps::SimuGps(const IODevice *parent, string name, uint32_t modelId,uint32_t deviceId)
[55]60 : NmeaGps(parent, name), Thread(parent, name, 0) {
61 dataRate = NULL;
62
[158]63 shmem = new SharedMem((Thread *)this,ShMemName(modelId, deviceId),
[55]64 sizeof(gps_states_t));
[157]65
66 SetIsReady(true);
[55]67}
68
[15]69SimuGps::~SimuGps() {
70 SafeStop();
71 Join();
[3]72}
73
[158]74string SimuGps::ShMemName(uint32_t modelId,uint32_t deviceId) {
75 ostringstream dev_name;
76 dev_name << "simu" << modelId << "_gps_" << deviceId;
77 return dev_name.str().c_str();
78}
79
[55]80void SimuGps::UpdateFrom(const io_data *data) {
81 if (data != NULL) {
[214]82 Matrix *input = (Matrix *)data;
[55]83 gps_states_t state;
84
85 input->GetMutex();
86 //simulator is ned, convert it to enu
87 //TODO: put simulator in enu?
[68]88 state.e = input->ValueNoMutex(5, 0);//y simulator
89 state.n = input->ValueNoMutex(4, 0);//x simulator
90 state.u = -input->ValueNoMutex(6, 0);//z simulator
91 state.ve = input->ValueNoMutex(11, 0);//vy simulator
92 state.vn = input->ValueNoMutex(10, 0);//vx simulator
[55]93 input->ReleaseMutex();
94
95 shmem->Write((char *)&state, sizeof(gps_states_t));
96 }
97}
98
[15]99void SimuGps::Run(void) {
[55]100 gps_states_t state;
[68]101 char buf[512];
[51]102 nmeaGPGGA gga;
103 nmeaGPVTG vtg;
[55]104 nmeaPOS pos;
105 nmeaINFO info;
106
[68]107 vtg.dir_t='T';
108 vtg.dec_m='M';
109 vtg.spk_k='K';
110 vtg.spn_n='N';
[214]111 gga.elv_units='M';
112 gga.diff_units='M';
[68]113
[55]114 if (dataRate == NULL) {
115 Thread::Err("not applicable for simulation part.\n");
116 return;
117 }
118
119 SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
120
[15]121 WarnUponSwitches(true);
[3]122
[51]123
[15]124 while (!ToBeStopped()) {
125 WaitPeriod();
[51]126
[55]127 if (dataRate->ValueChanged() == true) {
128 SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
129 }
130
131 shmem->Read((char *)&state, sizeof(gps_states_t));
132
133 double x, y, z;
[68]134 ENU_2_ECEF(state.e, state.n, state.u, x,y,z, Euler::ToRadian(longitudeRef->Value()), Euler::ToRadian(latitudeRef->Value()), altitudeRef->Value());
[55]135 ECEF_2_Geographique( x, y, z,pos.lon, pos.lat, gga.elv);
136 nmea_pos2info(&pos,&info);
137
138 if(pos.lat>0) {
139 gga.ns='N';
140 gga.lat=info.lat;
141 } else {
142 gga.ns='S';
143 gga.lat=-info.lat;
144 }
145 if(pos.lon>0) {
146 gga.ew='E';
147 gga.lon=info.lon;
148 } else {
149 gga.ew='W';
150 gga.lon=-info.lon;
151 }
152
153 gga.sig=fixQuality->Value();
154 gga.satinuse=numberOfSatellites->Value();
155
[214]156 int size=nmea_gen_GPGGA(buf,sizeof(buf),&gga);
157 parseFrame(buf,size);
158
[68]159 vtg.dir=90-Euler::ToDegree(atan2f(state.vn,state.ve));
160 vtg.spk=sqrtf(state.ve*state.ve+state.vn*state.vn)*3600./1000.;
[214]161 size=nmea_gen_GPVTG(buf,sizeof(buf),&vtg);
162 parseFrame(buf,size);
[15]163 }
[3]164
[15]165 WarnUponSwitches(false);
[3]166}
167
168} // end namespace sensor
[170]169} // end namespace flair
Note: See TracBrowser for help on using the repository browser.