Arducopter, RangeFinder - added SONAR_TYPE parameter and properly support the XL (default), LV and long distance XL (aka XLL) sonar types

ArduCopter/ArduCopter.pde

@@ -250,11 +250,7 @@ ModeFilter sonar_mode_filter;
 	#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
 		AP_AnalogSource_Arduino sonar_analog_source(CONFIG_SONAR_SOURCE_ANALOG_PIN);
 	#endif
-	#if SONAR_TYPE == MAX_SONAR_XL
-		AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter);
-	#else
-		#error Unrecognised SONAR_TYPE setting.
-	#endif
+	AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter);
 #endif
 
 // agmatthews USERHOOKS

ArduCopter/Parameters.h

@@ -105,6 +105,7 @@ public:
 	k_param_input_voltage,
 	k_param_low_voltage,
 	k_param_ch7_option,
+	k_param_sonar_type,  // 153
 
 	//
 	// 160: Navigation parameters
@@ -192,6 +193,7 @@ 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		battery_monitoring;	// 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current
 	AP_Int16	pack_capacity;		// Battery pack capacity less reserve
 	AP_Int8		compass_enabled;
@@ -306,6 +308,7 @@ public:
 
 	RTL_altitude			(ALT_HOLD_HOME * 100,		k_param_RTL_altitude,					PSTR("ALT_HOLD_RTL")),
 	sonar_enabled			(DISABLED,					k_param_sonar,							PSTR("SONAR_ENABLE")),
+	sonar_type				(AP_RANGEFINDER_MAXSONARXL,	k_param_sonar_type,						PSTR("SONAR_TYPE")),
 	battery_monitoring 		(DISABLED,					k_param_battery_monitoring,				PSTR("BATT_MONITOR")),
 	pack_capacity			(HIGH_DISCHARGE,			k_param_pack_capacity,					PSTR("BATT_CAPACITY")),
 	compass_enabled			(MAGNETOMETER,				k_param_compass_enabled,				PSTR("MAG_ENABLE")),

ArduCopter/config.h

@@ -161,10 +161,6 @@
 # define SONAR_ENABLED DISABLED
 #endif
 
-#ifndef SONAR_TYPE
-# define SONAR_TYPE		MAX_SONAR_XL
-#endif
-
 #ifndef CONFIG_SONAR
 # define CONFIG_SONAR ENABLED
 #endif

ArduCopter/defines.h

@@ -90,17 +90,12 @@
 #define GPS_PROTOCOL_MTK16	6
 #define GPS_PROTOCOL_AUTO	7
 
-// SONAR types:
-#define MAX_SONAR_UNKNOWN	0
-#define MAX_SONAR_XL		1
-
 #define CH_ROLL CH_1
 #define CH_PITCH CH_2
 #define CH_THROTTLE CH_3
 #define CH_RUDDER CH_4
 #define CH_YAW CH_4
 
-
 #define RC_CHANNEL_ANGLE 0
 #define RC_CHANNEL_RANGE 1
 #define RC_CHANNEL_ANGLE_RAW 2

ArduCopter/sensors.pde

@@ -5,6 +5,17 @@
 
 static void ReadSCP1000(void) {}
 
+#if CONFIG_SONAR == ENABLED
+static void init_sonar(void)
+{
+    #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
+	    sonar.calculate_scaler(g.sonar_type, 3.3);
+	#else
+        sonar.calculate_scaler(g.sonar_type, 5.0);
+	#endif
+}
+#endif
+
 static void init_barometer(void)
 {
 	#if HIL_MODE == HIL_MODE_SENSORS

ArduCopter/setup.pde

@@ -416,9 +416,13 @@ 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)) {
+	    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]\n"));
+		Serial.printf_P(PSTR("\nOp:[on, off, 0-2]\n"));
 		report_sonar();
 		return 0;
 	}
@@ -802,9 +806,11 @@ static void report_wp(byte index = 255)
 static void report_sonar()
 {
 	g.sonar_enabled.load();
+	g.sonar_type.load();
 	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);
 	print_blanks(2);
 }
 

ArduCopter/system.pde

@@ -333,6 +333,11 @@ static void init_ardupilot()
 	//-----------------------------
 	init_barometer();
 	#endif
+	
+	// initialise sonar
+	#if CONFIG_SONAR == ENABLED
+	init_sonar();
+	#endif
 
 	// initialize commands
 	// -------------------

ArduCopter/test.pde

@@ -973,6 +973,9 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
 		Serial.printf_P(PSTR("Sonar disabled\n"));
 		return (0);
 	}
+	
+	// make sure sonar is initialised
+	init_sonar();
 
 	print_hit_enter();
 	while(1) {

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp

@@ -33,10 +33,33 @@
 
 AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source,
                                                      ModeFilter *filter) :
-	RangeFinder(source, filter)
+	RangeFinder(source, filter), _scaler(AP_RANGEFINDER_MAXSONARXL_SCALER)
 {
     max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE;
 	min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE;
 }
 
 // Public Methods //////////////////////////////////////////////////////////////
+float AP_RangeFinder_MaxsonarXL::calculate_scaler(int sonar_type, float adc_refence_voltage)
+{
+    float type_scaler = 1.0;
+	switch(sonar_type) {
+		case AP_RANGEFINDER_MAXSONARXL:
+		    type_scaler = AP_RANGEFINDER_MAXSONARXL_SCALER;
+		    min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE;
+		    max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE;
+			break;
+		case AP_RANGEFINDER_MAXSONARLV:
+		    type_scaler = AP_RANGEFINDER_MAXSONARLV_SCALER;
+		    min_distance = AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE;
+		    max_distance = AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE;
+			break;
+		case AP_RANGEFINDER_MAXSONARXLL:
+		    type_scaler = AP_RANGEFINDER_MAXSONARXLL_SCALER;
+		    min_distance = AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE;
+		    max_distance = AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE;
+			break;
+    }
+	_scaler = type_scaler * adc_refence_voltage / 5.0;
+	return _scaler;
+}

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h

@@ -3,14 +3,31 @@
 
 #include "RangeFinder.h"
 
+// XL-EZ0 (aka XL)
+#define AP_RANGEFINDER_MAXSONARXL 0
+#define AP_RANGEFINDER_MAXSONARXL_SCALER 1.0
 #define AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE 20
-#define AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE 700
+#define AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE 765
+
+// LV-EZ0 (aka LV)
+#define AP_RANGEFINDER_MAXSONARLV 1
+#define AP_RANGEFINDER_MAXSONARLV_SCALER (2.54/2.0)
+#define AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE 15
+#define AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE 645
+
+// XL-EZL0 (aka XLL)
+#define AP_RANGEFINDER_MAXSONARXLL 2
+#define AP_RANGEFINDER_MAXSONARXLL_SCALER 2.0
+#define AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE 20
+#define AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE 1068
 
 class AP_RangeFinder_MaxsonarXL : public RangeFinder
 {
   public:
-	AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter);
-
-	int convert_raw_to_distance(int _raw_value) { return _raw_value; }   // read value from analog port and return distance in cm
+	AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter);
+	int convert_raw_to_distance(int _raw_value) { return _raw_value * _scaler; }   // read value from analog port and return distance in cm
+	float calculate_scaler(int sonar_type, float adc_refence_voltage); 
+  private:
+    float _scaler;  // used to account for different sonar types
 };
 #endif

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -30,17 +30,17 @@ void RangeFinder::set_orientation(int x, int y, int z)
 // Read Sensor data - only the raw_value is filled in by this parent class
 int RangeFinder::read()
 {
+    int temp_dist;
+	
 	raw_value = _analog_source->read();
-
-	raw_value = convert_raw_to_distance(raw_value);
 
 	// convert analog value to distance in cm (using child implementation most likely)
-	raw_value = convert_raw_to_distance(raw_value);
+	temp_dist = convert_raw_to_distance(raw_value);
 
 	// ensure distance is within min and max
-	raw_value = constrain(raw_value, min_distance, max_distance);
+	temp_dist = constrain(temp_dist, min_distance, max_distance);
 
-	distance = _mode_filter->get_filtered_with_sample(raw_value);
+	distance = _mode_filter->get_filtered_with_sample(temp_dist);
 	return distance;
 }
 

libraries/AP_RangeFinder/RangeFinder.h

@@ -19,13 +19,9 @@
 #define AP_RANGEFINDER_ORIENTATION_FRONT_LEFT 	  5,  5,  0
 */
 
-// define Pitot tube's ADC Channel
-#define AP_RANGEFINDER_PITOT_TYPE_ADC_CHANNEL 7
-
 class RangeFinder
 {
   protected:
-	//GPS(Stream *s) : _port(s) {};
 	RangeFinder(AP_AnalogSource * source, ModeFilter *filter) :
 		_analog_source(source),
 		_mode_filter(filter)

libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde

@@ -11,15 +11,27 @@
 #include <AP_AnalogSource.h>
 #include <ModeFilter.h>			// mode filter
 
+// comment out line below if using APM2 or analog pin instead of APM1's built in ADC
+#define USE_ADC_ADS7844  // use APM1's built in ADC and connect sonar to pitot tube
+
+// uncomment appropriate line corresponding to your sonar
+#define SONAR_TYPE AP_RANGEFINDER_MAXSONARXL   // 0 - XL (default)
+//#define SONAR_TYPE AP_RANGEFINDER_MAXSONARLV   // 1 - LV (cheaper)
+//#define SONAR_TYPE AP_RANGEFINDER_MAXSONARXLL   // 2 - XLL (XL with 10m range)
+
+// define Pitot tube's ADC Channel
+#define AP_RANGEFINDER_PITOT_TYPE_ADC_CHANNEL 7
+
 // declare global instances
 Arduino_Mega_ISR_Registry isr_registry;
-AP_TimerProcess  adc_scheduler;
-AP_ADC_ADS7844	adc;
 ModeFilter mode_filter;
-
-// uncomment the appropriate line corresponding to where sonar is connected 
-AP_AnalogSource_ADC	adc_source(&adc, AP_RANGEFINDER_PITOT_TYPE_ADC_CHANNEL, 0.25);    // uncomment this line to use Pitot tube
-//AP_AnalogSource_Arduino adc_source(A4);   // uncomment this line to use A5 analog pin (far back-right pin on the oilpan near the CLI switch
+#ifdef USE_ADC_ADS7844
+    AP_TimerProcess  adc_scheduler;
+    AP_ADC_ADS7844 adc;
+    AP_AnalogSource_ADC adc_source(&adc, AP_RANGEFINDER_PITOT_TYPE_ADC_CHANNEL, 0.25);    // use Pitot tube
+#else
+    AP_AnalogSource_Arduino adc_source(A4);   // use AN4 analog pin (APM1: on oilpan at back right near the CLI switch.  APM2: on left)
+#endif
 
 // create the range finder object
 //AP_RangeFinder_SharpGP2Y aRF(&adc_source, &mode_filter);
@@ -29,10 +41,19 @@ void setup()
 {
     Serial.begin(115200);
     Serial.println("Range Finder Test v1.1");
+	Serial.print("Sonar Type: ");
+	Serial.println(SONAR_TYPE);
+	
+#ifdef USE_ADC_ADS7844
     isr_registry.init();
     adc_scheduler.init(&isr_registry);
     adc.filter_result = true;
     adc.Init(&adc_scheduler);   // APM ADC initialization
+    aRF.calculate_scaler(SONAR_TYPE,3.3);   // setup scaling for sonar
+#else
+    aRF.calculate_scaler(SONAR_TYPE,5.0);   // setup scaling for sonar
+#endif
+	
 }
 
 void loop()