From 67855b207d822fd40207845c22c3628e6f101d4a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 11 Dec 2011 16:40:59 +0900 Subject: [PATCH] Arducopter, RangeFinder - added SONAR_TYPE parameter and properly support the XL (default), LV and long distance XL (aka XLL) sonar types --- ArduCopter/ArduCopter.pde | 6 +--- ArduCopter/Parameters.h | 3 ++ ArduCopter/config.h | 4 --- ArduCopter/defines.h | 5 --- ArduCopter/sensors.pde | 11 +++++++ ArduCopter/setup.pde | 10 ++++-- ArduCopter/system.pde | 5 +++ ArduCopter/test.pde | 3 ++ .../AP_RangeFinder_MaxsonarXL.cpp | 25 +++++++++++++- .../AP_RangeFinder_MaxsonarXL.h | 25 +++++++++++--- libraries/AP_RangeFinder/RangeFinder.cpp | 10 +++--- libraries/AP_RangeFinder/RangeFinder.h | 4 --- .../AP_RangeFinder_test.pde | 33 +++++++++++++++---- 13 files changed, 108 insertions(+), 36 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 5099b58c35..08af33fc7a 100644 --- a/ArduCopter/ArduCopter.pde +++ b/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 diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index fa53b80206..1bd53b77d0 100644 --- a/ArduCopter/Parameters.h +++ b/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")), diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 60ab7a16c0..d9ee78fb62 100644 --- a/ArduCopter/config.h +++ b/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 diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index f6ca48896b..67662503c4 100644 --- a/ArduCopter/defines.h +++ b/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 diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index b7da236f8a..7d04870b9d 100644 --- a/ArduCopter/sensors.pde +++ b/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 diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 993afb6bec..9c4ab27bc5 100644 --- a/ArduCopter/setup.pde +++ b/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); } diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index b3b9736eac..ad6c8c8391 100644 --- a/ArduCopter/system.pde +++ b/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 // ------------------- diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 7555cb8d28..60685ed9f9 100644 --- a/ArduCopter/test.pde +++ b/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) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp index 338660c69a..492fc9a2c3 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp +++ b/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; +} \ 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 6492bb2599..a589e987a5 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h +++ b/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 diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 6da25d4383..962148eb1c 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/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; } diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 5b08ce4723..64b38e7e89 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/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) diff --git a/libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde b/libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde index 58e5dab9d5..3d2f18c520 100644 --- a/libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde +++ b/libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde @@ -11,15 +11,27 @@ #include #include // 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()