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

made reference dynamic configurable from airframe file

Browse Source
This commit is contained in:
Antoine Drouin
2009-07-24 23:43:02 +00:00
parent 2331400ed7
commit 012031685b
3 changed files with 92 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz2_a1.xml
+32 -5
View File
@@ -91,21 +91,48 @@
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="3000"/>
<define name="SP_MAX_THETA" value="3000"/>
<define name="SP_MAX_R" value="5500"/>
<define name="DEADBAND_R" value="250"/>
<define name="PHI_THETA_PGAIN" value="-400"/>
<define name="PHI_THETA_DGAIN" value="-300"/>
<define name="PHI_THETA_DDGAIN" value=" 300"/>
<define name="PHI_THETA_IGAIN" value="-100"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="RadOfDeg(800)"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="BFP_OF_REAL(5,RATE_REF_RES)"/>
<define name="REF_MAX_PDOT" value="BFP_OF_REAL(128,ACCEL_REF_RES)"/>
<define name="REF_OMEGA_Q" value="RadOfDeg(800)"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="BFP_OF_REAL(5,RATE_REF_RES)"/>
<define name="REF_MAX_QDOT" value="BFP_OF_REAL(128,ACCEL_REF_RES)"/>
<define name="REF_OMEGA_R" value="RadOfDeg(500)"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="BFP_OF_REAL(3,RATE_REF_RES)"/>
<define name="REF_MAX_RDOT" value="BFP_OF_REAL(32,ACCEL_REF_RES)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_DDGAIN" value=" 300"/>
<define name="PSI_IGAIN" value="-75"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
<define name="THETA_DDGAIN" value=" 300"/>
<define name="PSI_DDGAIN" value=" 300"/>
</section>
<section name="INS" prefix="BOOZ_INS_">
sw/airborne/booz/stabilization/booz_stabilization_attitude_euler.c
+20 -17
View File
@@ -25,6 +25,8 @@
#include "booz_ahrs.h"
#include "booz_radio_control.h"
#include "airframe.h"
struct Int32Eulers booz_stabilization_att_sp;
@@ -54,24 +56,25 @@ void booz_stabilization_attitude_init(void) {
INT_VECT3_ZERO(booz_stabilization_accel_ref);
VECT3_ASSIGN(booz_stabilization_pgain,
BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_PGAIN,
BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_PGAIN,
BOOZ_STABILIZATION_ATTITUDE_PHI_PGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_PGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN);
VECT3_ASSIGN(booz_stabilization_dgain,
BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DGAIN,
BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DGAIN,
BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN);
VECT3_ASSIGN(booz_stabilization_igain,
BOOZ_STABILIZATION_ATTITUDE_PHI_IGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_IGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN);
VECT3_ASSIGN(booz_stabilization_ddgain,
BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DDGAIN,
BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DDGAIN,
BOOZ_STABILIZATION_ATTITUDE_PHI_DDGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN);
VECT3_ASSIGN(booz_stabilization_igain,
BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_IGAIN,
BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_IGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN);
INT_EULERS_ZERO( booz_stabilization_att_sum_err );
@@ -104,9 +107,9 @@ void booz_stabilization_attitude_run(bool_t in_flight) {
/* compute attitude error */
const struct Int32Eulers att_ref_scaled = {
OFFSET_AND_ROUND2(booz_stabilization_att_ref.phi, (ANGLE_REF_RES - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND2(booz_stabilization_att_ref.theta, (ANGLE_REF_RES - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND2(booz_stabilization_att_ref.psi, (ANGLE_REF_RES - INT32_ANGLE_FRAC)) };
OFFSET_AND_ROUND(booz_stabilization_att_ref.phi, (ANGLE_REF_RES - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND(booz_stabilization_att_ref.theta, (ANGLE_REF_RES - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND(booz_stabilization_att_ref.psi, (ANGLE_REF_RES - INT32_ANGLE_FRAC)) };
struct Int32Eulers att_err;
EULERS_DIFF(att_err, booz_ahrs.ltp_to_body_euler, att_ref_scaled);
INT32_ANGLE_NORMALIZE(att_err.psi);
@@ -122,9 +125,9 @@ void booz_stabilization_attitude_run(bool_t in_flight) {
/* compute rate error */
const struct Int32Rates rate_ref_scaled = {
OFFSET_AND_ROUND2(booz_stabilization_rate_ref.x, (RATE_REF_RES - INT32_RATE_FRAC)),
OFFSET_AND_ROUND2(booz_stabilization_rate_ref.y, (RATE_REF_RES - INT32_RATE_FRAC)),
OFFSET_AND_ROUND2(booz_stabilization_rate_ref.z, (RATE_REF_RES - INT32_RATE_FRAC)) };
OFFSET_AND_ROUND(booz_stabilization_rate_ref.x, (RATE_REF_RES - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(booz_stabilization_rate_ref.y, (RATE_REF_RES - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(booz_stabilization_rate_ref.z, (RATE_REF_RES - INT32_RATE_FRAC)) };
struct Int32Rates rate_err;
RATES_DIFF(rate_err, booz_ahrs.body_rate, rate_ref_scaled);
@@ -133,7 +136,7 @@ void booz_stabilization_attitude_run(bool_t in_flight) {
booz_stabilization_att_fb_cmd[COMMAND_ROLL] =
booz_stabilization_pgain.x * att_err.phi +
booz_stabilization_dgain.x * rate_err.p +
OFFSET_AND_ROUND((booz_stabilization_igain.x * booz_stabilization_att_sum_err.phi), 10);
OFFSET_AND_ROUND2((booz_stabilization_igain.x * booz_stabilization_att_sum_err.phi), 10);
booz_stabilization_att_ff_cmd[COMMAND_ROLL] =
OFFSET_AND_ROUND(booz_stabilization_ddgain.x * booz_stabilization_accel_ref.x, 5);
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_traj_euler.h
+40 -29
View File
@@ -35,12 +35,14 @@ extern struct Int32Vect3 booz_stabilization_accel_ref;
#define F_UPDATE (1<<F_UPDATE_RES)
#define ACCEL_REF_RES 12
#define ACCEL_REF_MAX_PQ BFP_OF_REAL(128,ACCEL_REF_RES)
#define ACCEL_REF_MAX_R BFP_OF_REAL( 32,ACCEL_REF_RES)
#define ACCEL_REF_MAX_P BOOZ_STABILIZATION_ATTITUDE_REF_MAX_PDOT
#define ACCEL_REF_MAX_Q BOOZ_STABILIZATION_ATTITUDE_REF_MAX_QDOT
#define ACCEL_REF_MAX_R BOOZ_STABILIZATION_ATTITUDE_REF_MAX_RDOT
#define RATE_REF_RES 16
#define RATE_REF_MAX_PQ BFP_OF_REAL(5,RATE_REF_RES)
#define RATE_REF_MAX_R BFP_OF_REAL(3,RATE_REF_RES)
#define RATE_REF_MAX_P BOOZ_STABILIZATION_ATTITUDE_REF_MAX_P
#define RATE_REF_MAX_Q BOOZ_STABILIZATION_ATTITUDE_REF_MAX_Q
#define RATE_REF_MAX_R BOOZ_STABILIZATION_ATTITUDE_REF_MAX_R
#define ANGLE_REF_RES 20
#define PI_ANGLE_REF BFP_OF_REAL(3.1415926535897932384626433832795029, ANGLE_REF_RES)
@@ -51,14 +53,23 @@ extern struct Int32Vect3 booz_stabilization_accel_ref;
}
#define OMEGA_PQ RadOfDeg(800)
#define ZETA_PQ 0.85
#define ZETA_OMEGA_PQ_RES 10
#define ZETA_OMEGA_PQ BFP_OF_REAL((ZETA_PQ*OMEGA_PQ), ZETA_OMEGA_PQ_RES)
#define OMEGA_2_PQ_RES 7
#define OMEGA_2_PQ BFP_OF_REAL((OMEGA_PQ*OMEGA_PQ), OMEGA_2_PQ_RES)
#define OMEGA_R RadOfDeg(500)
#define ZETA_R 0.85
#define OMEGA_P BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_P
#define ZETA_P BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_P
#define ZETA_OMEGA_P_RES 10
#define ZETA_OMEGA_P BFP_OF_REAL((ZETA_P*OMEGA_P), ZETA_OMEGA_P_RES)
#define OMEGA_2_P_RES 7
#define OMEGA_2_P BFP_OF_REAL((OMEGA_P*OMEGA_P), OMEGA_2_P_RES)
#define OMEGA_Q BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_Q
#define ZETA_Q BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_Q
#define ZETA_OMEGA_Q_RES 10
#define ZETA_OMEGA_Q BFP_OF_REAL((ZETA_Q*OMEGA_Q), ZETA_OMEGA_Q_RES)
#define OMEGA_2_Q_RES 7
#define OMEGA_2_Q BFP_OF_REAL((OMEGA_Q*OMEGA_Q), OMEGA_2_Q_RES)
#define OMEGA_R BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_R
#define ZETA_R BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_R
#define ZETA_OMEGA_R_RES 10
#define ZETA_OMEGA_R BFP_OF_REAL((ZETA_R*OMEGA_R), ZETA_OMEGA_R_RES)
#define OMEGA_2_R_RES 7
@@ -92,43 +103,43 @@ extern struct Int32Vect3 booz_stabilization_accel_ref;
\
/* compute reference angular accelerations */ \
const struct Int32Vect3 accel_rate = { \
((int32_t)(-2.*ZETA_OMEGA_PQ)* (booz_stabilization_rate_ref.x >> (RATE_REF_RES - ACCEL_REF_RES))) \
>> (ZETA_OMEGA_PQ_RES), \
((int32_t)(-2.*ZETA_OMEGA_PQ)* (booz_stabilization_rate_ref.y >> (RATE_REF_RES - ACCEL_REF_RES))) \
>> (ZETA_OMEGA_PQ_RES), \
((int32_t)(-2.*ZETA_OMEGA_P) * (booz_stabilization_rate_ref.x >> (RATE_REF_RES - ACCEL_REF_RES))) \
>> (ZETA_OMEGA_P_RES), \
((int32_t)(-2.*ZETA_OMEGA_Q) * (booz_stabilization_rate_ref.y >> (RATE_REF_RES - ACCEL_REF_RES))) \
>> (ZETA_OMEGA_Q_RES), \
((int32_t)(-2.*ZETA_OMEGA_R) * (booz_stabilization_rate_ref.z >> (RATE_REF_RES - ACCEL_REF_RES))) \
>> (ZETA_OMEGA_R_RES) }; \
\
const struct Int32Vect3 accel_angle = { \
((int32_t)(-OMEGA_2_PQ)* (ref_err.phi >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_PQ_RES), \
((int32_t)(-OMEGA_2_PQ)* (ref_err.theta >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_PQ_RES), \
((int32_t)(-OMEGA_2_R )* (ref_err.psi >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_R_RES ) }; \
((int32_t)(-OMEGA_2_P)* (ref_err.phi >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_P_RES), \
((int32_t)(-OMEGA_2_Q)* (ref_err.theta >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_Q_RES), \
((int32_t)(-OMEGA_2_R)* (ref_err.psi >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_R_RES) }; \
\
VECT3_SUM(booz_stabilization_accel_ref, accel_rate, accel_angle); \
\
/* saturate acceleration */ \
const struct Int32Vect3 MIN_ACCEL = { -ACCEL_REF_MAX_PQ, -ACCEL_REF_MAX_PQ, -ACCEL_REF_MAX_R }; \
const struct Int32Vect3 MAX_ACCEL = { ACCEL_REF_MAX_PQ, ACCEL_REF_MAX_PQ, ACCEL_REF_MAX_R }; \
const struct Int32Vect3 MIN_ACCEL = { -ACCEL_REF_MAX_P, -ACCEL_REF_MAX_Q, -ACCEL_REF_MAX_R }; \
const struct Int32Vect3 MAX_ACCEL = { ACCEL_REF_MAX_P, ACCEL_REF_MAX_Q, ACCEL_REF_MAX_R }; \
VECT3_BOUND_BOX(booz_stabilization_accel_ref, MIN_ACCEL, MAX_ACCEL); \
\
/* saturate speed and trim accel accordingly */ \
if (booz_stabilization_rate_ref.x >= RATE_REF_MAX_PQ) { \
booz_stabilization_rate_ref.x = RATE_REF_MAX_PQ; \
if (booz_stabilization_rate_ref.x >= RATE_REF_MAX_P) { \
booz_stabilization_rate_ref.x = RATE_REF_MAX_P; \
if (booz_stabilization_accel_ref.x > 0) \
booz_stabilization_accel_ref.x = 0; \
} \
else if (booz_stabilization_rate_ref.x <= -RATE_REF_MAX_PQ) { \
booz_stabilization_rate_ref.x = -RATE_REF_MAX_PQ; \
else if (booz_stabilization_rate_ref.x <= -RATE_REF_MAX_P) { \
booz_stabilization_rate_ref.x = -RATE_REF_MAX_P; \
if (booz_stabilization_accel_ref.x < 0) \
booz_stabilization_accel_ref.x = 0; \
} \
if (booz_stabilization_rate_ref.y >= RATE_REF_MAX_PQ) { \
booz_stabilization_rate_ref.y = RATE_REF_MAX_PQ; \
if (booz_stabilization_rate_ref.y >= RATE_REF_MAX_Q) { \
booz_stabilization_rate_ref.y = RATE_REF_MAX_Q; \
if (booz_stabilization_accel_ref.y > 0) \
booz_stabilization_accel_ref.y = 0; \
} \
else if (booz_stabilization_rate_ref.y <= -RATE_REF_MAX_PQ) { \
booz_stabilization_rate_ref.y = -RATE_REF_MAX_PQ; \
else if (booz_stabilization_rate_ref.y <= -RATE_REF_MAX_Q) { \
booz_stabilization_rate_ref.y = -RATE_REF_MAX_Q; \
if (booz_stabilization_accel_ref.y < 0) \
booz_stabilization_accel_ref.y = 0; \
} \