Copter: convert to new AP_RangeFinder API

ArduCopter/ArduCopter.pde

@@ -353,13 +353,10 @@ static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
 static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
 
 ////////////////////////////////////////////////////////////////////////////////
-// SONAR selection
-////////////////////////////////////////////////////////////////////////////////
-//
-ModeFilterInt16_Size3 sonar_mode_filter(1);
+// SONAR
 #if CONFIG_SONAR == ENABLED
-static AP_HAL::AnalogSource *sonar_analog_source;
-static AP_RangeFinder_MaxsonarXL *sonar;
+static RangeFinder sonar;
+static bool sonar_enabled = true; // enable user switch for sonar
 #endif
 
 ////////////////////////////////////////////////////////////////////////////////

ArduCopter/GCS_Mavlink.pde

@@ -487,7 +487,7 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
 static void NOINLINE send_rangefinder(mavlink_channel_t chan)
 {
     // exit immediately if sonar is disabled
-    if (!g.sonar_enabled) {
+    if (!sonar.healthy()) {
         return;
     }
     mavlink_msg_rangefinder_send(chan, sonar_alt * 0.01f, 0);

ArduCopter/Parameters.h

@@ -116,6 +116,7 @@ public:
         k_param_serial1_baud,
         k_param_serial2_baud,
         k_param_land_repositioning,     // 52
+        k_param_sonar, // sonar object
 
         // 65: AP_Limits Library
         k_param_limits = 65,            // deprecated - remove
@@ -182,13 +183,13 @@ public:
         k_param_pack_capacity,  // deprecated - can be deleted
         k_param_compass_enabled,
         k_param_compass,
-        k_param_sonar_enabled,
+        k_param_sonar_enabled_old, // deprecated
         k_param_frame_orientation,
         k_param_optflow_enabled,
         k_param_fs_batt_voltage,
         k_param_ch7_option,
         k_param_auto_slew_rate,     // deprecated - can be deleted
-        k_param_sonar_type,
+        k_param_sonar_type_old,     // deprecated
         k_param_super_simple = 155,
         k_param_axis_enabled = 157, // deprecated - remove with next eeprom number change
         k_param_copter_leds_mode,   // deprecated - remove with next eeprom number change
@@ -320,10 +321,6 @@ public:
     AP_Int8         telem_delay;
 
     AP_Int16        rtl_altitude;
-    AP_Int8         sonar_enabled;
-    AP_Int8         sonar_type;       // 0 = XL, 1 = LV,
-                                      // 2 = XLL (XL with 10m range)
-                                      // 3 = HRLV
     AP_Float        sonar_gain;
 
     AP_Int8         failsafe_battery_enabled;   // battery failsafe enabled

ArduCopter/Parameters.pde

@@ -93,20 +93,6 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(rtl_altitude,   "RTL_ALT",     RTL_ALT),
 
-    // @Param: SONAR_ENABLE
-    // @DisplayName: Sonar enable/disable
-    // @Description: Setting this to Enabled(1) will enable the sonar. Setting this to Disabled(0) will disable the sonar
-    // @Values: 0:Disabled,1:Enabled
-    // @User: Standard
-    GSCALAR(sonar_enabled,  "SONAR_ENABLE",     DISABLED),
-
-    // @Param: SONAR_TYPE
-    // @DisplayName: Sonar type
-    // @Description: Used to adjust scaling to match the sonar used (only Maxbotix sonars are supported at this time)
-    // @Values: 0:XL-EZ0 / XL-EZ4,1:LV-EZ0,2:XLL-EZ0,3:HRLV
-    // @User: Standard
-    GSCALAR(sonar_type,     "SONAR_TYPE",           AP_RANGEFINDER_MAXSONARXL),
-
     // @Param: SONAR_GAIN
     // @DisplayName: Sonar gain
     // @Description: Used to adjust the speed with which the target altitude is changed when objects are sensed below the copter
@@ -1089,6 +1075,12 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Path: ../libraries/AP_Mission/AP_Mission.cpp
     GOBJECT(mission, "MIS_",       AP_Mission),
 
+#if CONFIG_SONAR == ENABLED
+    // @Group: SONAR
+    // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp
+    GOBJECT(sonar,   "SONAR", RangeFinder),
+#endif
+
     AP_VAREND
 };
 

ArduCopter/control_land.pde

@@ -175,7 +175,7 @@ static void land_nogps_run()
 static float get_throttle_land()
 {
     // if we are above 10m and the sonar does not sense anything perform regular alt hold descent
-    if (current_loc.alt >= LAND_START_ALT && !(g.sonar_enabled && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
+    if (current_loc.alt >= LAND_START_ALT && !(sonar_enabled && sonar.healthy() && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
         return pos_control.get_speed_down();
     }else{
         return -abs(g.land_speed);

ArduCopter/control_modes.pde

@@ -259,9 +259,9 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
         case AUX_SWITCH_SONAR:
             // enable or disable the sonar
             if (ch_flag == AUX_SWITCH_HIGH) {
-                g.sonar_enabled = true;
+                sonar_enabled = true;
             }else{
-                g.sonar_enabled = false;
+                sonar_enabled = false;
             }
             break;
 

ArduCopter/sensors.pde

@@ -3,13 +3,9 @@
  #if CONFIG_SONAR == ENABLED
 static void init_sonar(void)
 {
-  #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
-    sonar->calculate_scaler(g.sonar_type, 3.3f);
-  #else
-    sonar->calculate_scaler(g.sonar_type, 5.0f);
-  #endif
+    sonar.init();
 }
- #endif
+#endif
 
 static void init_barometer(bool full_calibration)
 {
@@ -36,15 +32,18 @@ static int32_t read_barometer(void)
 static int16_t read_sonar(void)
 {
 #if CONFIG_SONAR == ENABLED
+    sonar.update();
+
     // exit immediately if sonar is disabled
-    if( !g.sonar_enabled ) {
+    if (!sonar_enabled || !sonar.healthy()) {
         sonar_alt_health = 0;
         return 0;
     }
 
-    int16_t temp_alt = sonar->read();
+    int16_t temp_alt = sonar.distance_cm();
 
-    if (temp_alt >= sonar->min_distance && temp_alt <= sonar->max_distance * SONAR_RELIABLE_DISTANCE_PCT) {
+    if (temp_alt >= sonar.min_distance_cm() && 
+        temp_alt <= sonar.max_distance_cm() * SONAR_RELIABLE_DISTANCE_PCT) {
         if ( sonar_alt_health < SONAR_ALT_HEALTH_MAX ) {
             sonar_alt_health++;
         }

ArduCopter/setup.pde

@@ -87,7 +87,6 @@ setup_show(uint8_t argc, const Menu::arg *argv)
     report_radio();
     report_frame();
     report_batt_monitor();
-    report_sonar();
     report_flight_modes();
     report_ins();
     report_compass();
@@ -112,15 +111,6 @@ static void report_batt_monitor()
     print_blanks(2);
 }
 
-static void report_sonar()
-{
-    cliSerial->printf_P(PSTR("Sonar\n"));
-    print_divider();
-    print_enabled(g.sonar_enabled.get());
-    cliSerial->printf_P(PSTR("Type: %d (0=XL, 1=LV, 2=XLL, 3=HRLV)"), (int)g.sonar_type);
-    print_blanks(2);
-}
-
 static void report_frame()
 {
     cliSerial->printf_P(PSTR("Frame\n"));

ArduCopter/system.pde

@@ -151,20 +151,6 @@ static void init_ardupilot()
     // initialise battery monitor
     battery.init();
     
-#if CONFIG_SONAR == ENABLED
- #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
-    sonar_analog_source = new AP_ADC_AnalogSource(
-            &adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25);
- #elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
-    sonar_analog_source = hal.analogin->channel(
-            CONFIG_SONAR_SOURCE_ANALOG_PIN);
- #else
-  #warning "Invalid CONFIG_SONAR_SOURCE"
- #endif
-    sonar = new AP_RangeFinder_MaxsonarXL(sonar_analog_source,
-            &sonar_mode_filter);
-#endif
-
     rssi_analog_source      = hal.analogin->channel(g.rssi_pin);
 
     barometer.init();

ArduCopter/test.pde

@@ -266,7 +266,7 @@ static int8_t
 test_sonar(uint8_t argc, const Menu::arg *argv)
 {
 #if CONFIG_SONAR == ENABLED
-    if(g.sonar_enabled == false) {
+    if(!sonar.healthy()) {
         cliSerial->printf_P(PSTR("Sonar disabled\n"));
         return (0);
     }
@@ -277,8 +277,8 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
     print_hit_enter();
     while(1) {
         delay(100);
-
-        cliSerial->printf_P(PSTR("Sonar: %d cm\n"), sonar->read());
+        sonar.update();
+        cliSerial->printf_P(PSTR("Sonar: %d cm\n"), sonar.distance_cm());
 
         if(cliSerial->available() > 0) {
             return (0);