[paparazzi-commits] [4629] add feedforward to rate stabilization

[Felix Ruess]

Revision: 4629
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4629
Author:   flixr
Date:     2010-03-05 00:27:19 +0000 (Fri, 05 Mar 2010)
Log Message:
-----------
add feedforward to rate stabilization

Modified Paths:
--------------
    paparazzi3/trunk/conf/airframes/booz2_flixr.xml
    paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
    paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c
    paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.h

Modified: paparazzi3/trunk/conf/airframes/booz2_flixr.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_flixr.xml     2010-03-05 00:27:11 UTC 
(rev 4628)
+++ paparazzi3/trunk/conf/airframes/booz2_flixr.xml     2010-03-05 00:27:19 UTC 
(rev 4629)
@@ -87,6 +87,11 @@
                <define name="GAIN_Q" value="-400" />
                <define name="GAIN_R" value="-350" />
 
+        <!-- feedforward -->
+               <define name="DDGAIN_P" value="100" />
+               <define name="DDGAIN_Q" value="100" />
+               <define name="DDGAIN_R" value="100" />
+        <define name="TAU" value="10" />
        </section>
 
 

Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c       
2010-03-05 00:27:11 UTC (rev 4628)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c       
2010-03-05 00:27:19 UTC (rev 4629)
@@ -107,6 +107,10 @@
 
   switch (new_mode) {
 
+  case BOOZ2_GUIDANCE_H_MODE_RATE:
+    booz_stabilization_rate_enter();
+    break;
+
   case BOOZ2_GUIDANCE_H_MODE_ATTITUDE:
     booz_stabilization_attitude_enter();
     break;

Modified: 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c   
2010-03-05 00:27:11 UTC (rev 4628)
+++ paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c   
2010-03-05 00:27:19 UTC (rev 4629)
@@ -2,6 +2,7 @@
  * $Id$
  *
  * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ * Copyright (C) 2010 Felix Ruess <address@hidden>
  *
  * This file is part of paparazzi.
  *
@@ -29,18 +30,48 @@
 #include "booz_radio_control.h"
 #include "airframe.h"
 
+#define F_UPDATE_RES 9
+#define REF_DOT_FRAC 11
+#define REF_FRAC  16
+
+#ifndef BOOZ_STABILIZATION_RATE_DDGAIN_P
+#define BOOZ_STABILIZATION_RATE_DDGAIN_P 0
+#endif
+#ifndef BOOZ_STABILIZATION_RATE_DDGAIN_Q
+#define BOOZ_STABILIZATION_RATE_DDGAIN_Q 0
+#endif
+#ifndef BOOZ_STABILIZATION_RATE_DDGAIN_R
+#define BOOZ_STABILIZATION_RATE_DDGAIN_R 0
+#endif
+#ifndef BOOZ_STABILIZATION_RATE_TAU
+#define BOOZ_STABILIZATION_RATE_TAU 1
+#endif
+
 struct Int32Rates booz_stabilization_rate_sp;
 struct Int32Rates booz_stabilization_rate_gain;
+struct Int32Rates booz_stabilization_rate_ddgain;
+struct Int32Rates booz_stabilization_rate_ref;
+struct Int32Rates booz_stabilization_rate_refdot;
 
+struct Int32Rates booz_stabilization_rate_fb_cmd;
+struct Int32Rates booz_stabilization_rate_ff_cmd;
+
 void booz_stabilization_rate_init(void) {
 
   INT_RATES_ZERO(booz_stabilization_rate_sp);
 
   RATES_ASSIGN(booz_stabilization_rate_gain,
-              BOOZ_STABILIZATION_RATE_GAIN_P,
-              BOOZ_STABILIZATION_RATE_GAIN_Q,
-              BOOZ_STABILIZATION_RATE_GAIN_R);
+               BOOZ_STABILIZATION_RATE_GAIN_P,
+               BOOZ_STABILIZATION_RATE_GAIN_Q,
+               BOOZ_STABILIZATION_RATE_GAIN_R);
 
+  RATES_ASSIGN(booz_stabilization_rate_ddgain,
+               BOOZ_STABILIZATION_RATE_DDGAIN_P,
+               BOOZ_STABILIZATION_RATE_DDGAIN_Q,
+               BOOZ_STABILIZATION_RATE_DDGAIN_R);
+
+  INT_RATES_ZERO(booz_stabilization_rate_ref);
+  INT_RATES_ZERO(booz_stabilization_rate_refdot);
 }
 
 
@@ -53,16 +84,34 @@
 
 }
 
+void booz_stabilization_rate_enter(void) {
+  RATES_COPY(booz_stabilization_rate_ref, booz_stabilization_rate_sp);
+}
 
 void booz_stabilization_rate_run(void) {
 
+  /* reference */
+  struct Int32Rates _r;
+  RATES_DIFF(_r, booz_stabilization_rate_sp, booz_stabilization_rate_ref);
+  RATES_SDIV(booz_stabilization_rate_refdot, _r, BOOZ_STABILIZATION_RATE_TAU);
+  /* integrate ref */
+  const struct Int32Rates _delta_ref = {
+    booz_stabilization_rate_refdot.p >> ( F_UPDATE_RES + REF_DOT_FRAC - 
REF_FRAC),
+    booz_stabilization_rate_refdot.q >> ( F_UPDATE_RES + REF_DOT_FRAC - 
REF_FRAC),
+    booz_stabilization_rate_refdot.r >> ( F_UPDATE_RES + REF_DOT_FRAC - 
REF_FRAC)};
+  RATES_ADD(booz_stabilization_rate_ref, _delta_ref);
+
+  /* compute feed-forward command */
+  RATES_EWMULT_RSHIFT(booz_stabilization_rate_ff_cmd, 
booz_stabilization_rate_ddgain, booz_stabilization_rate_refdot, 16);
+
+  /* compute feed-back command */
   struct Int32Rates _error;
   RATES_DIFF(_error, booz_ahrs.body_rate, booz_stabilization_rate_sp);
-  struct Int32Rates _cmd;
-  RATES_EWMULT_RSHIFT(_cmd, _error, booz_stabilization_rate_gain, 16);
+  RATES_EWMULT_RSHIFT(booz_stabilization_rate_fb_cmd, 
booz_stabilization_rate_gain, _error, 16);
 
-  booz_stabilization_cmd[COMMAND_ROLL]  = _cmd.p;
-  booz_stabilization_cmd[COMMAND_PITCH] = _cmd.q;
-  booz_stabilization_cmd[COMMAND_YAW]   = _cmd.r;
+  /* sum to final command */
+  booz_stabilization_cmd[COMMAND_ROLL]  = booz_stabilization_rate_ff_cmd.p + 
booz_stabilization_rate_fb_cmd.p;
+  booz_stabilization_cmd[COMMAND_PITCH] = booz_stabilization_rate_ff_cmd.q + 
booz_stabilization_rate_fb_cmd.q;
+  booz_stabilization_cmd[COMMAND_YAW]   = booz_stabilization_rate_ff_cmd.r + 
booz_stabilization_rate_fb_cmd.r;
 
 }

Modified: 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.h   
2010-03-05 00:27:11 UTC (rev 4628)
+++ paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.h   
2010-03-05 00:27:19 UTC (rev 4629)
@@ -29,8 +29,15 @@
 extern void booz_stabilization_rate_init(void);
 extern void booz_stabilization_rate_read_rc(void);
 extern void booz_stabilization_rate_run(void);
+extern void booz_stabilization_rate_enter(void);
 
 extern struct Int32Rates booz_stabilization_rate_sp;
 extern struct Int32Rates booz_stabilization_rate_gain;
+extern struct Int32Rates booz_stabilization_rate_ddgain;
+extern struct Int32Rates booz_stabilization_rate_ref;
+extern struct Int32Rates booz_stabilization_rate_refdot;
 
+extern struct Int32Rates booz_stabilization_rate_fb_cmd;
+extern struct Int32Rates booz_stabilization_rate_ff_cmd;
+
 #endif /* BOOZ_STABILIZATION_RATE */