diff --git a/src/control.c b/src/control.c
index b006013..432f095 100644
--- a/src/control.c
+++ b/src/control.c
@@ -25,12 +25,14 @@ void control_cycle(measured_state_t *measured_state,
   static pid_state_t yaw_pid_state = INITIAL_PID_STATE;
   float z_factor = pid_cycle(
     desired_state->z_vel,  // Desired Z acceleration.
-    measured_state->z_accel, // Actual (measured) Z acceleration.
+    measured_state->z_vel, // Actual (measured) Z acceleration.
     &z_pid_state,          // Structure in which to hold the PID controller's state.
     dt_float,              // The time that has passed since the last control cycle.
     PID_GAIN_Z_P,          // Proportional gain constant.
     PID_GAIN_Z_I,          // Integral gain constant.
-    PID_GAIN_Z_D);         // Derivative gain constant.
+    PID_GAIN_Z_D,          // Derivative gain constant.
+    MIN_INTEGRAL_Z,
+    MAX_INTEGRAL_Z);
   float roll_factor = pid_cycle(
     desired_state->roll,
     measured_state->roll,
@@ -38,7 +40,9 @@ void control_cycle(measured_state_t *measured_state,
     dt_float,
     PID_GAIN_ROLL_P,
     PID_GAIN_ROLL_I,
-    PID_GAIN_ROLL_D);
+    PID_GAIN_ROLL_D,
+    MIN_INTEGRAL_ROLL,
+    MAX_INTEGRAL_ROLL);
   float pitch_factor = pid_cycle(
     desired_state->pitch,
     measured_state->pitch,
@@ -46,7 +50,9 @@ void control_cycle(measured_state_t *measured_state,
     dt_float,
     PID_GAIN_PITCH_P,
     PID_GAIN_PITCH_I,
-    PID_GAIN_PITCH_D);
+    PID_GAIN_PITCH_D,
+    MIN_INTEGRAL_PITCH,
+    MAX_INTEGRAL_PITCH);
   float yaw_factor = pid_cycle(
     desired_state->yaw_vel,
     measured_state->yaw_vel,
@@ -54,18 +60,24 @@ void control_cycle(measured_state_t *measured_state,
     dt_float,
     PID_GAIN_YAW_P,
     PID_GAIN_YAW_I,
-    PID_GAIN_YAW_D);
+    PID_GAIN_YAW_D,
+    MIN_INTEGRAL_YAW,
+    MAX_INTEGRAL_YAW);
 
-  z_factor = 0.45;
+  static float t = 0.0;
+  z_factor = -0.3;
   roll_factor = 0.0;
   pitch_factor = 0.0;
   yaw_factor = 0.0;
+  t += 0.00001;
 
-  printf("LOG %f,%f,%f\n", dt_float, measured_state->z_accel, z_factor);
+  printf("LOG %f,%f,%f\n", dt_float, z_factor, measured_state->z_vel);
+
+  //printf("z_vel error=%f-%f zfactor=%f\n", desired_state->z_vel, measured_state->z_vel, z_factor);
 
   // Combine contributions to produce rotor speeds.
-  rotor_speeds->a = z_factor + roll_factor - pitch_factor + yaw_factor;
-  rotor_speeds->b = z_factor - roll_factor - pitch_factor - yaw_factor;
-  rotor_speeds->c = z_factor - roll_factor + pitch_factor + yaw_factor;
-  rotor_speeds->d = z_factor + roll_factor + pitch_factor - yaw_factor;
+  rotor_speeds->a = 0.4 + z_factor + roll_factor - pitch_factor + yaw_factor;
+  rotor_speeds->b = 0.4 + z_factor - roll_factor - pitch_factor - yaw_factor;
+  rotor_speeds->c = 0.4 + z_factor - roll_factor + pitch_factor + yaw_factor;
+  rotor_speeds->d = 0.4 + z_factor + roll_factor + pitch_factor - yaw_factor;
 }
diff --git a/src/escs.c b/src/escs.c
index 265abea..6616b83 100644
--- a/src/escs.c
+++ b/src/escs.c
@@ -1,3 +1,5 @@
+#include <stdio.h>
+
 #include <avr/interrupt.h>
 #include <avr/io.h>
 
@@ -130,4 +132,6 @@ void escs_update(rotor_speeds_t *rotor_speeds) {
   escs_levels.b = level_b;
   escs_levels.c = level_c;
   escs_levels.d = level_d;
+
+  //printf("a: raw=%f, filtered=%f, level=%d\n", rotor_speeds->a, filtered_a, level_a);
 }
diff --git a/src/imu.c b/src/imu.c
index 4b3ebde..85fcbc1 100644
--- a/src/imu.c
+++ b/src/imu.c
@@ -23,6 +23,8 @@ float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0};
 uint16_t accel_raw[3]; //possibly can be deined in the imu_read function
 uint16_t gyro_raw[3];
 
+float vert_accel_offset = 0.0;
+
 // Variables
 //float pitch = 0, roll = 0, pitch_accel = 0, roll_accel = 0;
 
@@ -148,6 +150,8 @@ static void read_raw_gyro(uint16_t *gyro){
 	gyro[0] = (((uint16_t)raw_gyro[0]<<8)|raw_gyro[1]);
 	gyro[1] = (((uint16_t)raw_gyro[2]<<8)|raw_gyro[3]);
 	gyro[2] = (((uint16_t)raw_gyro[4]<<8)|raw_gyro[5]);
+
+	gyro[0] = -gyro[0];
 }
 
 static void read_raw_accel(uint16_t *accel){
@@ -158,6 +162,7 @@ static void read_raw_accel(uint16_t *accel){
 	accel[1]= (((uint16_t)raw_accel[2]<<8)|raw_accel[3]);
 	accel[2] = (((uint16_t)raw_accel[4]<<8)|raw_accel[5]);
 
+	accel[0] = -accel[0];
 }
 
 // void calibrate_IMU(float * gyroBias, float * accelBias)
@@ -319,6 +324,30 @@ static void reset_IMU(){
 	_delay_ms(100);// Delay 100 ms
 }
 
+static float get_vert_accel(float accel_x, float accel_y, float accel_z) {
+	// Vertical component of acceleration.
+	float vert_accel = 2*(q1*q3-q2*q0)*accel_x + 2*(q2*q3+q1*q0)*accel_y + (1-2*q1*q1-2*q2*q2)*accel_z;
+	// Correct for gravity.
+	vert_accel -= 1.0;
+	// Convert from g to m/s^2.
+	vert_accel *= 9.81;
+	vert_accel -= vert_accel_offset;
+
+	// Lowpass:
+	// static float prev_vert_accel = 0.0;
+	// vert_accel = 0.95*prev_vert_accel + 0.05*vert_accel;
+
+	// Highpass:
+	static float prev_vert_accel_i = 0.0;
+	static float prev_vert_accel_o = 0.0;
+	float vert_accel_o = 0.5 * (prev_vert_accel_o + vert_accel - prev_vert_accel_i);
+	prev_vert_accel_i = vert_accel;
+	prev_vert_accel_o = vert_accel_o;
+	vert_accel = vert_accel_o;
+
+	return vert_accel;
+}
+
 void imu_init(){
 
 	//initialize I2C
@@ -371,6 +400,19 @@ void imu_init(){
 	//write_IMU_byte(MPU9250_DEFAULT_ADDRESS, 0x38, 0x01);  // Enable data ready (bit 0) interrupt
 	//_delay_ms(100);
 
+	const int n = 128;
+	float total = 0.0;
+	int i;
+	for (i = 0; i < n; i++) {
+		read_raw_accel(&accel_raw[0]);
+		float accel_x = (int)accel_raw[0]/(float)ACCEL_SENSITIVITY;//-accelBias[0];
+		float accel_y = (int)accel_raw[1]/(float)ACCEL_SENSITIVITY;//-accelBias[1];
+		float accel_z = (int)accel_raw[2]/(float)ACCEL_SENSITIVITY;//-accelBias[2];
+		total += get_vert_accel(accel_x, accel_y, accel_z);
+	}
+	vert_accel_offset = total / ((float) n);
+
+	printf("offset=%f\n", vert_accel_offset);
 }
 // This function has to be executed at 250Hz frequeny (every 4ms)
 void imu_read(measured_state_t *destination){
@@ -410,7 +452,10 @@ void imu_read(measured_state_t *destination){
 
 	static clock_time_t prev_time = 0;
 	clock_time_t curr_time = clock_get_time();
-	float dt = ((float) clock_diff(prev_time, curr_time)) * ((float) SECONDS_PER_CLOCK_TICK);
+	float dt = 0.0;
+	if (prev_time != 0) {
+		dt = ((float) clock_diff(prev_time, curr_time)) * ((float) SECONDS_PER_CLOCK_TICK);
+	}
 	prev_time = curr_time;
 
 	MadgwickAHRSupdateIMU(gyro_x, gyro_y, gyro_z, accel_x, accel_y, accel_z, dt);
@@ -419,15 +464,11 @@ void imu_read(measured_state_t *destination){
 	destination->pitch = -atan2f(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2));
 	destination->yaw_vel = gyro_z;
 
-	// Vertical component of acceleration.
-	float vert_accel = 2*(q1*q3-q2*q0)*accel_x + 2*(q2*q3+q1*q0)*accel_y + (1-2*q1*q1-2*q2*q2)*accel_z;
-	// Correct for gravity.
-	vert_accel -= 1.0;
-	// Convert from g to m/s^2.
-	vert_accel *= 9.81;
-	// Integrate to get vertical velocity.
-	// static float vert_vel = 0.0;
-	// vert_vel += vert_accel * dt;
-	destination->z_accel = vert_accel;
+	static float z_vel = 0.0;
+	float z_accel = get_vert_accel(accel_x, accel_y, accel_z);
+	z_vel += dt * z_accel * 50.0;
+	destination->z_vel = z_vel;
+
+	printf("z_accel=%f z_vel=%f\n", z_accel, z_vel);
 }
 
diff --git a/src/imu_interface.h b/src/imu_interface.h
index 8d676d2..ca0cf49 100644
--- a/src/imu_interface.h
+++ b/src/imu_interface.h
@@ -18,7 +18,7 @@
 typedef struct {
   // Acceleration along the world frame Z axis. Positive indicates upwards (away
   // from the ground). Units are metres/second.
-  float z_accel;
+  float z_vel;
 
   // Roll angle (rotation about the body frame X axis). Positive indicates
   // rolling to the right (the right-hand side of the vehicle is lower than the
diff --git a/src/main.c b/src/main.c
index dde1093..bee7952 100644
--- a/src/main.c
+++ b/src/main.c
@@ -19,10 +19,10 @@ static void convert_logg_packet(measured_state_t* measured_state){
 
 static void convert_rc_packet(desired_state_t *desired_state) {
   // TODO: fix this conversion!
-  desired_state->z_vel = rc_data_packet.channel_0;
-  desired_state->roll = rc_data_packet.channel_1;
-  desired_state->pitch = rc_data_packet.channel_2;
-  desired_state->yaw_vel = rc_data_packet.channel_3;
+  desired_state->z_vel = ((float) rc_data_packet.channel_0) / 128.0 - 1.0;
+  desired_state->roll = ((float) rc_data_packet.channel_1) / 128.0 - 1.0;
+  desired_state->pitch = ((float) rc_data_packet.channel_2) / 128.0 - 1.0;
+  desired_state->yaw_vel = ((float) rc_data_packet.channel_3) / 128.0 - 1.0;
 }
 
 int main() {
@@ -69,7 +69,7 @@ int main() {
   while (1) {
     // Communicate with IMU.
     imu_read(&measured_state);
-    //convert_logg_packet(&measured_state);
+    convert_logg_packet(&measured_state);
 
     // Run control algorithm.
     //convert_rc_packet(&desired_state);
diff --git a/src/pid.h b/src/pid.h
index 4c1de9b..184bb71 100644
--- a/src/pid.h
+++ b/src/pid.h
@@ -9,6 +9,16 @@ typedef struct {
 // Initial value for variables of type pid_state_t.
 #define INITIAL_PID_STATE {0.0, 0.0}
 
+static float clamp(float x, float min, float max) {
+  if (x < min) {
+    return min;
+  }
+  if (x > max) {
+    return max;
+  }
+  return x;
+}
+
 // This is an inline function so that accesses to 'state', which will usually be
 // a static (i.e. global) variable, can be changed from indirect to direct
 // addressing, which is much faster.
@@ -19,13 +29,15 @@ static inline float pid_cycle(
   const float dt,
   const float p_gain,
   const float i_gain,
-  const float d_gain) {
+  const float d_gain,
+  const float min_integral,
+  const float max_integral) {
 
   // Calculate error.
   const float error = desired - measured;
 
   // Calculate integral of error.
-  const float integral = state->integral + error * dt;
+  const float integral = clamp(state->integral + error * dt, min_integral, max_integral);
   state->integral = integral;
 
   // Calculate derivative of error.
diff --git a/src/settings.h b/src/settings.h
index ff04b60..27eec37 100644
--- a/src/settings.h
+++ b/src/settings.h
@@ -16,24 +16,42 @@
 #define PWM_TICKS_PER_MS (1000 / US_PER_PWM_TICK)
 
 // Z-factor PID parameters.
-#define PID_GAIN_Z_P 0
-#define PID_GAIN_Z_I 0
-#define PID_GAIN_Z_D 0
+// Iteration 1: 0.002548/0.0004406/0.003685 (PID disabled)
+// Iteration 2: 1.07/21.38/0.0005811 (PID disabled)
+// Iteration 3: -0.05018/-0.0364/0
+// Iteration 4: 1.013/192.4/6.746e-5 (bad)
+// Start again
+// Iteration 1 (z3-2.txt): 0.1506/0.03667/0.1546
+#define PID_GAIN_Z_P 0.1506
+#define PID_GAIN_Z_I 0.03667
+#define PID_GAIN_Z_D 0.1546
+#define MIN_INTEGRAL_Z -1.0
+#define MAX_INTEGRAL_Z 1.0
 
 // Roll-factor PID parameters.
-#define PID_GAIN_ROLL_P 0
-#define PID_GAIN_ROLL_I 0
-#define PID_GAIN_ROLL_D 0
+#define PID_GAIN_ROLL_P 0.0
+#define PID_GAIN_ROLL_I 0.0
+#define PID_GAIN_ROLL_D 0.0
+#define MIN_INTEGRAL_ROLL -1.0
+#define MAX_INTEGRAL_ROLL 1.0
 
 // Pitch-factor PID parameters.
-#define PID_GAIN_PITCH_P 0
-#define PID_GAIN_PITCH_I 0
-#define PID_GAIN_PITCH_D 0
+// Iteration 1: 0.01338/0.005628/0.007947
+// Iteration 2: -0.4264/-2.777/-0.0006686
+// Fixed axis
+// Iteration 3: 4.82/545.7/0.0003971
+#define PID_GAIN_PITCH_P 0.0
+#define PID_GAIN_PITCH_I 0.0
+#define PID_GAIN_PITCH_D 0.0
+#define MIN_INTEGRAL_PITCH (-0.3/PID_GAIN_PITCH_I)
+#define MAX_INTEGRAL_PITCH (-0.3/PID_GAIN_PITCH_I)
 
 // Yaw-factor PID parameters.
-#define PID_GAIN_YAW_P 0
-#define PID_GAIN_YAW_I 0
-#define PID_GAIN_YAW_D 0
+#define PID_GAIN_YAW_P 0.0
+#define PID_GAIN_YAW_I 0.0
+#define PID_GAIN_YAW_D 0.0
+#define MIN_INTEGRAL_YAW -1.0
+#define MAX_INTEGRAL_YAW 1.0
 
 // Accelerometer/gyroscope "who am I" ID.
 #define IMU_ID 0x71 // Grove IMU
@@ -45,13 +63,13 @@
 
 // Allowable limits for rotor speeds (0.0 = 1ms pulse, 1.0 = 2ms pulse).
 #define MIN_ROTOR_SPEED 0.0
-#define MAX_ROTOR_SPEED 0.7
+#define MAX_ROTOR_SPEED 0.6
 
 // Rotor speeds are filtered to reduce transients. The higher this parameter is,
 // the more filtering is applied (but the slower the rotors are to respond to
 // changes).
 // Range: 0.0 to 1.0
-#define ROTOR_SPEED_FILTERING 0.9
+#define ROTOR_SPEED_FILTERING 0.98
 
 // Specification of which pins on the microcontroller are connected to the PWM
 // inputs on the ESCS.