[paparazzi-commits] [4572] trajectory generation for horizontal guidance

[Gautier Hattenberger]

Revision: 4572
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4572
Author:   gautier
Date:     2010-02-24 09:49:58 +0000 (Wed, 24 Feb 2010)
Log Message:
-----------
trajectory generation for horizontal guidance
used in the navigation loop

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

Added Paths:
-----------
    paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h_ref.h

Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c       
2010-02-24 09:45:46 UTC (rev 4571)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c       
2010-02-24 09:49:58 UTC (rev 4572)
@@ -21,6 +21,8 @@
  * Boston, MA 02111-1307, USA.
  */
 
+#define B2_GUIDANCE_H_C
+//#define B2_GUIDANCE_H_USE_REF
 #include "booz2_guidance_h.h"
 
 #include "booz_ahrs.h"
@@ -34,7 +36,10 @@
 uint8_t booz2_guidance_h_mode;
 
 struct Int32Vect2 booz2_guidance_h_pos_sp;
-int32_t            booz2_guidance_h_psi_sp;
+int32_t           booz2_guidance_h_psi_sp;
+struct Int32Vect2 booz2_guidance_h_pos_ref;
+struct Int32Vect2 booz2_guidance_h_speed_ref;
+struct Int32Vect2 booz2_guidance_h_accel_ref;
 
 struct Int32Vect2 booz2_guidance_h_pos_err;
 struct Int32Vect2 booz2_guidance_h_speed_err;
@@ -65,7 +70,14 @@
 static inline void booz2_guidance_h_hover_enter(void);
 static inline void booz2_guidance_h_nav_enter(void);
 
+#define Booz2GuidanceHSetRef(_pos, _speed, _accel) { \
+    b2_gh_set_ref(_pos, _speed, _accel); \
+    VECT2_COPY(booz2_guidance_h_pos_ref,   _pos); \
+    VECT2_COPY(booz2_guidance_h_speed_ref, _speed); \
+    VECT2_COPY(booz2_guidance_h_accel_ref, _accel); \
+  }
 
+
 void booz2_guidance_h_init(void) {
 
   booz2_guidance_h_mode = BOOZ2_GUIDANCE_H_MODE_KILL;
@@ -179,6 +191,9 @@
       }
       else {
         INT32_VECT2_NED_OF_ENU(booz2_guidance_h_pos_sp, 
booz2_navigation_carrot);
+#ifdef B2_GUIDANCE_H_USE_REF
+        b2_gh_update_ref_from_pos_sp(booz2_guidance_h_pos_sp);
+#endif
 #ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
         booz2_guidance_h_psi_sp = (nav_heading << (REF_ANGLE_FRAC - 
INT32_ANGLE_FRAC));
 #endif
@@ -263,13 +278,25 @@
 
 static inline void  booz2_guidance_h_nav_run(bool_t in_flight) {
 
+  /* convert our reference to generic representation */
+#ifdef B2_GUIDANCE_H_USE_REF
+  INT32_VECT2_RSHIFT(booz2_guidance_h_pos_ref,   b2_gh_pos_ref,   
(B2_GH_POS_REF_FRAC - INT32_POS_FRAC));
+  INT32_VECT2_LSHIFT(booz2_guidance_h_speed_ref, b2_gh_speed_ref, 
(INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC));
+  INT32_VECT2_LSHIFT(booz2_guidance_h_accel_ref, b2_gh_accel_ref, 
(INT32_ACCEL_FRAC - B2_GH_ACCEL_REF_FRAC));
+#else
+  VECT2_COPY(booz2_guidance_h_pos_ref, booz2_guidance_h_pos_sp);
+  INT_VECT2_ZERO(booz2_guidance_v_speed_ref);
+  INT_VECT2_ZERO(booz2_guidance_v_accel_ref);
+#endif
+
   /* compute position error    */
-  VECT2_DIFF(booz2_guidance_h_pos_err, booz_ins_ltp_pos, 
booz2_guidance_h_pos_sp);
+  VECT2_DIFF(booz2_guidance_h_pos_err, booz_ins_ltp_pos, 
booz2_guidance_h_pos_ref);
   /* saturate it               */
   VECT2_STRIM(booz2_guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
 
   /* compute speed error    */
-  VECT2_COPY(booz2_guidance_h_speed_err, booz_ins_ltp_speed);
+  //VECT2_COPY(booz2_guidance_h_speed_err, booz_ins_ltp_speed);
+  VECT2_DIFF(booz2_guidance_h_speed_err, booz_ins_ltp_speed, 
booz2_guidance_h_speed_ref);
   /* saturate it               */
   VECT2_STRIM(booz2_guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
 
@@ -305,13 +332,13 @@
     booz2_guidance_h_dgain * (booz2_guidance_h_speed_err.x >> 
(INT32_SPEED_FRAC - 10)) +
     booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.x >> (12 + 
INT32_POS_FRAC - 10)) +
     booz2_guidance_h_ngain * booz2_guidance_h_nav_err.x +
-    booz2_guidance_h_again * booz_ins_ltp_accel.x;
+    booz2_guidance_h_again * booz2_guidance_h_accel_ref.x; /* feedforward gain 
*/
   booz2_guidance_h_command_earth.y =
     booz2_guidance_h_pgain * (booz2_guidance_h_pos_err.y << (10 - 
INT32_POS_FRAC)) +
     booz2_guidance_h_dgain * (booz2_guidance_h_speed_err.y >> 
(INT32_SPEED_FRAC - 10)) +
     booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.y >> (12 + 
INT32_POS_FRAC - 10)) +
     booz2_guidance_h_ngain * booz2_guidance_h_nav_err.y +
-    booz2_guidance_h_again * booz_ins_ltp_accel.y;
+    booz2_guidance_h_again * booz2_guidance_h_accel_ref.y; /* feedforward gain 
*/
 
   VECT2_STRIM(booz2_guidance_h_command_earth, -NAV_MAX_BANK, NAV_MAX_BANK);
   INT32_VECT2_RSHIFT(booz2_guidance_h_command_earth, 
booz2_guidance_h_command_earth, REF_ANGLE_FRAC - 16); // Reduice to 16 for 
trigo operation
@@ -327,11 +354,13 @@
   booz2_guidance_h_command_body.theta =
     - ( c_psi * booz2_guidance_h_command_earth.x + s_psi * 
booz2_guidance_h_command_earth.y) >> (INT32_TRIG_FRAC - (REF_ANGLE_FRAC - 16));
 
+  // Add RC setpoint
   booz2_guidance_h_command_body.phi   += booz2_guidance_h_rc_sp.phi;
   booz2_guidance_h_command_body.theta += booz2_guidance_h_rc_sp.theta;
   booz2_guidance_h_command_body.psi    = booz2_guidance_h_psi_sp + 
booz2_guidance_h_rc_sp.psi;
   ANGLE_REF_NORMALIZE(booz2_guidance_h_command_body.psi);
 
+  // Set attitude setpoint
   EULERS_COPY(booz_stab_att_sp_euler, booz2_guidance_h_command_body);
 
 }
@@ -352,6 +381,10 @@
 static inline void booz2_guidance_h_nav_enter(void) {
 
   INT32_VECT2_NED_OF_ENU(booz2_guidance_h_pos_sp, booz2_navigation_carrot);
+  struct Int32Vect2 pos,speed,zero;
+  VECT2_COPY(pos, booz_ins_ltp_pos);
+  VECT2_COPY(speed, booz_ins_ltp_speed);
+  Booz2GuidanceHSetRef(pos, speed, zero);
 
   struct Int32Eulers tmp_sp;
   BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( tmp_sp );

Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.h       
2010-02-24 09:45:46 UTC (rev 4571)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.h       
2010-02-24 09:49:58 UTC (rev 4572)
@@ -27,6 +27,8 @@
 
 #include "math/pprz_algebra_int.h"
 
+#include "booz2_guidance_h_ref.h"
+
 #define BOOZ2_GUIDANCE_H_MODE_KILL      0
 #define BOOZ2_GUIDANCE_H_MODE_RATE      1
 #define BOOZ2_GUIDANCE_H_MODE_ATTITUDE  2
@@ -40,6 +42,9 @@
 /* Q_int32_xx_8        */
 extern struct Int32Vect2 booz2_guidance_h_pos_sp;
 extern int32_t           booz2_guidance_h_psi_sp;
+extern struct Int32Vect2 booz2_guidance_h_pos_ref;
+extern struct Int32Vect2 booz2_guidance_h_speed_ref;
+extern struct Int32Vect2 booz2_guidance_h_accel_ref;
 
 extern struct Int32Vect2 booz2_guidance_h_pos_err;
 extern struct Int32Vect2 booz2_guidance_h_speed_err;

Added: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h_ref.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h_ref.h           
                (rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h_ref.h   
2010-02-24 09:49:58 UTC (rev 4572)
@@ -0,0 +1,192 @@
+/*
+ * $Id: booz2_guidance_v_ref.h 4173 2009-09-18 11:57:21Z flixr $
+ *
+ * Copyright (C) 2008-2009 ENAC <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#ifndef BOOZ2_GUIDANCE_H_REF_H
+#define BOOZ2_GUIDANCE_H_REF_H
+
+#include "airframe.h"
+#include "inttypes.h"
+#include "math/pprz_algebra.h"
+#include "math/pprz_algebra_int.h"
+
+/* update frequency                               */
+#define B2_GH_FREQ_FRAC 9
+#define B2_GH_FREQ (1<<B2_GH_FREQ_FRAC)
+
+/* reference model accel in meters/sec2 (output) */
+/* Q23.8 : accuracy 0.0039 , range 8388km/s2      */
+/* int32_4_8_t */
+extern struct Int32Vect2 b2_gh_accel_ref;
+#define B2_GH_ACCEL_REF_FRAC 8
+
+/* reference model speed in meters/sec (output)  */
+/* Q14.17 : accuracy 0.0000076 , range 16384m/s2  */
+extern struct Int32Vect2 b2_gh_speed_ref;
+#define B2_GH_SPEED_REF_FRAC (B2_GH_ACCEL_REF_FRAC + B2_GH_FREQ_FRAC)
+
+/* reference model position in meters (output)    */
+/* Q37.26 :                                       */
+extern struct Int64Vect2 b2_gh_pos_ref;
+#define B2_GH_POS_REF_FRAC (B2_GH_SPEED_REF_FRAC + B2_GH_FREQ_FRAC)
+
+/* Saturations definition */
+#ifndef BOOZ2_GUIDANCE_H_REF_MAX_ACCEL
+#define BOOZ2_GUIDANCE_H_REF_MAX_ACCEL ( tanf(RadOfDeg(30.))*9.81 )
+#endif
+#define B2_GH_MAX_ACCEL BFP_OF_REAL(BOOZ2_GUIDANCE_H_REF_MAX_ACCEL, 
B2_GH_ACCEL_REF_FRAC)
+
+#ifndef BOOZ2_GUIDANCE_H_REF_MAX_SPEED
+#define BOOZ2_GUIDANCE_H_REF_MAX_SPEED ( 5. )
+#endif
+#define B2_GH_MAX_SPEED BFP_OF_REAL(BOOZ2_GUIDANCE_H_REF_MAX_SPEED, 
B2_GH_SPEED_REF_FRAC)
+
+/* second order model natural frequency and damping */
+#ifndef BOOZ2_GUIDANCE_H_REF_OMEGA
+#define BOOZ2_GUIDANCE_H_REF_OMEGA RadOfDeg(67.)
+#endif
+#ifndef BOOZ2_GUIDANCE_H_REF_ZETA
+#define BOOZ2_GUIDANCE_H_REF_ZETA  0.85
+#endif
+#define B2_GH_ZETA_OMEGA_FRAC 10
+#define B2_GH_ZETA_OMEGA 
BFP_OF_REAL((BOOZ2_GUIDANCE_H_REF_ZETA*BOOZ2_GUIDANCE_H_REF_OMEGA), 
B2_GH_ZETA_OMEGA_FRAC)
+#define B2_GH_OMEGA_2_FRAC 7
+#define B2_GH_OMEGA_2    
BFP_OF_REAL((BOOZ2_GUIDANCE_H_REF_OMEGA*BOOZ2_GUIDANCE_H_REF_OMEGA), 
B2_GH_OMEGA_2_FRAC)
+
+/* first order time constant */
+#define B2_GH_REF_THAU_F  0.5
+#define B2_GH_REF_INV_THAU_FRAC 16
+#define B2_GH_REF_INV_THAU  BFP_OF_REAL((1./B2_GH_REF_THAU_F), 
B2_GH_REF_INV_THAU_FRAC)
+
+#ifdef B2_GUIDANCE_H_C
+static inline void b2_gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 
speed, struct Int32Vect2 accel);
+static inline void b2_gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp);
+static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp);
+
+struct Int64Vect2 b2_gh_pos_ref;
+struct Int32Vect2 b2_gh_speed_ref;
+struct Int32Vect2 b2_gh_accel_ref;
+
+static inline void b2_gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 
speed, struct Int32Vect2 accel) {
+  struct Int64Vect2 new_pos;
+  new_pos.x = ((int64_t)pos.x)<<(B2_GH_POS_REF_FRAC - INT32_POS_FRAC);
+  new_pos.y = ((int64_t)pos.y)<<(B2_GH_POS_REF_FRAC - INT32_POS_FRAC);
+  b2_gh_pos_ref = new_pos;
+  INT32_VECT2_RSHIFT(b2_gh_speed_ref, speed, (INT32_SPEED_FRAC - 
B2_GH_SPEED_REF_FRAC));
+  INT32_VECT2_RSHIFT(b2_gh_accel_ref, accel, (INT32_ACCEL_FRAC - 
B2_GH_ACCEL_REF_FRAC));
+}
+
+static inline void b2_gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) {
+
+  VECT2_ADD(b2_gh_pos_ref, b2_gh_speed_ref);
+  VECT2_ADD(b2_gh_speed_ref, b2_gh_accel_ref);
+
+  // compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - 
pos_sp)
+  struct Int32Vect2 speed;
+  INT32_VECT2_RSHIFT(speed, b2_gh_speed_ref, (B2_GH_SPEED_REF_FRAC - 
B2_GH_ACCEL_REF_FRAC));
+  VECT2_SMUL(speed, speed, -2*B2_GH_ZETA_OMEGA);
+  INT32_VECT2_RSHIFT(speed, speed, B2_GH_ZETA_OMEGA_FRAC);
+  // compute pos error in pos_sp resolution
+  struct Int32Vect2 pos_err;
+  INT32_VECT2_RSHIFT(pos_err, b2_gh_pos_ref, (B2_GH_POS_REF_FRAC - 
INT32_POS_FRAC));
+  VECT2_DIFF(pos_err, pos_err, pos_sp);
+  // convert to accel resolution
+  INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC - 
B2_GH_ACCEL_REF_FRAC));
+  // compute the "pos part" of accel
+  struct Int32Vect2 pos;
+  VECT2_SMUL(pos, pos_err, (-B2_GH_OMEGA_2));
+  INT32_VECT2_RSHIFT(pos, pos, B2_GH_OMEGA_2_FRAC);
+  // sum accel
+  VECT2_SUM(b2_gh_accel_ref, speed, pos);
+
+  /* Saturate accelerations */
+  VECT2_STRIM(b2_gh_accel_ref, -B2_GH_MAX_ACCEL, B2_GH_MAX_ACCEL);
+
+  /* Saturate speed and adjust acceleration accordingly */
+  if (b2_gh_speed_ref.x <= -B2_GH_MAX_SPEED) {
+    b2_gh_speed_ref.x = -B2_GH_MAX_SPEED;
+    if (b2_gh_accel_ref.x < 0)
+      b2_gh_accel_ref.x = 0;
+  }
+  else if (b2_gh_speed_ref.x >= B2_GH_MAX_SPEED) {
+    b2_gh_speed_ref.x = B2_GH_MAX_SPEED;
+    if (b2_gh_accel_ref.x > 0)
+      b2_gh_accel_ref.x = 0;
+  }
+  if (b2_gh_speed_ref.y <= -B2_GH_MAX_SPEED) {
+    b2_gh_speed_ref.y = -B2_GH_MAX_SPEED;
+    if (b2_gh_accel_ref.y < 0)
+      b2_gh_accel_ref.y = 0;
+  }
+  else if (b2_gh_speed_ref.y >= B2_GH_MAX_SPEED) {
+    b2_gh_speed_ref.y = B2_GH_MAX_SPEED;
+    if (b2_gh_accel_ref.y > 0)
+      b2_gh_accel_ref.y = 0;
+  }
+}
+
+
+static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
+
+  VECT2_ADD(b2_gh_pos_ref, b2_gh_speed_ref);
+  VECT2_ADD(b2_gh_speed_ref, b2_gh_accel_ref);
+
+  // compute speed error
+  struct Int32Vect2 speed_err;
+  INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - 
B2_GH_SPEED_REF_FRAC));
+  VECT2_DIFF(speed_err, b2_gh_speed_ref, speed_err);
+  // convert to accel resolution
+  INT32_VECT2_RSHIFT(speed_err, speed_err, (B2_GH_SPEED_REF_FRAC - 
B2_GH_ACCEL_REF_FRAC));
+  // compute accel from speed_sp
+  VECT2_SMUL(b2_gh_accel_ref, speed_err, -B2_GH_REF_INV_THAU);
+  INT32_VECT2_RSHIFT(b2_gh_accel_ref, b2_gh_accel_ref, 
B2_GH_REF_INV_THAU_FRAC);
+
+  /* Saturate accelerations */
+  VECT2_STRIM(b2_gh_accel_ref, -B2_GH_MAX_ACCEL, B2_GH_MAX_ACCEL);
+
+  /* Saturate speed and adjust acceleration accordingly */
+  if (b2_gh_speed_ref.x <= -B2_GH_MAX_SPEED) {
+    b2_gh_speed_ref.x = -B2_GH_MAX_SPEED;
+    if (b2_gh_accel_ref.x < 0)
+      b2_gh_accel_ref.x = 0;
+  }
+  else if (b2_gh_speed_ref.x >= B2_GH_MAX_SPEED) {
+    b2_gh_speed_ref.x = B2_GH_MAX_SPEED;
+    if (b2_gh_accel_ref.x > 0)
+      b2_gh_accel_ref.x = 0;
+  }
+  if (b2_gh_speed_ref.y <= -B2_GH_MAX_SPEED) {
+    b2_gh_speed_ref.y = -B2_GH_MAX_SPEED;
+    if (b2_gh_accel_ref.y < 0)
+      b2_gh_accel_ref.y = 0;
+  }
+  else if (b2_gh_speed_ref.y >= B2_GH_MAX_SPEED) {
+    b2_gh_speed_ref.y = B2_GH_MAX_SPEED;
+    if (b2_gh_accel_ref.y > 0)
+      b2_gh_accel_ref.y = 0;
+  }
+}
+
+#endif /* B2_GUIDANCE_H_C */
+
+#endif /* BOOZ2_GUIDANCE_H_REF_H */
+