diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 116401c76cefabfedeac5f248d59c223eb899413..7c1af8196811cdb7eaf7bfa4203306401fcb0a0a 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -131,7 +131,7 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->vrpn_y, CONST_VAL);
     graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);
     // Alt autonomous
-    graph_set_source(graph, ps->alt_pid, PID_DT, ps->pos_time, CONST_VAL);
+    graph_set_source(graph, ps->alt_pid, PID_DT, ps->angle_time, CONST_VAL);
     graph_set_source(graph, ps->alt_pid, PID_CUR_POINT, ps->lidar, CONST_VAL);
     graph_set_source(graph, ps->alt_pid, PID_SETPOINT, ps->alt_set, CONST_VAL);
     graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND1, ps->alt_pid, PID_CORRECTION);
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index e67a075394ee6099d89f0c326a07f656a72694ee..5714d32a2e3c41e2ca1dbd7db5bf2217a190132a 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -113,6 +113,7 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
 	addOutputToLog(log_struct, ps->vrpn_alt, CONST_VAL, m);
 	addOutputToLog(log_struct, ps->vrpn_pitch, CONST_VAL, rad);
 	addOutputToLog(log_struct, ps->vrpn_roll, CONST_VAL, rad);
+	addOutputToLog(log_struct, ps->lidar, CONST_VAL, m);
 	addOutputToLog(log_struct, ps->x_set, CONST_VAL, m);
 	addOutputToLog(log_struct, ps->y_set, CONST_VAL, m);
 	addOutputToLog(log_struct, ps->alt_set, CONST_VAL, m);
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
index eb0597de8277eb2d2e1bd327c4ac739e8fe9e0fb..2a4a32c8063c34b5c4bf2f56651e7ee9de57b7d7 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
@@ -59,10 +59,10 @@ int test_zybo_i2c() {
   lidar_t lidar = { };
   iic_set_globals(&i2c, &sys);
   if (iic0_lidarlite_init()) return 0;
-  short x;
+  float x;
   while (1) {
     iic0_lidarlite_read_distance(&lidar);
-    x = lidar.distance_cm;
+    x = lidar.distance_m;
   }
   return 0;
 }
@@ -136,7 +136,7 @@ int test_zybo_i2c_all() {
     iic0_mpu9150_read_gam(&gam);
     iic0_lidarlite_read_distance(&lidar);
 
-    if (lidar.distance_cm > 5000) {
+    if (lidar.distance_m > 50) {
     	lidarErrors += 1;
     }
     if (gam.accel_z > -0.8) {