Rover: added dual sonar support

APMrover2/APMrover2.pde

@@ -239,6 +239,7 @@ AP_HAL::AnalogSource * batt_curr_pin;
 ////////////////////////////////////////////////////////////////////////////////
 //
 static AP_RangeFinder_analog sonar;
+static AP_RangeFinder_analog sonar2;
 
 // relay support
 AP_Relay relay;
@@ -373,6 +374,8 @@ static uint32_t last_heartbeat_ms;
 static struct {
     // have we detected an obstacle?
     bool detected;
+    float turn_angle;
+
     // time when we last detected an obstacle, in milliseconds
     uint32_t detected_time_ms;
 } obstacle;
@@ -622,22 +625,7 @@ static void fast_loop()
 
 	ahrs.update();
 
-	// Read Sonar
-	// ----------
-	if (sonar.enabled()) {
-		float sonar_dist_cm = sonar.distance_cm();
-        if (sonar_dist_cm <= g.sonar_trigger_cm)  {
-            // obstacle detected in front 
-            obstacle.detected = true;
-            obstacle.detected_time_ms = hal.scheduler->millis();
-        } else if (obstacle.detected == true && 
-                   hal.scheduler->millis() > obstacle.detected_time_ms + g.sonar_turn_time*1000) { 
-            obstacle.detected = false;
-        }
-	} else {
-        // this makes it possible to disable sonar at runtime
-        obstacle.detected = false;
-    }
+    read_sonars();
 
 	// uses the yaw from the DCM to give more accurate turns
 	calc_bearing_error();

APMrover2/Parameters.h

@@ -104,6 +104,7 @@ public:
         k_param_sonar_trigger_cm,
         k_param_sonar_turn_angle,
         k_param_sonar_turn_time,
+        k_param_sonar2, // sonar2 object
         
         //
         // 210: driving modes

APMrover2/Parameters.pde

@@ -355,6 +355,7 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Group: SONAR_
     // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
     GOBJECT(sonar,                  "SONAR_", AP_RangeFinder_analog),
+    GOBJECT(sonar2,                 "SONAR2_", AP_RangeFinder_analog),
 
 #if HIL_MODE == HIL_MODE_DISABLED
     // @Group: INS_

APMrover2/Steering.pde

@@ -78,7 +78,7 @@ static void calc_nav_steer()
 	nav_steer = g.pidNavSteer.get_pid(bearing_error_cd, nav_gain_scaler);
 
     if (obstacle.detected) {  // obstacle avoidance 
-	    nav_steer += g.sonar_turn_angle*100;
+	    nav_steer += obstacle.turn_angle*100;
     }
 
     g.channel_steer.servo_out = nav_steer;

APMrover2/sensors.pde

@@ -4,8 +4,10 @@ static void init_sonar(void)
 {
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
     sonar.Init(&adc);
+    sonar2.Init(&adc);
 #else
     sonar.Init(NULL);
+    sonar2.Init(NULL);
 #endif
 }
 
@@ -45,3 +47,44 @@ void read_receiver_rssi(void)
     float ret = rssi_analog_source->read_average();
     receiver_rssi = constrain_int16(ret, 0, 255);
 }
+
+// read the sonars
+static void read_sonars(void)
+{
+    if (!sonar.enabled()) {
+        // this makes it possible to disable sonar at runtime
+        return;
+    }
+
+    if (sonar2.enabled()) {
+        // we have two sonars
+        float sonar1_dist_cm = sonar.distance_cm();
+        float sonar2_dist_cm = sonar2.distance_cm();
+        if (sonar1_dist_cm <= g.sonar_trigger_cm &&
+            sonar1_dist_cm <= sonar2_dist_cm)  {
+            // we have an object on the left
+            obstacle.detected = true;
+            obstacle.turn_angle = g.sonar_turn_angle;
+        } else if (sonar2_dist_cm <= g.sonar_trigger_cm) {
+            // we have an object on the right
+            obstacle.detected = true;
+            obstacle.turn_angle = -g.sonar_turn_angle;
+        }
+        return;
+    }
+
+    // we have a single sonar
+    float sonar_dist_cm = sonar.distance_cm();
+    if (sonar_dist_cm <= g.sonar_trigger_cm)  {
+        // obstacle detected in front 
+        obstacle.detected = true;
+        obstacle.detected_time_ms = hal.scheduler->millis();
+        obstacle.turn_angle = g.sonar_turn_angle;
+        return;
+    }
+
+    // no object detected - reset after the turn time
+    if (hal.scheduler->millis() > obstacle.detected_time_ms + g.sonar_turn_time*1000) { 
+        obstacle.detected = false;
+    }
+}

APMrover2/test.pde

@@ -561,8 +561,11 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
         delay(200);
         float sonar_dist_cm = sonar.distance_cm();
         float voltage = sonar.voltage();
-        cliSerial->printf_P(PSTR("sonar distance=%5.1fcm  voltage=%5.2f\n"), 
-                            sonar_dist_cm, voltage);
+        float sonar2_dist_cm = sonar2.distance_cm();
+        float voltage2 = sonar2.voltage();
+        cliSerial->printf_P(PSTR("sonar1 dist=%5.1fcm volt1=%5.2f   sonar2 dist=%5.1fcm volt2=%5.2f\n"), 
+                            sonar_dist_cm, voltage,
+                            sonar2_dist_cm, voltage2);
         if (cliSerial->available() > 0) {
             break;
 	    }