From 098f8169b0465afc3cc1d84ca41f394a10c7c00d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 23 Apr 2014 14:40:06 +0900 Subject: [PATCH] AC_PosControl: add keep_xy_I_terms method Stops horizontal PID's I terms from being reset when the controller is next updated --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 7 +++++-- libraries/AC_AttitudeControl/AC_PosControl.h | 6 +++++- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index bcd3b5cd7f..14f99bc2b6 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -64,6 +64,7 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, #endif _flags.recalc_leash_xy = true; _flags.recalc_leash_z = true; + _flags.keep_xy_I_terms = false; } /// @@ -428,17 +429,19 @@ float AC_PosControl::get_distance_to_target() const } /// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher -void AC_PosControl::update_xy_controller(bool use_desired_velocity, bool reset_I) +void AC_PosControl::update_xy_controller(bool use_desired_velocity) { // catch if we've just been started uint32_t now = hal.scheduler->millis(); if ((now - _last_update_ms) >= 1000) { _last_update_ms = now; - if (reset_I) { + if (!_flags.keep_xy_I_terms) { reset_I_xy(); } _xy_step = 0; } + // reset keep_xy_I_term flag in case it has been set + _flags.keep_xy_I_terms = false; // check if xy leash needs to be recalculated calc_leash_length_xy(); diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 9c81b065ad..6ba2c112a2 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -165,7 +165,7 @@ public: /// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher /// when use_desired_velocity is true the desired velocity (i.e. feed forward) is incorporated at the pos_to_rate step - void update_xy_controller(bool use_desired_velocity, bool reset_I=true); + void update_xy_controller(bool use_desired_velocity); /// get_stopping_point_xy - calculates stopping point based on current position, velocity, vehicle acceleration /// distance_max allows limiting distance to stopping point @@ -187,6 +187,9 @@ public: /// get_pos_xy_kP - returns xy position controller's kP gain float get_pos_xy_kP() const { return _p_pos_xy.kP(); } + /// keep_xy_I_terms - ensure xy position controller's PID's I terms are not cleared when the xy controller is next run. Reset automatically back to zero in update_xy_controller. + void keep_xy_I_terms() { _flags.keep_xy_I_terms = true; } + /// accessors for reporting const Vector3f& get_vel_target() const { return _vel_target; } const Vector3f& get_accel_target() const { return _accel_target; } @@ -201,6 +204,7 @@ private: uint8_t recalc_leash_xy : 1; // 1 if we should recalculate the xy axis leash length uint8_t force_recalc_xy : 1; // 1 if we want the xy position controller to run at it's next possible time. set by loiter and wp controllers after they have updated the target position. uint8_t slow_cpu : 1; // 1 if we are running on a slow_cpu machine. xy position control is broken up into multiple steps + uint8_t keep_xy_I_terms : 1; // 1 if we are to keep I terms when the position controller is next run. Reset automatically back to zero in update_xy_controller. } _flags; // limit flags structure