Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

noise filter and threshold for cmpl euler filter

Browse Source
This commit is contained in:
Gautier Hattenberger
2011-09-05 11:07:13 +02:00
parent d61acb3c1a
commit ca92bf002b
1 changed files with 58 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
+58 -4
View File
@@ -95,7 +95,41 @@ void ahrs_align(void) {
}
//#define USE_NOISE_CUT 1
//#define USE_NOISE_FILTER 1
//#define NOISE_FILTER_GAIN 50
#ifdef USE_NOISE_CUT
#include "led.h"
static inline bool_t cut_rates (struct Int32Rates i1, struct Int32Rates i2, int32_t threshold) {
struct Int32Rates diff;
RATES_DIFF(diff, i1, i2);
if (diff.p < -threshold || diff.p > threshold ||
diff.q < -threshold || diff.q > threshold ||
diff.r < -threshold || diff.r > threshold) {
return TRUE;
} else {
return FALSE;
}
}
#define RATE_CUT_THRESHOLD RATE_BFP_OF_REAL(1)
static inline bool_t cut_accel (struct Int32Vect3 i1, struct Int32Vect3 i2, int32_t threshold) {
struct Int32Vect3 diff;
VECT3_DIFF(diff, i1, i2);
if (diff.x < -threshold || diff.x > threshold ||
diff.y < -threshold || diff.y > threshold ||
diff.z < -threshold || diff.z > threshold) {
LED_ON(4);
return TRUE;
} else {
LED_OFF(4);
return FALSE;
}
}
#define ACCEL_CUT_THRESHOLD ACCEL_BFP_OF_REAL(20)
#endif
/*
*
@@ -116,9 +150,17 @@ void ahrs_propagate(void) {
/* unbias gyro */
struct Int32Rates uf_rate;
RATES_DIFF(uf_rate, imu.gyro, ahrs_impl.gyro_bias);
/* low pass rate */
RATES_ADD(ahrs.imu_rate, uf_rate);
RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2);
#ifdef USE_NOISE_CUT
static struct Int32Rates last_uf_rate = { 0, 0, 0 };
if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) {
#endif
/* low pass rate */
RATES_ADD(ahrs.imu_rate, uf_rate);
RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2);
#ifdef USE_NOISE_CUT
}
RATES_COPY(last_uf_rate, uf_rate);
#endif
/* integrate eulers */
struct Int32Eulers euler_dot;
@@ -152,7 +194,19 @@ void ahrs_propagate(void) {
void ahrs_update_accel(void) {
get_phi_theta_measurement_fom_accel(&ahrs_impl.measurement.phi, &ahrs_impl.measurement.theta, imu.accel);
#ifdef USE_NOISE_CUT
static struct Int32Vect3 last_accel = { 0, 0, 0 };
if (!cut_accel(imu.accel, last_accel, ACCEL_CUT_THRESHOLD)) {
#endif
#ifdef USE_NOISE_FILTER
VECT3_SUM_SCALED(imu.accel, imu.accel, last_accel, NOISE_FILTER_GAIN);
VECT3_SDIV(imu.accel, imu.accel, NOISE_FILTER_GAIN+1);
#endif
get_phi_theta_measurement_fom_accel(&ahrs_impl.measurement.phi, &ahrs_impl.measurement.theta, imu.accel);
#ifdef USE_NOISE_CUT
}
VECT3_COPY(last_accel, imu.accel);
#endif
}