From c14c2d67d491bb2f5a3f20676fdcb1b95ae80473 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 8 Nov 2023 09:23:41 +1100 Subject: [PATCH] ArduPlane: make AP_RANGEFINDER_ENABLED remove more code --- ArduPlane/ArduPlane.cpp | 8 ++++++++ ArduPlane/GCS_Plane.cpp | 2 ++ ArduPlane/Parameters.cpp | 2 ++ ArduPlane/Plane.h | 8 +++++++- ArduPlane/altitude.cpp | 10 +++++++++- ArduPlane/commands_logic.cpp | 13 ++++++++++++- ArduPlane/mode.cpp | 2 ++ ArduPlane/quadplane.cpp | 4 ++++ ArduPlane/sensors.cpp | 3 +++ ArduPlane/system.cpp | 2 ++ 10 files changed, 51 insertions(+), 3 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 6c7cff04d5..bc6d28fbfa 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -81,7 +81,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150, 63), #endif SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300, 66), +#if AP_RANGEFINDER_ENABLED SCHED_TASK(read_rangefinder, 50, 100, 78), +#endif #if AP_ICENGINE_ENABLED SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100, 81), #endif @@ -750,7 +752,9 @@ float Plane::tecs_hgt_afe(void) float hgt_afe; if (flight_stage == AP_FixedWing::FlightStage::LAND) { hgt_afe = height_above_target(); +#if AP_RANGEFINDER_ENABLED hgt_afe -= rangefinder_correction(); +#endif } else { // when in normal flight we pass the hgt_afe as relative // altitude to home @@ -967,7 +971,11 @@ bool Plane::flight_option_enabled(FlightOptions flight_option) const void Plane::precland_update(void) { // alt will be unused if we pass false through as the second parameter: +#if AP_RANGEFINDER_ENABLED return g2.precland.update(rangefinder_state.height_estimate*100, rangefinder_state.in_range); +#else + return g2.precland.update(0, false); +#endif } #endif diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp index 39989dba93..baf2cd9c92 100644 --- a/ArduPlane/GCS_Plane.cpp +++ b/ArduPlane/GCS_Plane.cpp @@ -114,6 +114,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void) } #endif +#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; @@ -124,4 +125,5 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void) control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } } +#endif } diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 4da8435d3a..941dcba774 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -764,6 +764,7 @@ const AP_Param::Info Plane::var_info[] = { GOBJECT(parachute, "CHUTE_", AP_Parachute), #endif +#if AP_RANGEFINDER_ENABLED // @Group: RNGFND // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp GOBJECT(rangefinder, "RNGFND", RangeFinder), @@ -774,6 +775,7 @@ const AP_Param::Info Plane::var_info[] = { // @Values: 0:Disabled,1:Enabled // @User: Standard GSCALAR(rangefinder_landing, "RNGFND_LANDING", 0), +#endif #if AP_TERRAIN_AVAILABLE // @Group: TERRAIN_ diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 613cc25a9b..489276ded4 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -39,7 +39,7 @@ #include // interface and maths for accelerometer calibration #include // ArduPilot Mega DCM Library #include -#include // Range finder library +#include // Range finder library #include // Filter library #include // Photo or video camera #include @@ -206,7 +206,9 @@ private: AP_Int8 *flight_modes = &g.flight_mode1; const uint8_t num_flight_modes = 6; +#if AP_RANGEFINDER_ENABLED AP_FixedWing::Rangefinder_State rangefinder_state; +#endif #if AP_RPM_ENABLED AP_RPM rpm_sensor; @@ -865,9 +867,11 @@ private: float mission_alt_offset(void); float height_above_target(void); float lookahead_adjustment(void); +#if AP_RANGEFINDER_ENABLED float rangefinder_correction(void); void rangefinder_height_update(void); void rangefinder_terrain_correction(float &height); +#endif void stabilize(); void calc_throttle(); void calc_nav_roll(); @@ -1074,8 +1078,10 @@ private: bool rc_throttle_value_ok(void) const; bool rc_failsafe_active(void) const; +#if AP_RANGEFINDER_ENABLED // sensors.cpp void read_rangefinder(void); +#endif // system.cpp void init_ardupilot() override; diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index d30fc76566..b20c1ffeb0 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -115,11 +115,13 @@ int32_t Plane::get_RTL_altitude_cm() const */ float Plane::relative_ground_altitude(bool use_rangefinder_if_available, bool use_terrain_if_available) { +#if AP_RANGEFINDER_ENABLED if (use_rangefinder_if_available && rangefinder_state.in_range) { return rangefinder_state.height_estimate; } +#endif -#if HAL_QUADPLANE_ENABLED +#if HAL_QUADPLANE_ENABLED && AP_RANGEFINDER_ENABLED if (use_rangefinder_if_available && quadplane.in_vtol_land_final() && rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::OutOfRangeLow) { // a special case for quadplane landing when rangefinder goes @@ -252,8 +254,10 @@ int32_t Plane::relative_target_altitude_cm(void) target_altitude.lookahead = lookahead_adjustment(); relative_home_height += target_altitude.lookahead; +#if AP_RANGEFINDER_ENABLED // correct for rangefinder data relative_home_height += rangefinder_correction(); +#endif // we are following terrain, and have terrain data for the // current location. Use it. @@ -262,7 +266,9 @@ int32_t Plane::relative_target_altitude_cm(void) #endif int32_t relative_alt = target_altitude.amsl_cm - home.alt; relative_alt += mission_alt_offset()*100; +#if AP_RANGEFINDER_ENABLED relative_alt += rangefinder_correction() * 100; +#endif return relative_alt; } @@ -608,6 +614,7 @@ float Plane::lookahead_adjustment(void) } +#if AP_RANGEFINDER_ENABLED /* correct target altitude using rangefinder data. Returns offset in meters to correct target altitude. A positive number means we need @@ -747,6 +754,7 @@ void Plane::rangefinder_height_update(void) } } +#endif // AP_RANGEFINDER_ENABLED /* determine if Non Auto Terrain Disable is active and allowed in present control mode diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 728ff97ad9..47a588b861 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -255,14 +255,23 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret } else { // use rangefinder to correct if possible +#if AP_RANGEFINDER_ENABLED float height = height_above_target() - rangefinder_correction(); +#else + float height = height_above_target(); +#endif // for flare calculations we don't want to use the terrain // correction as otherwise we will flare early on rising // ground height -= auto_state.terrain_correction; return landing.verify_land(prev_WP_loc, next_WP_loc, current_loc, height, auto_state.sink_rate, auto_state.wp_proportion, auto_state.last_flying_ms, arming.is_armed(), is_flying(), - g.rangefinder_landing && rangefinder_state.in_range); +#if AP_RANGEFINDER_ENABLED + g.rangefinder_landing && rangefinder_state.in_range +#else + false +#endif + ); } case MAV_CMD_NAV_LOITER_UNLIM: @@ -421,8 +430,10 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd) auto_state.takeoff_pitch_cd = 1000; } +#if AP_RANGEFINDER_ENABLED // zero rangefinder state, start to accumulate good samples now memset(&rangefinder_state, 0, sizeof(rangefinder_state)); +#endif landing.do_land(cmd, relative_altitude); diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 0a4415ff8b..a24db831fd 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -168,7 +168,9 @@ void Mode::update_target_altitude() plane.set_target_altitude_location(plane.next_WP_loc); } else if (plane.landing.is_on_approach()) { plane.landing.setup_landing_glide_slope(plane.prev_WP_loc, plane.next_WP_loc, plane.current_loc, plane.target_altitude.offset_cm); +#if AP_RANGEFINDER_ENABLED plane.landing.adjust_landing_slope_for_rangefinder_bump(plane.rangefinder_state, plane.prev_WP_loc, plane.next_WP_loc, plane.current_loc, plane.auto_state.wp_distance, plane.target_altitude.offset_cm); +#endif } else if (plane.landing.get_target_altitude_location(target_location)) { plane.set_target_altitude_location(target_location); #if HAL_SOARING_ENABLED diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index d2f74a5b55..13003b0da9 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3777,7 +3777,11 @@ float QuadPlane::forward_throttle_pct() // approach the landing point when landing below the takeoff point vel_forward.last_pct = vel_forward.integrator; } else if ((in_vtol_land_final() && motors->limit.throttle_lower) || +#if AP_RANGEFINDER_ENABLED (plane.g.rangefinder_landing && (plane.rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::OutOfRangeLow))) { +#else + false) { +#endif // we're in the settling phase of landing or using a rangefinder that is out of range low, disable fwd motor vel_forward.last_pct = 0; vel_forward.integrator = 0; diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index 91d3ed5b77..a0aebe06b8 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -2,6 +2,7 @@ #include #include +#if AP_RANGEFINDER_ENABLED /* read the rangefinder and update height estimate */ @@ -33,3 +34,5 @@ void Plane::read_rangefinder(void) rangefinder_height_update(); } + +#endif // AP_RANGEFINDER_ENABLED diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 47a1bd7796..d931ac158d 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -36,9 +36,11 @@ void Plane::init_ardupilot() // init baro barometer.init(); +#if AP_RANGEFINDER_ENABLED // initialise rangefinder rangefinder.set_log_rfnd_bit(MASK_LOG_SONAR); rangefinder.init(ROTATION_PITCH_270); +#endif // initialise battery monitoring battery.init();