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.
ArduPilot · May 7, 2024 · 5824a12 · 5824a12
1 parent 7a21f2b
commit 5824a12
Show file tree

Hide file tree

Showing 10 changed files with 5 additions and 15 deletions.
diff --git 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
@@ -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
@@ -400,9 +400,6 @@ class Plane : public AP_Vehicle {
     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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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));