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

zero attitude ref if not used

Browse Source
This commit is contained in:
Felix Ruess
2010-03-05 00:27:11 +00:00
parent 8a63ef575f
commit fdc52ead1d
2 changed files with 36 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.c
+8 -11
View File
@@ -43,15 +43,15 @@ void booz_stabilization_attitude_ref_init(void) {
void booz_stabilization_attitude_ref_update() {
#ifdef USE_REF
/* dumb integrate reference attitude */
/* dumb integrate reference attitude */
struct FloatRates delta_rate;
RATES_SMUL(delta_rate, booz_stab_att_ref_rate, DT_UPDATE);
struct FloatEulers delta_angle;
EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r);
EULERS_ADD(booz_stab_att_ref_euler, delta_angle );
FLOAT_ANGLE_NORMALIZE(booz_stab_att_ref_euler.psi);
/* integrate reference rotational speeds */
struct FloatRates delta_accel;
RATES_SMUL(delta_accel, booz_stab_att_ref_accel, DT_UPDATE);
@@ -68,21 +68,18 @@ void booz_stabilization_attitude_ref_update() {
booz_stab_att_ref_accel.q = -2.*ZETA_Q*OMEGA_P*booz_stab_att_ref_rate.q - OMEGA_Q*OMEGA_Q*ref_err.theta;
booz_stab_att_ref_accel.r = -2.*ZETA_R*OMEGA_P*booz_stab_att_ref_rate.r - OMEGA_R*OMEGA_R*ref_err.psi;
/* saturate acceleration */
/* saturate acceleration */
const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; \
RATES_BOUND_BOX(booz_stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
/* saturate speed and trim accel accordingly */
SATURATE_SPEED_TRIM_ACCEL();
#else /* !USE_REF */
EULERS_COPY(booz_stabilization_att_ref, booz_stabilization_att_sp);
FLOAT_RATES_ZERO(booz_stabilization_rate_ref);
FLOAT_RATES_ZERO(booz_stabilization_accel_ref);
EULERS_COPY(booz_stab_att_ref_euler, booz_stabilization_att_sp);
FLOAT_RATES_ZERO(booz_stab_att_ref_rate);
FLOAT_RATES_ZERO(booz_stab_att_ref_accel);
#endif /* USE_REF */
}
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.c
+28 -29
View File
@@ -1,6 +1,6 @@
/*
* $Id: booz_stabilization_attitude_ref_euler_int.h -1 $
*
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
* Boston, MA 02111-1307, USA.
*/
#include "booz_stabilization.h"
@@ -75,56 +75,55 @@ void booz_stabilization_attitude_ref_update() {
#ifdef USE_REF
/* dumb integrate reference attitude */
const struct Int32Eulers d_angle = {
/* dumb integrate reference attitude */
const struct Int32Eulers d_angle = {
booz_stab_att_ref_rate.p >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC),
booz_stab_att_ref_rate.q >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC),
booz_stab_att_ref_rate.r >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC)};
EULERS_ADD(booz_stab_att_ref_euler, d_angle );
ANGLE_REF_NORMALIZE(booz_stab_att_ref_euler.psi);
/* integrate reference rotational speeds */
const struct Int32Rates d_rate = {
/* integrate reference rotational speeds */
const struct Int32Rates d_rate = {
booz_stab_att_ref_accel.p >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC),
booz_stab_att_ref_accel.q >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC),
booz_stab_att_ref_accel.r >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC)};
RATES_ADD(booz_stab_att_ref_rate, d_rate);
/* compute reference attitude error */
struct Int32Eulers ref_err;
/* compute reference attitude error */
struct Int32Eulers ref_err;
EULERS_DIFF(ref_err, booz_stab_att_ref_euler, booz_stab_att_sp_euler);
/* wrap it in the shortest direction */
ANGLE_REF_NORMALIZE(ref_err.psi);
/* compute reference angular accelerations */
const struct Int32Rates accel_rate = {
/* wrap it in the shortest direction */
ANGLE_REF_NORMALIZE(ref_err.psi);
/* compute reference angular accelerations */
const struct Int32Rates accel_rate = {
((int32_t)(-2.*ZETA_OMEGA_P) * (booz_stab_att_ref_rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
>> (ZETA_OMEGA_P_RES),
>> (ZETA_OMEGA_P_RES),
((int32_t)(-2.*ZETA_OMEGA_Q) * (booz_stab_att_ref_rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
>> (ZETA_OMEGA_Q_RES),
>> (ZETA_OMEGA_Q_RES),
((int32_t)(-2.*ZETA_OMEGA_R) * (booz_stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
>> (ZETA_OMEGA_R_RES) };
const struct Int32Rates accel_angle = {
>> (ZETA_OMEGA_R_RES) };
const struct Int32Rates accel_angle = {
((int32_t)(-OMEGA_2_P)* (ref_err.phi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES),
((int32_t)(-OMEGA_2_Q)* (ref_err.theta >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES),
((int32_t)(-OMEGA_2_R)* (ref_err.psi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) };
RATES_SUM(booz_stab_att_ref_accel, accel_rate, accel_angle);
/* saturate acceleration */
/* saturate acceleration */
const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
RATES_BOUND_BOX(booz_stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
/* saturate speed and trim accel accordingly */
SATURATE_SPEED_TRIM_ACCEL();
#else /* !USE_REF */
EULERS_COPY(booz_stabilization_att_ref, booz_stabilization_att_sp);
INT_RATES_ZERO(booz_stabilization_rate_ref);
INT_RATES_ZERO(booz_stabilization_accel_ref);
EULERS_COPY(booz_stab_att_ref_euler, booz_stabilization_att_sp);
INT_RATES_ZERO(booz_stab_att_ref_rate);
INT_RATES_ZERO(booz_stab_att_ref_accel);
#endif /* USE_REF */
}
}