[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5035] Add rates_d gains and reference module schedu
|
From: |
Allen Ibara |
|
Subject: |
[paparazzi-commits] [5035] Add rates_d gains and reference module scheduling ( different ref model for different mode) |
|
Date: |
Fri, 09 Jul 2010 05:41:34 +0000 |
Revision: 5035
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5035
Author: aibara
Date: 2010-07-09 05:41:34 +0000 (Fri, 09 Jul 2010)
Log Message:
-----------
Add rates_d gains and reference module scheduling (different ref model for
different mode)
Modified Paths:
--------------
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_float.h
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_float.h
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/sw/airborne/booz/stabilization/booz_stabilization_attitude_float.h
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_float.h
2010-07-09 05:24:58 UTC (rev 5034)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_float.h
2010-07-09 05:41:34 UTC (rev 5035)
@@ -32,6 +32,7 @@
struct FloatVect3 p;
struct FloatVect3 d;
struct FloatVect3 dd;
+ struct FloatVect3 rates_d;
struct FloatVect3 i;
struct FloatVect3 surface_p;
struct FloatVect3 surface_d;
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-07-09 05:24:58 UTC (rev 5034)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
2010-07-09 05:41:34 UTC (rev 5035)
@@ -59,6 +59,10 @@
static const float theta_ddgain[] = BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN;
static const float psi_ddgain[] = BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN;
+static const float phi_dgain_d[] = BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN_D;
+static const float theta_dgain_d[] = BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN_D;
+static const float psi_dgain_d[] = BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN_D;
+
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;
@@ -86,6 +90,7 @@
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]);
+ VECT3_ASSIGN(booz_stabilization_gains[i].rates_d, phi_dgain_d[i],
theta_dgain_d[i], psi_dgain_d[i]);
VECT3_ASSIGN(booz_stabilization_gains[i].surface_p, phi_pgain_surface[i],
theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN(booz_stabilization_gains[i].surface_d, phi_dgain_surface[i],
theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN(booz_stabilization_gains[i].surface_i, phi_igain_surface[i],
theta_igain_surface[i], psi_igain_surface[i]);
@@ -103,6 +108,7 @@
return;
}
gain_idx = idx;
+ booz_stabilization_attitude_ref_schedule(idx);
}
void booz_stabilization_attitude_enter(void) {
@@ -120,28 +126,43 @@
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_ROLL_SURFACE] = GAIN_PRESCALER_FF * gains->surface_dd.x
* ref_accel->p;
+ ff_commands[COMMAND_PITCH_SURFACE] = GAIN_PRESCALER_FF * gains->surface_dd.y
* ref_accel->q;
ff_commands[COMMAND_YAW_SURFACE] = GAIN_PRESCALER_FF * gains->surface_dd.z
* ref_accel->r;
}
static void attitude_run_fb(float fb_commands[], struct FloatAttitudeGains
*gains, struct FloatQuat *att_err,
- struct FloatRates *rate_err, struct FloatQuat *sum_err)
+ struct FloatRates *rate_err, struct FloatRates *rate_err_d, struct
FloatQuat *sum_err)
{
/* 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_D * gains->rates_d.x * rate_err_d->p +
GAIN_PRESCALER_I * gains->i.x * sum_err->qx;
fb_commands[COMMAND_PITCH] =
GAIN_PRESCALER_P * -gains->p.y * att_err->qy +
GAIN_PRESCALER_D * gains->d.y * rate_err->q +
+ GAIN_PRESCALER_D * gains->rates_d.y * rate_err_d->q +
GAIN_PRESCALER_I * gains->i.y * sum_err->qy;
fb_commands[COMMAND_YAW] =
GAIN_PRESCALER_P * -gains->p.z * att_err->qz +
GAIN_PRESCALER_D * gains->d.z * rate_err->r +
+ GAIN_PRESCALER_D * gains->rates_d.z * rate_err_d->r +
GAIN_PRESCALER_I * gains->i.z * sum_err->qz;
+ fb_commands[COMMAND_ROLL_SURFACE] =
+ GAIN_PRESCALER_P * -gains->surface_p.x * att_err->qx +
+ GAIN_PRESCALER_D * gains->surface_d.x * rate_err->p +
+ GAIN_PRESCALER_I * gains->surface_i.x * sum_err->qx;
+
+ fb_commands[COMMAND_PITCH_SURFACE] =
+ GAIN_PRESCALER_P * -gains->surface_p.y * att_err->qy +
+ GAIN_PRESCALER_D * gains->surface_d.y * rate_err->q +
+ GAIN_PRESCALER_I * gains->surface_i.y * sum_err->qy;
+
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 +
@@ -190,7 +211,7 @@
attitude_run_ff(booz_stabilization_att_ff_cmd,
&booz_stabilization_gains[gain_idx], &booz_stab_att_ref_accel);
- attitude_run_fb(booz_stabilization_att_fb_cmd,
&booz_stabilization_gains[gain_idx], &att_err, &rate_err,
&booz_stabilization_att_sum_err_quat);
+ attitude_run_fb(booz_stabilization_att_fb_cmd,
&booz_stabilization_gains[gain_idx], &att_err, &rate_err,
&booz_ahrs_float.body_rate_d, &booz_stabilization_att_sum_err_quat);
for (int i = COMMAND_ROLL; i <= COMMAND_YAW_SURFACE; 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_float.h
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_float.h
2010-07-09 05:24:58 UTC (rev 5034)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_float.h
2010-07-09 05:41:34 UTC (rev 5035)
@@ -23,6 +23,8 @@
#ifndef BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H
#define BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H
+#include "airframe.h"
+
extern struct FloatEulers booz_stab_att_sp_euler;
extern struct FloatQuat booz_stab_att_sp_quat;
extern struct FloatEulers booz_stab_att_ref_euler;
@@ -31,15 +33,11 @@
extern struct FloatRates booz_stab_att_ref_accel;
struct FloatRefModel {
- float omega_p;
- float zeta_p;
- float omega_q;
- float zeta_q;
- float omega_r;
- float zeta_r;
+ struct FloatRates omega;
+ struct FloatRates zeta;
};
-extern struct FloatRefModel booz_stab_att_ref_model;
+extern struct FloatRefModel booz_stab_att_ref_model[];
#endif /* BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H */
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-07-09 05:24:58 UTC (rev 5034)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c
2010-07-09 05:41:34 UTC (rev 5035)
@@ -26,6 +26,7 @@
*
*/
+#include "airframe.h"
#include "booz_stabilization.h"
#include "booz_ahrs.h"
@@ -43,8 +44,17 @@
struct FloatRates booz_stab_att_ref_rate;
struct FloatRates booz_stab_att_ref_accel;
-struct FloatRefModel booz_stab_att_ref_model;
+struct FloatRefModel
booz_stab_att_ref_model[BOOZ_STABILIZATION_ATTITUDE_GAIN_NB];
+static int ref_idx = BOOZ_STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT;
+
+static const float omega_p[] = BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_P;
+static const float zeta_p[] = BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_P;
+static const float omega_q[] = BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_Q;
+static const float zeta_q[] = BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_Q;
+static const float omega_r[] = BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_R;
+static const float zeta_r[] = BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_R;
+
static void reset_psi_ref_from_body(void) {
booz_stab_att_ref_euler.psi = booz_ahrs_float.ltp_to_body_euler.psi;
booz_stab_att_ref_rate.r = 0;
@@ -72,15 +82,18 @@
FLOAT_RATES_ZERO( booz_stab_att_ref_rate);
FLOAT_RATES_ZERO( booz_stab_att_ref_accel);
- booz_stab_att_ref_model.omega_p = BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_P;
- booz_stab_att_ref_model.zeta_p = BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_P;
- booz_stab_att_ref_model.omega_q = BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_Q;
- booz_stab_att_ref_model.zeta_q = BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_Q;
- booz_stab_att_ref_model.omega_r = BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_R;
- booz_stab_att_ref_model.zeta_r = BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_R;
+ for (int i = 0; i < BOOZ_STABILIZATION_ATTITUDE_GAIN_NB; i++) {
+ RATES_ASSIGN(booz_stab_att_ref_model[i].omega, omega_p[i], omega_q[i],
omega_r[i]);
+ RATES_ASSIGN(booz_stab_att_ref_model[i].zeta, zeta_p[i], zeta_q[i],
zeta_r[i]);
+ }
}
+void booz_stabilization_attitude_ref_schedule(uint8_t idx)
+{
+ ref_idx = idx;
+}
+
void booz_stabilization_attitude_ref_enter()
{
reset_psi_ref_from_body();
@@ -118,12 +131,12 @@
/* wrap it in the shortest direction */
FLOAT_QUAT_WRAP_SHORTEST(err);
/* propagate the 2nd order linear model */
- booz_stab_att_ref_accel.p =
-2.*booz_stab_att_ref_model.zeta_p*booz_stab_att_ref_model.omega_p*booz_stab_att_ref_rate.p
- - booz_stab_att_ref_model.omega_p*booz_stab_att_ref_model.omega_p*err.qx;
- booz_stab_att_ref_accel.q =
-2.*booz_stab_att_ref_model.zeta_q*booz_stab_att_ref_model.omega_q*booz_stab_att_ref_rate.q
- - booz_stab_att_ref_model.omega_q*booz_stab_att_ref_model.omega_q*err.qy;
- booz_stab_att_ref_accel.r =
-2.*booz_stab_att_ref_model.zeta_r*booz_stab_att_ref_model.omega_r*booz_stab_att_ref_rate.r
- - booz_stab_att_ref_model.omega_r*booz_stab_att_ref_model.omega_r*err.qz;
+ booz_stab_att_ref_accel.p =
-2.*booz_stab_att_ref_model[ref_idx].zeta.p*booz_stab_att_ref_model[ref_idx].omega.p*booz_stab_att_ref_rate.p
+ -
booz_stab_att_ref_model[ref_idx].omega.p*booz_stab_att_ref_model[ref_idx].omega.p*err.qx;
+ booz_stab_att_ref_accel.q =
-2.*booz_stab_att_ref_model[ref_idx].zeta.q*booz_stab_att_ref_model[ref_idx].omega.q*booz_stab_att_ref_rate.q
+ -
booz_stab_att_ref_model[ref_idx].omega.q*booz_stab_att_ref_model[ref_idx].omega.q*err.qy;
+ booz_stab_att_ref_accel.r =
-2.*booz_stab_att_ref_model[ref_idx].zeta.r*booz_stab_att_ref_model[ref_idx].omega.r*booz_stab_att_ref_rate.r
+ -
booz_stab_att_ref_model[ref_idx].omega.r*booz_stab_att_ref_model[ref_idx].omega.r*err.qz;
/* saturate acceleration */
const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q,
-REF_ACCEL_MAX_R };
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-07-09 05:24:58 UTC (rev 5034)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h
2010-07-09 05:41:34 UTC (rev 5035)
@@ -31,13 +31,14 @@
#include "stabilization/booz_stabilization_attitude_ref_float.h"
#define RC_UPDATE_FREQ 40.
-#define ROLL_COEF (-BOOZ_STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ)
+#define ROLL_COEF (BOOZ_STABILIZATION_ATTITUDE_SP_MAX_P / MAX_PPRZ)
+#define ROLL_COEF_H (BOOZ_STABILIZATION_ATTITUDE_SP_MAX_P_H / MAX_PPRZ)
#define PITCH_COEF ( BOOZ_STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ)
-#define YAW_COEF (-BOOZ_STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ)
-#define YAW_COEF_H (-BOOZ_STABILIZATION_ATTITUDE_SP_MAX_H_R / MAX_PPRZ)
+#define YAW_COEF (BOOZ_STABILIZATION_ATTITUDE_SP_MAX_PSI / MAX_PPRZ)
#define ROLL_COEF_RATE (-BOOZ_STABILIZATION_ATTITUDE_SP_MAX_P / MAX_PPRZ)
#define PITCH_COEF_RATE ( BOOZ_STABILIZATION_ATTITUDE_SP_MAX_Q / MAX_PPRZ)
+#define YAW_COEF_RATE ( BOOZ_STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ)
#define DEADBAND_EXCEEDED(VARIABLE, VALUE) ((VARIABLE > VALUE) || (VARIABLE <
-VALUE))
#define APPLY_DEADBAND(VARIABLE, VALUE) (DEADBAND_EXCEEDED(VARIABLE, VALUE) ?
VARIABLE : 0.0)
@@ -53,5 +54,6 @@
radio_control.values[RADIO_CONTROL_YAW] <
-BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R)
void booz_stabilization_attitude_ref_enter(void);
+void booz_stabilization_attitude_ref_schedule(uint8_t idx);
#endif /* BOOZ_STABILIZATION_ATTITUDE_REF_QUAT_FLOAT_H */
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5035] Add rates_d gains and reference module scheduling ( different ref model for different mode),
Allen Ibara <=