Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[ahrs] configurable gravity_heuristic_factor

Browse Source 
if set to zero, gravity heuristic is disabled
- removed AHRS_USE_GRAVITY_UPDATE_NORM_HEURISTIC
This commit is contained in:
Felix Ruess
2013-09-06 18:16:27 +02:00
parent 61b4507917
commit e31901900f
13 changed files with 63 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml
+1 -1
View File
@@ -112,7 +112,7 @@
<section name="AHRS" prefix="AHRS_">
<define name="PROPAGATE_LOW_PASS_RATES" value="1"/>
<define name="GRAVITY_UPDATE_NORM_HEURISTIC" value="1"/>
<define name="GRAVITY_HEURISTIC_FACTOR" value="30"/>
<define name="H_X" value="0.3586845"/>
<define name="H_Y" value="0.0168651"/>
<define name="H_Z" value="0.933303"/>
conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml
+1 -1
View File
@@ -115,7 +115,7 @@
<section name="AHRS" prefix="AHRS_">
<define name="PROPAGATE_LOW_PASS_RATES" value="1"/>
<define name="GRAVITY_UPDATE_NORM_HEURISTIC" value="1"/>
<define name="GRAVITY_HEURISTIC_FACTOR" value="30"/>
<define name="H_X" value="0.3586845"/>
<define name="H_Y" value="0.0168651"/>
<define name="H_Z" value="0.933303"/>
conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml
+1 -1
View File
@@ -105,7 +105,7 @@
<section name="AHRS" prefix="AHRS_">
<define name="PROPAGATE_LOW_PASS_RATES" value="1"/>
<define name="GRAVITY_UPDATE_NORM_HEURISTIC" value="1"/>
<define name="GRAVITY_HEURISTIC_FACTOR" value="30"/>
<define name="H_X" value="0.3586845"/>
<define name="H_Y" value="0.0168651"/>
<define name="H_Z" value="0.933303"/>
conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml
+1 -1
View File
@@ -101,7 +101,7 @@
<section name="AHRS" prefix="AHRS_">
<define name="PROPAGATE_LOW_PASS_RATES" value="1"/>
<define name="GRAVITY_UPDATE_NORM_HEURISTIC" value="1"/>
<define name="GRAVITY_HEURISTIC_FACTOR" value="30"/>
<define name="H_X" value="0.365258"/>
<define name="H_Y" value="0.015505"/>
<define name="H_Z" value="0.930777"/>
conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
+1 -1
View File
@@ -35,7 +35,7 @@
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat">
<define name="AHRS_GRAVITY_UPDATE_NORM_HEURISTIC" value="TRUE"/>
<define name="AHRS_GRAVITY_HEURISTIC_FACTOR" value="30"/>
</subsystem>
<subsystem name="ins"/>
</firmware>
conf/airframes/examples/quadrotor_mlkf.xml
+1 -3
View File
@@ -27,9 +27,7 @@
<subsystem name="imu" type="aspirin_v2.1"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="float_quat"/>
<subsystem name="ahrs" type="float_mlkf">
<define name="AHRS_GRAVITY_UPDATE_NORM_HEURISTIC" value="TRUE"/>
</subsystem>
<subsystem name="ahrs" type="float_mlkf"/>
<subsystem name="ins"/>
</firmware>
conf/airframes/examples/stm32f4_discovery_test.xml
+1 -1
View File
@@ -24,7 +24,7 @@
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat">
<define name="AHRS_GRAVITY_UPDATE_NORM_HEURISTIC" value="TRUE"/>
<define name="AHRS_GRAVITY_HEURISTIC_FACTOR" value="30"/>
</subsystem>
<subsystem name="ins"/>
</firmware>
conf/airframes/fraser_lisa_m_rotorcraft.xml
+1 -1
View File
@@ -30,7 +30,7 @@
<subsystem name="gps" type="ublox"/>
<subsystem name="imu" type="aspirin_v1.5"/>
<subsystem name="ahrs" type="int_cmpl_quat">
<define name="AHRS_GRAVITY_UPDATE_NORM_HEURISTIC" value="TRUE"/>
<define name="AHRS_GRAVITY_HEURISTIC_FACTOR" value="30"/>
</subsystem>
<subsystem name="ins_extended"/>
conf/settings/estimation/ahrs_int_cmpl_quat.xml
+1 -1
View File
@@ -4,7 +4,7 @@
<dl_settings>
<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" param="AHRS_GRAVITY_UPDATE_NORM_HEURISTIC"/>
<dl_setting var="ahrs_impl.gravity_heuristic_factor" min="0" step="1" max="50" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="g_heuristic" param="AHRS_GRAVITY_HEURISTIC_FACTOR"/>
<dl_setting var="ahrs_impl.accel_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s" handler="SetAccelOmega"/>
<dl_setting var="ahrs_impl.accel_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_zeta" param="AHRS_ACCEL_ZETA" handler="SetAccelZeta"/>
<dl_setting var="ahrs_impl.mag_omega" min="0.02" step="0.01" max="0.1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_omega" param="AHRS_MAG_OMEGA" unit="rad/s" handler="SetMagOmega"/>
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
+6 -11
View File
@@ -91,8 +91,8 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
#endif
/** by default use the gravity heuristic to reduce gain */
#ifndef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
#define AHRS_GRAVITY_UPDATE_NORM_HEURISTIC TRUE
#ifndef AHRS_GRAVITY_HEURISTIC_FACTOR
#define AHRS_GRAVITY_HEURISTIC_FACTOR 30
#endif
#ifdef AHRS_UPDATE_FW_ESTIMATOR
@@ -146,11 +146,7 @@ void ahrs_init(void) {
ahrs_impl.correct_gravity = FALSE;
#endif
#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
ahrs_impl.use_gravity_heuristic = TRUE;
#else
ahrs_impl.use_gravity_heuristic = FALSE;
#endif
ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR;
VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
}
@@ -263,16 +259,15 @@ void ahrs_update_accel(void) {
VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
if (ahrs_impl.use_gravity_heuristic) {
if (ahrs_impl.gravity_heuristic_factor) {
/* heuristic on acceleration (gravity estimate) norm */
/* Factor how strongly to change the weight.
* e.g. for WEIGHT_FACTOR 3:
* e.g. for gravity_heuristic_factor 30:
* <0.66G = 0, 1G = 1.0, >1.33G = 0
*/
#define WEIGHT_FACTOR 3
const float g_meas_norm = FLOAT_VECT3_NORM(filtered_gravity_measurement) / 9.81;
ahrs_impl.weight = 1.0 - WEIGHT_FACTOR * fabs(1.0 - g_meas_norm);
ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0;
Bound(ahrs_impl.weight, 0.15, 1.0);
}
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h
+11 -5
View File
@@ -38,16 +38,22 @@ struct AhrsFloatCmpl {
struct FloatRates imu_rate;
struct FloatQuat ltp_to_imu_quat;
struct FloatRMat ltp_to_imu_rmat;
/* for gravity correction during coordinated turns */
float ltp_vel_norm;
bool_t correct_gravity; ///< enable gravity correction during coordinated turns
float ltp_vel_norm; ///< velocity norm for gravity correction during coordinated turns
bool_t ltp_vel_norm_valid;
float accel_omega; ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement)
float accel_zeta; ///< filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement)
float mag_omega; ///< filter cut-off frequency for correcting the attitude (heading) from magnetometer
float mag_zeta; ///< filter damping for correcting the gyro bias from magnetometer
/** sets how strongly the gravity heuristic reduces accel correction.
* Set to zero in order to disable gravity heuristic.
*/
uint8_t gravity_heuristic_factor;
float weight;
bool_t ltp_vel_norm_valid;
bool_t correct_gravity;
bool_t use_gravity_heuristic;
bool_t heading_aligned;
struct FloatVect3 mag_h;
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+9 -9
View File
@@ -94,8 +94,8 @@ PRINT_CONFIG_VAR(AHRS_MAG_OMEGA)
PRINT_CONFIG_VAR(AHRS_MAG_ZETA)
/** by default use the gravity heuristic to reduce gain */
#ifndef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
#define AHRS_GRAVITY_UPDATE_NORM_HEURISTIC TRUE
#ifndef AHRS_GRAVITY_HEURISTIC_FACTOR
#define AHRS_GRAVITY_HEURISTIC_FACTOR 30
#endif
@@ -141,7 +141,7 @@ void ahrs_init(void) {
ahrs_set_mag_gains();
/* set default gravity heuristic */
ahrs_impl.weight = 1.0;
ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR;
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE;
@@ -149,8 +149,6 @@ void ahrs_init(void) {
ahrs_impl.correct_gravity = FALSE;
#endif
ahrs_impl.use_gravity_heuristic = AHRS_GRAVITY_UPDATE_NORM_HEURISTIC;
VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X),
MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));
@@ -287,20 +285,22 @@ void ahrs_update_accel(void) {
VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
if (ahrs_impl.use_gravity_heuristic) {
if (ahrs_impl.gravity_heuristic_factor) {
/* heuristic on acceleration (gravity estimate) norm */
/* Factor how strongly to change the weight.
* e.g. for WEIGHT_FACTOR 3:
* e.g. for gravity_heuristic_factor 30:
* <0.66G = 0, 1G = 1.0, >1.33G = 0
*/
#define WEIGHT_FACTOR 5
struct FloatVect3 g_meas_f;
ACCELS_FLOAT_OF_BFP(g_meas_f, filtered_gravity_measurement);
const float g_meas_norm = FLOAT_VECT3_NORM(g_meas_f)/9.81;
ahrs_impl.weight = 1.0 - WEIGHT_FACTOR * fabs(1.0 - g_meas_norm);
ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10;
Bound(ahrs_impl.weight, 0.15, 1.0);
}
else {
ahrs_impl.weight = 1.0;
}
/* Complementary filter proportional gain.
* Kp = 2 * zeta * omega * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h
+28 -5
View File
@@ -57,12 +57,35 @@ struct AhrsIntCmplQuat {
float mag_ki;
/* parameters/options that can be changed */
/** enable gravity vector correction by removing centrifugal acceleration */
bool_t correct_gravity;
bool_t use_gravity_heuristic;
float accel_omega; ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement)
float accel_zeta; ///< filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement)
float mag_omega; ///< filter cut-off frequency for correcting the attitude (heading) from magnetometer
float mag_zeta; ///< filter damping for correcting the gyro bias from magnetometer
/** sets how strongly the gravity heuristic reduces accel correction.
* Set to zero in order to disable gravity heuristic.
*/
uint8_t gravity_heuristic_factor;
/** filter cut-off frequency for correcting the attitude from accels.
* (pseudo-gravity measurement)
* only update through #ahrs_int_cmpl_quat_SetAccelOmega(omega)
*/
float accel_omega;
/** filter damping for correcting the gyro-bias from accels.
* (pseudo-gravity measurement)
* only update through #ahrs_int_cmpl_quat_SetAccelZeta(zeta)
*/
float accel_zeta;
/** filter cut-off frequency for correcting the attitude (heading) from magnetometer.
* only update through #ahrs_int_cmpl_quat_SetMagOmega(omega)
*/
float mag_omega;
/** filter damping for correcting the gyro bias from magnetometer.
* only update through #ahrs_int_cmpl_quat_SetMagZeta(zeta)
*/
float mag_zeta;
};
extern struct AhrsIntCmplQuat ahrs_impl;