[paparazzi-commits] [4272] parse fms messages in nav mode

[Gautier Hattenberger]

Revision: 4272
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4272
Author:   gautier
Date:     2009-10-19 09:39:06 +0000 (Mon, 19 Oct 2009)
Log Message:
-----------
parse fms messages in nav mode

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_datalink.h

Modified: paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_datalink.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_datalink.h   2009-10-19 
09:33:55 UTC (rev 4271)
+++ paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_datalink.h   2009-10-19 
09:39:06 UTC (rev 4272)
@@ -33,42 +33,50 @@
 #endif
 
 #define BOOZ_FMS_PARSE_DATALINK(_dl_buffer) {                          \
-    booz_fms_last_msg = 0;                                             \
-    booz_fms_input.h_mode = DL_BOOZ2_FMS_COMMAND_h_mode(_dl_buffer);   \
-    booz_fms_input.v_mode = DL_BOOZ2_FMS_COMMAND_v_mode(_dl_buffer);   \
-    switch (booz_fms_input.h_mode) {                                   \
+    fms.last_msg = 0;                                          \
+    fms.input.h_mode = DL_BOOZ2_FMS_COMMAND_h_mode(_dl_buffer);        \
+    fms.input.v_mode = DL_BOOZ2_FMS_COMMAND_v_mode(_dl_buffer);        \
+    switch (fms.input.h_mode) {                                        \
     case BOOZ2_GUIDANCE_H_MODE_KILL:                                   \
     case BOOZ2_GUIDANCE_H_MODE_RATE :                                  \
       break;                                                           \
     case BOOZ2_GUIDANCE_H_MODE_ATTITUDE :                              \
       {                                                                        
\
-         booz_fms_input.h_sp.attitude.phi   = 
DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer);                 \
-         booz_fms_input.h_sp.attitude.theta = 
DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer);                 \
-         booz_fms_input.h_sp.attitude.psi   = 
DL_BOOZ2_FMS_COMMAND_h_sp_3(_dl_buffer);                 \
-        ANGLE_REF_NORMALIZE(booz_fms_input.h_sp.attitude.psi); \
-        BOOZ_FMS_LIMIT_ATTITUDE(booz_fms_input.h_sp.attitude); \
+             fms.input.h_sp.attitude.phi   = 
DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer);                  \
+             fms.input.h_sp.attitude.theta = 
DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer);                  \
+             fms.input.h_sp.attitude.psi   = 
DL_BOOZ2_FMS_COMMAND_h_sp_3(_dl_buffer);                  \
+        ANGLE_REF_NORMALIZE(fms.input.h_sp.attitude.psi); \
+        BOOZ_FMS_LIMIT_ATTITUDE(fms.input.h_sp.attitude); \
       }                                                                        
\
       break;  \
     case BOOZ2_GUIDANCE_H_MODE_HOVER :                                 \
       {                                                                        
\
-         booz_fms_input.h_sp.pos.x   = 
DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer);                        \
-         booz_fms_input.h_sp.pos.y   = 
DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer);                        \
+             fms.input.h_sp.pos.x   = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); 
                \
+             fms.input.h_sp.pos.y   = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); 
                \
       }                                                                        
\
       break;  \
     case BOOZ2_GUIDANCE_H_MODE_NAV :                                   \
+      { \
+        fms.input.h_sp.speed.x = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \
+        fms.input.h_sp.speed.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \
+        fms.input.h_sp.speed.z = DL_BOOZ2_FMS_COMMAND_h_sp_3(_dl_buffer); \
+      } \
       break;                                                           \
     }                                                                  \
-    switch (booz_fms_input.v_mode) {                                   \
+    switch (fms.input.v_mode) {                                        \
     case BOOZ2_GUIDANCE_V_MODE_KILL:                                   \
     case BOOZ2_GUIDANCE_V_MODE_RC_DIRECT:                              \
     case BOOZ2_GUIDANCE_V_MODE_RC_CLIMB:                               \
       break;                                                           \
     case BOOZ2_GUIDANCE_V_MODE_CLIMB :                                 \
-      booz_fms_input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
+      fms.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
       break;                                                           \
     case BOOZ2_GUIDANCE_V_MODE_HOVER :                                 \
-      booz_fms_input.v_sp.height = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
+      fms.input.v_sp.height = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
       break;                                                           \
+    case BOOZ2_GUIDANCE_V_MODE_NAV :                                   \
+      fms.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
+      break;                                                           \
     }                                                                  \
   }