Changeset 55 in flair-src for trunk/lib/FlairSensorActuator/src/SimuGps.cpp
- Timestamp:
- 07/28/16 17:55:31 (7 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairSensorActuator/src/SimuGps.cpp
r51 r55 18 18 #include "SimuGps.h" 19 19 #include <FrameworkManager.h> 20 #include <string.h>21 20 #include <GeoCoordinate.h> 22 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" 23 30 24 31 using std::string; 32 using std::ostringstream; 25 33 using namespace flair::core; 34 using namespace flair::gui; 35 using namespace Geodesie; 26 36 27 37 namespace flair { … … 29 39 30 40 SimuGps::SimuGps(const FrameworkManager *parent, string name, 31 NmeaGps::NMEAFlags_t NMEAFlags, uint8_t priority) 32 : Thread(parent, name, priority), NmeaGps(parent, name, NMEAFlags) {} 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 58 SimuGps::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 } 33 67 34 68 SimuGps::~SimuGps() { … … 37 71 } 38 72 73 void 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.x = input->ValueNoMutex(5, 0); 82 state.y = input->ValueNoMutex(4, 0); 83 state.z = -input->ValueNoMutex(6, 0); 84 input->ReleaseMutex(); 85 86 shmem->Write((char *)&state, sizeof(gps_states_t)); 87 } 88 } 89 39 90 void SimuGps::Run(void) { 40 // double lat=0;91 gps_states_t state; 41 92 char buf[500]; 42 93 nmeaGPGGA gga; 43 94 nmeaGPVTG vtg; 44 SetPeriodMS(500); 95 nmeaPOS pos; 96 nmeaINFO info; 97 98 if (dataRate == NULL) { 99 Thread::Err("not applicable for simulation part.\n"); 100 return; 101 } 102 103 SetPeriodUS((uint32_t)(1000000. / dataRate->Value())); 104 45 105 WarnUponSwitches(true); 46 47 gga.sig=1;48 gga.satinuse=2;49 gga.lat=49.402313;50 gga.lon=2.795463;51 106 52 107 vtg.spn=1; … … 56 111 WaitPeriod(); 57 112 113 if (dataRate->ValueChanged() == true) { 114 SetPeriodUS((uint32_t)(1000000. / dataRate->Value())); 115 } 116 117 shmem->Read((char *)&state, sizeof(gps_states_t)); 118 119 double x, y, z; 120 ENU_2_ECEF(state.x, state.y, state.z, x,y,z, Euler::ToRadian(longitudeRef->Value()), Euler::ToRadian(latitudeRef->Value()), altitudeRef->Value()); 121 ECEF_2_Geographique( x, y, z,pos.lon, pos.lat, gga.elv); 122 nmea_pos2info(&pos,&info); 123 124 if(pos.lat>0) { 125 gga.ns='N'; 126 gga.lat=info.lat; 127 } else { 128 gga.ns='S'; 129 gga.lat=-info.lat; 130 } 131 if(pos.lon>0) { 132 gga.ew='E'; 133 gga.lon=info.lon; 134 } else { 135 gga.ew='W'; 136 gga.lon=-info.lon; 137 } 138 139 gga.sig=fixQuality->Value(); 140 gga.satinuse=numberOfSatellites->Value(); 141 58 142 nmea_gen_GPGGA(buf,sizeof(buf),&gga); 59 143 parseFrame(buf,sizeof(buf)); 60 position->SetCoordinates(49.402313, 2.795463, 0);61 // lat+=.5;62 144 } 63 145
Note:
See TracChangeset
for help on using the changeset viewer.