From 78be1067a85280f2e5a8afbc9694ce5a302edd4f Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 16 Jul 2019 11:43:55 +0930 Subject: [PATCH] AC_AttitudeControl: Alt Hold init bug --- .../AC_AttitudeControl/AC_PosControl.cpp | 21 +++++++------------ libraries/AC_AttitudeControl/AC_PosControl.h | 1 - .../AC_AttitudeControl/AC_PosControl_Sub.cpp | 4 +++- 3 files changed, 11 insertions(+), 15 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index c4246d76d1..ff9cb40faa 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -215,7 +215,6 @@ AC_PosControl::AC_PosControl(const AP_AHRS_View& ahrs, const AP_InertialNav& ina _flags.reset_desired_vel_to_pos = true; _flags.reset_accel_to_lean_xy = true; _flags.reset_rate_to_accel_z = true; - _flags.reset_accel_to_throttle = true; _flags.freeze_ff_z = true; _flags.use_desvel_ff_z = true; _limit.pos_up = true; @@ -378,9 +377,10 @@ void AC_PosControl::relax_alt_hold_controllers(float throttle_setting) _vel_last.z = _inav.get_velocity_z(); _accel_desired.z = 0.0f; _accel_last_z_cms = 0.0f; - _accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f; - _flags.reset_accel_to_throttle = true; + _flags.reset_rate_to_accel_z = true; _pid_accel_z.set_integrator((throttle_setting - _motors.get_throttle_hover()) * 1000.0f); + _accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f; + _pid_accel_z.reset_filter(); } // get_alt_error - returns altitude error in cm @@ -468,7 +468,9 @@ void AC_PosControl::update_z_controller() const uint64_t now_us = AP_HAL::micros64(); if (now_us - _last_update_z_us > POSCONTROL_ACTIVE_TIMEOUT_US) { _flags.reset_rate_to_accel_z = true; - _flags.reset_accel_to_throttle = true; + _pid_accel_z.set_integrator((_motors.get_throttle() - _motors.get_throttle_hover()) * 1000.0f); + _accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f; + _pid_accel_z.reset_filter(); } _last_update_z_us = now_us; @@ -588,15 +590,8 @@ void AC_PosControl::run_z_controller() // Calculate Earth Frame Z acceleration z_accel_meas = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f; - // reset target acceleration if this controller has just been engaged - if (_flags.reset_accel_to_throttle) { - // Reset Filter - _accel_error.z = 0; - _flags.reset_accel_to_throttle = false; - } else { - // calculate accel error - _accel_error.z = _accel_target.z - z_accel_meas; - } + // calculate accel error + _accel_error.z = _accel_target.z - z_accel_meas; // set input to PID _pid_accel_z.set_input_filter_all(_accel_error.z); diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index ed88569c8a..86db19321f 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -309,7 +309,6 @@ protected: uint16_t reset_desired_vel_to_pos : 1; // 1 if we should reset the rate_to_accel_xy step uint16_t reset_accel_to_lean_xy : 1; // 1 if we should reset the accel to lean angle step uint16_t reset_rate_to_accel_z : 1; // 1 if we should reset the rate_to_accel_z step - uint16_t reset_accel_to_throttle : 1; // 1 if we should reset the accel_to_throttle step of the z-axis controller uint16_t freeze_ff_z : 1; // 1 used to freeze velocity to accel feed forward for one iteration uint16_t use_desvel_ff_z : 1; // 1 to use z-axis desired velocity as feed forward into velocity step uint16_t vehicle_horiz_vel_override : 1; // 1 if we should use _vehicle_horiz_vel as our velocity process variable for one timestep diff --git a/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp b/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp index 2b6821a9ed..ef0a9a125f 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp @@ -99,6 +99,8 @@ void AC_PosControl_Sub::relax_alt_hold_controllers() _vel_last.z = _inav.get_velocity_z(); _accel_desired.z = 0.0f; _accel_last_z_cms = 0.0f; + _flags.reset_rate_to_accel_z = true; + _pid_accel_z.set_integrator(-_motors.get_throttle_hover() * 1000.0f); _accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f; - _flags.reset_accel_to_throttle = true; + _pid_accel_z.reset_filter(); }