From 5824a12b2e0c4a5d8cf1accdf07383c6cce6e5d9 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Mon, 6 May 2024 02:01:08 -0500 Subject: [PATCH] Plane: remove altitude_error_cm variable This variable updated unpredictably, and it was easy to introduce bugs. It was not used in many places and is clearer to calculate the error directly when needed. --- ArduPlane/GCS_Mavlink.cpp | 4 ++-- ArduPlane/Log.cpp | 2 +- ArduPlane/Plane.h | 3 --- ArduPlane/commands_logic.cpp | 1 - ArduPlane/mode.cpp | 2 -- ArduPlane/mode_guided.cpp | 1 - ArduPlane/mode_qrtl.cpp | 1 - ArduPlane/mode_rtl.cpp | 2 +- ArduPlane/navigation.cpp | 2 -- ArduPlane/quadplane.cpp | 2 +- 10 files changed, 5 insertions(+), 15 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index aabe9cfbe5..8ffa3f8297 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -234,7 +234,7 @@ void GCS_MAVLINK_Plane::send_nav_controller_output() const nav_controller->nav_bearing_cd() * 0.01, nav_controller->target_bearing_cd() * 0.01, MIN(plane.auto_state.wp_distance, UINT16_MAX), - plane.altitude_error_cm * 0.01, + plane.calc_altitude_error_cm() * 0.01, plane.airspeed_error * 100, // incorrect units; see PR#7933 nav_controller->crosstrack_error()); } @@ -1463,7 +1463,7 @@ int16_t GCS_MAVLINK_Plane::high_latency_target_altitude() const return (plane.control_mode != &plane.mode_qstabilize) ? 0.01 * (global_position_current.alt + quadplane.pos_control->get_pos_error_z_cm()) : 0; } #endif - return 0.01 * (global_position_current.alt + plane.altitude_error_cm); + return 0.01 * (global_position_current.alt + plane.calc_altitude_error_cm()); } uint8_t GCS_MAVLINK_Plane::high_latency_tgt_heading() const diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 12067036e5..2eac3ece63 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -178,7 +178,7 @@ void Plane::Log_Write_Nav_Tuning() wp_distance : auto_state.wp_distance, target_bearing_cd : (int16_t)nav_controller->target_bearing_cd(), nav_bearing_cd : (int16_t)nav_controller->nav_bearing_cd(), - altitude_error_cm : (int16_t)altitude_error_cm, + altitude_error_cm : (int16_t)plane.calc_altitude_error_cm(), xtrack_error : nav_controller->crosstrack_error(), xtrack_error_i : nav_controller->crosstrack_error_integrator(), airspeed_error : airspeed_error, diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index ed07ed9433..613cc25a9b 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -400,9 +400,6 @@ private: int32_t groundspeed_undershoot; bool groundspeed_undershoot_is_valid; - // Difference between current altitude and desired altitude. Centimeters - int32_t altitude_error_cm; - // speed scaler for control surfaces, updated at 10Hz float surface_speed_scaler = 1.0; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index a904d7ce7e..728ff97ad9 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -350,7 +350,6 @@ void Plane::do_RTL(int32_t rtl_altitude_AMSL_cm) next_WP_loc = calc_best_rally_or_home_location(current_loc, rtl_altitude_AMSL_cm); setup_terrain_target_alt(next_WP_loc); set_target_altitude_location(next_WP_loc); - plane.altitude_error_cm = calc_altitude_error_cm(); if (aparm.loiter_radius < 0) { loiter.direction = -1; diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 30ec5fc5b0..0a4415ff8b 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -191,8 +191,6 @@ void Mode::update_target_altitude() } else { plane.set_target_altitude_location(plane.next_WP_loc); } - - plane.altitude_error_cm = plane.calc_altitude_error_cm(); } // returns true if the vehicle can be armed in this mode diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 85462166d1..86a3bf60c1 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -147,7 +147,6 @@ void ModeGuided::update_target_altitude() } plane.guided_state.last_target_alt = temp.alt; plane.set_target_altitude_location(temp); - plane.altitude_error_cm = plane.calc_altitude_error_cm(); } else #endif // OFFBOARD_GUIDED == ENABLED { diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp index 7de44dbc9e..87c3b77128 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/ArduPlane/mode_qrtl.cpp @@ -211,7 +211,6 @@ void ModeQRTL::update_target_altitude() Location loc = plane.next_WP_loc; loc.alt += alt*100; plane.set_target_altitude_location(loc); - plane.altitude_error_cm = plane.calc_altitude_error_cm(); } // only nudge during approach diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index c35a3b64a6..15d9728006 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -106,7 +106,7 @@ void ModeRTL::navigate() if ((plane.g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START) || (plane.g.rtl_autoland == RtlAutoland::RTL_THEN_DO_LAND_START && plane.reached_loiter_target() && - labs(plane.altitude_error_cm) < 1000)) + labs(plane.calc_altitude_error_cm()) < 1000)) { // we've reached the RTL point, see if we have a landing sequence if (plane.have_position && plane.mission.jump_to_landing_sequence(plane.current_loc)) { diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 4dc71bbee3..6a78965871 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -425,8 +425,6 @@ void Plane::update_fbwb_speed_height(void) check_fbwb_altitude(); - altitude_error_cm = calc_altitude_error_cm(); - calc_throttle(); calc_nav_pitch(); } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 36e0b21619..2809694fce 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1417,7 +1417,7 @@ float QuadPlane::assist_climb_rate_cms(void) const float climb_rate; if (plane.control_mode->does_auto_throttle()) { // use altitude_error_cm, spread over 10s interval - climb_rate = plane.altitude_error_cm * 0.1f; + climb_rate = plane.calc_altitude_error_cm() * 0.1f; } else { // otherwise estimate from pilot input climb_rate = plane.g.flybywire_climb_rate * (plane.nav_pitch_cd/(plane.aparm.pitch_limit_max*100));