Skip to content

Commit

Permalink
[imu] Fix accelerometer and gyroscope filters (#3170)
Browse files Browse the repository at this point in the history
  • Loading branch information
@fvantienen
fvantienen committed Nov 10, 2023
1 parent 90e1d71 commit 8f507c3
Show file tree
Hide file tree
Showing 3 changed files with 16 additions and 21 deletions.
7 changes: 4 additions & 3 deletions conf/airframes/tudelft/rot_wing_25kg.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -257,9 +257,10 @@
</section>

<section name="IMU" prefix="IMU_">
<define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true},.neutral={-3,3,13}, .scale={{15146,60067,2567},{1551,6124,263}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-26,9,2}, .scale={{32863,18346,55169},{6721,3745,11294}}}}"/>
<define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true},.neutral={-96,-95,-292}, .scale={{33136,8709,8764},{59233,15343,15761}}}}"/> <!-- Calibrated 't Harde 17-08-2023 -->

<define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true, .filter=true},.neutral={-3,3,13}, .scale={{15146,60067,2567},{1551,6124,263}}, .filter_sample_freq=1138.7, .filter_freq=30}, {.abi_id=22, .calibrated={.neutral=true, .scale=true, .filter=true},.neutral={-26,9,2}, .scale={{32863,18346,55169},{6721,3745,11294}}, .filter_sample_freq=1139.3, .filter_freq=30}}"/>
<define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true},.neutral={-96,-95,-292}, .scale={{33136,8709,8764},{59233,15343,15761}}}}"/> <!-- Calibrated 't Harde 17-08-2023 -->
<define name="GYRO_CALIB" value="{{.abi_id=20, .calibrated={.filter=true}, .filter_sample_freq=1138.7, .filter_freq=30}, {.abi_id=22, .calibrated={.filter=true}, .filter_sample_freq=1139.3, .filter_freq=30}}"/>

<!-- Define axis in hover frame -->
<define name="BODY_TO_IMU_PHI" value="180." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
Expand Down
26 changes: 10 additions & 16 deletions sw/airborne/modules/imu/imu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -389,7 +389,7 @@ void imu_init(void)
float tau = 1.0 / (2.0 * M_PI * imu.gyros[i].filter_freq);
float sample_time = 1 / imu.gyros[i].filter_sample_freq;
for(uint8_t j = 0; j < 3; j++)
init_butterworth_2_low_pass_int(&imu.gyros[i].filter[j], tau, sample_time, 0.0);
init_butterworth_2_low_pass(&imu.gyros[i].filter[j], tau, sample_time, 0.0);
}

/* Copy accel calibration if needed */
Expand Down Expand Up @@ -421,7 +421,7 @@ void imu_init(void)
float tau = 1.0 / (2.0 * M_PI * imu.accels[i].filter_freq);
float sample_time = 1 / imu.accels[i].filter_sample_freq;
for(uint8_t j = 0; j < 3; j++)
init_butterworth_2_low_pass_int(&imu.accels[i].filter[j], tau, sample_time, 0.0);
init_butterworth_2_low_pass(&imu.accels[i].filter[j], tau, sample_time, 0.0);
}

/* Copy mag calibrated if needed */
Expand Down Expand Up @@ -576,15 +576,12 @@ static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates
return;

// Filter the gyro
struct Int32Rates data_filtered[samples];
if(gyro->calibrated.filter) {
struct Int32Rates data_filtered[samples];
for(uint8_t i = 0; i < samples; i++) {
update_butterworth_2_low_pass_int(&gyro->filter[0], data[i].p);
data_filtered[i].p = get_butterworth_2_low_pass_int(&gyro->filter[0]);
update_butterworth_2_low_pass_int(&gyro->filter[1], data[i].q);
data_filtered[i].q = get_butterworth_2_low_pass_int(&gyro->filter[1]);
update_butterworth_2_low_pass_int(&gyro->filter[2], data[i].r);
data_filtered[i].r = get_butterworth_2_low_pass_int(&gyro->filter[2]);
data_filtered[i].p = update_butterworth_2_low_pass(&gyro->filter[0], data[i].p);
data_filtered[i].q = update_butterworth_2_low_pass(&gyro->filter[1], data[i].q);
data_filtered[i].r = update_butterworth_2_low_pass(&gyro->filter[2], data[i].r);
}
data = data_filtered;
}
Expand Down Expand Up @@ -673,15 +670,12 @@ static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect
return;

// Filter the accel
struct Int32Vect3 data_filtered[samples];
if(accel->calibrated.filter) {
struct Int32Vect3 data_filtered[samples];
for(uint8_t i = 0; i < samples; i++) {
update_butterworth_2_low_pass_int(&accel->filter[0], data[i].x);
data_filtered[i].x = get_butterworth_2_low_pass_int(&accel->filter[0]);
update_butterworth_2_low_pass_int(&accel->filter[1], data[i].y);
data_filtered[i].y = get_butterworth_2_low_pass_int(&accel->filter[1]);
update_butterworth_2_low_pass_int(&accel->filter[2], data[i].z);
data_filtered[i].z = get_butterworth_2_low_pass_int(&accel->filter[2]);
data_filtered[i].x = update_butterworth_2_low_pass(&accel->filter[0], data[i].x);
data_filtered[i].y = update_butterworth_2_low_pass(&accel->filter[1], data[i].y);
data_filtered[i].z = update_butterworth_2_low_pass(&accel->filter[2], data[i].z);
}
data = data_filtered;
}
Expand Down
4 changes: 2 additions & 2 deletions sw/airborne/modules/imu/imu.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ struct imu_gyro_t {
struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame (body to imu combined with imu to sensor)
float filter_freq; ///< Filter frequency
float filter_sample_freq; ///< Lowpass filter sample frequency (Hz)
Butterworth2LowPass_int filter[3]; ///< Lowpass filter optional
Butterworth2LowPass filter[3]; ///< Lowpass filter optional
};

struct imu_accel_t {
Expand All @@ -73,7 +73,7 @@ struct imu_accel_t {
struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame (body to imu combined with imu to sensor)
float filter_freq; ///< Lowpass filter frequency (Hz)
float filter_sample_freq; ///< Lowpass filter sample frequency (Hz)
Butterworth2LowPass_int filter[3]; ///< Lowpass filter optional
Butterworth2LowPass filter[3]; ///< Lowpass filter optional
};

struct imu_mag_t {
Expand Down

0 comments on commit 8f507c3

Please sign in to comment.