[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4820] Clean up booz_stabilization_attitude_quat_flo
|
From: |
Allen Ibara |
|
Subject: |
[paparazzi-commits] [4820] Clean up booz_stabilization_attitude_quat_float. c by removing plenty of junk from it and pushing said bits down into ref or setpoint |
|
Date: |
Tue, 20 Apr 2010 03:37:56 +0000 |
Revision: 4820
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4820
Author: aibara
Date: 2010-04-20 03:37:55 +0000 (Tue, 20 Apr 2010)
Log Message:
-----------
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)
Modified Paths:
--------------
paparazzi3/trunk/conf/settings/settings_booz2.xml
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_float.h
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_int.h
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h
Modified: paparazzi3/trunk/conf/settings/settings_booz2.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_booz2.xml 2010-04-20 03:34:53 UTC
(rev 4819)
+++ paparazzi3/trunk/conf/settings/settings_booz2.xml 2010-04-20 03:37:55 UTC
(rev 4820)
@@ -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">
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h
2010-04-20 03:34:53 UTC (rev 4819)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h
2010-04-20 03:37:55 UTC (rev 4820)
@@ -1,3 +1,4 @@
+
/*
* $Id$
*
@@ -38,25 +39,8 @@
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; \
}
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c
2010-04-20 03:34:53 UTC (rev 4819)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c
2010-04-20 03:37:55 UTC (rev 4820)
@@ -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 @@
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 @@
/* 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 @@
/* 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] =
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c
2010-04-20 03:34:53 UTC (rev 4819)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c
2010-04-20 03:37:55 UTC (rev 4820)
@@ -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 @@
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 @@
/* 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 @@
/* 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] =
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_float.h
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_float.h
2010-04-20 03:34:53 UTC (rev 4819)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_float.h
2010-04-20 03:37:55 UTC (rev 4820)
@@ -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 */
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_int.h
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_int.h
2010-04-20 03:34:53 UTC (rev 4819)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_int.h
2010-04-20 03:37:55 UTC (rev 4820)
@@ -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];
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
2010-04-20 03:34:53 UTC (rev 4819)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
2010-04-20 03:37:55 UTC (rev 4820)
@@ -1,5 +1,5 @@
/*
- * $Id: booz_stabilization_attitude_euler.c 3787 2009-07-24 15:33:54Z poine $
+ * $Id: booz_stabilization_attitude_quat_float.c 3787 2009-07-24 15:33:54Z
poine $
*
* Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
*
@@ -21,445 +21,139 @@
* Boston, MA 02111-1307, USA.
*/
+/** \file booz_stabilization_attitude_quat_float.c
+ * \brief Booz quaternion attitude stabilization
+ */
+
#include "booz_stabilization.h"
+#include <stdio.h>
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
#include "booz_ahrs.h"
-#include "booz_radio_control.h"
#include "airframe.h"
+struct FloatAttitudeGains
booz_stabilization_gains[BOOZ_STABILIZATION_ATTITUDE_GAIN_NB];
-struct FloatVect3 booz_stabilization_pgain;
-struct FloatVect3 booz_stabilization_dgain;
-struct FloatVect3 booz_stabilization_ddgain;
-struct FloatVect3 booz_stabilization_igain;
-struct FloatQuat booz_stabilization_sum_err;
-struct FloatEulers booz_stabilization_att_sum_err;
+struct FloatQuat booz_stabilization_att_sum_err_quat;
+struct FloatEulers booz_stabilization_att_sum_err_eulers;
float booz_stabilization_att_fb_cmd[COMMANDS_NB];
float booz_stabilization_att_ff_cmd[COMMANDS_NB];
-float booz_stabilization_attitude_beta_vane_gain;
-float booz_stabilization_attitude_alpha_vane_gain;
+static int gain_idx = BOOZ_STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT;
-float booz_stabilization_attitude_alpha_alt_dgain;
-float booz_stabilization_attitude_alpha_alt_pgain;
+static const float phi_pgain[] = BOOZ_STABILIZATION_ATTITUDE_PHI_PGAIN;
+static const float theta_pgain[] = BOOZ_STABILIZATION_ATTITUDE_THETA_PGAIN;
+static const float psi_pgain[] = BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN;
-float booz_stabilization_attitude_pitch_wish;
+static const float phi_dgain[] = BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN;
+static const float theta_dgain[] = BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN;
+static const float psi_dgain[] = BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN;
-float booz_stabilization_att_ff_adap_gain[COMMANDS_NB];
-float booz_stabilization_att_ff_gain_wish[COMMANDS_NB];
-float booz_stab_att_ff_lambda;
-float booz_stab_att_ff_alpha0;
-float booz_stab_att_ff_k0;
-float booz_stab_att_ff_update_min;
-float booz_stab_att_ff_update_max;
+static const float phi_igain[] = BOOZ_STABILIZATION_ATTITUDE_PHI_IGAIN;
+static const float theta_igain[] = BOOZ_STABILIZATION_ATTITUDE_THETA_IGAIN;
+static const float psi_igain[] = BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN;
-#ifdef USE_VANE
-static float beta_vane_stick_cmd = 0;
-static struct FloatQuat alpha_setpoint_quat = { 1., 0., 0., 0. };
-static float alpha_error = 0;
-#endif // USE_VANE
+static const float phi_ddgain[] = BOOZ_STABILIZATION_ATTITUDE_PHI_DDGAIN;
+static const float theta_ddgain[] = BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN;
+static const float psi_ddgain[] = BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN;
-void booz_stabilization_attitude_init(void) {
+static const float phi_pgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_PHI_PGAIN_SURFACE;
+static const float theta_pgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_THETA_PGAIN_SURFACE;
+static const float psi_pgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN_SURFACE;
- booz_stabilization_attitude_ref_init();
+static const float phi_dgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN_SURFACE;
+static const float theta_dgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN_SURFACE;
+static const float psi_dgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN_SURFACE;
- VECT3_ASSIGN(booz_stabilization_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_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);
+static const float phi_igain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_PHI_IGAIN_SURFACE;
+static const float theta_igain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_THETA_IGAIN_SURFACE;
+static const float psi_igain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN_SURFACE;
- VECT3_ASSIGN(booz_stabilization_ddgain,
- BOOZ_STABILIZATION_ATTITUDE_PHI_DDGAIN,
- BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN,
- BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN);
+static const float phi_ddgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_PHI_DDGAIN_SURFACE;
+static const float theta_ddgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN_SURFACE;
+static const float psi_ddgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN_SURFACE;
- FLOAT_QUAT_ZERO( booz_stabilization_sum_err );
- FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err );
+#define IERROR_SCALE 1024
-#ifdef USE_VANE
- booz_stabilization_attitude_alpha_vane_gain =
BOOZ_STABILIZATION_ATTITUDE_ALPHA_VANE_T;
- booz_stabilization_attitude_beta_vane_gain =
BOOZ_STABILIZATION_ATTITUDE_BETA_VANE_T;
- booz_stabilization_attitude_alpha_alt_dgain = booz_stabilization_dgain.y;
- booz_stabilization_attitude_alpha_alt_pgain = booz_stabilization_pgain.y/4;
-#endif
+void booz_stabilization_attitude_init(void) {
- booz_stabilization_att_ff_adap_gain[COMMAND_ROLL] = 0.001;
- booz_stabilization_att_ff_adap_gain[COMMAND_PITCH] = 0.001;
- booz_stabilization_att_ff_adap_gain[COMMAND_YAW] = 0.001;
+ booz_stabilization_attitude_ref_init();
- booz_stabilization_att_ff_gain_wish[COMMAND_ROLL] = 550;
- booz_stabilization_att_ff_gain_wish[COMMAND_PITCH] = 400;
- booz_stabilization_att_ff_gain_wish[COMMAND_YAW] = 300;
+ for (int i = 0; i < BOOZ_STABILIZATION_ATTITUDE_GAIN_NB; i++) {
+ VECT3_ASSIGN(booz_stabilization_gains[i].p, phi_pgain[i], theta_pgain[i],
psi_pgain[i]);
+ VECT3_ASSIGN(booz_stabilization_gains[i].d, phi_dgain[i], theta_dgain[i],
psi_dgain[i]);
+ VECT3_ASSIGN(booz_stabilization_gains[i].i, phi_igain[i], theta_igain[i],
psi_igain[i]);
+ VECT3_ASSIGN(booz_stabilization_gains[i].dd, phi_ddgain[i],
theta_ddgain[i], psi_ddgain[i]);
+ }
- booz_stab_att_ff_lambda = 0.05;
- booz_stab_att_ff_alpha0 = 0.005;
- booz_stab_att_ff_k0 = 10;
- booz_stab_att_ff_update_min = 0.1;
- booz_stab_att_ff_update_max = 500;
-
+ FLOAT_QUAT_ZERO( booz_stabilization_att_sum_err_quat );
+ FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err_eulers );
}
-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;
-}
-
-// Returns rotation about Y axis from dcm from -2 * pi to 2 * pi
-static float get_pitch_rotation_angle(struct FloatRMat *rmat)
+void booz_stabilization_attitude_gain_schedule(uint8_t idx)
{
- float dcm02 = rmat->m[2];
- float dcm22 = rmat->m[8];
-
- return -atan2f(dcm02, dcm22);
+ if (gain_idx >= BOOZ_STABILIZATION_ATTITUDE_GAIN_NB) {
+ // This could be bad -- Just say no.
+ return;
+ }
+ gain_idx = idx;
}
+void booz_stabilization_attitude_enter(void) {
-// complicated function to reset setpoint quaternion to pitch theta, roll phi
using provided
-// quaternion initial rotating first about pitch, then roll (and not yaw)
-static void reset_sp_quat(float phi, float theta, struct FloatQuat *initial)
-{
- float pitch_rotation_angle, roll_rotation_angle;
- struct FloatQuat pitch_axis_quat, roll_axis_quat;
-
- struct FloatRMat ltp_to_body_rmat;
-
- struct FloatQuat pitch_rotated_quat, roll_rotated_quat;
-
- struct FloatVect3 x_axis = { 1, 0, 0 };
- struct FloatVect3 y_axis = { 0, 1, 0 };
-
- struct FloatEulers rotated_euler;
+ booz_stabilization_attitude_ref_enter();
- // Convert body orientation to rotation matrix
- FLOAT_RMAT_OF_QUAT(ltp_to_body_rmat, *initial);
-
- // compose rotation about Y axis (pitch axis) to theta
- pitch_rotation_angle = theta - get_pitch_rotation_angle(<p_to_body_rmat);
- FLOAT_QUAT_OF_AXIS_ANGLE(pitch_axis_quat, y_axis, pitch_rotation_angle);
- FLOAT_QUAT_COMP(pitch_rotated_quat, *initial, pitch_axis_quat);
-
- // compose rotation about X axis (roll axis) to phi
- FLOAT_EULERS_OF_QUAT(rotated_euler, pitch_rotated_quat);
- roll_rotation_angle = phi - rotated_euler.phi;
- FLOAT_QUAT_OF_AXIS_ANGLE(roll_axis_quat, x_axis, roll_rotation_angle);
- FLOAT_QUAT_COMP(roll_rotated_quat, pitch_rotated_quat, roll_axis_quat);
-
- // store result into setpoint
- FLOAT_QUAT_COPY(booz_stab_att_sp_quat, roll_rotated_quat);
+ FLOAT_QUAT_ZERO( booz_stabilization_att_sum_err_quat );
+ FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err_eulers );
}
-static void update_sp_quat_from_eulers(void) {
- struct FloatRMat sp_rmat;
-
-#ifdef STICKS_RMAT312
- FLOAT_RMAT_OF_EULERS_312(sp_rmat, booz_stab_att_sp_euler);
-#else
- FLOAT_RMAT_OF_EULERS_321(sp_rmat, booz_stab_att_sp_euler);
-#endif
- FLOAT_QUAT_OF_RMAT(booz_stab_att_sp_quat, sp_rmat);
-}
-
-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);
-}
-
-#ifdef USE_VANE
-void booz_stabilization_attitude_read_beta_vane(float beta)
+static void attitude_run_ff(float ff_commands[], struct FloatAttitudeGains
*gains, struct FloatRates *ref_accel)
{
- struct FloatEulers sticks_eulers;
- struct FloatQuat sticks_quat, prev_sp_quat;
+ /* Compute feedforward based on reference acceleration */
- sticks_eulers.phi = booz_stabilization_attitude_beta_vane_gain * beta /
RC_UPDATE_FREQ;
- sticks_eulers.theta = 0;
- sticks_eulers.psi = 0;
-
- beta_vane_stick_cmd = sticks_eulers.phi;
-
- // convert eulers to quaternion
- FLOAT_QUAT_OF_EULERS(sticks_quat, sticks_eulers);
- FLOAT_QUAT_COPY(prev_sp_quat, booz_stab_att_sp_quat)
-
- // rotate previous setpoint by commanded rotation
- FLOAT_QUAT_COMP(booz_stab_att_sp_quat, prev_sp_quat, sticks_quat);
+ ff_commands[COMMAND_ROLL] = GAIN_PRESCALER_FF * gains->dd.x *
ref_accel->p;
+ ff_commands[COMMAND_PITCH] = GAIN_PRESCALER_FF * gains->dd.y *
ref_accel->q;
+ ff_commands[COMMAND_YAW] = GAIN_PRESCALER_FF * gains->dd.z *
ref_accel->r;
+ ff_commands[COMMAND_YAW_SURFACE] = GAIN_PRESCALER_FF * gains->surface_dd.z
* ref_accel->r;
}
-// when doing closed-loop angle of attack control, the pitch
-// setpoint is not based on a rate stick but on the AoA error.
-void booz_stabilization_attitude_read_alpha_vane(float alpha)
+static void attitude_run_fb(float fb_commands[], struct FloatAttitudeGains
*gains, struct FloatQuat *att_err,
+ struct FloatRates *rate_err, struct FloatQuat *sum_err)
{
- // argument alpha is error between measurement and setpoint, I believe
- struct FloatVect3 y_axis = { 0, 1, 0 };
+ /* PID feedback */
+ fb_commands[COMMAND_ROLL] =
+ GAIN_PRESCALER_P * -gains->p.x * att_err->qx +
+ GAIN_PRESCALER_D * gains->d.x * rate_err->p +
+ GAIN_PRESCALER_I * gains->i.x * sum_err->qx;
- alpha_error = alpha;
- FLOAT_QUAT_OF_AXIS_ANGLE(alpha_setpoint_quat, y_axis, alpha_error +
booz_ahrs_float.ltp_to_body_euler.theta);
-}
-
-#endif
-
-void booz_stabilization_attitude_read_rc(bool_t in_flight) {
-
- uint32_t rate_stick_mode = radio_control.values[RADIO_CONTROL_MODE] < -150;
- static uint32_t last_rate_stick_mode;
- pprz_t roll = radio_control.values[RADIO_CONTROL_ROLL];
- pprz_t pitch = radio_control.values[RADIO_CONTROL_PITCH];
- pprz_t yaw = radio_control.values[RADIO_CONTROL_YAW];
- struct FloatEulers sticks_eulers;
- struct FloatQuat sticks_quat, prev_sp_quat;
-
-#ifdef USE_VANE
- struct FloatQuat setpoint_quat_old;
- static int vane_transition = 0;
- static float p_gain_y = 0;
- static float d_gain_y = 0;
-#endif // USE_VANE
-
- // convert sticks to commanded rates
- sticks_eulers.phi = APPLY_DEADBAND(roll,
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_A) * ROLL_COEF_RATE / RC_UPDATE_FREQ;
- sticks_eulers.psi = APPLY_DEADBAND(yaw,
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R) * YAW_COEF_H / RC_UPDATE_FREQ;
- sticks_eulers.theta = APPLY_DEADBAND(pitch,
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_E) * PITCH_COEF_RATE / RC_UPDATE_FREQ;
-
- // RC stick commands rate or position?
- if (rate_stick_mode) {
-#ifdef USE_VANE
- // is vane engaged?
- if (radio_control.values[RADIO_CONTROL_AUX4] < 0) {
- sticks_eulers.theta = 0;
-
- // generate new rotation based on current stick commands
- if (radio_control.values[RADIO_CONTROL_UNUSED] < 0) {
- sticks_eulers.phi = sticks_eulers.phi + beta_vane_stick_cmd;
- }
- FLOAT_QUAT_OF_EULERS(sticks_quat, sticks_eulers);
-
- // if first time on the vane, set setpoint to existing attitude
- if (vane_transition == 0) {
-
- // new setpoint
- FLOAT_QUAT_COPY(booz_stab_att_sp_quat,
booz_ahrs_float.ltp_to_body_quat);
-
- // store old gains
- d_gain_y = booz_stabilization_dgain.y;
- p_gain_y = booz_stabilization_pgain.y;
- vane_transition = 1;
- }
-
- // swap in new D gain and reference model
- booz_stabilization_dgain.y = booz_stabilization_attitude_alpha_alt_dgain;
- booz_stabilization_pgain.y = booz_stabilization_attitude_alpha_alt_pgain;
-
- // integrate stick commands in phi and psi
- FLOAT_QUAT_COPY(setpoint_quat_old, booz_stab_att_sp_quat);
- FLOAT_QUAT_COMP(booz_stab_att_sp_quat, setpoint_quat_old, sticks_quat);
-
- // make new trajectory setpoint
- } else {
- if (vane_transition == 1) {
- // just switched out of vane mode
- booz_stabilization_dgain.y = d_gain_y;
- booz_stabilization_pgain.y = p_gain_y;
- vane_transition = 0;
- }
-#endif // USE_VANE
- // convert eulers to quaternion
- FLOAT_QUAT_OF_EULERS(sticks_quat, sticks_eulers);
- FLOAT_QUAT_COPY(prev_sp_quat, booz_stab_att_sp_quat);
-
- // rotate previous setpoint by commanded rotation
- FLOAT_QUAT_COMP(booz_stab_att_sp_quat, prev_sp_quat, sticks_quat);
-#ifdef USE_VANE
- }
-#endif // USE_VANE
- } else {
- // First time switching from rate to position reset the setpoint based on
the body
- if (last_rate_stick_mode) {
- reset_sp_quat(roll * ROLL_COEF, pitch * PITCH_COEF,
&booz_ahrs_float.ltp_to_body_quat);
- }
-#ifdef USE_VANE
- if (vane_transition == 1) {
- // just switched out of vane mode
- booz_stabilization_dgain.y = d_gain_y;
- booz_stabilization_pgain.y = p_gain_y;
- vane_transition = 0;
- }
-#endif // USE_VANE
-
- // heading hold?
- if (in_flight) {
- // compose setpoint based on previous setpoint + pitch/roll sticks
- reset_sp_quat(roll * ROLL_COEF, pitch * PITCH_COEF,
&booz_stab_att_sp_quat);
-
- // get commanded yaw rate from sticks
- sticks_eulers.phi = 0;
- sticks_eulers.theta = 0;
- sticks_eulers.psi = APPLY_DEADBAND(yaw,
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R) * YAW_COEF / RC_UPDATE_FREQ;
-
- // convert yaw rate * dt into quaternion
- FLOAT_QUAT_OF_EULERS(sticks_quat, sticks_eulers);
- FLOAT_QUAT_COPY(prev_sp_quat, booz_stab_att_sp_quat)
+ fb_commands[COMMAND_PITCH] =
+ GAIN_PRESCALER_P * -gains->p.y * att_err->qy +
+ GAIN_PRESCALER_D * gains->d.y * rate_err->q +
+ GAIN_PRESCALER_I * gains->i.y * sum_err->qy;
- // update setpoint by rotating by yaw command
- FLOAT_QUAT_COMP(booz_stab_att_sp_quat, prev_sp_quat, sticks_quat);
- } else { /* if not flying, use current body position + pitch/roll from
sticks to compose setpoint */
- reset_sp_quat(roll * ROLL_COEF, pitch * PITCH_COEF,
&booz_ahrs_float.ltp_to_body_quat);
- }
- }
- // update euler setpoints for telemetry
- FLOAT_EULERS_OF_QUAT(booz_stab_att_sp_euler, booz_stab_att_sp_quat);
- last_rate_stick_mode = rate_stick_mode;
-}
+ fb_commands[COMMAND_YAW] =
+ GAIN_PRESCALER_P * -gains->p.z * att_err->qz +
+ GAIN_PRESCALER_D * gains->d.z * rate_err->r +
+ GAIN_PRESCALER_I * gains->i.z * sum_err->qz;
+ fb_commands[COMMAND_YAW_SURFACE] =
+ GAIN_PRESCALER_P * -gains->surface_p.z * att_err->qz +
+ GAIN_PRESCALER_D * gains->surface_d.z * rate_err->r +
+ GAIN_PRESCALER_I * gains->surface_i.z * sum_err->qz;
-
-void booz_stabilization_attitude_enter(void) {
-
- reset_psi_ref_from_body();
- update_sp_quat_from_eulers();
- update_ref_quat_from_eulers();
-
- FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err );
- FLOAT_QUAT_ZERO( booz_stabilization_sum_err );
-
}
-#define CMD_SCALE 0.0001
-
-static void booz_stabilization_attitude_ffgain_adap(void) {
- static float miac_covariance_roll = 1;
- static float miac_covariance_pitch = 1;
- static float miac_covariance_yaw = 1;
-
- static float miac_alpha_roll = 0.005;
- static float miac_alpha_pitch = 0.005;
- static float miac_alpha_yaw = 0.005;
+void booz_stabilization_attitude_run(bool_t enable_integrator) {
- static float miac_command_roll = 0;
- static float miac_command_pitch = 0;
- static float miac_command_yaw = 0;
-
- float update[COMMANDS_NB];
-
- /* Roll */
- /* Update the feedforward gains based on how well we can predict the
- vehicle's response (MIAC); see Slotine Ch. 8.7-8 */
- update[COMMAND_ROLL] = -1/miac_covariance_roll * miac_command_roll
- * (booz_ahrs_float.body_rate.p - miac_command_roll *
booz_stabilization_att_ff_adap_gain[COMMAND_ROLL]);
-
- /* Update the feedforward gains based on how well we have tracked
- the reference trajectory (MRAC); see Slotine Ch. 8.2-4 */
- update[COMMAND_ROLL] += CMD_SCALE *
booz_stabilization_att_ff_adap_gain[COMMAND_ROLL] *
- copysign(booz_stabilization_att_fb_cmd[COMMAND_ROLL],
- booz_stabilization_att_fb_cmd[COMMAND_ROLL] *
booz_stabilization_att_ff_cmd[COMMAND_ROLL]);
-
- /* Update the gain for the MIAC adaptive controller. This is a
- crude approximation of the parameter error covariance. */
- miac_covariance_roll += booz_ahrs_float.body_rate.p *
booz_ahrs_float.body_rate.p
- - miac_alpha_roll * miac_covariance_roll;
-
- /* Low-pass filter the axis commands so that we don't need to
- measure rotational accelerations to learn the command-to-torque
- coupling; see Slotine example 8.9 pg. 360 */
- miac_command_roll =
booz_stabilization_cmd[COMMAND_ROLL]*booz_stab_att_ff_lambda
- + miac_command_roll*(1 - booz_stab_att_ff_lambda);
-
- /* Calculate a dynamic forgetting factor so we can learn faster when
- signals are more persistently exciting; see Slotine 8.7.6 */
- miac_alpha_roll = booz_stab_att_ff_alpha0 * (1 - 1/(booz_stab_att_ff_k0 *
miac_covariance_roll));
-
- /* Remove noisy and excessive updates and actually update the
- feedforward gain */
- if (fabs(update[COMMAND_ROLL]) < booz_stab_att_ff_update_min)
- update[COMMAND_ROLL] = 0;
- else if (fabs(update[COMMAND_ROLL]) > booz_stab_att_ff_update_max)
- update[COMMAND_ROLL] = copysign(booz_stab_att_ff_update_max,
update[COMMAND_ROLL]);
- booz_stabilization_att_ff_gain_wish[COMMAND_ROLL] += update[COMMAND_ROLL];
-
- /* Pitch */
- update[COMMAND_PITCH] = -1/miac_covariance_pitch * miac_command_pitch
- * (booz_ahrs_float.body_rate.q - miac_command_pitch *
booz_stabilization_att_ff_adap_gain[COMMAND_PITCH]);
- update[COMMAND_PITCH] += CMD_SCALE *
booz_stabilization_att_ff_adap_gain[COMMAND_PITCH] *
- copysign(booz_stabilization_att_fb_cmd[COMMAND_PITCH],
- booz_stabilization_att_fb_cmd[COMMAND_PITCH] *
booz_stabilization_att_ff_cmd[COMMAND_PITCH]);
- miac_covariance_pitch += booz_ahrs_float.body_rate.q *
booz_ahrs_float.body_rate.q
- - miac_alpha_pitch * miac_covariance_pitch;
- miac_command_pitch =
booz_stabilization_cmd[COMMAND_PITCH]*booz_stab_att_ff_lambda
- + miac_command_pitch*(1 - booz_stab_att_ff_lambda);
- miac_alpha_pitch = booz_stab_att_ff_alpha0 * (1 - 1/(booz_stab_att_ff_k0 *
miac_covariance_pitch));
- if (fabs(update[COMMAND_PITCH]) < booz_stab_att_ff_update_min)
- update[COMMAND_PITCH] = 0;
- else if (fabs(update[COMMAND_PITCH]) > booz_stab_att_ff_update_max)
- update[COMMAND_PITCH] = copysign(booz_stab_att_ff_update_max,
update[COMMAND_PITCH]);
- booz_stabilization_att_ff_gain_wish[COMMAND_PITCH] += update[COMMAND_PITCH];
-
- /* Yaw */
- update[COMMAND_YAW] = -1/miac_covariance_yaw * miac_command_yaw
- * (booz_ahrs_float.body_rate.r - miac_command_yaw *
booz_stabilization_att_ff_adap_gain[COMMAND_YAW]);
- update[COMMAND_YAW] += CMD_SCALE *
booz_stabilization_att_ff_adap_gain[COMMAND_YAW] *
- copysign(booz_stabilization_att_fb_cmd[COMMAND_YAW],
- booz_stabilization_att_fb_cmd[COMMAND_YAW] *
booz_stabilization_att_ff_cmd[COMMAND_YAW]);
- miac_covariance_yaw += booz_ahrs_float.body_rate.r *
booz_ahrs_float.body_rate.r
- - miac_alpha_yaw * miac_covariance_yaw;
- miac_command_yaw =
booz_stabilization_cmd[COMMAND_YAW]*booz_stab_att_ff_lambda
- + miac_command_yaw*(1 - booz_stab_att_ff_lambda);
- miac_alpha_yaw = booz_stab_att_ff_alpha0 * (1 - 1/(booz_stab_att_ff_k0 *
miac_covariance_yaw));
- if (fabs(update[COMMAND_YAW]) < booz_stab_att_ff_update_min)
- update[COMMAND_YAW] = 0;
- else if (fabs(update[COMMAND_YAW]) > booz_stab_att_ff_update_max)
- update[COMMAND_YAW] = copysign(booz_stab_att_ff_update_max,
update[COMMAND_YAW]);
- booz_stabilization_att_ff_gain_wish[COMMAND_YAW] += update[COMMAND_YAW];
-
-}
-
-
-#define IERROR_SCALE 1024
-
-#define MAX_SUM_ERR RadOfDeg(56000)
-#include <stdio.h>
-void booz_stabilization_attitude_run(bool_t in_flight) {
-#ifdef BOOZ_AHRS_FIXED_POINT
- BOOZ_AHRS_FLOAT_OF_INT32();
-#endif
-
-
/*
* Update reference
*/
booz_stabilization_attitude_ref_update();
- /*
- * Compute feedforward
- */
- /* ref accel bfp is 2^12 and int controller offsets by 5 */
- booz_stabilization_att_ff_cmd[COMMAND_ROLL] =
- booz_stabilization_ddgain.x * BFP_OF_REAL(booz_stab_att_ref_accel.p,
(12-5));
- booz_stabilization_att_ff_cmd[COMMAND_PITCH] =
- booz_stabilization_ddgain.y * BFP_OF_REAL(booz_stab_att_ref_accel.q,
(12-5));
- booz_stabilization_att_ff_cmd[COMMAND_YAW] =
- booz_stabilization_ddgain.z * BFP_OF_REAL(booz_stab_att_ref_accel.r,
(12-5));
-
/*
- * Compute feedback
+ * Compute errors for feedback
*/
/* attitude error */
@@ -468,70 +162,33 @@
/* wrap it in the shortest direction */
FLOAT_QUAT_WRAP_SHORTEST(att_err);
- if (in_flight) {
+ /* rate error */
+ struct FloatRates rate_err;
+ RATES_DIFF(rate_err, booz_ahrs_float.body_rate, booz_stab_att_ref_rate);
+
+ /* integrated error */
+ if (enable_integrator) {
struct FloatQuat new_sum_err, scaled_att_err;
/* update accumulator */
- FLOAT_QUAT_COPY(scaled_att_err, att_err);
- scaled_att_err.qx /= IERROR_SCALE;
- scaled_att_err.qy /= IERROR_SCALE;
- scaled_att_err.qz /= IERROR_SCALE;
- FLOAT_QUAT_COMP_INV(new_sum_err, booz_stabilization_sum_err,
scaled_att_err);
+ scaled_att_err.qi = att_err.qi;
+ scaled_att_err.qx = att_err.qx / IERROR_SCALE;
+ scaled_att_err.qy = att_err.qy / IERROR_SCALE;
+ scaled_att_err.qz = att_err.qz / IERROR_SCALE;
+ FLOAT_QUAT_COMP_INV(new_sum_err, booz_stabilization_att_sum_err_quat,
scaled_att_err);
FLOAT_QUAT_NORMALISE(new_sum_err);
- FLOAT_QUAT_COPY(booz_stabilization_sum_err, new_sum_err);
- }
- else {
+ FLOAT_QUAT_COPY(booz_stabilization_att_sum_err_quat, new_sum_err);
+ FLOAT_EULERS_OF_QUAT(booz_stabilization_att_sum_err_eulers,
booz_stabilization_att_sum_err_quat);
+ } else {
/* reset accumulator */
- FLOAT_EULERS_ZERO(booz_stabilization_att_sum_err);
- FLOAT_QUAT_ZERO( booz_stabilization_sum_err );
+ FLOAT_QUAT_ZERO( booz_stabilization_att_sum_err_quat );
+ FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err_eulers );
}
-
- /* rate error */
- struct FloatRates rate_err;
- RATES_DIFF(rate_err, booz_ahrs_float.body_rate, booz_stab_att_ref_rate);
-
- /* what the quaternion controller would have commanded in
- alpha-vane mode */
- booz_stabilization_attitude_pitch_wish =
- -2. * booz_stabilization_pgain.y * QUAT1_BFP_OF_REAL(att_err.qy)+
- booz_stabilization_dgain.y * RATE_BFP_OF_REAL(rate_err.q) +
- booz_stabilization_igain.y *
QUAT1_BFP_OF_REAL(booz_stabilization_sum_err.qy);
-#ifdef USE_VANE
- uint32_t rate_stick_mode = radio_control.values[RADIO_CONTROL_MODE] < -150;
+ attitude_run_ff(booz_stabilization_att_ff_cmd,
&booz_stabilization_gains[gain_idx], &booz_stab_att_ref_accel);
- /* override qy in alpha mode */
- if (rate_stick_mode) {
- if (radio_control.values[RADIO_CONTROL_AUX4] < 0) {
- att_err.qy = alpha_error;
- }
- }
- FLOAT_QUAT_NORMALISE(att_err);
-#endif // USE_VANE
+ attitude_run_fb(booz_stabilization_att_fb_cmd,
&booz_stabilization_gains[gain_idx], &att_err, &rate_err,
&booz_stabilization_att_sum_err_quat);
- /* PID */
- booz_stabilization_att_fb_cmd[COMMAND_ROLL] =
- -2. * booz_stabilization_pgain.x * QUAT1_BFP_OF_REAL(att_err.qx)+
- booz_stabilization_dgain.x * RATE_BFP_OF_REAL(rate_err.p) +
- booz_stabilization_igain.x *
QUAT1_BFP_OF_REAL(booz_stabilization_sum_err.qx);
-
- booz_stabilization_att_fb_cmd[COMMAND_PITCH] =
- -2. * booz_stabilization_pgain.y * QUAT1_BFP_OF_REAL(att_err.qy)+
- booz_stabilization_dgain.y * RATE_BFP_OF_REAL(rate_err.q) +
- booz_stabilization_igain.y *
QUAT1_BFP_OF_REAL(booz_stabilization_sum_err.qy);
-
- booz_stabilization_att_fb_cmd[COMMAND_YAW] =
- -2. * booz_stabilization_pgain.z * QUAT1_BFP_OF_REAL(att_err.qz)+
- booz_stabilization_dgain.z * RATE_BFP_OF_REAL(rate_err.r) +
- booz_stabilization_igain.z *
QUAT1_BFP_OF_REAL(booz_stabilization_sum_err.qz);
-
- booz_stabilization_cmd[COMMAND_ROLL] =
-
((booz_stabilization_att_fb_cmd[COMMAND_ROLL]+booz_stabilization_att_ff_cmd[COMMAND_ROLL]))/(1<<16);
- booz_stabilization_cmd[COMMAND_PITCH] =
-
((booz_stabilization_att_fb_cmd[COMMAND_PITCH]+booz_stabilization_att_ff_cmd[COMMAND_PITCH]))/(1<<16);
- booz_stabilization_cmd[COMMAND_YAW] =
-
((booz_stabilization_att_fb_cmd[COMMAND_YAW]+booz_stabilization_att_ff_cmd[COMMAND_YAW]))/(1<<16);
-
- if (in_flight) {
- booz_stabilization_attitude_ffgain_adap();
+ for (int i = COMMAND_ROLL; i <= COMMAND_YAW; i++) {
+ booz_stabilization_cmd[i] =
booz_stabilization_att_fb_cmd[i]+booz_stabilization_att_ff_cmd[i];
}
}
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c
2010-04-20 03:34:53 UTC (rev 4819)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c
2010-04-20 03:37:55 UTC (rev 4820)
@@ -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 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_enter()
+{
+ reset_psi_ref_from_body();
+ update_ref_quat_from_eulers();
+ booz_stabilization_attitude_sp_enter();
+}
+
/*
* Reference
*/
@@ -91,7 +123,4 @@
/* compute ref_euler */
FLOAT_EULERS_OF_QUAT(booz_stab_att_ref_euler, booz_stab_att_ref_quat);
-
}
-
-
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h
2010-04-20 03:34:53 UTC (rev 4819)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h
2010-04-20 03:37:55 UTC (rev 4820)
@@ -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 */
-
-
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4820] Clean up booz_stabilization_attitude_quat_float. c by removing plenty of junk from it and pushing said bits down into ref or setpoint,
Allen Ibara <=