[paparazzi-commits] [5012] precommand consider bank angle

[Gautier Hattenberger]

Revision: 5012
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5012
Author:   gautier
Date:     2010-07-02 12:06:06 +0000 (Fri, 02 Jul 2010)
Log Message:
-----------
precommand consider bank angle
correct nav_circle_radians

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

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c        2010-07-01 
14:03:49 UTC (rev 5011)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c        2010-07-02 
12:06:06 UTC (rev 5012)
@@ -155,9 +155,15 @@
     // compute qdr
     INT32_ATAN2(nav_circle_qdr, pos_diff.y, pos_diff.x);
     // increment circle radians
-    int32_t angle_diff = nav_circle_qdr - last_qdr;
-    INT32_ANGLE_NORMALIZE(angle_diff);
-    nav_circle_radians += angle_diff;
+    if (nav_circle_radians != 0) {
+      int32_t angle_diff = nav_circle_qdr - last_qdr;
+      INT32_ANGLE_NORMALIZE(angle_diff);
+      nav_circle_radians += angle_diff;
+    }
+    else {
+      // Smallest angle to increment at next step
+      nav_circle_radians = 1;
+    }
 
     // direction of rotation
     int8_t sign_radius = radius > 0 ? 1 : -1;

Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c       
2010-07-01 14:03:49 UTC (rev 5011)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c       
2010-07-02 12:06:06 UTC (rev 5012)
@@ -28,6 +28,7 @@
 
 #include "booz_radio_control.h"
 #include "booz_stabilization.h"
+#include "booz_ahrs.h"
 #include "booz_fms.h"
 #include "booz2_navigation.h"
 
@@ -225,6 +226,8 @@
 
 #define FF_CMD_FRAC 18
 
+#define BOOZ2_MAX_BANK_COEF (BFP_OF_REAL(RadOfDeg(30.),INT32_TRIG_FRAC))
+
 static inline void run_hover_loop(bool_t in_flight) {
 
   /* convert our reference to generic representation */
@@ -260,8 +263,13 @@
     booz2_guidance_v_ff_cmd = ( g_m_zdd - (inv_m>>1)) / inv_m;
 #else
   booz2_guidance_v_ff_cmd = g_m_zdd / inv_m;
+  int32_t cphi,ctheta,cphitheta;
+  PPRZ_ITRIG_COS(cphi, booz_ahrs.ltp_to_body_euler.phi);
+  PPRZ_ITRIG_COS(ctheta, booz_ahrs.ltp_to_body_euler.theta);
+  cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC;
+  if (cphitheta < BOOZ2_MAX_BANK_COEF) cphitheta = BOOZ2_MAX_BANK_COEF;
+  booz2_guidance_v_ff_cmd = (booz2_guidance_v_ff_cmd << INT32_TRIG_FRAC) / 
cphitheta;
 #endif
-  //  booz2_guidance_v_ff_cmd = BOOZ2_GUIDANCE_V_HOVER_POWER;
 
   /* our error command                   */
   booz2_guidance_v_fb_cmd = ((-booz2_guidance_v_kp * err_z)  >> 12) +