diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 12d959155a..ffe08425dc 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -864,7 +864,6 @@ private: void rpm_update(); void update_optical_flow(void); void compass_cal_update(void); - void init_proximity(); // RC_Channel.cpp void save_trim(); diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 741eb85f08..b37103b0f9 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -159,12 +159,3 @@ void Copter::rpm_update(void) } #endif } - - -// initialise proximity sensor -void Copter::init_proximity(void) -{ -#if HAL_PROXIMITY_ENABLED - g2.proximity.init(); -#endif -} diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index b35afe83d6..3d27915028 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -149,8 +149,10 @@ void Copter::init_ardupilot() // initialise rangefinder init_rangefinder(); +#if HAL_PROXIMITY_ENABLED // init proximity sensor - init_proximity(); + g2.proximity.init(); +#endif #if BEACON_ENABLED == ENABLED // init beacons used for non-gps position estimation