[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4631] add integral part to rate stabilization
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [4631] add integral part to rate stabilization |
|
Date: |
Fri, 05 Mar 2010 00:27:32 +0000 |
Revision: 4631
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4631
Author: flixr
Date: 2010-03-05 00:27:32 +0000 (Fri, 05 Mar 2010)
Log Message:
-----------
add integral part to rate stabilization
Modified Paths:
--------------
paparazzi3/trunk/conf/messages.xml
paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
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/messages.xml
===================================================================
--- paparazzi3/trunk/conf/messages.xml 2010-03-05 00:27:25 UTC (rev 4630)
+++ paparazzi3/trunk/conf/messages.xml 2010-03-05 00:27:32 UTC (rev 4631)
@@ -812,6 +812,9 @@
<field name="refdot_p" type="int32"/>
<field name="refdot_q" type="int32"/>
<field name="refdot_r" type="int32"/>
+ <field name="sumerr_p" type="int32"/>
+ <field name="sumerr_q" type="int32"/>
+ <field name="sumerr_r" type="int32"/>
<field name="ff_p" type="int32"/>
<field name="ff_q" type="int32"/>
<field name="ff_r" type="int32"/>
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2010-03-05 00:27:25 UTC
(rev 4630)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2010-03-05 00:27:32 UTC
(rev 4631)
@@ -172,12 +172,15 @@
&booz_stabilization_rate_sp.p, \
&booz_stabilization_rate_sp.q, \
&booz_stabilization_rate_sp.r, \
- &booz_stabilization_rate_ref.p, \
+ &booz_stabilization_rate_ref.p, \
&booz_stabilization_rate_ref.q, \
&booz_stabilization_rate_ref.r, \
- &booz_stabilization_rate_refdot.p, \
- &booz_stabilization_rate_refdot.q, \
- &booz_stabilization_rate_refdot.r, \
+ &booz_stabilization_rate_refdot.p, \
+ &booz_stabilization_rate_refdot.q, \
+ &booz_stabilization_rate_refdot.r, \
+ &booz_stabilization_rate_sum_err.p, \
+ &booz_stabilization_rate_sum_err.q, \
+ &booz_stabilization_rate_sum_err.r, \
&booz_stabilization_rate_ff_cmd.p, \
&booz_stabilization_rate_ff_cmd.q, \
&booz_stabilization_rate_ff_cmd.r, \
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:25 UTC (rev 4630)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
2010-03-05 00:27:32 UTC (rev 4631)
@@ -171,7 +171,7 @@
switch ( booz2_guidance_h_mode ) {
case BOOZ2_GUIDANCE_H_MODE_RATE:
- booz_stabilization_rate_run();
+ booz_stabilization_rate_run(in_flight);
break;
case BOOZ2_GUIDANCE_H_MODE_ATTITUDE:
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:25 UTC (rev 4630)
+++ paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c
2010-03-05 00:27:32 UTC (rev 4631)
@@ -34,6 +34,8 @@
#define REF_DOT_FRAC 11
#define REF_FRAC 16
+#define MAX_SUM_ERR 4000000
+
#ifndef BOOZ_STABILIZATION_RATE_DDGAIN_P
#define BOOZ_STABILIZATION_RATE_DDGAIN_P 0
#endif
@@ -43,15 +45,29 @@
#ifndef BOOZ_STABILIZATION_RATE_DDGAIN_R
#define BOOZ_STABILIZATION_RATE_DDGAIN_R 0
#endif
+#ifndef BOOZ_STABILIZATION_RATE_IGAIN_P
+#define BOOZ_STABILIZATION_RATE_IGAIN_P -64
+#endif
+#ifndef BOOZ_STABILIZATION_RATE_IGAIN_Q
+#define BOOZ_STABILIZATION_RATE_IGAIN_Q -64
+#endif
+#ifndef BOOZ_STABILIZATION_RATE_IGAIN_R
+#define BOOZ_STABILIZATION_RATE_IGAIN_R -32
+#endif
#ifndef BOOZ_STABILIZATION_RATE_TAU
-#define BOOZ_STABILIZATION_RATE_TAU 1
+#define BOOZ_STABILIZATION_RATE_TAU 4
#endif
+#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
+#define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
+
struct Int32Rates booz_stabilization_rate_sp;
struct Int32Rates booz_stabilization_rate_gain;
+struct Int32Rates booz_stabilization_rate_igain;
struct Int32Rates booz_stabilization_rate_ddgain;
struct Int32Rates booz_stabilization_rate_ref;
struct Int32Rates booz_stabilization_rate_refdot;
+struct Int32Rates booz_stabilization_rate_sum_err;
struct Int32Rates booz_stabilization_rate_fb_cmd;
struct Int32Rates booz_stabilization_rate_ff_cmd;
@@ -64,7 +80,10 @@
BOOZ_STABILIZATION_RATE_GAIN_P,
BOOZ_STABILIZATION_RATE_GAIN_Q,
BOOZ_STABILIZATION_RATE_GAIN_R);
-
+ RATES_ASSIGN(booz_stabilization_rate_igain,
+ BOOZ_STABILIZATION_RATE_IGAIN_P,
+ BOOZ_STABILIZATION_RATE_IGAIN_Q,
+ BOOZ_STABILIZATION_RATE_IGAIN_R);
RATES_ASSIGN(booz_stabilization_rate_ddgain,
BOOZ_STABILIZATION_RATE_DDGAIN_P,
BOOZ_STABILIZATION_RATE_DDGAIN_Q,
@@ -72,6 +91,7 @@
INT_RATES_ZERO(booz_stabilization_rate_ref);
INT_RATES_ZERO(booz_stabilization_rate_refdot);
+ INT_RATES_ZERO(booz_stabilization_rate_sum_err);
}
@@ -86,9 +106,10 @@
void booz_stabilization_rate_enter(void) {
RATES_COPY(booz_stabilization_rate_ref, booz_stabilization_rate_sp);
+ INT_RATES_ZERO(booz_stabilization_rate_sum_err);
}
-void booz_stabilization_rate_run(void) {
+void booz_stabilization_rate_run(bool_t in_flight) {
/* reference */
struct Int32Rates _r;
@@ -107,8 +128,29 @@
/* compute feed-back command */
struct Int32Rates _error;
RATES_DIFF(_error, booz_ahrs.body_rate, booz_stabilization_rate_sp);
- RATES_EWMULT_RSHIFT(booz_stabilization_rate_fb_cmd,
booz_stabilization_rate_gain, _error, 16);
+ if (in_flight) {
+ /* update integrator */
+ RATES_ADD(booz_stabilization_rate_sum_err, _error);
+ RATES_BOUND_CUBE(booz_stabilization_rate_sum_err, -MAX_SUM_ERR,
MAX_SUM_ERR);
+ }
+ else {
+ INT_RATES_ZERO(booz_stabilization_rate_sum_err);
+ }
+ /* PI */
+ booz_stabilization_rate_fb_cmd.p = booz_stabilization_rate_gain.p * _error.p
+
+ OFFSET_AND_ROUND2((booz_stabilization_rate_igain.p *
booz_stabilization_rate_sum_err.p), 10);
+
+ booz_stabilization_rate_fb_cmd.q = booz_stabilization_rate_gain.q * _error.q
+
+ OFFSET_AND_ROUND2((booz_stabilization_rate_igain.q *
booz_stabilization_rate_sum_err.q), 10);
+
+ booz_stabilization_rate_fb_cmd.r = booz_stabilization_rate_gain.r * _error.r
+
+ OFFSET_AND_ROUND2((booz_stabilization_rate_igain.r *
booz_stabilization_rate_sum_err.r), 10);
+
+ booz_stabilization_rate_fb_cmd.p = booz_stabilization_rate_fb_cmd.p >> 16;
+ booz_stabilization_rate_fb_cmd.q = booz_stabilization_rate_fb_cmd.q >> 16;
+ booz_stabilization_rate_fb_cmd.r = booz_stabilization_rate_fb_cmd.r >> 16;
+
/* 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;
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:25 UTC (rev 4630)
+++ paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.h
2010-03-05 00:27:32 UTC (rev 4631)
@@ -28,14 +28,16 @@
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_run(bool_t in_flight);
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_igain;
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_sum_err;
extern struct Int32Rates booz_stabilization_rate_fb_cmd;
extern struct Int32Rates booz_stabilization_rate_ff_cmd;
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4631] add integral part to rate stabilization,
Felix Ruess <=