Added check to verify that LiDAR is within reasonable ranges, since it sometimes spikes randomly.

quad/src/quad_app/control_algorithm.c

@@ -14,6 +14,7 @@
 #include "timer.h"
 
 //#define USE_LIDAR
+#define MAX_VALID_LIDAR (10.0) // Maximum valid distance to read from LiDAR to update
 
 #define PX4FLOW_QUAL_MIN			(100)
 #define OF_OFFSET_ANGLE             (-0.62204) // -35.64 degrees
@@ -388,7 +389,9 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_param_val(graph, ps->gyro_y, CONST_SET, sensor_struct->gyr_y);
     graph_set_param_val(graph, ps->gyro_x, CONST_SET, sensor_struct->gyr_x);
     graph_set_param_val(graph, ps->gyro_z, CONST_SET, sensor_struct->gyr_z);
-    graph_set_param_val(graph, ps->lidar, CONST_SET, sensor_struct->lidar_altitude);
+    if (fabs(sensor_struct->lidar_altitude) <= MAX_VALID_LIDAR) {
+        graph_set_param_val(graph, ps->lidar, CONST_SET, sensor_struct->lidar_altitude);
+    }
     graph_set_param_val(graph, ps->flow_quality, CONST_SET, sensor_struct->optical_flow.quality);
     //As per documentation, disregard frames with low quality, as they contain unreliable data
     if (sensor_struct->optical_flow.quality >= PX4FLOW_QUAL_MIN) {