Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 23:49:00 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

add deadbands to rate mode

Browse Source
This commit is contained in:
Felix Ruess
2010-03-05 23:22:11 +00:00
parent a1147fe35b
commit 78b9cd3f12
2 changed files with 47 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz2_flixr.xml
+7 -3
View File
@@ -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>
sw/airborne/booz/stabilization/booz_stabilization_rate.c
+40 -7
View File
@@ -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_sum_err;
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_init(void) {
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 @@ void booz_stabilization_rate_run(bool_t in_flight) {
/* 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),