From ad7e8bd9c44b92d739a70d12372dfa41df9a8f70 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 29 Mar 2013 10:49:08 +1100
Subject: [PATCH] Rover: added SONAR_DEBOUNCE option

this allows us to debounce the sonar triggering, which will make it
handle noise better
---
 APMrover2/APMrover2.pde  |  2 +-
 APMrover2/Parameters.h   |  2 ++
 APMrover2/Parameters.pde |  8 ++++++++
 APMrover2/Steering.pde   |  5 ++---
 APMrover2/sensors.pde    | 23 +++++++++++++++--------
 5 files changed, 28 insertions(+), 12 deletions(-)

diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index 34a9ab1b1f..c2a7794598 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -372,7 +372,7 @@ static uint32_t last_heartbeat_ms;
 // obstacle detection information
 static struct {
     // have we detected an obstacle?
-    bool detected;
+    uint8_t detected_count;
     float turn_angle;
 
     // time when we last detected an obstacle, in milliseconds
diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
index f4e9837481..e9f86243ab 100644
--- a/APMrover2/Parameters.h
+++ b/APMrover2/Parameters.h
@@ -107,6 +107,7 @@ public:
         k_param_sonar_turn_angle,
         k_param_sonar_turn_time,
         k_param_sonar2, // sonar2 object
+        k_param_sonar_debounce,
         
         //
         // 210: driving modes
@@ -213,6 +214,7 @@ public:
     AP_Int16    sonar_trigger_cm;
     AP_Float    sonar_turn_angle;
     AP_Float    sonar_turn_time;
+    AP_Int8     sonar_debounce;
     
 
     // driving modes
diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde
index 390d06b670..59e68761a5 100644
--- a/APMrover2/Parameters.pde
+++ b/APMrover2/Parameters.pde
@@ -299,6 +299,14 @@ const AP_Param::Info var_info[] PROGMEM = {
 	// @User: Standard
 	GSCALAR(sonar_turn_time,    "SONAR_TURN_TIME",     1.0f),
 
+	// @Param: SONAR_DEBOUNCE
+	// @DisplayName: Sonar debounce count
+	// @Description: The number of 50Hz sonar hits needed to trigger an obstacle avoidance event. If you get a lot of false sonar events then raise this number
+    // @Range: 1 100
+    // @Increment: 1
+	// @User: Standard
+	GSCALAR(sonar_debounce,   "SONAR_DEBOUNCE",    2),
+
     // @Param: MODE_CH
     // @DisplayName: Mode channel
     // @Description: RC Channel to use for driving mode control
diff --git a/APMrover2/Steering.pde b/APMrover2/Steering.pde
index 3144bee240..20171b892d 100644
--- a/APMrover2/Steering.pde
+++ b/APMrover2/Steering.pde
@@ -138,9 +138,8 @@ static void calc_nav_steer()
 	// positive error = right turn
 	nav_steer = g.pidNavSteer.get_pid(bearing_error_cd, nav_gain_scaler);
 
-    if (obstacle.detected) {  // obstacle avoidance 
-	    nav_steer += obstacle.turn_angle*100;
-    }
+    // avoid obstacles, if any
+    nav_steer += obstacle.turn_angle*100;
 
     g.channel_steer.servo_out = nav_steer;
 }
diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde
index 677e0ac987..88ddb05d02 100644
--- a/APMrover2/sensors.pde
+++ b/APMrover2/sensors.pde
@@ -63,20 +63,24 @@ static void read_sonars(void)
         if (sonar1_dist_cm <= g.sonar_trigger_cm &&
             sonar1_dist_cm <= sonar2_dist_cm)  {
             // we have an object on the left
-            if (!obstacle.detected) {
+            if (obstacle.detected_count < 127) {
+                obstacle.detected_count++;
+            }
+            if (obstacle.detected_count == g.sonar_debounce) {
                 gcs_send_text_fmt(PSTR("Sonar1 obstacle %.0fcm"),
                                   sonar1_dist_cm);
             }
-            obstacle.detected = true;
             obstacle.detected_time_ms = hal.scheduler->millis();
             obstacle.turn_angle = g.sonar_turn_angle;
         } else if (sonar2_dist_cm <= g.sonar_trigger_cm) {
             // we have an object on the right
-            if (!obstacle.detected) {
+            if (obstacle.detected_count < 127) {
+                obstacle.detected_count++;
+            }
+            if (obstacle.detected_count == g.sonar_debounce) {
                 gcs_send_text_fmt(PSTR("Sonar2 obstacle %.0fcm"),
                                   sonar2_dist_cm);
             }
-            obstacle.detected = true;
             obstacle.detected_time_ms = hal.scheduler->millis();
             obstacle.turn_angle = -g.sonar_turn_angle;
         }
@@ -85,20 +89,23 @@ static void read_sonars(void)
         float sonar_dist_cm = sonar.distance_cm();
         if (sonar_dist_cm <= g.sonar_trigger_cm)  {
             // obstacle detected in front 
-            if (!obstacle.detected) {
+            if (obstacle.detected_count < 127) {
+                obstacle.detected_count++;
+            }
+            if (obstacle.detected_count == g.sonar_debounce) {
                 gcs_send_text_fmt(PSTR("Sonar obstacle %.0fcm"),
                                   sonar_dist_cm);
             }
-            obstacle.detected = true;
             obstacle.detected_time_ms = hal.scheduler->millis();
             obstacle.turn_angle = g.sonar_turn_angle;
         }
     }
 
     // no object detected - reset after the turn time
-    if (obstacle.detected &&
+    if (obstacle.detected_count >= g.sonar_debounce &&
         hal.scheduler->millis() > obstacle.detected_time_ms + g.sonar_turn_time*1000) { 
         gcs_send_text_fmt(PSTR("Obstacle passed"));
-        obstacle.detected = false;
+        obstacle.detected_count = 0;
+        obstacle.turn_angle = 0;
     }
 }