Added low-pass filter at 10hz to accelerometer

quad/src/quad_app/biquad_filter.c

+#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

quad/src/quad_app/biquad_filter.h

+#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

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;

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++;

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);

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;
 
 /**