[nps][gps] calc utm, ned_vel, gspeed, course

paparazzi · Aug 8, 2013 · 1827ed1 · 1827ed1
1 parent d2e0296
commit 1827ed1
Show file tree

Hide file tree

Showing 7 changed files with 45 additions and 5 deletions.
diff --git a/conf/airframes/examples/MentorEnergy.xml b/conf/airframes/examples/MentorEnergy.xml
@@ -260,6 +260,7 @@
     <define name="ACTUATOR_NAMES"  value="{&quot;throttle-cmd-norm&quot;, &quot;aileron-cmd-norm&quot;, &quot;elevator-cmd-norm&quot;, &quot;rudder-cmd-norm&quot;}"/>
     <define name="INITIAL_CONDITITONS" value="&quot;reset00&quot;"/>
     <define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
+    <define name="JS_MODE_AXIS" value="4"/>
   </section>
 
 </airframe>
diff --git a/conf/firmwares/subsystems/fixedwing/gps_mediatek_diy.makefile b/conf/firmwares/subsystems/fixedwing/gps_mediatek_diy.makefile
@@ -24,7 +24,7 @@ sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
 jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
 jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
 
-nps.CFLAGS += -DUSE_GPS
+nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
 nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
 nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
 nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
diff --git a/conf/firmwares/subsystems/fixedwing/gps_nmea.makefile b/conf/firmwares/subsystems/fixedwing/gps_nmea.makefile
@@ -24,7 +24,7 @@ sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
 jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
 jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
 
-nps.CFLAGS += -DUSE_GPS
+nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
 nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
 nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
 nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
diff --git a/conf/firmwares/subsystems/fixedwing/gps_skytraq.makefile b/conf/firmwares/subsystems/fixedwing/gps_skytraq.makefile
@@ -21,7 +21,7 @@ sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
 jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
 jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.
 
-nps.CFLAGS += -DUSE_GPS
+nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
 nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
 nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
 nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
diff --git a/conf/firmwares/subsystems/fixedwing/gps_ublox.makefile b/conf/firmwares/subsystems/fixedwing/gps_ublox.makefile
@@ -23,7 +23,7 @@ sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
 jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
 jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
 
-nps.CFLAGS += -DUSE_GPS
+nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
 nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
 nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
 nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
diff --git a/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile b/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile
@@ -22,7 +22,7 @@ sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
 jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
 jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
 
-nps.CFLAGS += -DUSE_GPS
+nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
 nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
 nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
 nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.c b/sw/airborne/subsystems/gps/gps_sim_nps.c
@@ -22,6 +22,12 @@
 #include "subsystems/gps/gps_sim_nps.h"
 #include "subsystems/gps.h"
 
+#if GPS_USE_LATLONG
+/* currently needed to get nav_utm_zone0 */
+#include "subsystems/navigation/common_nav.h"
+#include "math/pprz_geodetic_float.h"
+#endif
+
 bool_t gps_available;
 
 void  gps_feed_value() {
@@ -36,6 +42,39 @@ void  gps_feed_value() {
   gps.lla_pos.lon = sensors.gps.lla_pos.lon * 1e7;
   gps.lla_pos.alt = sensors.gps.lla_pos.alt * 1000.;
   gps.hmsl        = sensors.gps.hmsl * 1000.;
+
+  /* calc NED speed from ECEF */
+  struct LtpDef_d ref_ltp;
+  ltp_def_from_ecef_d(&ref_ltp, &sensors.gps.ecef_pos);
+  struct NedCoor_d ned_vel_d;
+  ned_of_ecef_vect_d(&ned_vel_d, &ref_ltp, &sensors.gps.ecef_vel);
+  gps.ned_vel.x = ned_vel_d.x * 100;
+  gps.ned_vel.y = ned_vel_d.y * 100;
+  gps.ned_vel.z = ned_vel_d.z * 100;
+
+  /* horizontal and 3d ground speed in cm/s */
+  gps.gspeed = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y) * 100;
+  gps.speed_3d = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y + ned_vel_d.z * ned_vel_d.z) * 100;
+
+  /* ground course in radians * 1e7 */
+  gps.course = atan2(ned_vel_d.y, ned_vel_d.x) * 1e7;
+
+#if GPS_USE_LATLONG
+  /* Computes from (lat, long) in the referenced UTM zone */
+  struct LlaCoor_f lla_f;
+  lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
+  lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
+  struct UtmCoor_f utm_f;
+  utm_f.zone = nav_utm_zone0;
+  /* convert to utm */
+  utm_of_lla_f(&utm_f, &lla_f);
+  /* copy results of utm conversion */
+  gps.utm_pos.east = utm_f.east*100;
+  gps.utm_pos.north = utm_f.north*100;
+  gps.utm_pos.alt = gps.lla_pos.alt;
+  gps.utm_pos.zone = nav_utm_zone0;
+#endif
+
   gps.fix = GPS_FIX_3D;
   gps_available = TRUE;
 }