Copter: integrate AP_Proximity into main vehicle

ArduCopter/APM_Config.h

@@ -25,6 +25,7 @@
 //#define AC_FENCE              DISABLED            // disable fence to save 2k of flash
 //#define CAMERA                DISABLED            // disable camera trigger to save 1k of flash
 //#define RANGEFINDER_ENABLED   DISABLED            // disable rangefinder to save 1k of flash
+//#define PROXIMITY_ENABLED     DISABLED            // disable proximity sensors
 //#define POSHOLD_ENABLED       DISABLED            // disable PosHold flight mode to save 4.5k of flash
 //#define AC_RALLY              DISABLED            // disable rally points library (must also disable terrain which relies on rally)
 //#define AC_TERRAIN            DISABLED            // disable terrain library

ArduCopter/ArduCopter.cpp

@@ -95,6 +95,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK(auto_disarm_check,     10,     50),
     SCHED_TASK(auto_trim,             10,     75),
     SCHED_TASK(read_rangefinder,      20,    100),
+    SCHED_TASK(update_proximity,     100,     50),
     SCHED_TASK(update_altitude,       10,    100),
     SCHED_TASK(run_nav_updates,       50,    100),
     SCHED_TASK(update_throttle_hover,100,     90),

ArduCopter/Copter.h

@@ -60,6 +60,7 @@
 #include <RC_Channel/RC_Channel.h>         // RC Channel Library
 #include <AP_Motors/AP_Motors.h>          // AP Motors library
 #include <AP_RangeFinder/AP_RangeFinder.h>     // Range finder library
+#include <AP_Proximity/AP_Proximity.h>
 #include <AP_OpticalFlow/AP_OpticalFlow.h>     // Optical Flow library
 #include <AP_RSSI/AP_RSSI.h>                   // RSSI Library
 #include <Filter/Filter.h>             // Filter library
@@ -196,6 +197,10 @@ private:
         LowPassFilterFloat alt_cm_filt; // altitude filter
     } rangefinder_state = { false, false, 0, 0 };
 
+#if PROXIMITY_ENABLED == ENABLED
+    AP_Proximity proximity {serial_manager};
+#endif
+
     AP_RPM rpm_sensor;
 
     // Inertial Navigation EKF
@@ -712,6 +717,8 @@ private:
     void send_rpm(mavlink_channel_t chan);
     void rpm_update();
     void button_update();
+    void init_proximity();
+    void update_proximity();
     void send_pid_tuning(mavlink_channel_t chan);
     void gcs_send_message(enum ap_message id);
     void gcs_send_mission_item_reached_message(uint16_t mission_index);

ArduCopter/Parameters.cpp

@@ -911,6 +911,12 @@ const AP_Param::Info Copter::var_info[] = {
     // @Path: ../libraries/AP_Avoidance/AP_Avoidance.cpp
     GOBJECT(avoidance_adsb, "AVD_", AP_Avoidance_Copter),
 
+#if PROXIMITY_ENABLED == ENABLED
+    // @Group: PRX_
+    // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp
+    GOBJECT(proximity, "PRX_", AP_Proximity),
+#endif
+
     // @Param: AUTOTUNE_AXES
     // @DisplayName: Autotune axis bitmask
     // @Description: 1-byte bitmap of axes to autotune

ArduCopter/Parameters.h

@@ -362,6 +362,7 @@ public:
         k_param_rtl_climb_min,
         k_param_rpm_sensor,
         k_param_autotune_min_d, // 251
+        k_param_proximity,
         k_param_DataFlash = 253, // 253 - Logging Group
 
         // 254,255: reserved

ArduCopter/config.h

@@ -152,6 +152,12 @@
  # define RANGEFINDER_TILT_CORRECTION ENABLED
 #endif
 
+//////////////////////////////////////////////////////////////////////////////
+// Proximity sensor
+//
+#ifndef PROXIMITY_ENABLED
+ # define PROXIMITY_ENABLED ENABLED
+#endif
 
 #ifndef MAV_SYSTEM_ID
  # define MAV_SYSTEM_ID          1

ArduCopter/sensors.cpp

@@ -253,3 +253,20 @@ void Copter::button_update(void)
 {
     g2.button.update();
 }
+
+// initialise proximity sensor
+void Copter::init_proximity(void)
+{
+#if PROXIMITY_ENABLED == ENABLED
+    proximity.init();
+#endif
+}
+
+// update proximity sensor
+void Copter::update_proximity(void)
+{
+#if PROXIMITY_ENABLED == ENABLED
+    proximity.update();
+#endif
+}
+

ArduCopter/system.cpp

@@ -258,6 +258,9 @@ void Copter::init_ardupilot()
     // initialise rangefinder
     init_rangefinder();
 
+    // init proximity sensor
+    init_proximity();
+
     // initialise AP_RPM library
     rpm_sensor.init();