diff --git a/quad/src/quad_app/biquad_filter.c b/quad/src/quad_app/biquad_filter.c
new file mode 100644
index 0000000000000000000000000000000000000000..3fb3327e4106aea63f68b1f017508b20879337e3
--- /dev/null
+++ b/quad/src/quad_app/biquad_filter.c
@@ -0,0 +1,29 @@
+#include "biquad_filter.h"
+
+struct biquadState filter_make_state(float a0, float a1, float a2,
+ float b1, float b2) {
+ struct biquadState state = {
+ .delayed = {0,0},
+ .a0 = a0,
+ .a1 = a1,
+ .a2 = a2,
+ .b1 = b1,
+ .b2 = b2
+ };
+ return state;
+}
+
+// http://www.earlevel.com/main/2003/02/28/biquads/
+// Direct form II
+float biquad_execute(struct biquadState* state, float new_input) {
+ float left_sum = new_input -
+ (state->delayed[0] * state->b1) -
+ (state->delayed[1] * state->b2);
+ float output = (left_sum * state->a0) +
+ (state->delayed[0] * state->a1) +
+ (state->delayed[1] * state->a2);
+
+ state->delayed[1] = state->delayed[0];
+ state->delayed[0] = left_sum;
+ return output;
+}
\ No newline at end of file
diff --git a/quad/src/quad_app/biquad_filter.h b/quad/src/quad_app/biquad_filter.h
new file mode 100644
index 0000000000000000000000000000000000000000..a9076573883767901440683df9fe1c45abd52ae7
--- /dev/null
+++ b/quad/src/quad_app/biquad_filter.h
@@ -0,0 +1,14 @@
+#ifndef BIQUAD_FILTER_H
+#define BIQUAD_FILTER_H
+
+struct biquadState {
+ float delayed[2];
+ float a0, a1, a2, b1, b2;
+};
+
+float biquad_execute(struct biquadState* state, float new_input);
+
+
+struct biquadState filter_make_state(float a0, float a1, float a2,
+ float b1, float b2);
+#endif // BIQUAD_FILTER_H
\ No newline at end of file
diff --git a/quad/src/quad_app/iic_utils.c b/quad/src/quad_app/iic_utils.c
index a0061e6b00432768feded1b15495c850fad5a914..2f3875a0f035a9c1f2f98f4251d07fed6e4fc329 100644
--- a/quad/src/quad_app/iic_utils.c
+++ b/quad/src/quad_app/iic_utils.c
@@ -182,10 +182,6 @@ int iic0_mpu9150_read_gam(gam_t* gam) {
gam->accel_y = (raw_accel_y / 4096.0) + ACCEL_Y_BIAS;
gam->accel_z = (raw_accel_z / 4096.0) + ACCEL_Z_BIAS;
- //Get X and Y angles
- // javey: this assigns accel_(pitch/roll) in units of radians
- gam->accel_pitch = atan(gam->accel_x / sqrt(gam->accel_y*gam->accel_y + gam->accel_z*gam->accel_z));
- gam->accel_roll = -atan(gam->accel_y / sqrt(gam->accel_x*gam->accel_x + gam->accel_z*gam->accel_z)); // negative because sensor board is upside down
//Convert gyro data to rate (we're only using the most 12 significant bits)
gyro_x = (sensor_data[GYR_X_H] << 8) | (sensor_data[GYR_X_L]); //* G_GAIN;
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index d7bd7b707ac23b2b87e8c5aab8029840dce80b31..dee473219dad3fff0beacfd192de2a99526a6a1e 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -14,8 +14,33 @@
#include "timer.h"
#include <math.h>
+#define ALPHA 0.98
+
+int sensor_processing_init(sensor_t* sensor_struct) {
+ float a0 = 0.0200833310260;
+ float a1 = 0.0401666620520;
+ float a2 = 0.0200833310260;
+ float b1 = -1.561015391253;
+ float b2 = 0.6413487153577;
+ sensor_struct->accel_x_filt = filter_make_state(a0, a1, a2, b1, b2);
+ sensor_struct->accel_y_filt = filter_make_state(a0, a1, a2, b1, b2);
+ sensor_struct->accel_z_filt = filter_make_state(a0, a1, a2, b1, b2);
+ return 0;
+}
+
int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_sensor_t* raw_sensor_struct, sensor_t* sensor_struct)
{
+ // Filter accelerometer values
+ gam_t* gam = &(raw_sensor_struct->gam);
+ float accel_x = biquad_execute(&sensor_struct->accel_x_filt, gam->accel_x);
+ float accel_y = biquad_execute(&sensor_struct->accel_y_filt, gam->accel_y);
+ float accel_z = biquad_execute(&sensor_struct->accel_z_filt, gam->accel_z);
+ //Get X and Y angles
+ // javey: this assigns accel_(pitch/roll) in units of radians
+ float accel_pitch = atan(accel_x / sqrt(accel_y*accel_y + accel_z*accel_z));
+ float accel_roll = -atan(accel_y / sqrt(accel_x*accel_x + accel_z*accel_z)); // negative because sensor board is upside down
+
+
// copy currentQuadPosition and trimmedRCValues from raw_sensor_struct to sensor_struct
deep_copy_Qpos(&(sensor_struct->currentQuadPosition), &(raw_sensor_struct->currentQuadPosition));
@@ -85,11 +110,11 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
sensor_struct->gyr_z = raw_sensor_struct->gam.gyro_zVel_r;
// Complementary Filter Calculations
- sensor_struct->pitch_angle_filtered = 0.98 * (sensor_struct->pitch_angle_filtered + sensor_struct->theta_dot * get_last_loop_time())
- + 0.02 * raw_sensor_struct->gam.accel_pitch;
+ sensor_struct->pitch_angle_filtered = ALPHA * (sensor_struct->pitch_angle_filtered + sensor_struct->theta_dot * get_last_loop_time())
+ + (1. - ALPHA) * accel_pitch;
- sensor_struct->roll_angle_filtered = 0.98 * (sensor_struct->roll_angle_filtered + sensor_struct->phi_dot* get_last_loop_time())
- + 0.02 * raw_sensor_struct->gam.accel_roll;
+ sensor_struct->roll_angle_filtered = ALPHA * (sensor_struct->roll_angle_filtered + sensor_struct->phi_dot* get_last_loop_time())
+ + (1. - ALPHA) * accel_roll;
// static int loop_counter = 0;
// loop_counter++;
diff --git a/quad/src/quad_app/sensor_processing.h b/quad/src/quad_app/sensor_processing.h
index 04b5efebe48ab20bdecfcfaab11e1ffe2f63a540..52b626eafa173ac5c806ad29d52ca03138017060 100644
--- a/quad/src/quad_app/sensor_processing.h
+++ b/quad/src/quad_app/sensor_processing.h
@@ -77,6 +77,7 @@
*/
int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_sensor_t * raw_sensor_struct, sensor_t* sensor_struct);
+int sensor_processing_init(sensor_t* sensor_struct);
void deep_copy_Qpos(quadPosition_t * dest, quadPosition_t * src);
void set_pitch_angle_filtered(sensor_t * sensor_struct, float accel_roll);
void set_roll_angle_filtered(sensor_t * sensor_struct, float accel_roll);
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index c32b202fa201762212adc068a3cee21e43c109f4..a268af2ca6e796a3ddb171c74d7d0a3ca6130d30 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -12,6 +12,7 @@
#include "commands.h"
#include "computation_graph.h"
#include "hw_iface.h"
+#include "biquad_filter.h"
typedef unsigned char u8;
typedef unsigned short u16;
@@ -278,6 +279,10 @@ typedef struct sensor {
quadPosition_t currentQuadPosition;
quadTrims_t trims;
+ struct biquadState accel_x_filt;
+ struct biquadState accel_y_filt;
+ struct biquadState accel_z_filt;
+
} sensor_t;
/**