From a605d27b23fd5c1886f6188aac7123ca80160e9d Mon Sep 17 00:00:00 2001
From: David Wehr <dawehr@iastate.edu>
Date: Fri, 14 Apr 2017 00:24:29 -0500
Subject: [PATCH] Added check to verify that LiDAR is within reasonable ranges,
since it sometimes spikes randomly.
---
quad/src/quad_app/control_algorithm.c | 5 ++++-
1 file changed, 4 insertions(+), 1 deletion(-)
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 3af671161..c58886dd3 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/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) {
--
GitLab