Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 03:57:45 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[ahrs] int_cmpl_quat: scale the correction in update_accel with AHRS_CORRECT_FREQUENCY

Browse Source 
Allow tuning of the rate and bias gains.
Scaling correction by AHRS_CORRECT_FREQUENCY should also give you
roughly correct tuning for 100Hz fixedwings, see #240.
This commit is contained in:
Felix Ruess
2013-02-20 15:24:15 +01:00
parent 08bcb665d6
commit 5545851d20
6 changed files with 59 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
+4
View File
@@ -18,6 +18,7 @@
</subsystem> </subsystem>
<!-- MPU6000 is configured to output data at 500Hz --> <!-- MPU6000 is configured to output data at 500Hz -->
<configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/> <configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
<configure name="AHRS_CORRECT_FREQUENCY" value="500"/>
</target> </target>
<target name="nps" board="pc"> <target name="nps" board="pc">
@@ -25,6 +26,9 @@
<subsystem name="radio_control" type="ppm"/> <subsystem name="radio_control" type="ppm"/>
</target> </target>
<!-- print the configuration during compiling -->
<define name="PRINT_CONFIG"/>
<subsystem name="motor_mixing"/> <subsystem name="motor_mixing"/>
<subsystem name="actuators" type="pwm"> <subsystem name="actuators" type="pwm">
<define name="SERVO_HZ" value="400"/> <define name="SERVO_HZ" value="400"/>
conf/conf.xml.example
+1 -1
View File
@@ -41,7 +41,7 @@
radio="radios/cockpitSX.xml" radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml" telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml" flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml" settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml settings/estimation/ahrs_int_cmpl_quat.xml"
gui_color="white" gui_color="white"
/> />
<aircraft <aircraft
conf/settings/estimation/ahrs_int_cmpl_quat.xml
+3 -1
View File
@@ -4,7 +4,9 @@
<dl_settings> <dl_settings>
<dl_settings NAME="AHRS"> <dl_settings NAME="AHRS">
<dl_setting var="ahrs_impl.use_gravity_heuristic" min="0" step="1" max="1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="gravity_heuristic" values="OFF|ON"/> <dl_setting var="ahrs_impl.use_gravity_heuristic" min="0" step="1" max="1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="gravity_heuristic" values="OFF|ON" param="AHRS_GRAVITY_UPDATE_NORM_HEURISTIC"/>
<dl_setting var="ahrs_impl.rate_correction_gain" min="1" step="1" max="100" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="rate_gain" param="AHRS_RATE_CORRECTION_GAIN"/>
<dl_setting var="ahrs_impl.bias_correction_gain" min="1" step="1" max="100" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="bias_gain" param="AHRS_BIAS_CORRECTION_GAIN"/>
</dl_settings> </dl_settings>
</dl_settings> </dl_settings>
conf/tests_conf.xml
+1 -1
View File
@@ -41,7 +41,7 @@
radio="radios/cockpitSX.xml" radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml" telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml" flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml" settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml settings/estimation/ahrs_int_cmpl_quat.xml"
gui_color="white" gui_color="white"
/> />
<aircraft <aircraft
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+47 -18
View File
@@ -59,8 +59,18 @@ static inline void ahrs_update_mag_2d(void);
#ifndef AHRS_PROPAGATE_FREQUENCY #ifndef AHRS_PROPAGATE_FREQUENCY
#define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
#endif #endif
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
#ifndef AHRS_CORRECT_FREQUENCY
#define AHRS_CORRECT_FREQUENCY PERIODIC_FREQUENCY
#endif
PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
struct AhrsIntCmpl ahrs_impl; #ifndef AHRS_RATE_CORRECTION_GAIN
#define AHRS_RATE_CORRECTION_GAIN 25
#endif
#ifndef AHRS_BIAS_CORRECTION_GAIN
#define AHRS_BIAS_CORRECTION_GAIN 16
#endif
#ifdef AHRS_UPDATE_FW_ESTIMATOR #ifdef AHRS_UPDATE_FW_ESTIMATOR
// remotely settable // remotely settable
@@ -74,6 +84,8 @@ float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
#endif #endif
struct AhrsIntCmpl ahrs_impl;
static inline void set_body_state_from_quat(void); static inline void set_body_state_from_quat(void);
@@ -91,6 +103,10 @@ void ahrs_init(void) {
INT_RATES_ZERO(ahrs_impl.rate_correction); INT_RATES_ZERO(ahrs_impl.rate_correction);
INT_RATES_ZERO(ahrs_impl.high_rez_bias); INT_RATES_ZERO(ahrs_impl.high_rez_bias);
/* set default correction gains */
ahrs_impl.rate_correction_gain = AHRS_RATE_CORRECTION_GAIN;
ahrs_impl.bias_correction_gain = AHRS_BIAS_CORRECTION_GAIN;
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE; ahrs_impl.correct_gravity = TRUE;
#else #else
@@ -151,7 +167,7 @@ void ahrs_propagate(void) {
RATES_COPY(ahrs_impl.imu_rate, omega); RATES_COPY(ahrs_impl.imu_rate, omega);
#endif #endif
/* add correction */ /* add correction */
RATES_ADD(omega, ahrs_impl.rate_correction); RATES_ADD(omega, ahrs_impl.rate_correction);
/* and zeros it */ /* and zeros it */
INT_RATES_ZERO(ahrs_impl.rate_correction); INT_RATES_ZERO(ahrs_impl.rate_correction);
@@ -229,24 +245,37 @@ void ahrs_update_accel(void) {
inv_weight = 1; inv_weight = 1;
} }
// residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 /* Correct the drift by adding a rate correction.
// rate_correction FRAC = RATE_FRAC = 12 * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
// 2^12 / 2^24 * 5e-2 = 1/81920 * rate_correction FRAC = RATE_FRAC = 12
ahrs_impl.rate_correction.p += -residual.x/82000/inv_weight; * FRAC conversion: 2^12 / 2^24 = 1/(2^12) = 1/4096
ahrs_impl.rate_correction.q += -residual.y/82000/inv_weight; */
ahrs_impl.rate_correction.r += -residual.z/82000/inv_weight; /* scale gain with the update_accel freq and */
uint32_t inv_rate_scale = AHRS_CORRECT_FREQUENCY * (4096 / ahrs_impl.rate_correction_gain);
Bound(inv_rate_scale, 2000, 500000);
ahrs_impl.rate_correction.p += -residual.x / inv_rate_scale / inv_weight;
ahrs_impl.rate_correction.q += -residual.y / inv_rate_scale / inv_weight;
ahrs_impl.rate_correction.r += -residual.z / inv_rate_scale / inv_weight;
// residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
// high_rez_bias = RATE_FRAC+28 = 40
// 2^40 / 2^24 * 5e-6 = 1/3.05
// ahrs_impl.high_rez_bias.p += residual.x*3; /* Correct the gyro bias.
// ahrs_impl.high_rez_bias.q += residual.y*3; * Also make mitigating effect of gravity heuristic weight
// ahrs_impl.high_rez_bias.r += residual.z*3; * twice as large on bias as on rate.
*
ahrs_impl.high_rez_bias.p += residual.x/(2*inv_weight); * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
ahrs_impl.high_rez_bias.q += residual.y/(2*inv_weight); * high_rez_bias = RATE_FRAC+28 = 40
ahrs_impl.high_rez_bias.r += residual.z/(2*inv_weight); * FRAC conversion: 2^40 / 2^24 = 2^16
*
* Use bias_correction_gain scale of 1/(2^11) compared to rate_correction_gain.
* 2^16 / 2^11 = 32
* To allow a suitable range of the freq and bias gain, multily by 32 separately.
*/
/* scale gain with the update_accel freq */
uint32_t inv_bias_gain = AHRS_CORRECT_FREQUENCY / ahrs_impl.bias_correction_gain;
Bound(inv_bias_gain, 1, 1000)
ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) * 32 / (2*inv_weight);
ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) * 32 / (2*inv_weight);
ahrs_impl.high_rez_bias.r += (residual.z / inv_bias_gain) * 32 / (2*inv_weight);
/* */ /* */
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h
+3 -1
View File
@@ -43,7 +43,9 @@ struct AhrsIntCmpl {
struct Int64Rates high_rez_bias; struct Int64Rates high_rez_bias;
struct Int32Quat ltp_to_imu_quat; struct Int32Quat ltp_to_imu_quat;
struct Int32Eulers ltp_to_imu_euler; // FIXME to compile telemetry struct Int32Eulers ltp_to_imu_euler; // FIXME to compile telemetry
struct Int32Vect3 mag_h; struct Int32Vect3 mag_h;
uint32_t rate_correction_gain;
uint32_t bias_correction_gain;
int32_t ltp_vel_norm; int32_t ltp_vel_norm;
bool_t ltp_vel_norm_valid; bool_t ltp_vel_norm_valid;
bool_t correct_gravity; bool_t correct_gravity;