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

stabilization_rate: compute feedback error between reference and actual rate instead of stick setpoint and actual rate

Browse Source
This commit is contained in:
Felix Ruess
2010-09-24 14:10:22 +00:00
parent 6252c082e6
commit 6bef819be9
1 changed files with 7 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/stabilization/booz_stabilization_rate.c
+7 -1
View File
@@ -158,9 +158,15 @@ void booz_stabilization_rate_run(bool_t in_flight) {
/* 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 */
/* error for feedback */
const struct Int32Rates _ref_scaled = {
OFFSET_AND_ROUND(booz_stabilization_rate_ref.p, (REF_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(booz_stabilization_rate_ref.q, (REF_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(booz_stabilization_rate_ref.r, (REF_FRAC - INT32_RATE_FRAC)) };
struct Int32Rates _error;
RATES_DIFF(_error, booz_ahrs.body_rate, booz_stabilization_rate_sp);
RATES_DIFF(_error, booz_ahrs.body_rate, _ref_scaled);
if (in_flight) {
/* update integrator */
RATES_ADD(booz_stabilization_rate_sum_err, _error);