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 26 Packages Projects Releases Wiki Activity

add feedforward to rate stabilization

Browse Source
This commit is contained in:
Felix Ruess
2010-03-05 00:27:19 +00:00
parent fdc52ead1d
commit a083f4a7e7
4 changed files with 73 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz2_flixr.xml
+5
View File
@@ -87,6 +87,11 @@
<define name="GAIN_Q" value="-400" />
<define name="GAIN_R" value="-350" />
<!-- feedforward -->
<define name="DDGAIN_P" value="100" />
<define name="DDGAIN_Q" value="100" />
<define name="DDGAIN_R" value="100" />
<define name="TAU" value="10" />
</section>
sw/airborne/booz/guidance/booz2_guidance_h.c
+4
View File
@@ -107,6 +107,10 @@ void booz2_guidance_h_mode_changed(uint8_t new_mode) {
switch (new_mode) {
case BOOZ2_GUIDANCE_H_MODE_RATE:
booz_stabilization_rate_enter();
break;
case BOOZ2_GUIDANCE_H_MODE_ATTITUDE:
booz_stabilization_attitude_enter();
break;
sw/airborne/booz/stabilization/booz_stabilization_rate.c
+57 -8
View File
@@ -2,6 +2,7 @@
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
* Copyright (C) 2010 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
@@ -29,18 +30,48 @@
#include "booz_radio_control.h"
#include "airframe.h"
#define F_UPDATE_RES 9
#define REF_DOT_FRAC 11
#define REF_FRAC 16
#ifndef BOOZ_STABILIZATION_RATE_DDGAIN_P
#define BOOZ_STABILIZATION_RATE_DDGAIN_P 0
#endif
#ifndef BOOZ_STABILIZATION_RATE_DDGAIN_Q
#define BOOZ_STABILIZATION_RATE_DDGAIN_Q 0
#endif
#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
#endif
struct Int32Rates booz_stabilization_rate_sp;
struct Int32Rates booz_stabilization_rate_gain;
struct Int32Rates booz_stabilization_rate_ddgain;
struct Int32Rates booz_stabilization_rate_ref;
struct Int32Rates booz_stabilization_rate_refdot;
struct Int32Rates booz_stabilization_rate_fb_cmd;
struct Int32Rates booz_stabilization_rate_ff_cmd;
void booz_stabilization_rate_init(void) {
INT_RATES_ZERO(booz_stabilization_rate_sp);
RATES_ASSIGN(booz_stabilization_rate_gain,
BOOZ_STABILIZATION_RATE_GAIN_P,
BOOZ_STABILIZATION_RATE_GAIN_Q,
BOOZ_STABILIZATION_RATE_GAIN_R);
BOOZ_STABILIZATION_RATE_GAIN_P,
BOOZ_STABILIZATION_RATE_GAIN_Q,
BOOZ_STABILIZATION_RATE_GAIN_R);
RATES_ASSIGN(booz_stabilization_rate_ddgain,
BOOZ_STABILIZATION_RATE_DDGAIN_P,
BOOZ_STABILIZATION_RATE_DDGAIN_Q,
BOOZ_STABILIZATION_RATE_DDGAIN_R);
INT_RATES_ZERO(booz_stabilization_rate_ref);
INT_RATES_ZERO(booz_stabilization_rate_refdot);
}
@@ -53,16 +84,34 @@ void booz_stabilization_rate_read_rc( void ) {
}
void booz_stabilization_rate_enter(void) {
RATES_COPY(booz_stabilization_rate_ref, booz_stabilization_rate_sp);
}
void booz_stabilization_rate_run(void) {
/* 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);
/* integrate ref */
const struct Int32Rates _delta_ref = {
booz_stabilization_rate_refdot.p >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC),
booz_stabilization_rate_refdot.q >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC),
booz_stabilization_rate_refdot.r >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC)};
RATES_ADD(booz_stabilization_rate_ref, _delta_ref);
/* 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 */
struct Int32Rates _error;
RATES_DIFF(_error, booz_ahrs.body_rate, booz_stabilization_rate_sp);
struct Int32Rates _cmd;
RATES_EWMULT_RSHIFT(_cmd, _error, booz_stabilization_rate_gain, 16);
RATES_EWMULT_RSHIFT(booz_stabilization_rate_fb_cmd, booz_stabilization_rate_gain, _error, 16);
booz_stabilization_cmd[COMMAND_ROLL] = _cmd.p;
booz_stabilization_cmd[COMMAND_PITCH] = _cmd.q;
booz_stabilization_cmd[COMMAND_YAW] = _cmd.r;
/* 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;
booz_stabilization_cmd[COMMAND_YAW] = booz_stabilization_rate_ff_cmd.r + booz_stabilization_rate_fb_cmd.r;
}
sw/airborne/booz/stabilization/booz_stabilization_rate.h
+7
View File
@@ -29,8 +29,15 @@
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_enter(void);
extern struct Int32Rates booz_stabilization_rate_sp;
extern struct Int32Rates booz_stabilization_rate_gain;
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_fb_cmd;
extern struct Int32Rates booz_stabilization_rate_ff_cmd;
#endif /* BOOZ_STABILIZATION_RATE */