diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 3002cfc2697bc760dd657e4f7d6fa11df0264577..0391ca2dd23cca86ae5b5c0fd9191da3e28eaca1 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -59,6 +59,10 @@ int control_algorithm_init(parameter_t * ps)
     ps->cur_pitch = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch"); // ID 15
     ps->cur_roll = graph_add_defined_block(graph, BLOCK_CONSTANT, "Roll");
     ps->cur_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "Yaw");
+    // Sensor trims
+    ps->pitch_trim = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch trim");
+    ps->pitch_trim_add = graph_add_defined_block(graph, BLOCK_ADD, "Pitch trim add");
+
 	// Yaw angular velocity PID
     // theta_dot is the angular velocity about the y-axis
     // phi_dot is the angular velocity about the x-axis
@@ -90,10 +94,14 @@ int control_algorithm_init(parameter_t * ps)
     ps->pos_time = graph_add_defined_block(graph, BLOCK_CONSTANT, "Ts_VRPN");
 
     // Connect pitch PID chain
+    // Trims
+    graph_set_source(graph, ps->pitch_trim_add, ADD_SUMMAND1, ps->pitch_trim, CONST_VAL);
+    graph_set_source(graph, ps->pitch_trim_add, ADD_SUMMAND2, ps->cur_pitch, CONST_VAL);
+    // Controllers
     graph_set_source(graph, ps->pitch_r_pid, PID_SETPOINT, ps->pitch_pid, PID_CORRECTION);
     graph_set_source(graph, ps->pitch_r_pid, PID_CUR_POINT, ps->theta_dot, CONST_VAL);
     graph_set_source(graph, ps->pitch_r_pid, PID_DT, ps->angle_time, CONST_VAL);
-    graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->cur_pitch, CONST_VAL);
+    graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->pitch_trim_add, ADD_SUM);
     //graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->vrpn_pitch, CONST_VAL);
     graph_set_source(graph, ps->pitch_pid, PID_DT, ps->angle_time, CONST_VAL);
 
@@ -187,6 +195,8 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MIN, -PWM_DIFF_BOUNDS);
     graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MAX, PWM_DIFF_BOUNDS);
 
+    // Set trims
+    graph_set_param_val(graph, ps->pitch_trim, CONST_SET, 0.02);
 
     // Initial value for sampling periods
     graph_set_param_val(graph, ps->pos_time, CONST_SET, 0.04);
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index 53af7f2f873cd25c07ac4f25ac6127e33a35c768..c32b202fa201762212adc068a3cee21e43c109f4 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -341,6 +341,8 @@ typedef struct parameter_t {
 	// "trim" for autonomous
 	int throttle_trim;
 	int throttle_trim_add;
+	int pitch_trim;
+	int pitch_trim_add;
 } parameter_t;
 
 /**