[paparazzi-commits] [5066] parse datalink messages in nps

[Felix Ruess]

Revision: 5066
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5066
Author:   flixr
Date:     2010-07-16 20:37:04 +0000 (Fri, 16 Jul 2010)
Log Message:
-----------
parse datalink messages in nps

Modified Paths:
--------------
    paparazzi3/trunk/sw/simulator/nps/nps_ivy.c

Modified: paparazzi3/trunk/sw/simulator/nps/nps_ivy.c
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_ivy.c 2010-07-16 18:44:13 UTC (rev 
5065)
+++ paparazzi3/trunk/sw/simulator/nps/nps_ivy.c 2010-07-16 20:37:04 UTC (rev 
5066)
@@ -11,26 +11,50 @@
 
 #include NPS_SENSORS_PARAMS
 
+
+/* Datalink Ivy functions */
 static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
-                         void *user_data __attribute__ ((unused)),
-                         int argc __attribute__ ((unused)), char *argv[]);
+                          void *user_data __attribute__ ((unused)),
+                          int argc __attribute__ ((unused)), char *argv[]);
 
+static void on_DL_GET_SETTING(IvyClientPtr app __attribute__ ((unused)),
+                              void *user_data __attribute__ ((unused)),
+                              int argc __attribute__ ((unused)), char *argv[]);
+
+static void on_DL_PING(IvyClientPtr app __attribute__ ((unused)),
+                       void *user_data __attribute__ ((unused)),
+                       int argc __attribute__ ((unused)), char *argv[]);
+
+static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
+                        void *user_data __attribute__ ((unused)),
+                        int argc __attribute__ ((unused)), char *argv[]);
+
+static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
+                          void *user_data __attribute__ ((unused)),
+                          int argc __attribute__ ((unused)), char *argv[]);
+
 void nps_ivy_init(void) {
   const char* agent_name = AIRFRAME_NAME"_NPS";
   const char* ready_msg = AIRFRAME_NAME"_NPS Ready";
   IvyInit(agent_name, ready_msg, NULL, NULL, NULL, NULL);
+  IvyBindMsg(on_DL_PING, NULL, "^(\\S*) DL_PING");
   IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)");
-  //IvyBindMsg(on_DL_BLOCK, NULL,   "^(\\S*) BLOCK (\\S*) (\\S*)");
-  //IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) 
(\\S*) (\\S*)");
+  IvyBindMsg(on_DL_GET_SETTING, NULL, "^(\\S*) DL_GET_SETTING (\\S*) (\\S*)");
+  IvyBindMsg(on_DL_BLOCK, NULL,   "^(\\S*) BLOCK (\\S*) (\\S*)");
+  IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) 
(\\S*)");
   IvyStart("127.255.255.255");
 }
 
+//TODO use datalink parsing from booz or fw instead of doing it here explicitly
+//FIXME currently parsed correctly for booz only
+
+
 #include "settings.h"
 #include "dl_protocol.h"
 #include "downlink.h"
 static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
-                         void *user_data __attribute__ ((unused)),
-                         int argc __attribute__ ((unused)), char *argv[]) {
+                          void *user_data __attribute__ ((unused)),
+                          int argc __attribute__ ((unused)), char *argv[]) {
   uint8_t index = atoi(argv[2]);
   float value = atof(argv[3]);
   DlSetting(index, value);
@@ -38,9 +62,50 @@
   printf("setting %d %f\n", index, value);
 }
 
-void nps_ivy_display(void) {
+static void on_DL_GET_SETTING(IvyClientPtr app __attribute__ ((unused)),
+                              void *user_data __attribute__ ((unused)),
+                              int argc __attribute__ ((unused)), char *argv[]) 
{
+  uint8_t index = atoi(argv[2]);
+  float value = settings_get_value(index);
+  DOWNLINK_SEND_DL_VALUE(DefaultChannel,&index, &value);
+  printf("get setting %d %f\n", index, value);
+}
 
+static void on_DL_PING(IvyClientPtr app __attribute__ ((unused)),
+                       void *user_data __attribute__ ((unused)),
+                       int argc __attribute__ ((unused)), char *argv[]) {
+  DOWNLINK_SEND_PONG(DefaultChannel);
+}
 
+static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
+                        void *user_data __attribute__ ((unused)),
+                        int argc __attribute__ ((unused)), char *argv[]){
+  int block = atoi(argv[1]);
+  nav_goto_block(block);
+  printf("goto block %d\n", block);
+}
+
+static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
+                          void *user_data __attribute__ ((unused)),
+                          int argc __attribute__ ((unused)), char *argv[]) {
+  uint8_t wp_id = atoi(argv[1]);
+
+  struct LlaCoor_i lla;
+  struct EnuCoor_i enu;
+  lla.lat = INT32_RAD_OF_DEG(atoi(argv[3]));
+  lla.lon = INT32_RAD_OF_DEG(atoi(argv[4]));
+  lla.alt = atoi(argv[5]) - booz_ins_ltp_def.hmsl + booz_ins_ltp_def.lla.alt;
+  enu_of_lla_point_i(&enu,&booz_ins_ltp_def,&lla);
+  enu.x = POS_BFP_OF_REAL(enu.x)/100;
+  enu.y = POS_BFP_OF_REAL(enu.y)/100;
+  enu.z = POS_BFP_OF_REAL(enu.z)/100;
+  VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
+  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, &wp_id, &enu.x, &enu.y, &enu.z);
+  printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, enu.x, enu.y, enu.z);
+}
+
+
+void nps_ivy_display(void) {
   /*
     IvySendMsg("%d COMMANDS %f %f %f %f",
     AC_ID,