diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 3049e75818..69a59ef0d8 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -4,7 +4,6 @@ //#define LOGGING_ENABLED DISABLED // disable logging to save 11K of flash space //#define MOUNT DISABLED // disable the camera gimbal to save 8K of flash space //#define AUTOTUNE_ENABLED DISABLED // disable the auto tune functionality to save 7k of flash -//#define RANGEFINDER_ENABLED DISABLED // disable rangefinder to save 1k of flash //#define PARACHUTE DISABLED // disable parachute release to save 1k of flash //#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands //#define MODE_ACRO_ENABLED DISABLED // disable acrobatic mode support diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 9af1d32a4d..12d037f076 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -257,6 +257,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) return false; break; case AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER: +#if AP_RANGEFINDER_ENABLED if (!copter.rangefinder_state.enabled || !copter.rangefinder.has_orientation(ROTATION_PITCH_270)) { check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "no rangefinder"); return false; @@ -266,6 +267,9 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "RTL_ALT>RNGFND_MAX_CM"); return false; } +#else + check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "rangefinder not in firmware"); +#endif break; case AC_WPNav::TerrainSource::TERRAIN_FROM_TERRAINDATABASE: // these checks are done in AP_Arming diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 5b5f39df7e..b02628b1bd 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -164,7 +164,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #endif SCHED_TASK(auto_disarm_check, 10, 50, 27), SCHED_TASK(auto_trim, 10, 75, 30), -#if RANGEFINDER_ENABLED == ENABLED +#if AP_RANGEFINDER_ENABLED SCHED_TASK(read_rangefinder, 20, 100, 33), #endif #if HAL_PROXIMITY_ENABLED diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 9084069c2c..8438aa1fa8 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -121,7 +121,7 @@ #if AP_TERRAIN_AVAILABLE # include #endif -#if RANGEFINDER_ENABLED == ENABLED +#if AP_RANGEFINDER_ENABLED # include #endif @@ -258,6 +258,7 @@ private: // helper function to get inertially interpolated rangefinder height. bool get_rangefinder_height_interpolated_cm(int32_t& ret) const; +#if AP_RANGEFINDER_ENABLED class SurfaceTracking { public: @@ -292,6 +293,7 @@ private: bool valid_for_logging; // true if we have a desired target altitude bool reset_target; // true if target should be reset because of change in surface being tracked } surface_tracking; +#endif #if AP_RPM_ENABLED AP_RPM rpm_sensor; diff --git a/ArduCopter/GCS_Copter.cpp b/ArduCopter/GCS_Copter.cpp index 2a5f776b5e..67b49e7cb2 100644 --- a/ArduCopter/GCS_Copter.cpp +++ b/ArduCopter/GCS_Copter.cpp @@ -91,7 +91,7 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void) } #endif -#if RANGEFINDER_ENABLED == ENABLED +#if AP_RANGEFINDER_ENABLED const RangeFinder *rangefinder = RangeFinder::get_singleton(); if (rangefinder && rangefinder->has_orientation(ROTATION_PITCH_270)) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index d1d530ca13..a59da5d47a 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -39,11 +39,15 @@ void Copter::Log_Write_Control_Tuning() target_climb_rate_cms = pos_control->get_vel_target_z_cms(); } - // get surface tracking alts float desired_rangefinder_alt; +#if AP_RANGEFINDER_ENABLED if (!surface_tracking.get_target_dist_for_logging(desired_rangefinder_alt)) { desired_rangefinder_alt = AP::logger().quiet_nan(); } +#else + // get surface tracking alts + desired_rangefinder_alt = AP::logger().quiet_nan(); +#endif struct log_Control_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG), @@ -56,7 +60,11 @@ void Copter::Log_Write_Control_Tuning() inav_alt : inertial_nav.get_position_z_up_cm() * 0.01f, baro_alt : baro_alt, desired_rangefinder_alt : desired_rangefinder_alt, +#if AP_RANGEFINDER_ENABLED rangefinder_alt : surface_tracking.get_dist_for_logging(), +#else + rangefinder_alt : AP::logger().quiet_nanf(), +#endif terr_alt : terr_alt, target_climb_rate : target_climb_rate_cms, climb_rate : int16_t(inertial_nav.get_velocity_z_up_cms()) // float -> int16_t diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 4b1f79833b..a1dd234b0c 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -638,7 +638,7 @@ const AP_Param::Info Copter::var_info[] = { // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp GOBJECT(rssi, "RSSI_", AP_RSSI), -#if RANGEFINDER_ENABLED == ENABLED +#if AP_RANGEFINDER_ENABLED // @Group: RNGFND // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp GOBJECT(rangefinder, "RNGFND", RangeFinder), @@ -1004,7 +1004,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced AP_GROUPINFO("FLIGHT_OPTIONS", 44, ParametersG2, flight_options, 0), -#if RANGEFINDER_ENABLED == ENABLED +#if AP_RANGEFINDER_ENABLED // @Param: RNGFND_FILT // @DisplayName: Rangefinder filter // @Description: Rangefinder filter to smooth distance. Set to zero to disable filtering @@ -1028,6 +1028,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // ACRO_PR_RATE (47), ACRO_Y_RATE (48), PILOT_Y_RATE (49) and PILOT_Y_EXPO (50) moved to command model class +#if AP_RANGEFINDER_ENABLED // @Param: SURFTRAK_MODE // @DisplayName: Surface Tracking Mode // @Description: set which surface to track in surface tracking @@ -1035,6 +1036,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced // @RebootRequired: True AP_GROUPINFO("SURFTRAK_MODE", 51, ParametersG2, surftrak_mode, (uint8_t)Copter::SurfaceTracking::Surface::GROUND), +#endif // @Param: FS_DR_ENABLE // @DisplayName: DeadReckon Failsafe Action diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 0e3d8920aa..087c6f9e3d 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -652,7 +652,7 @@ public: AP_Int32 flight_options; -#if RANGEFINDER_ENABLED == ENABLED +#if AP_RANGEFINDER_ENABLED AP_Float rangefinder_filt; #endif diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index 02bc8d7bcf..3cb3b99ea2 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -258,7 +258,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc break; #endif -#if RANGEFINDER_ENABLED == ENABLED +#if AP_RANGEFINDER_ENABLED case AUX_FUNC::RANGEFINDER: // enable or disable the rangefinder if ((ch_flag == AuxSwitchPos::HIGH) && @@ -544,6 +544,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc break; } +#if AP_RANGEFINDER_ENABLED case AUX_FUNC::SURFACE_TRACKING: switch (ch_flag) { case AuxSwitchPos::LOW: @@ -557,6 +558,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc break; } break; +#endif case AUX_FUNC::FLIGHTMODE_PAUSE: switch (ch_flag) { diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 6e177a4b86..682d2e1c6c 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -70,10 +70,6 @@ // Rangefinder // -#ifndef RANGEFINDER_ENABLED - # define RANGEFINDER_ENABLED ENABLED -#endif - #ifndef RANGEFINDER_FILT_DEFAULT # define RANGEFINDER_FILT_DEFAULT 0.5f // filter for rangefinder distance #endif diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 0be2f610b3..91d9e79a65 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -42,11 +42,13 @@ void Copter::check_dynamic_flight(void) moving = (motors->get_throttle() > 0.8f || ahrs.pitch_sensor < -1500); } +#if AP_RANGEFINDER_ENABLED if (!moving && rangefinder_state.enabled && rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) { // when we are more than 2m from the ground with good // rangefinder lock consider it to be dynamic flight moving = (rangefinder.distance_cm_orient(ROTATION_PITCH_270) > 200); } +#endif if (moving) { // if moving for 2 seconds, set the dynamic flight flag diff --git a/ArduCopter/landing_gear.cpp b/ArduCopter/landing_gear.cpp index 2aec67d2b2..5c77b8231f 100644 --- a/ArduCopter/landing_gear.cpp +++ b/ArduCopter/landing_gear.cpp @@ -14,6 +14,7 @@ void Copter::landinggear_update() int32_t height_cm = flightmode->get_alt_above_ground_cm(); // use rangefinder if available +#if AP_RANGEFINDER_ENABLED switch (rangefinder.status_orient(ROTATION_PITCH_270)) { case RangeFinder::Status::NotConnected: case RangeFinder::Status::NoData: @@ -31,6 +32,7 @@ void Copter::landinggear_update() height_cm = rangefinder_state.alt_cm_filt.get(); break; } +#endif // AP_RANGEFINDER_ENABLED landinggear.update(height_cm * 0.01f); // convert cm->m for update call } diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index c4e8068982..ce191b0c1e 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -420,7 +420,9 @@ bool Copter::set_mode(const uint8_t new_mode, const ModeReason reason) // called at 100hz or more void Copter::update_flight_mode() { +#if AP_RANGEFINDER_ENABLED surface_tracking.invalidate_for_logging(); // invalidate surface tracking alt, flight mode will set to true if used +#endif flightmode->run(); } diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index 91c7943867..09e5ad0a88 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -87,8 +87,10 @@ void ModeAltHold::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); +#if AP_RANGEFINDER_ENABLED // update the vertical offset based on the surface measurement copter.surface_tracking.update_surface_offset(); +#endif // Send the commanded climb rate to the position controller pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 3733093544..f6bd5d9377 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1197,8 +1197,10 @@ void ModeAuto::loiter_to_alt_run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); +#if AP_RANGEFINDER_ENABLED // update the vertical offset based on the surface measurement copter.surface_tracking.update_surface_offset(); +#endif // Send the commanded climb rate to the position controller pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index 2c31f8d0a3..8b2578d49b 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -114,8 +114,10 @@ void ModeCircle::run() // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); +#if AP_RANGEFINDER_ENABLED // update the vertical offset based on the surface measurement copter.surface_tracking.update_surface_offset(); +#endif copter.failsafe_terrain_set_status(copter.circle_nav->update(target_climb_rate)); pos_control->update_z_controller(); diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 24ab8806be..1e53107c6b 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -302,8 +302,10 @@ void ModeFlowHold::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); +#if AP_RANGEFINDER_ENABLED // update the vertical offset based on the surface measurement copter.surface_tracking.update_surface_offset(); +#endif // Send the commanded climb rate to the position controller pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 6e0d0b7459..a771c0fdaa 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -128,6 +128,7 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm) // calculate target altitude and frame (either alt-above-ekf-origin or alt-above-terrain) int32_t alt_target_cm; bool alt_target_terrain = false; +#if AP_RANGEFINDER_ENABLED if (wp_nav->rangefinder_used_and_healthy() && wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER && takeoff_alt_cm < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) { @@ -138,7 +139,9 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm) // provide target altitude as alt-above-terrain alt_target_cm = takeoff_alt_cm; alt_target_terrain = true; - } else { + } else +#endif + { // interpret altitude as alt-above-home Location target_loc = copter.current_loc; target_loc.set_alt_cm(takeoff_alt_cm, Location::AltFrame::ABOVE_HOME); diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 1b76c41430..7acb8c19ba 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -191,8 +191,10 @@ void ModeLoiter::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); +#if AP_RANGEFINDER_ENABLED // update the vertical offset based on the surface measurement copter.surface_tracking.update_surface_offset(); +#endif // Send the commanded climb rate to the position controller pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index c90e36a18e..08e65cfa09 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -158,8 +158,10 @@ void ModePosHold::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); +#if AP_RANGEFINDER_ENABLED // update the vertical offset based on the surface measurement copter.surface_tracking.update_surface_offset(); +#endif // Send the commanded climb rate to the position controller pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 7647e4973c..44f3935a82 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -108,8 +108,10 @@ void ModeSport::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); +#if AP_RANGEFINDER_ENABLED // update the vertical offset based on the surface measurement copter.surface_tracking.update_surface_offset(); +#endif // Send the commanded climb rate to the position controller pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 3887f1bd52..135ad32cc9 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -244,9 +244,11 @@ void ModeZigZag::return_to_manual_control(bool maintain_target) if (maintain_target) { const Vector3f& wp_dest = wp_nav->get_wp_destination(); loiter_nav->init_target(wp_dest.xy()); +#if AP_RANGEFINDER_ENABLED if (wp_nav->origin_and_destination_are_terrain_alt()) { copter.surface_tracking.set_target_alt_cm(wp_dest.z); } +#endif } else { loiter_nav->init_target(); } @@ -377,8 +379,10 @@ void ModeZigZag::manual_control() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); +#if AP_RANGEFINDER_ENABLED // update the vertical offset based on the surface measurement copter.surface_tracking.update_surface_offset(); +#endif // Send the commanded climb rate to the position controller pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); @@ -454,9 +458,11 @@ bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Ve // if we have a downward facing range finder then use terrain altitude targets terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy(); if (terrain_alt) { +#if AP_RANGEFINDER_ENABLED if (!copter.surface_tracking.get_target_alt_cm(next_dest.z)) { next_dest.z = copter.rangefinder_state.alt_cm_filt.get(); } +#endif } else { next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_position_z_up_cm(); } @@ -506,9 +512,11 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con // if we have a downward facing range finder then use terrain altitude targets terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy(); if (terrain_alt) { +#if AP_RANGEFINDER_ENABLED if (!copter.surface_tracking.get_target_alt_cm(next_dest.z)) { next_dest.z = copter.rangefinder_state.alt_cm_filt.get(); } +#endif } else { next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_position_z_up_cm(); } diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 8cb09dcd93..f83a1c25e3 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -8,9 +8,9 @@ void Copter::read_barometer(void) baro_alt = barometer.get_altitude() * 100.0f; } +#if AP_RANGEFINDER_ENABLED void Copter::init_rangefinder(void) { -#if RANGEFINDER_ENABLED == ENABLED && AP_RANGEFINDER_ENABLED rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN); rangefinder.init(ROTATION_PITCH_270); rangefinder_state.alt_cm_filt.set_cutoff_frequency(g2.rangefinder_filt); @@ -19,13 +19,11 @@ void Copter::init_rangefinder(void) // upward facing range finder rangefinder_up_state.alt_cm_filt.set_cutoff_frequency(g2.rangefinder_filt); rangefinder_up_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_90); -#endif } // return rangefinder altitude in centimeters void Copter::read_rangefinder(void) { -#if RANGEFINDER_ENABLED == ENABLED && AP_RANGEFINDER_ENABLED rangefinder.update(); rangefinder_state.update(); @@ -36,9 +34,8 @@ void Copter::read_rangefinder(void) g2.proximity.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get()); } #endif - -#endif } +#endif // AP_RANGEFINDER_ENABLED // return true if rangefinder_alt can be used bool Copter::rangefinder_alt_ok() const @@ -73,7 +70,7 @@ void Copter::update_rangefinder_terrain_offset() // helper function to get inertially interpolated rangefinder height. bool Copter::get_rangefinder_height_interpolated_cm(int32_t& ret) const { -#if RANGEFINDER_ENABLED == ENABLED && AP_RANGEFINDER_ENABLED +#if AP_RANGEFINDER_ENABLED return rangefinder_state.get_rangefinder_height_interpolated_cm(ret); #else return false; diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp index 57ceda03ef..65b145b000 100644 --- a/ArduCopter/surface_tracking.cpp +++ b/ArduCopter/surface_tracking.cpp @@ -1,10 +1,12 @@ #include "Copter.h" +#if AP_RANGEFINDER_ENABLED + // update_surface_offset - manages the vertical offset of the position controller to follow the measured ground or ceiling // level measured using the range finder. void Copter::SurfaceTracking::update_surface_offset() { -#if RANGEFINDER_ENABLED == ENABLED +#if AP_RANGEFINDER_ENABLED // check for timeout const uint32_t now_ms = millis(); const bool timeout = (now_ms - last_update_ms) > SURFACE_TRACKING_TIMEOUT_MS; @@ -111,3 +113,5 @@ void Copter::SurfaceTracking::set_surface(Surface new_surface) surface = new_surface; reset_target = true; } + +#endif // AP_RANGEFINDER_ENABLED diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 40534a68cc..864b1569f8 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -52,9 +52,11 @@ void Copter::init_ardupilot() init_rc_in(); // sets up rc channels from radio +#if AP_RANGEFINDER_ENABLED // initialise surface to be tracked in SurfaceTracking // must be before rc init to not override initial switch position surface_tracking.init((SurfaceTracking::Surface)copter.g2.surftrak_mode.get()); +#endif // allocate the motors class allocate_motors(); @@ -133,7 +135,7 @@ void Copter::init_ardupilot() barometer.set_log_baro_bit(MASK_LOG_IMU); barometer.calibrate(); -#if RANGEFINDER_ENABLED == ENABLED +#if AP_RANGEFINDER_ENABLED // initialise rangefinder init_rangefinder(); #endif diff --git a/ArduCopter/terrain.cpp b/ArduCopter/terrain.cpp index 5933de460e..ae02aab3b5 100644 --- a/ArduCopter/terrain.cpp +++ b/ArduCopter/terrain.cpp @@ -8,7 +8,7 @@ void Copter::terrain_update() // tell the rangefinder our height, so it can go into power saving // mode if available -#if RANGEFINDER_ENABLED == ENABLED +#if AP_RANGEFINDER_ENABLED float height; if (terrain.height_above_terrain(height, true)) { rangefinder.set_estimated_terrain_height(height);