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

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

iadded isready to iodevice:
avoid problem of imu not ready in ardrone2

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