From ef62a2c697e95511a334dafce380e4f3eb074408 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 11 Nov 2014 11:33:51 +1100
Subject: [PATCH] Rover: fixed CLI sonartest

---
 APMrover2/test.pde | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/APMrover2/test.pde b/APMrover2/test.pde
index 29b0b92364..297922a814 100644
--- a/APMrover2/test.pde
+++ b/APMrover2/test.pde
@@ -455,12 +455,15 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 static int8_t
 test_sonar(uint8_t argc, const Menu::arg *argv)
 {
+    init_sonar();
+    delay(20);
+    sonar.update();
+
     if (!sonar.healthy()) {
         cliSerial->println_P(PSTR("WARNING: Sonar is not enabled"));
     }
 
     print_hit_enter();
-    init_sonar();
     
     float sonar_dist_cm_min = 0.0f;
     float sonar_dist_cm_max = 0.0f;
@@ -472,8 +475,9 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
 
 	while (true) {
         delay(20);
+        sonar.update();
         uint32_t now = millis();
-
+    
         float dist_cm = sonar.distance_cm(0);
         float voltage = sonar.voltage_mv(0);
         if (sonar_dist_cm_min == 0.0f) {