From e228bbfebf32ed2f9bd2592dc9b2afd8d16cb5b0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 1 Mar 2013 13:06:04 +1100
Subject: [PATCH] Rover: implement SONAR_TURN_TIME

keep turning for at least that time
---
 APMrover2/APMrover2.pde  | 8 +++++++-
 APMrover2/Parameters.pde | 4 ++--
 2 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index a6e13d1a59..fdd9019612 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -374,6 +374,10 @@ static uint32_t last_heartbeat_ms;
 // Set to true when an obstacle is detected
 static bool obstacle = false;
 
+// time when we last detected an obstacle, in milliseconds
+static uint32_t obstacle_detected_time_ms;
+
+
 ////////////////////////////////////////////////////////////////////////////////
 // Ground speed
 ////////////////////////////////////////////////////////////////////////////////
@@ -627,7 +631,9 @@ static void fast_loop()
         if (sonar_dist_cm <= g.sonar_trigger_cm)  {
             // obstacle detected in front 
             obstacle = true;
-        } else  { 
+            obstacle_detected_time_ms = hal.scheduler->millis();
+        } else if (obstacle == true && 
+                   hal.scheduler->millis() > obstacle_detected_time_ms + g.sonar_turn_time*1000) { 
             obstacle = false;
         }
 	} else {
diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde
index 59d1b11c78..ec48d9d76e 100644
--- a/APMrover2/Parameters.pde
+++ b/APMrover2/Parameters.pde
@@ -268,7 +268,7 @@ const AP_Param::Info var_info[] PROGMEM = {
 	// @DisplayName: Sonar trigger angle
 	// @Description: The course deviation in degrees to apply while avoiding an obstacle detected with the sonar. A positive number means to turn right, and a negative angle means to turn left.
 	// @Units: centimeters
-    // @Range: 0 90
+    // @Range: -90 90
     // @Increment: 1
 	// @User: Standard
 	GSCALAR(sonar_turn_angle,   "SONAR_TURN_ANGLE",    45),
@@ -280,7 +280,7 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Range: 0 100
     // @Increment: 0.1
 	// @User: Standard
-	GSCALAR(sonar_turn_time,    "SONAR_TURN_TIME",     2.0f),
+	GSCALAR(sonar_turn_time,    "SONAR_TURN_TIME",     1.0f),
 
     // @Param: MODE_CH
     // @DisplayName: Mode channel