diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index e2571439b0..24a3666d97 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -382,7 +382,6 @@ void Plane::send_pid_tuning(mavlink_channel_t chan) void Plane::send_rangefinder(mavlink_channel_t chan) { -#if RANGEFINDER_ENABLED == ENABLED if (!rangefinder.has_data()) { // no sonar to report return; @@ -391,7 +390,6 @@ void Plane::send_rangefinder(mavlink_channel_t chan) chan, rangefinder.distance_cm() * 0.01f, rangefinder.voltage_mv()*0.001f); -#endif } void Plane::send_current_waypoint(mavlink_channel_t chan) @@ -1897,11 +1895,9 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) handle_gps_inject(msg, plane.gps); break; -#if RANGEFINDER_ENABLED == ENABLED case MAVLINK_MSG_ID_DISTANCE_SENSOR: plane.rangefinder.handle_msg(msg); break; -#endif case MAVLINK_MSG_ID_TERRAIN_DATA: case MAVLINK_MSG_ID_TERRAIN_CHECK: diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 817a5b12ec..de6a95722e 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -342,7 +342,6 @@ struct PACKED log_Sonar { // Write a sonar packet void Plane::Log_Write_Sonar() { -#if RANGEFINDER_ENABLED == ENABLED uint16_t distance = 0; if (rangefinder.status() == RangeFinder::RangeFinder_Good) { distance = rangefinder.distance_cm(); @@ -359,7 +358,6 @@ void Plane::Log_Write_Sonar() DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.Log_Write_RFND(rangefinder); -#endif } struct PACKED log_Optflow { diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index feea092dc7..4bf01743e7 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1116,11 +1116,9 @@ const AP_Param::Info Plane::var_info[] = { GSCALAR(parachute_channel, "CHUTE_CHAN", 0), #endif -#if RANGEFINDER_ENABLED == ENABLED // @Group: RNGFND // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp GOBJECT(rangefinder, "RNGFND", RangeFinder), -#endif // @Param: RNGFND_LANDING // @DisplayName: Enable rangefinder for landing diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 1b90ab536d..022c4bcfe8 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -215,8 +215,6 @@ private: AP_InertialSensor ins; -#if RANGEFINDER_ENABLED == ENABLED - // rangefinder RangeFinder rangefinder {serial_manager}; struct { @@ -232,7 +230,6 @@ private: float height_estimate; float last_distance; } rangefinder_state; -#endif AP_RPM rpm_sensor; diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 4e5bfa212a..3abb3d0123 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -139,11 +139,9 @@ int32_t Plane::relative_altitude_abs_cm(void) */ float Plane::relative_ground_altitude(bool use_rangefinder_if_available) { -#if RANGEFINDER_ENABLED == ENABLED if (use_rangefinder_if_available && rangefinder_state.in_range) { return rangefinder_state.height_estimate; } -#endif #if AP_TERRAIN_AVAILABLE float altitude; @@ -571,7 +569,6 @@ float Plane::lookahead_adjustment(void) */ float Plane::rangefinder_correction(void) { -#if RANGEFINDER_ENABLED == ENABLED if (millis() - rangefinder_state.last_correction_time_ms > 5000) { // we haven't had any rangefinder data for 5s - don't use it return 0; @@ -588,12 +585,8 @@ float Plane::rangefinder_correction(void) } return rangefinder_state.correction; -#else - return 0; -#endif } -#if RANGEFINDER_ENABLED == ENABLED /* update the offset between rangefinder height and terrain height */ @@ -680,4 +673,3 @@ void Plane::rangefinder_height_update(void) } } -#endif diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 9ca6f79076..3c07be7536 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -401,10 +401,8 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd) auto_state.land_slope = 0; -#if RANGEFINDER_ENABLED == ENABLED // zero rangefinder state, start to accumulate good samples now memset(&rangefinder_state, 0, sizeof(rangefinder_state)); -#endif } void Plane::loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd) diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 4411078c7e..8692af5c1a 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -70,7 +70,6 @@ #endif #endif -#define RANGEFINDER_ENABLED ENABLED ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduPlane/landing.cpp b/ArduPlane/landing.cpp index befaf8002d..00862fce12 100644 --- a/ArduPlane/landing.cpp +++ b/ArduPlane/landing.cpp @@ -51,11 +51,7 @@ bool Plane::verify_land() rangefinder data (to prevent us keeping throttle on after landing if we've had positive baro drift) */ -#if RANGEFINDER_ENABLED == ENABLED bool rangefinder_in_range = rangefinder_state.in_range; -#else - bool rangefinder_in_range = false; -#endif // flare check: // 1) below flare alt/sec requires approach stage check because if sec/alt are set too @@ -160,7 +156,6 @@ void Plane::disarm_if_autoland_complete() void Plane::adjust_landing_slope_for_rangefinder_bump(void) { -#if RANGEFINDER_ENABLED == ENABLED // check the rangefinder correction for a large change. When found, recalculate the glide slope. This is done by // determining the slope from your current location to the land point then following that back up to the approach // altitude and moving the prev_wp to that location. From there @@ -206,7 +201,6 @@ void Plane::adjust_landing_slope_for_rangefinder_bump(void) g.land_slope_recalc_steep_threshold_to_abort = 0; // disable this feature so we only perform it once } } -#endif } /* diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index 6ddfe4cc61..f16169e941 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -14,9 +14,7 @@ void Plane::init_barometer(bool full_calibration) void Plane::init_rangefinder(void) { -#if RANGEFINDER_ENABLED == ENABLED rangefinder.init(); -#endif } /* @@ -24,7 +22,6 @@ void Plane::init_rangefinder(void) */ void Plane::read_rangefinder(void) { -#if RANGEFINDER_ENABLED == ENABLED // notify the rangefinder of our approximate altitude above ground to allow it to power on // during low-altitude flight when configured to power down during higher-altitude flight @@ -55,7 +52,6 @@ void Plane::read_rangefinder(void) Log_Write_Sonar(); rangefinder_height_update(); -#endif } /* @@ -347,7 +343,6 @@ void Plane::update_sensor_status_flags(void) } #endif -#if RANGEFINDER_ENABLED == ENABLED if (rangefinder.num_sensors() > 0) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; if (g.rangefinder_landing) { @@ -357,7 +352,6 @@ void Plane::update_sensor_status_flags(void) control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } } -#endif if (aparm.throttle_min < 0 && channel_throttle->get_servo_out() < 0) { control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR;