Properly setting sampling time for altitude PID with LiDAR. Fixed HW impl...

Properly setting sampling time for altitude PID with LiDAR. Fixed HW impl tests for lidar_t changes.

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

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

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