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

Timestamp:

Aug 30, 2016, 6:42:07 PM (8 years ago)

Author:

Sanahuja Guillaume

Message:

gps vtg

File:

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

trunk/lib/FlairSensorActuator/src/SimuGps.cpp

@@ -79 +79 @@
     //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);
+    state.e = input->ValueNoMutex(5, 0);//y simulator
+    state.n = input->ValueNoMutex(4, 0);//x simulator
+    state.u = -input->ValueNoMutex(6, 0);//z simulator
+    state.ve = input->ValueNoMutex(11, 0);//vy simulator
+    state.vn = input->ValueNoMutex(10, 0);//vx simulator
     input->ReleaseMutex();
 
@@ -90 +92 @@
 void SimuGps::Run(void) {
   gps_states_t state;
-  char buf[500];
+  char buf[512];
   nmeaGPGGA gga;
   nmeaGPVTG vtg;
   nmeaPOS pos;
   nmeaINFO info;
+
+  vtg.dir_t='T';
+  vtg.dec_m='M';
+  vtg.spk_k='K';
+  vtg.spn_n='N';
 
   if (dataRate == NULL) {
@@ -105 +112 @@
   WarnUponSwitches(true);
 
-  vtg.spn=1;
-  vtg.dir=0;
 
   while (!ToBeStopped()) {
@@ -118 +123 @@
 
     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());
+    ENU_2_ECEF(state.e, state.n, state.u, 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);
@@ -142 +147 @@
     nmea_gen_GPGGA(buf,sizeof(buf),&gga);
     parseFrame(buf,sizeof(buf));
+
+    vtg.dir=90-Euler::ToDegree(atan2f(state.vn,state.ve));
+    vtg.spk=sqrtf(state.ve*state.ve+state.vn*state.vn)*3600./1000.;
+    nmea_gen_GPVTG(buf,sizeof(buf),&vtg);
+    parseFrame(buf,sizeof(buf));
   }