[paparazzi-commits] [4279] use fms joystick in nav

[Gautier Hattenberger]

Revision: 4279
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4279
Author:   gautier
Date:     2009-10-19 13:00:34 +0000 (Mon, 19 Oct 2009)
Log Message:
-----------
use fms joystick in nav

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
    paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c        2009-10-19 
12:58:38 UTC (rev 4278)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c        2009-10-19 
13:00:34 UTC (rev 4279)
@@ -31,6 +31,8 @@
 #include "booz2_autopilot.h"
 #include "flight_plan.h"
 
+#include "booz_fms.h"
+
 #include "math/pprz_algebra_int.h"
 
 const uint8_t nb_waypoint = NB_WAYPOINT;
@@ -294,6 +296,26 @@
   }
 }
 
+void nav_update_wp_from_fms(uint8_t _wp) {
+  if (fms.enabled && _wp < nb_waypoint) {
+    int32_t s_heading, c_heading;
+    PPRZ_ITRIG_SIN(s_heading, nav_heading);
+    PPRZ_ITRIG_COS(c_heading, nav_heading);
+    struct Int32Vect3 dpos;
+    dpos.x = (NavFmsMaxHSpeed * fms.input.h_sp.speed.x >> (INT32_SPEED_FRAC - 
INT32_POS_FRAC)) / (BOOZ2_AP_NAV_RATE * 128);
+    dpos.y = (NavFmsMaxHSpeed * fms.input.h_sp.speed.y >> (INT32_SPEED_FRAC - 
INT32_POS_FRAC)) / (BOOZ2_AP_NAV_RATE * 128);
+    dpos.z = (NavFmsMaxVSpeed * fms.input.v_sp.climb >> (INT32_SPEED_FRAC - 
INT32_POS_FRAC)) / (BOOZ2_AP_NAV_RATE * 128);
+    waypoints[_wp].x += (s_heading * dpos.x + c_heading * dpos.y) >> 
INT32_TRIG_FRAC;
+    waypoints[_wp].y += (c_heading * dpos.x - s_heading * dpos.y) >> 
INT32_TRIG_FRAC;
+    waypoints[_wp].z += dpos.z;
+    int32_t dheading = (NavFmsMaxHeadingRate * fms.input.h_sp.speed.z >> 
(INT32_RATE_FRAC - INT32_ANGLE_FRAC)) / (BOOZ2_AP_NAV_RATE * 128);
+    if (dheading != 0) {
+      nav_heading += dheading;
+      INT32_COURSE_NORMALIZE(nav_heading);
+    }
+  }
+}
+
 bool_t nav_detect_ground(void) {
   if (!booz2_autopilot_detect_ground) return FALSE;
   booz2_autopilot_detect_ground = FALSE;

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h        2009-10-19 
12:58:38 UTC (rev 4278)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h        2009-10-19 
13:00:34 UTC (rev 4279)
@@ -218,7 +218,14 @@
 #define GetPosY() POS_FLOAT_OF_BFP(booz_ins_enu_pos.y)
 #define GetPosAlt() (POS_FLOAT_OF_BFP(booz_ins_enu_pos.z+ground_alt))
 
+#define NavFmsMaxHSpeed ((int32_t)SPEED_BFP_OF_REAL(4.))
+#define NavFmsMaxVSpeed ((int32_t)SPEED_BFP_OF_REAL(2.))
+#define NavFmsMaxHeadingRate ((int32_t)RATE_BFP_OF_REAL(RadOfDeg(60.)))
 
+extern void nav_update_wp_from_fms(uint8_t _wp);
+#define NavUpdateWPFromFms(_wp) { \
+  nav_update_wp_from_fms(_wp); \
+}
 
 
 #endif /* BOOZ2_NAVIGATION_H */