mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-26 16:30:07 +08:00
Merge pull request #448 from Drumettaz/ahrs_int_cmpl_quat_correction_scaling
Ahrs int cmpl quat correction scaling
This commit is contained in:
Gautier Hattenberger
@@ -127,6 +127,12 @@ struct FloatRates {
|
||||
n = sqrtf((v).x*(v).x + (v).y*(v).y); \
|
||||
}
|
||||
|
||||
#define FLOAT_VECT2_NORMALIZE(_v) { \
|
||||
float n; \
|
||||
FLOAT_VECT2_NORM(n, _v); \
|
||||
FLOAT_VECT2_SMUL(_v, _v, 1 / n); \
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Dimension 3 Vectors
|
||||
|
||||
@@ -61,6 +61,8 @@ struct Int16Vect3 {
|
||||
#define INT32_ACCEL_FRAC 10
|
||||
#define INT32_MAG_FRAC 11
|
||||
|
||||
#define INT32_PERCENTAGE_FRAC 10
|
||||
|
||||
struct Int32Vect2 {
|
||||
int32_t x;
|
||||
int32_t y;
|
||||
@@ -211,11 +213,18 @@ struct Int64Vect3 {
|
||||
|
||||
#define INT_VECT2_ASSIGN(_a, _x, _y) VECT2_ASSIGN(_a, _x, _y)
|
||||
|
||||
#define INT32_VECT2_NORM(n, v) { \
|
||||
int32_t n2 = (v).x*(v).x + (v).y*(v).y; \
|
||||
INT32_SQRT(n, n2); \
|
||||
#define INT32_VECT2_NORM(_n, _v) { \
|
||||
int32_t n2 = (_v).x*(_v).x + (_v).y*(_v).y; \
|
||||
INT32_SQRT(_n, n2); \
|
||||
}
|
||||
|
||||
#define INT32_VECT2_NORMALIZE(_v,_frac) { \
|
||||
int32_t n; \
|
||||
INT32_VECT2_NORM(n, _v); \
|
||||
INT32_VECT2_SCALE_2(_v, _v, BFP_OF_REAL((1.),_frac) , n); \
|
||||
}
|
||||
|
||||
|
||||
#define INT32_VECT2_RSHIFT(_o, _i, _r) { \
|
||||
(_o).x = ((_i).x >> (_r)); \
|
||||
(_o).y = ((_i).y >> (_r)); \
|
||||
|
||||
@@ -55,12 +55,45 @@
|
||||
#endif
|
||||
|
||||
#if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING
|
||||
#warning "Using magnetometer and GPS course to update heading. Probably better to set USE_MAGNETOMETER=0 if you want to use GPS course."
|
||||
#warning "Using magnetometer _and_ GPS course to update heading. Probably better to <configure name="USE_MAGNETOMETER" value="0"/> if you want to use GPS course."
|
||||
#endif
|
||||
|
||||
#ifndef AHRS_PROPAGATE_FREQUENCY
|
||||
#define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
|
||||
|
||||
#ifndef AHRS_CORRECT_FREQUENCY
|
||||
#define AHRS_CORRECT_FREQUENCY AHRS_PROPAGATE_FREQUENCY
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
|
||||
|
||||
#ifndef AHRS_MAG_CORRECT_FREQUENCY
|
||||
#define AHRS_MAG_CORRECT_FREQUENCY 50
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
|
||||
|
||||
/*
|
||||
* default gains for correcting attitude and bias from accel/mag
|
||||
*/
|
||||
#ifndef AHRS_ACCEL_OMEGA
|
||||
#define AHRS_ACCEL_OMEGA 0.063
|
||||
#endif
|
||||
#ifndef AHRS_ACCEL_ZETA
|
||||
#define AHRS_ACCEL_ZETA 0.9
|
||||
#endif
|
||||
|
||||
#ifndef AHRS_MAG_OMEGA
|
||||
#define AHRS_MAG_OMEGA 0.04
|
||||
#endif
|
||||
#ifndef AHRS_MAG_ZETA
|
||||
#define AHRS_MAG_ZETA 0.9
|
||||
#endif
|
||||
|
||||
/** by default use the gravity heursitic to reduce gain */
|
||||
#ifndef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
|
||||
#define AHRS_GRAVITY_UPDATE_NORM_HEURISTIC TRUE
|
||||
#endif
|
||||
|
||||
#ifdef AHRS_UPDATE_FW_ESTIMATOR
|
||||
// remotely settable
|
||||
@@ -101,12 +134,26 @@ void ahrs_init(void) {
|
||||
|
||||
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
|
||||
|
||||
/* set default filter cut-off frequency and damping */
|
||||
ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA;
|
||||
ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA;
|
||||
ahrs_impl.mag_omega = AHRS_MAG_OMEGA;
|
||||
ahrs_impl.mag_zeta = AHRS_MAG_ZETA;
|
||||
|
||||
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
|
||||
ahrs_impl.correct_gravity = TRUE;
|
||||
#else
|
||||
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
|
||||
|
||||
/* TO DO add local magnetic field
|
||||
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)); */
|
||||
}
|
||||
|
||||
void ahrs_align(void) {
|
||||
@@ -131,7 +178,6 @@ void ahrs_align(void) {
|
||||
struct Int32Rates bias0;
|
||||
RATES_COPY(bias0, ahrs_aligner.lp_gyro);
|
||||
RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0);
|
||||
|
||||
ahrs.status = AHRS_RUNNING;
|
||||
}
|
||||
|
||||
@@ -184,6 +230,8 @@ void ahrs_update_accel(void) {
|
||||
|
||||
struct FloatVect3 residual;
|
||||
|
||||
struct FloatVect3 pseudo_gravity_measurement;
|
||||
|
||||
if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) {
|
||||
/*
|
||||
* centrifugal acceleration in body frame
|
||||
@@ -200,30 +248,43 @@ void ahrs_update_accel(void) {
|
||||
struct FloatVect3 acc_c_imu;
|
||||
FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body);
|
||||
|
||||
/* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */
|
||||
struct FloatVect3 corrected_gravity;
|
||||
VECT3_DIFF(corrected_gravity, imu_accel_float, acc_c_imu);
|
||||
/* and subtract it from imu measurement to get a corrected measurement of the gravity vector */
|
||||
VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu);
|
||||
|
||||
/* compute the residual of gravity vector in imu frame */
|
||||
FLOAT_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2);
|
||||
} else {
|
||||
FLOAT_VECT3_CROSS_PRODUCT(residual, imu_accel_float, c2);
|
||||
VECT3_COPY(pseudo_gravity_measurement, imu_accel_float);
|
||||
}
|
||||
|
||||
#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
|
||||
/* heuristic on acceleration norm */
|
||||
const float acc_norm = FLOAT_VECT3_NORM(imu_accel_float);
|
||||
const float weight = Chop(1.-6*fabs((9.81-acc_norm)/9.81), 0., 1.);
|
||||
#else
|
||||
const float weight = 1.;
|
||||
#endif
|
||||
FLOAT_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
|
||||
|
||||
/* compute correction */
|
||||
const float gravity_rate_update_gain = -5e-2; // -5e-2
|
||||
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain);
|
||||
if (ahrs_impl.use_gravity_heuristic) {
|
||||
/* heuristic on acceleration (gravity estimate) norm */
|
||||
/* Factor how strongly to change the weight.
|
||||
* e.g. for WEIGHT_FACTOR 3:
|
||||
* <0.66G = 0, 1G = 1.0, >1.33G = 0
|
||||
*/
|
||||
#define WEIGHT_FACTOR 3
|
||||
|
||||
const float gravity_bias_update_gain = 1e-5; // -5e-6
|
||||
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain);
|
||||
/* TODO filter pseudo_gravity_measurement */
|
||||
/* g_meas_f=filter(pseudo_gravity_measurement) */
|
||||
|
||||
const float g_meas_norm = FLOAT_VECT3_NORM(pseudo_gravity_measurement) / 9.81;
|
||||
ahrs_impl.weight = 1.0 - WEIGHT_FACTOR * fabs(1.0 - g_meas_norm);
|
||||
Bound(ahrs_impl.weight, 0.15, 1.0);
|
||||
}
|
||||
|
||||
/* Complementary filter proportional gain.
|
||||
* Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
|
||||
*/
|
||||
const float gravity_rate_update_gain = -2 / 9.81 * ahrs_impl.accel_zeta * ahrs_impl.accel_omega * ahrs_impl.weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY;
|
||||
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, gravity_rate_update_gain);
|
||||
|
||||
/* Complementary filter integral gain
|
||||
* Correct the gyro bias.
|
||||
* Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
|
||||
*/
|
||||
const float gravity_bias_update_gain = ( ahrs_impl.accel_omega * ahrs_impl.accel_omega * ahrs_impl.weight * ahrs_impl.weight) / (AHRS_CORRECT_FREQUENCY * 9.81);
|
||||
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, gravity_bias_update_gain);
|
||||
|
||||
/* FIXME: saturate bias */
|
||||
|
||||
@@ -253,39 +314,61 @@ void ahrs_update_mag_full(void) {
|
||||
// DISPLAY_FLOAT_VECT3("# measured", measured_imu);
|
||||
// DISPLAY_FLOAT_VECT3("# residual", residual);
|
||||
|
||||
const float mag_rate_update_gain = 2.5;
|
||||
/* Complementary filter proportional gain.
|
||||
* Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
|
||||
*/
|
||||
|
||||
const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY;
|
||||
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
|
||||
|
||||
const float mag_bias_update_gain = -2.5e-3;
|
||||
/* Complementary filter integral gain
|
||||
* Correct the gyro bias.
|
||||
* Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
|
||||
*/
|
||||
const float mag_bias_update_gain = -( ahrs_impl.mag_omega * ahrs_impl.mag_omega) / AHRS_MAG_CORRECT_FREQUENCY;
|
||||
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
|
||||
|
||||
}
|
||||
|
||||
void ahrs_update_mag_2d(void) {
|
||||
|
||||
const struct FloatVect2 expected_ltp = {AHRS_H_X, AHRS_H_Y};
|
||||
struct FloatVect2 expected_ltp = {AHRS_H_X, AHRS_H_Y};
|
||||
// normalize expected ltp in 2D (x,y)
|
||||
FLOAT_VECT2_NORMALIZE(expected_ltp);
|
||||
|
||||
struct FloatVect3 measured_imu;
|
||||
MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
|
||||
struct FloatVect3 measured_ltp;
|
||||
FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_impl.ltp_to_imu_rmat, measured_imu);
|
||||
|
||||
struct FloatVect2 measured_ltp_2d={measured_ltp.x, measured_ltp.y};
|
||||
|
||||
// normalize measured ltp in 2D (x,y)
|
||||
FLOAT_VECT2_NORMALIZE(measured_ltp_2d);
|
||||
|
||||
const struct FloatVect3 residual_ltp =
|
||||
{ 0,
|
||||
0,
|
||||
measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x };
|
||||
measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x };
|
||||
|
||||
// printf("res : %f\n", residual_ltp.z);
|
||||
|
||||
struct FloatVect3 residual_imu;
|
||||
FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp);
|
||||
|
||||
const float mag_rate_update_gain = 2.5;
|
||||
|
||||
/* Complementary filter proportional gain.
|
||||
* Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
|
||||
*/
|
||||
const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY;
|
||||
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
|
||||
|
||||
const float mag_bias_update_gain = -2.5e-3;
|
||||
/* Complementary filter integral gain
|
||||
* Correct the gyro bias.
|
||||
* Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
|
||||
*/
|
||||
const float mag_bias_update_gain = -( ahrs_impl.mag_omega * ahrs_impl.mag_omega) / AHRS_MAG_CORRECT_FREQUENCY;
|
||||
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -43,6 +43,9 @@
|
||||
#include "math/pprz_trig_int.h"
|
||||
#include "math/pprz_algebra_int.h"
|
||||
|
||||
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
|
||||
PRINT_CONFIG_MSG("LOW PASS FILTER ON GYRO RATES")
|
||||
#endif
|
||||
|
||||
#ifdef AHRS_MAG_UPDATE_YAW_ONLY
|
||||
#error "The define AHRS_MAG_UPDATE_YAW_ONLY doesn't exist anymore, please remove it. This is the default behaviour. Define AHRS_MAG_UPDATE_ALL_AXES to use mag for all axes and not only yaw."
|
||||
@@ -77,6 +80,9 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
|
||||
#ifndef AHRS_ACCEL_ZETA
|
||||
#define AHRS_ACCEL_ZETA 0.9
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(AHRS_ACCEL_OMEGA)
|
||||
PRINT_CONFIG_VAR(AHRS_ACCEL_ZETA)
|
||||
|
||||
|
||||
#ifndef AHRS_MAG_OMEGA
|
||||
#define AHRS_MAG_OMEGA 0.04
|
||||
@@ -84,6 +90,8 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
|
||||
#ifndef AHRS_MAG_ZETA
|
||||
#define AHRS_MAG_ZETA 0.9
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(AHRS_MAG_OMEGA)
|
||||
PRINT_CONFIG_VAR(AHRS_MAG_ZETA)
|
||||
|
||||
/** by default use the gravity heursitic to reduce gain */
|
||||
#ifndef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
|
||||
@@ -246,7 +254,7 @@ void ahrs_update_accel(void) {
|
||||
INT32_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
|
||||
|
||||
|
||||
/* FIR filtered pseudo_gravity_measurement */
|
||||
/* FIR filtered pseudo_gravity_measurement TO DO MOVE IN USE HEURISTIC*/
|
||||
#define FIR_FILTER_SIZE 8
|
||||
static struct Int32Vect3 filtered_gravity_measurement = {0, 0, 0};
|
||||
VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1);
|
||||
@@ -254,14 +262,13 @@ void ahrs_update_accel(void) {
|
||||
VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
|
||||
|
||||
|
||||
|
||||
if (ahrs_impl.use_gravity_heuristic) {
|
||||
/* heuristic on acceleration (gravity estimate) norm */
|
||||
/* Factor how strongly to change the weight.
|
||||
* e.g. for WEIGHT_FACTOR 3:
|
||||
* <0.66G = 0, 1G = 1.0, >1.33G = 0
|
||||
*/
|
||||
#define WEIGHT_FACTOR 3
|
||||
#define WEIGHT_FACTOR 5
|
||||
|
||||
struct FloatVect3 g_meas_f;
|
||||
ACCELS_FLOAT_OF_BFP(g_meas_f, filtered_gravity_measurement);
|
||||
@@ -274,15 +281,14 @@ void ahrs_update_accel(void) {
|
||||
* Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
|
||||
* residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
|
||||
* rate_correction FRAC: RATE_FRAC = 12
|
||||
* FRAC_conversion: 2^12 / 2^24 = 1/(2^12) = 1/4096
|
||||
* Feedback_FRAC: 8
|
||||
*
|
||||
* Kp = 1 / inv_rate_scale / FRAC_conversion * feedback_FRAC
|
||||
* inv_rate_scale= 4096 * 8 / (2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY )
|
||||
* inv_rate_scale= 16384 * AHRS_CORRECT_FREQUENCY / (omega * zeta * weight * AHRS_PROPAGATE_FREQUENCY )
|
||||
* FRAC_conversion: 2^12 / 2^24 = 1 / 4096
|
||||
* cross_product_gain : 9.81 m/s2
|
||||
* Kp = 1 / inv_rate_scale / FRAC_conversion * cross_product_gain
|
||||
* inv_rate_scale= 4096 * 9.81 / (2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY )
|
||||
* inv_rate_scale= 2048 * 9.81 * AHRS_CORRECT_FREQUENCY / (omega * zeta * weight * AHRS_PROPAGATE_FREQUENCY )
|
||||
*/
|
||||
|
||||
int32_t inv_rate_scale = 16384 * AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * AHRS_PROPAGATE_FREQUENCY);
|
||||
int32_t inv_rate_scale = 2048 * 9.81 * AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * AHRS_PROPAGATE_FREQUENCY);
|
||||
Bound(inv_rate_scale, 8192, 4194304);
|
||||
ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale;
|
||||
ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale;
|
||||
@@ -295,13 +301,13 @@ void ahrs_update_accel(void) {
|
||||
* residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
|
||||
* high_rez_bias = RATE_FRAC+28 = 40
|
||||
* FRAC_conversion: 2^40 / 2^24 = 2^16
|
||||
* Feedback_FRAC: 8
|
||||
* Ki = 1 / FRAC_conversion / inv_bias_gain * 2^5 * feedback_FRAC
|
||||
* inv_bias_gain = 1/2^16/(omega*omega*weight*weight/AHRS_CORRECT_FREQUENCY) * 2^5 * 8
|
||||
* inv_bias_gain = AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 256)
|
||||
* cross_product_gain : 9.81 m/s2
|
||||
* Ki = 1 / FRAC_conversion / inv_bias_gain * cross_product_gain * 2^5
|
||||
* inv_bias_gain = 9.81 / 2^16 / (omega * omega * weight * weight / AHRS_CORRECT_FREQUENCY) * 2^5
|
||||
* inv_bias_gain = AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 2048)
|
||||
*/
|
||||
|
||||
int32_t inv_bias_gain = AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 256);
|
||||
int32_t inv_bias_gain = 9.81 * AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 2048);
|
||||
Bound(inv_bias_gain, 8, 65536)
|
||||
ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5;
|
||||
ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) << 5;
|
||||
@@ -331,21 +337,21 @@ static inline void ahrs_update_mag_full(void) {
|
||||
|
||||
struct Int32Vect3 residual;
|
||||
INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
|
||||
//INT32_VECT3_RSHIFT(residual,residual,5);
|
||||
|
||||
/* Complementary filter proportionnal gain.
|
||||
* Kp = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
|
||||
* residual FRAC: 2 * MAG_FRAC = 22
|
||||
* rate_correction FRAC: RATE_FRAC = 12
|
||||
* FRAC conversion: 2^12 / 2^22 = 1/1024
|
||||
* Feedback_FRAC: 1/8
|
||||
*
|
||||
* Kp = 1/ inv_rate_gain / FRAC_conversion * Feedback_FRAC
|
||||
* Kp = 1/ inv_rate_gain / FRAC_conversion
|
||||
* inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY
|
||||
* / FRAC_conversion * Feedback_FRAC
|
||||
* inv_rate_gain = 1024 / 8 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY
|
||||
* / FRAC_conversion
|
||||
* inv_rate_gain = 1024 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY
|
||||
*/
|
||||
|
||||
int32_t inv_rate_gain = 64 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY;
|
||||
int32_t inv_rate_gain = 512 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY;
|
||||
|
||||
ahrs_impl.rate_correction.p += residual.x / inv_rate_gain;
|
||||
ahrs_impl.rate_correction.q += residual.y / inv_rate_gain;
|
||||
@@ -357,13 +363,12 @@ static inline void ahrs_update_mag_full(void) {
|
||||
* residual FRAC: 2* MAG_FRAC = 22
|
||||
* high_rez_bias FRAC: RATE_FRAC+28 = 40
|
||||
* FRAC conversion: 2^40 / 2^22 = 2^18
|
||||
* Feedback_FRAC: 1/8
|
||||
*
|
||||
* Ki = bias_gain / FRAC_conversion * Feedback_FRAC = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY
|
||||
* bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion / Feedback_FRAC
|
||||
* bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^21
|
||||
* Ki = bias_gain / FRAC_conversion = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY
|
||||
* bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion
|
||||
* bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^18
|
||||
*/
|
||||
int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2097152;
|
||||
int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * (1 << 18);
|
||||
ahrs_impl.high_rez_bias.p -= residual.x * bias_gain;
|
||||
ahrs_impl.high_rez_bias.q -= residual.y * bias_gain;
|
||||
ahrs_impl.high_rez_bias.r -= residual.z * bias_gain;
|
||||
@@ -376,17 +381,25 @@ static inline void ahrs_update_mag_full(void) {
|
||||
|
||||
static inline void ahrs_update_mag_2d(void) {
|
||||
|
||||
struct Int32Vect2 expected_ltp = {ahrs_impl.mag_h.x, ahrs_impl.mag_h.y};
|
||||
/* normalize expected ltp in 2D (x,y) */
|
||||
INT32_VECT2_NORMALIZE(expected_ltp, INT32_MAG_FRAC);
|
||||
|
||||
struct Int32RMat ltp_to_imu_rmat;
|
||||
INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
|
||||
|
||||
struct Int32Vect3 measured_ltp;
|
||||
INT32_RMAT_TRANSP_VMULT(measured_ltp, ltp_to_imu_rmat, imu.mag);
|
||||
|
||||
/* normalize measured ltp in 2D (x,y) */
|
||||
struct Int32Vect2 measured_ltp_2d = {measured_ltp.x, measured_ltp.y};
|
||||
INT32_VECT2_NORMALIZE(measured_ltp_2d, INT32_MAG_FRAC);
|
||||
|
||||
/* residual_ltp FRAC: 2 * MAG_FRAC - 5 = 17 */
|
||||
struct Int32Vect3 residual_ltp =
|
||||
{ 0,
|
||||
0,
|
||||
(measured_ltp.x * ahrs_impl.mag_h.y - measured_ltp.y * ahrs_impl.mag_h.x)/(1<<5)};
|
||||
(measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x)/(1<<5)};
|
||||
|
||||
|
||||
struct Int32Vect3 residual_imu;
|
||||
@@ -397,19 +410,18 @@ static inline void ahrs_update_mag_2d(void) {
|
||||
* residual_imu FRAC = residual_ltp FRAC = 17
|
||||
* rate_correction FRAC: RATE_FRAC = 12
|
||||
* FRAC conversion: 2^12 / 2^17 = 1/32
|
||||
* Feedback_FRAC: 1/8
|
||||
*
|
||||
* Kp = 1/ inv_rate_gain / FRAC_conversion * 2^5
|
||||
* inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY )* AHRS_MAG_CORRECT_FREQUENCY
|
||||
* / FRAC_conversion * Feedback_FRAC * 2^5
|
||||
* inv_rate_gain = 32 * 32 / 8 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY
|
||||
* inv_rate_gain = 64 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY
|
||||
* / FRAC_conversion * 2^5
|
||||
* inv_rate_gain = 32 * 32 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY
|
||||
* inv_rate_gain = 512 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY
|
||||
*/
|
||||
|
||||
int32_t inv_rate_gain = 64 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY;
|
||||
|
||||
ahrs_impl.rate_correction.p += (residual_imu.x / inv_rate_gain) << 5;
|
||||
ahrs_impl.rate_correction.q += (residual_imu.y / inv_rate_gain) << 5;
|
||||
ahrs_impl.rate_correction.r += (residual_imu.z / inv_rate_gain) << 5;
|
||||
int32_t inv_rate_gain = 16 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY;
|
||||
ahrs_impl.rate_correction.p += (residual_imu.x / inv_rate_gain);
|
||||
ahrs_impl.rate_correction.q += (residual_imu.y / inv_rate_gain);
|
||||
ahrs_impl.rate_correction.r += (residual_imu.z / inv_rate_gain);
|
||||
|
||||
/* Complementary filter integral gain
|
||||
* Correct the gyro bias.
|
||||
@@ -417,18 +429,16 @@ static inline void ahrs_update_mag_2d(void) {
|
||||
* residual_imu FRAC = residual_ltp FRAC = 17
|
||||
* high_rez_bias FRAC: RATE_FRAC+28 = 40
|
||||
* FRAC conversion: 2^40 / 2^17 = 2^23
|
||||
* Feedback_FRAC: 1/8
|
||||
*
|
||||
* Ki = bias_gain / FRAC_conversion * Feedback_frac * 2^5= ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY
|
||||
* bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion/Feedback_frac
|
||||
* bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^21
|
||||
* Ki = bias_gain / FRAC_conversion = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY
|
||||
* bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion
|
||||
* bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^23
|
||||
*/
|
||||
|
||||
int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2097152;
|
||||
ahrs_impl.high_rez_bias.p -= (residual_imu.x * bias_gain) << 5;
|
||||
ahrs_impl.high_rez_bias.q -= (residual_imu.y * bias_gain) << 5;
|
||||
ahrs_impl.high_rez_bias.r -= (residual_imu.z * bias_gain) << 5;
|
||||
|
||||
int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * (1 << 23);
|
||||
ahrs_impl.high_rez_bias.p -= (residual_imu.x * bias_gain);
|
||||
ahrs_impl.high_rez_bias.q -= (residual_imu.y * bias_gain);
|
||||
ahrs_impl.high_rez_bias.r -= (residual_imu.z * bias_gain);
|
||||
|
||||
INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user