[paparazzi-commits] [4549] parse dl msg for jsbsim simulator

[Gautier Hattenberger]

Revision: 4549
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4549
Author:   gautier
Date:     2010-02-11 10:24:59 +0000 (Thu, 11 Feb 2010)
Log Message:
-----------
parse dl msg for jsbsim simulator

Added Paths:
-----------
    paparazzi3/trunk/sw/airborne/sim/jsbsim_transport.c

Added: paparazzi3/trunk/sw/airborne/sim/jsbsim_transport.c
===================================================================
--- paparazzi3/trunk/sw/airborne/sim/jsbsim_transport.c                         
(rev 0)
+++ paparazzi3/trunk/sw/airborne/sim/jsbsim_transport.c 2010-02-11 10:24:59 UTC 
(rev 4549)
@@ -0,0 +1,61 @@
+/* Datalink parsing functions */
+
+#include "jsbsim_hw.h"
+
+#include <stdlib.h>
+
+#define MOfCm(_x) (((float)(_x))/100.)
+
+void parse_dl_ping(char* argv[] __attribute__ ((unused))) {
+  DOWNLINK_SEND_PONG(DefaultChannel);
+}
+
+void parse_dl_acinfo(char* argv[] __attribute__ ((unused))) {
+#ifdef TRAFFIC_INFO
+  uint8_t id = atoi(argv[8]);
+  float ux = MOfCm(atoi(argv[2]));
+  float uy = MOfCm(atoi(argv[3]));
+  float a = MOfCm(atoi(argv[4]));
+  float c = RadOfDeg(((float)atoi(argv[1]))/ 10.);
+  float s = MOfCm(atoi(argv[6]));
+  float cl = MOfCm(atoi(argv[7]));
+  uint32_t t = atoi(argv[5]);
+  SetAcInfo(id, ux, uy, c, a, s, cl, t);
+#endif
+}
+
+void parse_dl_setting(char* argv[]) {
+  uint8_t index = atoi(argv[2]);
+  float value = atof(argv[3]);
+  DlSetting(index, value);
+  DOWNLINK_SEND_DL_VALUE(DefaultChannel,&index, &value);
+}
+
+void parse_dl_get_setting(char* argv[]) {
+  uint8_t index = atoi(argv[2]);
+  float value = settings_get_value(index);
+  DOWNLINK_SEND_DL_VALUE(DefaultChannel,&index, &value);
+}
+
+void parse_dl_block(char* argv[]) {
+  int block = atoi(argv[1]);
+  nav_goto_block(block);
+}
+
+void parse_dl_move_wp(char* argv[]) {
+  uint8_t wp_id = atoi(argv[1]);
+  float a = MOfCm(atoi(argv[5]));
+
+  /* Computes from (lat, long) in the referenced UTM zone */
+  float lat = RadOfDeg((float)(atoi(argv[3]) / 1e7));
+  float lon = RadOfDeg((float)(atoi(argv[4]) / 1e7));
+  latlong_utm_of(lat, lon, nav_utm_zone0);
+  nav_move_waypoint(wp_id, latlong_utm_x, latlong_utm_y, a);
+
+  /* Waypoint range is limited. Computes the UTM pos back from the relative
+     coordinates */
+  latlong_utm_x = waypoints[wp_id].x + nav_utm_east0;
+  latlong_utm_y = waypoints[wp_id].y + nav_utm_north0;
+  DOWNLINK_SEND_WP_MOVED(DefaultChannel,&wp_id, &latlong_utm_x, 
&latlong_utm_y, &a, &nav_utm_zone0);
+}
+