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: