Rover: implement SONAR_TURN_TIME

keep turning for at least that time

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 {

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