Changeset 55 in flair-src for trunk/lib/FlairSensorActuator/src/SimuGps.cpp

Timestamp:

Jul 28, 2016, 5:55:31 PM (9 years ago)

Author:

Sanahuja Guillaume

Message:

simu gps

File:

• trunk/lib/FlairSensorActuator/src/SimuGps.cpp (modified) (4 diffs)

trunk/lib/FlairSensorActuator/src/SimuGps.cpp

@@ -18 +18 @@
 #include "SimuGps.h"
 #include <FrameworkManager.h>
-#include <string.h>
 #include <GeoCoordinate.h>
 #include <GpsData.h>
+#include <SharedMem.h>
+#include <SpinBox.h>
+#include <DoubleSpinBox.h>
+#include <GroupBox.h>
+#include <Euler.h>
+#include <cvmatrix.h>
+#include <sstream>
+#include "geodesie.h"
 
 using std::string;
+using std::ostringstream;
 using namespace flair::core;
+using namespace flair::gui;
+using namespace Geodesie;
 
 namespace flair {
@@ -29 +39 @@
 
 SimuGps::SimuGps(const FrameworkManager *parent, string name,
-                 NmeaGps::NMEAFlags_t NMEAFlags, uint8_t priority)
-    : Thread(parent, name, priority), NmeaGps(parent, name, NMEAFlags) {}
+                 NmeaGps::NMEAFlags_t NMEAFlags, uint32_t deviceId,uint8_t priority)
+    : NmeaGps(parent, name, NMEAFlags),Thread(parent, name, priority) {
+
+  dataRate = new SpinBox(GetGroupBox()->NewRow(), "data rate", " Hz", 1, 500, 1, 200);
+  latitudeRef = new DoubleSpinBox(GetGroupBox()->NewRow(), "latitude ref", " deg", -90, 90, 1, 6,49.402313);
+  longitudeRef = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "longitude ref", " deg", -180, 180, 1, 6,2.795463);
+  altitudeRef= new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "altitude ref", " m", 0, 6000, 100, 1,0);
+  fixQuality = new SpinBox(GetGroupBox()->NewRow(), "fix quality", 1, 8, 1, 1);
+  numberOfSatellites = new SpinBox(GetGroupBox()->NewRow(), "number of satellites", 1, 15, 1, 5);
+
+  ostringstream dev_name;
+  dev_name << "simu_gps_" << deviceId;
+  shmem = new SharedMem((Thread *)this, dev_name.str().c_str(),
+                        sizeof(gps_states_t));
+}
+
+
+SimuGps::SimuGps(const IODevice *parent, string name, uint32_t deviceId)
+    : NmeaGps(parent, name), Thread(parent, name, 0) {
+  dataRate = NULL;
+
+  ostringstream dev_name;
+  dev_name << "simu_gps_" << deviceId;
+  shmem = new SharedMem((Thread *)this, dev_name.str().c_str(),
+                        sizeof(gps_states_t));
+}
 
 SimuGps::~SimuGps() {
@@ -37 +71 @@
 }
 
+void SimuGps::UpdateFrom(const io_data *data) {
+  if (data != NULL) {
+    cvmatrix *input = (cvmatrix *)data;
+    gps_states_t state;
+
+    input->GetMutex();
+    //simulator is ned, convert it to enu
+    //TODO: put simulator in enu?
+    state.x = input->ValueNoMutex(5, 0);
+    state.y = input->ValueNoMutex(4, 0);
+    state.z = -input->ValueNoMutex(6, 0);
+    input->ReleaseMutex();
+
+    shmem->Write((char *)&state, sizeof(gps_states_t));
+  }
+}
+
 void SimuGps::Run(void) {
-  // double lat=0;
+  gps_states_t state;
   char buf[500];
   nmeaGPGGA gga;
   nmeaGPVTG vtg;
-  SetPeriodMS(500);
+  nmeaPOS pos;
+  nmeaINFO info;
+
+  if (dataRate == NULL) {
+    Thread::Err("not applicable for simulation part.\n");
+    return;
+  }
+
+  SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
+
   WarnUponSwitches(true);
-
-  gga.sig=1;
-  gga.satinuse=2;
-  gga.lat=49.402313;
-  gga.lon=2.795463;
 
   vtg.spn=1;
@@ -56 +111 @@
     WaitPeriod();
 
+    if (dataRate->ValueChanged() == true) {
+      SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
+    }
+
+    shmem->Read((char *)&state, sizeof(gps_states_t));
+
+    double x, y, z;
+    ENU_2_ECEF(state.x, state.y, state.z, x,y,z, Euler::ToRadian(longitudeRef->Value()), Euler::ToRadian(latitudeRef->Value()), altitudeRef->Value());
+    ECEF_2_Geographique( x, y, z,pos.lon, pos.lat, gga.elv);
+    nmea_pos2info(&pos,&info);
+
+    if(pos.lat>0) {
+      gga.ns='N';
+      gga.lat=info.lat;
+    } else {
+      gga.ns='S';
+      gga.lat=-info.lat;
+    }
+    if(pos.lon>0) {
+      gga.ew='E';
+      gga.lon=info.lon;
+    } else {
+      gga.ew='W';
+      gga.lon=-info.lon;
+    }
+
+    gga.sig=fixQuality->Value();
+    gga.satinuse=numberOfSatellites->Value();
+
     nmea_gen_GPGGA(buf,sizeof(buf),&gga);
     parseFrame(buf,sizeof(buf));
-    position->SetCoordinates(49.402313, 2.795463, 0);
-    //      lat+=.5;
   }