[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4638] add deadbands to rate mode
From: |
Felix Ruess |
Subject: |
[paparazzi-commits] [4638] add deadbands to rate mode |
Date: |
Fri, 05 Mar 2010 23:22:11 +0000 |
Revision: 4638
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4638
Author: flixr
Date: 2010-03-05 23:22:11 +0000 (Fri, 05 Mar 2010)
Log Message:
-----------
add deadbands to rate mode
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/booz2_flixr.xml
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c
Modified: paparazzi3/trunk/conf/airframes/booz2_flixr.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_flixr.xml 2010-03-05 20:56:24 UTC
(rev 4637)
+++ paparazzi3/trunk/conf/airframes/booz2_flixr.xml 2010-03-05 23:22:11 UTC
(rev 4638)
@@ -78,24 +78,28 @@
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
-
+ <!-- setpoints -->
<define name="SP_MAX_P" value="10000" />
<define name="SP_MAX_Q" value="10000" />
<define name="SP_MAX_R" value="10000" />
+ <define name="DEADBAND_P" value="0" />
+ <define name="DEADBAND_Q" value="0" />
+ <define name="DEADBAND_R" value="200" />
+ <define name="REF_TAU" value="4" />
+ <!-- feedback -->
<define name="GAIN_P" value="-400" />
<define name="GAIN_Q" value="-400" />
<define name="GAIN_R" value="-350" />
- <!-- feedforward -->
<define name="IGAIN_P" value="-75" />
<define name="IGAIN_Q" value="-75" />
<define name="IGAIN_R" value="-50" />
+ <!-- feedforward -->
<define name="DDGAIN_P" value="300" />
<define name="DDGAIN_Q" value="300" />
<define name="DDGAIN_R" value="300" />
- <define name="TAU" value="4" />
</section>
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c
2010-03-05 20:56:24 UTC (rev 4637)
+++ paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c
2010-03-05 23:22:11 UTC (rev 4638)
@@ -54,8 +54,8 @@
#ifndef BOOZ_STABILIZATION_RATE_IGAIN_R
#define BOOZ_STABILIZATION_RATE_IGAIN_R 0
#endif
-#ifndef BOOZ_STABILIZATION_RATE_TAU
-#define BOOZ_STABILIZATION_RATE_TAU 4
+#ifndef BOOZ_STABILIZATION_RATE_REF_TAU
+#define BOOZ_STABILIZATION_RATE_REF_TAU 4
#endif
#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
@@ -72,6 +72,30 @@
struct Int32Rates booz_stabilization_rate_fb_cmd;
struct Int32Rates booz_stabilization_rate_ff_cmd;
+
+#ifndef BOOZ_STABILIZATION_RATE_DEADBAND_P
+#define BOOZ_STABILIZATION_RATE_DEADBAND_P 0
+#endif
+#ifndef BOOZ_STABILIZATION_RATE_DEADBAND_Q
+#define BOOZ_STABILIZATION_RATE_DEADBAND_Q 0
+#endif
+#ifndef BOOZ_STABILIZATION_RATE_DEADBAND_R
+#define BOOZ_STABILIZATION_RATE_DEADBAND_R 200
+#endif
+
+#define ROLL_RATE_DEADBAND_EXCEEDED() \
+ (radio_control.values[RADIO_CONTROL_ROLL] >
BOOZ_STABILIZATION_RATE_DEADBAND_P || \
+ radio_control.values[RADIO_CONTROL_ROLL] <
-BOOZ_STABILIZATION_RATE_DEADBAND_P)
+
+#define PITCH_RATE_DEADBAND_EXCEEDED()
\
+ (radio_control.values[RADIO_CONTROL_PITCH] >
BOOZ_STABILIZATION_RATE_DEADBAND_Q || \
+ radio_control.values[RADIO_CONTROL_PITCH] <
-BOOZ_STABILIZATION_RATE_DEADBAND_Q)
+
+#define YAW_RATE_DEADBAND_EXCEEDED() \
+ (radio_control.values[RADIO_CONTROL_YAW] >
BOOZ_STABILIZATION_RATE_DEADBAND_R || \
+ radio_control.values[RADIO_CONTROL_YAW] <
-BOOZ_STABILIZATION_RATE_DEADBAND_R)
+
+
void booz_stabilization_rate_init(void) {
INT_RATES_ZERO(booz_stabilization_rate_sp);
@@ -97,11 +121,20 @@
void booz_stabilization_rate_read_rc( void ) {
- RATES_ASSIGN(booz_stabilization_rate_sp,
- (int32_t)-radio_control.values[RADIO_CONTROL_ROLL] *
BOOZ_STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ,
- (int32_t) radio_control.values[RADIO_CONTROL_PITCH] *
BOOZ_STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ,
- (int32_t)-radio_control.values[RADIO_CONTROL_YAW] *
BOOZ_STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ);
+ if(ROLL_RATE_DEADBAND_EXCEEDED())
+ booz_stabilization_rate_sp.p =
(int32_t)-radio_control.values[RADIO_CONTROL_ROLL] *
BOOZ_STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ;
+ else
+ booz_stabilization_rate_sp.p = 0;
+ if(PITCH_RATE_DEADBAND_EXCEEDED())
+ booz_stabilization_rate_sp.q =
(int32_t)radio_control.values[RADIO_CONTROL_PITCH] *
BOOZ_STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ;
+ else
+ booz_stabilization_rate_sp.q = 0;
+
+ if(YAW_RATE_DEADBAND_EXCEEDED())
+ booz_stabilization_rate_sp.r =
(int32_t)-radio_control.values[RADIO_CONTROL_YAW] *
BOOZ_STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ;
+ else
+ booz_stabilization_rate_sp.r = 0;
}
void booz_stabilization_rate_enter(void) {
@@ -114,7 +147,7 @@
/* 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);
+ RATES_SDIV(booz_stabilization_rate_refdot, _r,
BOOZ_STABILIZATION_RATE_REF_TAU);
/* integrate ref */
const struct Int32Rates _delta_ref = {
booz_stabilization_rate_refdot.p >> ( F_UPDATE_RES + REF_DOT_FRAC -
REF_FRAC),
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4638] add deadbands to rate mode,
Felix Ruess <=