Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 20:36:06 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[ahrs] quaternion integrator fix

Browse Source 
closes #807
This commit is contained in:
Ewoud Smeur
2014-10-24 15:33:17 +02:00
committed by Felix Ruess
parent fefb624448
commit d57bf1ea0a
8 changed files with 25 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/control/stabilization_att_int.xml
+1 -1
View File
@@ -6,7 +6,7 @@
<dl_settings NAME="Att Loop">
<dl_setting var="stabilization_gains.p.x" min="1" step="1" max="8000" module="stabilization/stabilization_attitude_common_int" shortname="pgain phi" param="STABILIZATION_ATTITUDE_PHI_PGAIN"/>
<dl_setting var="stabilization_gains.d.x" min="1" step="1" max="4000" module="stabilization/stabilization_attitude_common_int" shortname="dgain p" param="STABILIZATION_ATTITUDE_PHI_DGAIN"/>
<dl_setting var="stabilization_gains.i.x" min="0" step="1" max="800" module="stabilization/stabilization_attitude_common_int" shortname="igain phi" handler="SetKiPhi"param="STABILIZATION_ATTITUDE_PHI_IGAIN" />
<dl_setting var="stabilization_gains.i.x" min="0" step="1" max="800" module="stabilization/stabilization_attitude_common_int" shortname="igain phi" param="STABILIZATION_ATTITUDE_PHI_IGAIN" />
<dl_setting var="stabilization_gains.dd.x" min="0" step="1" max="1000" module="stabilization/stabilization_attitude_common_int" shortname="ddgain p" param="STABILIZATION_ATTITUDE_PHI_DDGAIN"/>
<dl_setting var="stabilization_gains.p.y" min="1" step="1" max="8000" module="stabilization/stabilization_attitude_common_int" shortname="pgain theta" param="STABILIZATION_ATTITUDE_THETA_PGAIN"/>
<dl_setting var="stabilization_gains.d.y" min="1" step="1" max="4000" module="stabilization/stabilization_attitude_common_int" shortname="dgain q" param="STABILIZATION_ATTITUDE_THETA_DGAIN"/>
conf/settings/control/stabilization_att_int_quat.xml
+1 -1
View File
@@ -6,7 +6,7 @@
<dl_settings NAME="Att Loop">
<dl_setting var="stabilization_gains.p.x" min="1" step="1" max="8000" module="stabilization/stabilization_attitude_common_int" shortname="pgain phi" param="STABILIZATION_ATTITUDE_PHI_PGAIN"/>
<dl_setting var="stabilization_gains.d.x" min="1" step="1" max="4000" module="stabilization/stabilization_attitude_common_int" shortname="dgain p" param="STABILIZATION_ATTITUDE_PHI_DGAIN"/>
<dl_setting var="stabilization_gains.i.x" min="0" step="1" max="800" module="stabilization/stabilization_attitude_common_int" shortname="igain phi" handler="SetKiPhi"param="STABILIZATION_ATTITUDE_PHI_IGAIN" />
<dl_setting var="stabilization_gains.i.x" min="0" step="1" max="800" module="stabilization/stabilization_attitude_common_int" shortname="igain phi" param="STABILIZATION_ATTITUDE_PHI_IGAIN"/>
<dl_setting var="stabilization_gains.dd.x" min="0" step="1" max="1000" module="stabilization/stabilization_attitude_common_int" shortname="ddgain p" param="STABILIZATION_ATTITUDE_PHI_DDGAIN"/>
<dl_setting var="stabilization_gains.p.y" min="1" step="1" max="8000" module="stabilization/stabilization_attitude_common_int" shortname="pgain theta" param="STABILIZATION_ATTITUDE_THETA_PGAIN"/>
<dl_setting var="stabilization_gains.d.y" min="1" step="1" max="4000" module="stabilization/stabilization_attitude_common_int" shortname="dgain q" param="STABILIZATION_ATTITUDE_THETA_DGAIN"/>
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h
-2
View File
@@ -46,8 +46,6 @@ struct FloatAttitudeGains {
struct FloatVect3 surface_i;
};
extern struct FloatEulers stabilization_att_sum_err;
extern float stabilization_att_fb_cmd[COMMANDS_NB];
extern float stabilization_att_ff_cmd[COMMANDS_NB];
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h
-6
View File
@@ -40,14 +40,8 @@ struct Int32AttitudeGains {
};
extern struct Int32AttitudeGains stabilization_gains;
extern struct Int32Eulers stabilization_att_sum_err;
extern int32_t stabilization_att_fb_cmd[COMMANDS_NB];
extern int32_t stabilization_att_ff_cmd[COMMANDS_NB];
#define stabilization_attitude_common_int_SetKiPhi(_val) { \
stabilization_gains.i.x = _val; \
stabilization_att_sum_err.phi = 0; \
}
#endif /* STABILIZATION_ATTITUDE_COMMON_INT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h
+1
View File
@@ -33,5 +33,6 @@
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h"
extern struct FloatAttitudeGains stabilization_gains;
extern struct FloatEulers stabilization_att_sum_err;
#endif /* STABILIZATION_ATTITUDE_EULER_FLOAT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h
+2
View File
@@ -25,4 +25,6 @@
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h"
extern struct Int32Eulers stabilization_att_sum_err;
#endif /* STABILIZATION_ATTITUDE_EULER_INT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+10 -17
View File
@@ -38,7 +38,6 @@
struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_GAIN_NB];
struct FloatQuat stabilization_att_sum_err_quat;
struct FloatEulers stabilization_att_sum_err;
struct FloatRates last_body_rate;
struct FloatRates body_rate_d;
@@ -105,9 +104,9 @@ static void send_att(void) {
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
&stabilization_att_sum_err.phi,
&stabilization_att_sum_err.theta,
&stabilization_att_sum_err.psi,
&stabilization_att_sum_err_quat.qx,
&stabilization_att_sum_err_quat.qy,
&stabilization_att_sum_err_quat.qz,
&stabilization_att_fb_cmd[COMMAND_ROLL],
&stabilization_att_fb_cmd[COMMAND_PITCH],
&stabilization_att_fb_cmd[COMMAND_YAW],
@@ -156,7 +155,6 @@ void stabilization_attitude_init(void) {
}
float_quat_identity(&stabilization_att_sum_err_quat);
FLOAT_EULERS_ZERO( stabilization_att_sum_err );
FLOAT_RATES_ZERO( last_body_rate );
FLOAT_RATES_ZERO( body_rate_d );
@@ -184,7 +182,6 @@ void stabilization_attitude_enter(void) {
stabilization_attitude_ref_enter();
float_quat_identity(&stabilization_att_sum_err_quat);
FLOAT_EULERS_ZERO( stabilization_att_sum_err );
}
void stabilization_attitude_set_failsafe_setpoint(void) {
@@ -305,22 +302,18 @@ void stabilization_attitude_run(bool_t enable_integrator) {
RATES_DIFF(body_rate_d, *body_rate, last_body_rate);
RATES_COPY(last_body_rate, *body_rate);
#define INTEGRATOR_BOUND 1.0
/* integrated error */
if (enable_integrator) {
struct FloatQuat new_sum_err, scaled_att_err;
/* update accumulator */
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(&new_sum_err, &stabilization_att_sum_err_quat, &scaled_att_err);
float_quat_normalize(&new_sum_err);
QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
float_eulers_of_quat(&stabilization_att_sum_err, &stabilization_att_sum_err_quat);
stabilization_att_sum_err_quat.qx += att_err.qx /IERROR_SCALE;
stabilization_att_sum_err_quat.qy += att_err.qy /IERROR_SCALE;
stabilization_att_sum_err_quat.qz += att_err.qz /IERROR_SCALE;
Bound(stabilization_att_sum_err_quat.qx,-INTEGRATOR_BOUND,INTEGRATOR_BOUND);
Bound(stabilization_att_sum_err_quat.qy,-INTEGRATOR_BOUND,INTEGRATOR_BOUND);
Bound(stabilization_att_sum_err_quat.qz,-INTEGRATOR_BOUND,INTEGRATOR_BOUND);
} else {
/* reset accumulator */
float_quat_identity(&stabilization_att_sum_err_quat);
FLOAT_EULERS_ZERO( stabilization_att_sum_err );
}
attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+10 -17
View File
@@ -56,7 +56,6 @@ struct Int32AttitudeGains stabilization_gains = {
#endif
struct Int32Quat stabilization_att_sum_err_quat;
struct Int32Eulers stabilization_att_sum_err;
int32_t stabilization_att_fb_cmd[COMMANDS_NB];
int32_t stabilization_att_ff_cmd[COMMANDS_NB];
@@ -79,9 +78,9 @@ static void send_att(void) { //FIXME really use this message here ?
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
&stabilization_att_sum_err.phi,
&stabilization_att_sum_err.theta,
&stabilization_att_sum_err.psi,
&stabilization_att_sum_err_quat.qx,
&stabilization_att_sum_err_quat.qy,
&stabilization_att_sum_err_quat.qz,
&stabilization_att_fb_cmd[COMMAND_ROLL],
&stabilization_att_fb_cmd[COMMAND_PITCH],
&stabilization_att_fb_cmd[COMMAND_YAW],
@@ -128,7 +127,6 @@ void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
int32_quat_identity(&stabilization_att_sum_err_quat);
INT_EULERS_ZERO( stabilization_att_sum_err );
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att);
@@ -145,7 +143,6 @@ void stabilization_attitude_enter(void) {
stabilization_attitude_ref_enter();
int32_quat_identity(&stabilization_att_sum_err_quat);
INT_EULERS_ZERO(stabilization_att_sum_err);
}
@@ -242,22 +239,18 @@ void stabilization_attitude_run(bool_t enable_integrator) {
struct Int32Rates* body_rate = stateGetBodyRates_i();
RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate));
#define INTEGRATOR_BOUND 100000
/* integrated error */
if (enable_integrator) {
struct Int32Quat new_sum_err, scaled_att_err;
/* update accumulator */
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;
int32_quat_comp(&new_sum_err, &stabilization_att_sum_err_quat, &scaled_att_err);
int32_quat_normalize(&new_sum_err);
QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
int32_eulers_of_quat(&stabilization_att_sum_err, &stabilization_att_sum_err_quat);
stabilization_att_sum_err_quat.qx += att_err.qx /IERROR_SCALE;
stabilization_att_sum_err_quat.qy += att_err.qy /IERROR_SCALE;
stabilization_att_sum_err_quat.qz += att_err.qz /IERROR_SCALE;
Bound(stabilization_att_sum_err_quat.qx,-INTEGRATOR_BOUND,INTEGRATOR_BOUND);
Bound(stabilization_att_sum_err_quat.qy,-INTEGRATOR_BOUND,INTEGRATOR_BOUND);
Bound(stabilization_att_sum_err_quat.qz,-INTEGRATOR_BOUND,INTEGRATOR_BOUND);
} else {
/* reset accumulator */
int32_quat_identity(&stabilization_att_sum_err_quat);
INT_EULERS_ZERO( stabilization_att_sum_err );
}
/* compute the feed forward command */