mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-22 20:36:06 +08:00
Clean up booz_stabilization_attitude_quat_float.c by removing plenty of junk from it and pushing said bits down into ref or setpoint
generation, or elsewhere entirely. Also move all of booz_stabilization gains into a single struct (potentially useful for gain scheduling or other hackery)
This commit is contained in:
Allen Ibara
@@ -29,18 +29,18 @@
|
||||
|
||||
|
||||
<dl_settings NAME="Att Loop">
|
||||
<dl_setting var="booz_stabilization_pgain.x" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="pgain phi" />
|
||||
<dl_setting var="booz_stabilization_dgain.x" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="dgain p"/>
|
||||
<dl_setting var="booz_stabilization_igain.x" min="-300" step="1" max="0" module="stabilization/booz_stabilization_attitude" shortname="igain phi" handler="SetKiPhi"/>
|
||||
<dl_setting var="booz_stabilization_ddgain.x" min="0" step="1" max="1000" module="stabilization/booz_stabilization_attitude" shortname="ddgain p"/>
|
||||
<dl_setting var="booz_stabilization_pgain.y" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="pgain theta"/>
|
||||
<dl_setting var="booz_stabilization_dgain.y" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="dgain q"/>
|
||||
<dl_setting var="booz_stabilization_igain.y" min="-300" step="1" max="0" module="stabilization/booz_stabilization_attitude" shortname="igain theta"/>
|
||||
<dl_setting var="booz_stabilization_ddgain.y" min="0" step="1" max="500" module="stabilization/booz_stabilization_attitude" shortname="ddgain q"/>
|
||||
<dl_setting var="booz_stabilization_pgain.z" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="pgain psi"/>
|
||||
<dl_setting var="booz_stabilization_dgain.z" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="dgain r"/>
|
||||
<dl_setting var="booz_stabilization_igain.z" min="-300" step="1" max="0" module="stabilization/booz_stabilization_attitude" shortname="igain psi"/>
|
||||
<dl_setting var="booz_stabilization_ddgain.z" min="0" step="1" max="2000" module="stabilization/booz_stabilization_attitude" shortname="ddgain r"/>
|
||||
<dl_setting var="booz_stabilization_gains.p.x" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="pgain phi" />
|
||||
<dl_setting var="booz_stabilization_gains.d.x" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="dgain p"/>
|
||||
<dl_setting var="booz_stabilization_gains.i.x" min="-300" step="1" max="0" module="stabilization/booz_stabilization_attitude" shortname="igain phi" handler="SetKiPhi"/>
|
||||
<dl_setting var="booz_stabilization_gains.dd.x" min="0" step="1" max="1000" module="stabilization/booz_stabilization_attitude" shortname="ddgain p"/>
|
||||
<dl_setting var="booz_stabilization_gains.p.y" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="pgain theta"/>
|
||||
<dl_setting var="booz_stabilization_gains.d.y" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="dgain q"/>
|
||||
<dl_setting var="booz_stabilization_gains.i.y" min="-300" step="1" max="0" module="stabilization/booz_stabilization_attitude" shortname="igain theta"/>
|
||||
<dl_setting var="booz_stabilization_gains.dd.y" min="0" step="1" max="500" module="stabilization/booz_stabilization_attitude" shortname="ddgain q"/>
|
||||
<dl_setting var="booz_stabilization_gains.p.z" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="pgain psi"/>
|
||||
<dl_setting var="booz_stabilization_gains.d.z" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="dgain r"/>
|
||||
<dl_setting var="booz_stabilization_gains.i.z" min="-300" step="1" max="0" module="stabilization/booz_stabilization_attitude" shortname="igain psi"/>
|
||||
<dl_setting var="booz_stabilization_gains.dd.z" min="0" step="1" max="2000" module="stabilization/booz_stabilization_attitude" shortname="ddgain r"/>
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings NAME="Vert Loop">
|
||||
|
||||
@@ -1,3 +1,4 @@
|
||||
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
@@ -38,25 +39,8 @@ extern void booz_stabilization_attitude_run(bool_t in_flight);
|
||||
extern void booz_stabilization_attitude_ref_init(void);
|
||||
extern void booz_stabilization_attitude_ref_update(void);
|
||||
|
||||
extern float booz_stabilization_attitude_beta_vane_gain;
|
||||
extern float booz_stabilization_attitude_alpha_vane_gain;
|
||||
|
||||
extern float booz_stabilization_attitude_alpha_alt_dgain;
|
||||
extern float booz_stabilization_attitude_alpha_alt_pgain;
|
||||
|
||||
extern float booz_stabilization_attitude_pitch_wish;
|
||||
|
||||
extern float booz_stabilization_att_ff_adap_gain[COMMANDS_NB];
|
||||
extern float booz_stabilization_att_ff_gain_wish[COMMANDS_NB];
|
||||
extern float booz_stab_att_ff_lambda;
|
||||
extern float booz_stab_att_ff_alpha0;
|
||||
extern float booz_stab_att_ff_k0;
|
||||
extern float booz_stab_att_ff_update_min;
|
||||
extern float booz_stab_att_ff_update_max;
|
||||
|
||||
|
||||
#define booz_stabilization_attitude_SetKiPhi(_val) { \
|
||||
booz_stabilization_igain.x = _val; \
|
||||
booz_stabilization_gains.i.x = _val; \
|
||||
booz_stabilization_att_sum_err.phi = 0; \
|
||||
}
|
||||
|
||||
|
||||
@@ -30,10 +30,7 @@
|
||||
#include "airframe.h"
|
||||
|
||||
|
||||
struct FloatVect3 booz_stabilization_pgain;
|
||||
struct FloatVect3 booz_stabilization_dgain;
|
||||
struct FloatVect3 booz_stabilization_ddgain;
|
||||
struct FloatVect3 booz_stabilization_igain;
|
||||
struct FloatAttitudeGains booz_stabilization_gains;
|
||||
struct FloatEulers booz_stabilization_att_sum_err;
|
||||
|
||||
float booz_stabilization_att_fb_cmd[COMMANDS_NB];
|
||||
@@ -44,22 +41,22 @@ void booz_stabilization_attitude_init(void) {
|
||||
|
||||
booz_stabilization_attitude_ref_init();
|
||||
|
||||
VECT3_ASSIGN(booz_stabilization_pgain,
|
||||
VECT3_ASSIGN(booz_stabilization_gains.p,
|
||||
BOOZ_STABILIZATION_ATTITUDE_PHI_PGAIN,
|
||||
BOOZ_STABILIZATION_ATTITUDE_THETA_PGAIN,
|
||||
BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN);
|
||||
|
||||
VECT3_ASSIGN(booz_stabilization_dgain,
|
||||
VECT3_ASSIGN(booz_stabilization_gains.d,
|
||||
BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN,
|
||||
BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN,
|
||||
BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN);
|
||||
|
||||
VECT3_ASSIGN(booz_stabilization_igain,
|
||||
VECT3_ASSIGN(booz_stabilization_gains.i,
|
||||
BOOZ_STABILIZATION_ATTITUDE_PHI_IGAIN,
|
||||
BOOZ_STABILIZATION_ATTITUDE_THETA_IGAIN,
|
||||
BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN);
|
||||
|
||||
VECT3_ASSIGN(booz_stabilization_ddgain,
|
||||
VECT3_ASSIGN(booz_stabilization_gains.dd,
|
||||
BOOZ_STABILIZATION_ATTITUDE_PHI_DDGAIN,
|
||||
BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN,
|
||||
BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN);
|
||||
@@ -92,11 +89,11 @@ void booz_stabilization_attitude_run(bool_t in_flight) {
|
||||
|
||||
/* Compute feedforward */
|
||||
booz_stabilization_att_ff_cmd[COMMAND_ROLL] =
|
||||
booz_stabilization_ddgain.x * booz_stab_att_ref_accel.p / 32.;
|
||||
booz_stabilization_gains.dd.x * booz_stab_att_ref_accel.p / 32.;
|
||||
booz_stabilization_att_ff_cmd[COMMAND_PITCH] =
|
||||
booz_stabilization_ddgain.y * booz_stab_att_ref_accel.q / 32.;
|
||||
booz_stabilization_gains.dd.y * booz_stab_att_ref_accel.q / 32.;
|
||||
booz_stabilization_att_ff_cmd[COMMAND_YAW] =
|
||||
booz_stabilization_ddgain.z * booz_stab_att_ref_accel.r / 32.;
|
||||
booz_stabilization_gains.dd.z * booz_stab_att_ref_accel.r / 32.;
|
||||
|
||||
/* Compute feedback */
|
||||
/* attitude error */
|
||||
@@ -124,19 +121,19 @@ void booz_stabilization_attitude_run(bool_t in_flight) {
|
||||
/* PID */
|
||||
|
||||
booz_stabilization_att_fb_cmd[COMMAND_ROLL] =
|
||||
booz_stabilization_pgain.x * att_err.phi +
|
||||
booz_stabilization_dgain.x * rate_err.p +
|
||||
booz_stabilization_igain.x * booz_stabilization_att_sum_err.phi / 1024.;
|
||||
booz_stabilization_gains.p.x * att_err.phi +
|
||||
booz_stabilization_gains.d.x * rate_err.p +
|
||||
booz_stabilization_gains.i.x * booz_stabilization_att_sum_err.phi / 1024.;
|
||||
|
||||
booz_stabilization_att_fb_cmd[COMMAND_PITCH] =
|
||||
booz_stabilization_pgain.y * att_err.theta +
|
||||
booz_stabilization_dgain.y * rate_err.q +
|
||||
booz_stabilization_igain.y * booz_stabilization_att_sum_err.theta / 1024.;
|
||||
booz_stabilization_gains.p.y * att_err.theta +
|
||||
booz_stabilization_gains.d.y * rate_err.q +
|
||||
booz_stabilization_gains.i.y * booz_stabilization_att_sum_err.theta / 1024.;
|
||||
|
||||
booz_stabilization_att_fb_cmd[COMMAND_YAW] =
|
||||
booz_stabilization_pgain.z * att_err.psi +
|
||||
booz_stabilization_dgain.z * rate_err.r +
|
||||
booz_stabilization_igain.z * booz_stabilization_att_sum_err.psi / 1024.;
|
||||
booz_stabilization_gains.p.z * att_err.psi +
|
||||
booz_stabilization_gains.d.z * rate_err.r +
|
||||
booz_stabilization_gains.i.z * booz_stabilization_att_sum_err.psi / 1024.;
|
||||
|
||||
|
||||
booz_stabilization_cmd[COMMAND_ROLL] =
|
||||
|
||||
@@ -29,10 +29,8 @@
|
||||
|
||||
#include "airframe.h"
|
||||
|
||||
struct Int32Vect3 booz_stabilization_pgain;
|
||||
struct Int32Vect3 booz_stabilization_dgain;
|
||||
struct Int32Vect3 booz_stabilization_ddgain;
|
||||
struct Int32Vect3 booz_stabilization_igain;
|
||||
struct Int32AttitudeGains booz_stabilization_gains;
|
||||
|
||||
struct Int32Eulers booz_stabilization_att_sum_err;
|
||||
|
||||
int32_t booz_stabilization_att_fb_cmd[COMMANDS_NB];
|
||||
@@ -43,22 +41,22 @@ void booz_stabilization_attitude_init(void) {
|
||||
booz_stabilization_attitude_ref_init();
|
||||
|
||||
|
||||
VECT3_ASSIGN(booz_stabilization_pgain,
|
||||
VECT3_ASSIGN(booz_stabilization_gains.p,
|
||||
BOOZ_STABILIZATION_ATTITUDE_PHI_PGAIN,
|
||||
BOOZ_STABILIZATION_ATTITUDE_THETA_PGAIN,
|
||||
BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN);
|
||||
|
||||
VECT3_ASSIGN(booz_stabilization_dgain,
|
||||
VECT3_ASSIGN(booz_stabilization_gains.d,
|
||||
BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN,
|
||||
BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN,
|
||||
BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN);
|
||||
|
||||
VECT3_ASSIGN(booz_stabilization_igain,
|
||||
VECT3_ASSIGN(booz_stabilization_gains.i,
|
||||
BOOZ_STABILIZATION_ATTITUDE_PHI_IGAIN,
|
||||
BOOZ_STABILIZATION_ATTITUDE_THETA_IGAIN,
|
||||
BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN);
|
||||
|
||||
VECT3_ASSIGN(booz_stabilization_ddgain,
|
||||
VECT3_ASSIGN(booz_stabilization_gains.dd,
|
||||
BOOZ_STABILIZATION_ATTITUDE_PHI_DDGAIN,
|
||||
BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN,
|
||||
BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN);
|
||||
@@ -97,11 +95,11 @@ void booz_stabilization_attitude_run(bool_t in_flight) {
|
||||
|
||||
/* compute feedforward command */
|
||||
booz_stabilization_att_ff_cmd[COMMAND_ROLL] =
|
||||
OFFSET_AND_ROUND(booz_stabilization_ddgain.x * booz_stab_att_ref_accel.p, 5);
|
||||
OFFSET_AND_ROUND(booz_stabilization_gains.dd.x * booz_stab_att_ref_accel.p, 5);
|
||||
booz_stabilization_att_ff_cmd[COMMAND_PITCH] =
|
||||
OFFSET_AND_ROUND(booz_stabilization_ddgain.y * booz_stab_att_ref_accel.q, 5);
|
||||
OFFSET_AND_ROUND(booz_stabilization_gains.dd.y * booz_stab_att_ref_accel.q, 5);
|
||||
booz_stabilization_att_ff_cmd[COMMAND_YAW] =
|
||||
OFFSET_AND_ROUND(booz_stabilization_ddgain.z * booz_stab_att_ref_accel.r, 5);
|
||||
OFFSET_AND_ROUND(booz_stabilization_gains.dd.z * booz_stab_att_ref_accel.r, 5);
|
||||
|
||||
/* compute feedback command */
|
||||
/* attitude error */
|
||||
@@ -132,19 +130,19 @@ void booz_stabilization_attitude_run(bool_t in_flight) {
|
||||
|
||||
/* PID */
|
||||
booz_stabilization_att_fb_cmd[COMMAND_ROLL] =
|
||||
booz_stabilization_pgain.x * att_err.phi +
|
||||
booz_stabilization_dgain.x * rate_err.p +
|
||||
OFFSET_AND_ROUND2((booz_stabilization_igain.x * booz_stabilization_att_sum_err.phi), 10);
|
||||
booz_stabilization_gains.p.x * att_err.phi +
|
||||
booz_stabilization_gains.d.x * rate_err.p +
|
||||
OFFSET_AND_ROUND2((booz_stabilization_gains.i.x * booz_stabilization_att_sum_err.phi), 10);
|
||||
|
||||
booz_stabilization_att_fb_cmd[COMMAND_PITCH] =
|
||||
booz_stabilization_pgain.y * att_err.theta +
|
||||
booz_stabilization_dgain.y * rate_err.q +
|
||||
OFFSET_AND_ROUND2((booz_stabilization_igain.y * booz_stabilization_att_sum_err.theta), 10);
|
||||
booz_stabilization_gains.p.y * att_err.theta +
|
||||
booz_stabilization_gains.d.y * rate_err.q +
|
||||
OFFSET_AND_ROUND2((booz_stabilization_gains.i.y * booz_stabilization_att_sum_err.theta), 10);
|
||||
|
||||
booz_stabilization_att_fb_cmd[COMMAND_YAW] =
|
||||
booz_stabilization_pgain.z * att_err.psi +
|
||||
booz_stabilization_dgain.z * rate_err.r +
|
||||
OFFSET_AND_ROUND2((booz_stabilization_igain.z * booz_stabilization_att_sum_err.psi), 10);
|
||||
booz_stabilization_gains.p.z * att_err.psi +
|
||||
booz_stabilization_gains.d.z * rate_err.r +
|
||||
OFFSET_AND_ROUND2((booz_stabilization_gains.i.z * booz_stabilization_att_sum_err.psi), 10);
|
||||
|
||||
/* sum feedforward and feedback */
|
||||
booz_stabilization_cmd[COMMAND_ROLL] =
|
||||
|
||||
@@ -28,14 +28,24 @@
|
||||
|
||||
#include "airframe.h"
|
||||
|
||||
extern struct FloatVect3 booz_stabilization_pgain;
|
||||
extern struct FloatVect3 booz_stabilization_dgain;
|
||||
extern struct FloatVect3 booz_stabilization_ddgain;
|
||||
extern struct FloatVect3 booz_stabilization_igain;
|
||||
extern struct FloatEulers booz_stabilization_att_sum_err;
|
||||
struct FloatAttitudeGains {
|
||||
struct FloatVect3 p;
|
||||
struct FloatVect3 d;
|
||||
struct FloatVect3 dd;
|
||||
struct FloatVect3 i;
|
||||
struct FloatVect3 surface_p;
|
||||
struct FloatVect3 surface_d;
|
||||
struct FloatVect3 surface_dd;
|
||||
struct FloatVect3 surface_i;
|
||||
};
|
||||
|
||||
extern struct FloatAttitudeGains booz_stabilization_gains[];
|
||||
extern struct FloatEulers booz_stabilization_att_sum_err_eulers;
|
||||
|
||||
extern float booz_stabilization_att_fb_cmd[COMMANDS_NB];
|
||||
extern float booz_stabilization_att_ff_cmd[COMMANDS_NB];
|
||||
|
||||
void booz_stabilization_attitude_gain_schedule(uint8_t idx);
|
||||
|
||||
#endif /* BOOZ_STABILIZATION_ATTITUDE_FLOAT_H */
|
||||
|
||||
|
||||
@@ -28,10 +28,14 @@
|
||||
|
||||
#include "airframe.h"
|
||||
|
||||
extern struct Int32Vect3 booz_stabilization_igain;
|
||||
extern struct Int32Vect3 booz_stabilization_pgain;
|
||||
extern struct Int32Vect3 booz_stabilization_dgain;
|
||||
extern struct Int32Vect3 booz_stabilization_ddgain;
|
||||
struct Int32AttitudeGains {
|
||||
struct Int32Vect3 p;
|
||||
struct Int32Vect3 d;
|
||||
struct Int32Vect3 dd;
|
||||
struct Int32Vect3 i;
|
||||
};
|
||||
|
||||
extern struct Int32AttitudeGains booz_stabilization_gains;
|
||||
extern struct Int32Eulers booz_stabilization_att_sum_err;
|
||||
|
||||
extern int32_t booz_stabilization_att_fb_cmd[COMMANDS_NB];
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -21,9 +21,16 @@
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/** \file booz_stabilization_attitude_ref_float.c
|
||||
* \brief Booz attitude reference generation (quaternion float version)
|
||||
*
|
||||
*/
|
||||
|
||||
#include "booz_stabilization.h"
|
||||
#include "booz_ahrs.h"
|
||||
|
||||
#include "booz_stabilization_attitude_ref_float.h"
|
||||
#include "quat_setpoint.h"
|
||||
|
||||
struct FloatEulers booz_stab_att_sp_euler;
|
||||
struct FloatQuat booz_stab_att_sp_quat;
|
||||
@@ -34,6 +41,24 @@ struct FloatRates booz_stab_att_ref_accel;
|
||||
|
||||
struct FloatRefModel booz_stab_att_ref_model;
|
||||
|
||||
static void reset_psi_ref_from_body(void) {
|
||||
booz_stab_att_sp_euler.psi = booz_ahrs_float.ltp_to_body_euler.psi;
|
||||
booz_stab_att_ref_euler.psi = booz_ahrs_float.ltp_to_body_euler.psi;
|
||||
booz_stab_att_ref_rate.r = 0;
|
||||
booz_stab_att_ref_accel.r = 0;
|
||||
}
|
||||
|
||||
static void update_ref_quat_from_eulers(void) {
|
||||
struct FloatRMat ref_rmat;
|
||||
|
||||
#ifdef STICKS_RMAT312
|
||||
FLOAT_RMAT_OF_EULERS_312(ref_rmat, booz_stab_att_ref_euler);
|
||||
#else
|
||||
FLOAT_RMAT_OF_EULERS_321(ref_rmat, booz_stab_att_ref_euler);
|
||||
#endif
|
||||
FLOAT_QUAT_OF_RMAT(booz_stab_att_ref_quat, ref_rmat);
|
||||
}
|
||||
|
||||
void booz_stabilization_attitude_ref_init(void) {
|
||||
|
||||
FLOAT_EULERS_ZERO(booz_stab_att_sp_euler);
|
||||
@@ -52,6 +77,13 @@ void booz_stabilization_attitude_ref_init(void) {
|
||||
|
||||
}
|
||||
|
||||
void booz_stabilization_attitude_ref_enter()
|
||||
{
|
||||
reset_psi_ref_from_body();
|
||||
update_ref_quat_from_eulers();
|
||||
booz_stabilization_attitude_sp_enter();
|
||||
}
|
||||
|
||||
/*
|
||||
* Reference
|
||||
*/
|
||||
@@ -91,7 +123,4 @@ void booz_stabilization_attitude_ref_update() {
|
||||
|
||||
/* compute ref_euler */
|
||||
FLOAT_EULERS_OF_QUAT(booz_stab_att_ref_euler, booz_stab_att_ref_quat);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -52,7 +52,6 @@
|
||||
(radio_control.values[RADIO_CONTROL_YAW] > BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R || \
|
||||
radio_control.values[RADIO_CONTROL_YAW] < -BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R)
|
||||
|
||||
void booz_stabilization_attitude_ref_enter(void);
|
||||
|
||||
#endif /* BOOZ_STABILIZATION_ATTITUDE_REF_QUAT_FLOAT_H */
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user