[paparazzi-commits] [5942] stabilization_rate: compute feedback error between reference and actual rate instead of stick setpoint and actual rate

[Felix Ruess]

Revision: 5942
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5942
Author:   flixr
Date:     2010-09-24 14:10:22 +0000 (Fri, 24 Sep 2010)
Log Message:
-----------
stabilization_rate: compute feedback error between reference and actual rate 
instead of stick setpoint and actual rate

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c

Modified: 
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c   
2010-09-24 08:44:42 UTC (rev 5941)
+++ paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c   
2010-09-24 14:10:22 UTC (rev 5942)
@@ -158,9 +158,15 @@
   /* 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 */
+  /* error for feedback */
+  const struct Int32Rates _ref_scaled = {
+    OFFSET_AND_ROUND(booz_stabilization_rate_ref.p, (REF_FRAC - 
INT32_RATE_FRAC)),
+    OFFSET_AND_ROUND(booz_stabilization_rate_ref.q, (REF_FRAC - 
INT32_RATE_FRAC)),
+    OFFSET_AND_ROUND(booz_stabilization_rate_ref.r, (REF_FRAC - 
INT32_RATE_FRAC)) };
   struct Int32Rates _error;
-  RATES_DIFF(_error, booz_ahrs.body_rate, booz_stabilization_rate_sp);
+  RATES_DIFF(_error, booz_ahrs.body_rate, _ref_scaled);
   if (in_flight) {
     /* update integrator */
     RATES_ADD(booz_stabilization_rate_sum_err, _error);