From d4a0cb5db1f5d64200070b5977113bead626ef3c Mon Sep 17 00:00:00 2001
From: "Andreas M. Antonopoulos" <andreas@antonopoulos.com>
Date: Wed, 11 Jul 2012 18:46:47 -0700
Subject: [PATCH] Added Michael Pursifull's Maxsonar HRLV model support

---
 APMrover2/Parameters.h                                 | 4 +++-
 ArduCopter/Parameters.h                                | 4 +++-
 ArduCopter/setup.pde                                   | 6 +++---
 libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp | 7 ++++++-
 libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h   | 6 ++++++
 5 files changed, 21 insertions(+), 6 deletions(-)

diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
index 43e249dc04..07dce9ea55 100644
--- a/APMrover2/Parameters.h
+++ b/APMrover2/Parameters.h
@@ -336,7 +336,9 @@ public:
 #if HIL_MODE != HIL_MODE_ATTITUDE
 #if CONFIG_SONAR == ENABLED     
         AP_Int8	    sonar_enabled;
-	AP_Int8	    sonar_type;   // 0 = XL, 1 = LV, 2 = XLL (XL with 10m range)   
+	AP_Int8	    sonar_type;   // 0 = XL, 1 = LV,
+				  // 2 = XLL (XL with 10m range)   
+				  // 3 = HRLV 
 #endif
 #endif
         AP_Int8	    airspeed_enabled;
diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
index 9058b13339..5328212339 100644
--- a/ArduCopter/Parameters.h
+++ b/ArduCopter/Parameters.h
@@ -210,7 +210,9 @@ public:
 
 	AP_Int16	RTL_altitude;
 	AP_Int8		sonar_enabled;
-	AP_Int8		sonar_type;   // 0 = XL, 1 = LV, 2 = XLL (XL with 10m range)
+	AP_Int8		sonar_type;   // 0 = XL, 1 = LV,
+				      // 2 = XLL (XL with 10m range)
+				      // 3 = HRLV 
     AP_Int8		battery_monitoring;	// 0=disabled, 3=voltage only, 4=voltage and current
     AP_Float	volt_div_ratio;
     AP_Float	curr_amp_per_volt;
diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde
index 85632f7095..b723597ee3 100644
--- a/ArduCopter/setup.pde
+++ b/ArduCopter/setup.pde
@@ -431,12 +431,12 @@ setup_sonar(uint8_t argc, const Menu::arg *argv)
 	} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
 		g.sonar_enabled.set_and_save(false);
 
-	} else if (argc > 1 && (argv[1].i >= 0 && argv[1].i <= 2)) {
+	} else if (argc > 1 && (argv[1].i >= 0 && argv[1].i <= 3)) {
 	    g.sonar_enabled.set_and_save(true);  // if you set the sonar type, surely you want it on
 		g.sonar_type.set_and_save(argv[1].i);
 
 	}else{
-		Serial.printf_P(PSTR("\nOp:[on, off, 0-2]\n"));
+		Serial.printf_P(PSTR("\nOp:[on, off, 0-3]\n"));
 		report_sonar();
 		return 0;
 	}
@@ -824,7 +824,7 @@ static void report_sonar()
 	Serial.printf_P(PSTR("Sonar\n"));
 	print_divider();
 	print_enabled(g.sonar_enabled.get());
-	Serial.printf_P(PSTR("Type: %d (0=XL, 1=LV, 2=XLL)"), (int)g.sonar_type);
+	Serial.printf_P(PSTR("Type: %d (0=XL, 1=LV, 2=XLL, 3=HRLV)"), (int)g.sonar_type);
 	print_blanks(2);
 }
 
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp
index e5e865e615..339de3f821 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp
@@ -63,7 +63,12 @@ float AP_RangeFinder_MaxsonarXL::calculate_scaler(int sonar_type, float adc_refe
 		    min_distance = AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE;
 		    max_distance = AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE;
 			break;
+		case AP_RANGEFINDER_MAXSONARHRLV:
+		    type_scaler = AP_RANGEFINDER_MAXSONARHRLV_SCALER;
+		    min_distance = AP_RANGEFINDER_MAXSONARHRLV_MIN_DISTANCE;
+		    max_distance = AP_RANGEFINDER_MAXSONARHRLV_MAX_DISTANCE;
+			break;
     }
 	_scaler = type_scaler * adc_refence_voltage / 5.0;
 	return _scaler;
-}
\ No newline at end of file
+}
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h
index 63b8065e00..c0b75fd766 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h
@@ -21,6 +21,12 @@
 #define AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE 20
 #define AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE 1068
 
+// HRLV-MaxSonar-EZ0 (aka HRLV)
+#define AP_RANGEFINDER_MAXSONARHRLV 3
+#define AP_RANGEFINDER_MAXSONARHRLV_SCALER 0.512
+#define AP_RANGEFINDER_MAXSONARHRLV_MIN_DISTANCE 30
+#define AP_RANGEFINDER_MAXSONARHRLV_MAX_DISTANCE 500
+
 class AP_RangeFinder_MaxsonarXL : public RangeFinder
 {
   public: