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

add integral part to rate stabilization

Browse Source
This commit is contained in:
Felix Ruess
2010-03-05 00:27:32 +00:00
parent 4f52b60fd4
commit d6b0c1d67c
5 changed files with 61 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+3
View File
@@ -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"/>
sw/airborne/booz/booz2_telemetry.h
+7 -4
View File
@@ -172,12 +172,15 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&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, \
sw/airborne/booz/guidance/booz2_guidance_h.c
+1 -1
View File
@@ -171,7 +171,7 @@ void booz2_guidance_h_run(bool_t in_flight) {
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:
sw/airborne/booz/stabilization/booz_stabilization_rate.c
+47 -5
View File
@@ -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_TAU
#define BOOZ_STABILIZATION_RATE_TAU 1
#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 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 @@ void booz_stabilization_rate_init(void) {
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 @@ void booz_stabilization_rate_init(void) {
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_read_rc( void ) {
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,7 +128,28 @@ void booz_stabilization_rate_run(void) {
/* 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;
sw/airborne/booz/stabilization/booz_stabilization_rate.h
+3 -1
View File
@@ -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;