From 0d8729822106d9ea31bad2ea40588e3741249f13 Mon Sep 17 00:00:00 2001 From: lthall Date: Sat, 31 May 2014 23:05:14 +0930 Subject: [PATCH] AC_PosControl: freeze feed forward and vector fixes --- .../AC_AttitudeControl/AC_PosControl.cpp | 53 ++++++++++++------- libraries/AC_AttitudeControl/AC_PosControl.h | 8 +-- 2 files changed, 39 insertions(+), 22 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 05eeb988fc..4d8eb5710b 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -45,7 +45,6 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, _leash(POSCONTROL_LEASH_LENGTH_MIN), _roll_target(0.0), _pitch_target(0.0), - _vel_target_filt_z(0), _alt_max(0), _distance_to_target(0), _xy_step(0), @@ -281,15 +280,14 @@ void AC_PosControl::pos_to_rate_z() } // call rate based throttle controller which will update accel based throttle controller targets - rate_to_accel_z(_vel_target.z); + rate_to_accel_z(); } // rate_to_accel_z - calculates desired accel required to achieve the velocity target // calculates desired acceleration and calls accel throttle controller -void AC_PosControl::rate_to_accel_z(float vel_target_z) +void AC_PosControl::rate_to_accel_z() { const Vector3f& curr_vel = _inav.get_velocity(); - float z_target_speed_delta; // The change in requested speed float p; // used to capture pid values for logging float desired_accel; // the target acceleration if the accel based throttle is enabled, otherwise the output to be sent to the motors @@ -306,28 +304,41 @@ void AC_PosControl::rate_to_accel_z(float vel_target_z) _limit.vel_up = true; } + // reset last velocity target to current target + if (_flags.reset_rate_to_accel_z) { + _vel_last.z = _vel_target.z; + _flags.reset_rate_to_accel_z = false; + } + + // feed forward desired acceleration calculation + if (_dt > 0.0f) { + if(!_limit.freeze_ff_z) { + _accel_feedforward.z = (_vel_target.z - _vel_last.z)/_dt; + } else { + _limit.freeze_ff_z = false; + } + } else { + _accel_feedforward.z = 0.0f; + } + + // store this iteration's velocities for the next iteration + _vel_last.z = _vel_target.z; + // reset velocity error and filter if this controller has just been engaged if (_flags.reset_rate_to_accel_z) { // Reset Filter _vel_error.z = 0; - _vel_target_filt_z = vel_target_z; desired_accel = 0; _flags.reset_rate_to_accel_z = false; } else { - // calculate rate error and filter with cut off frequency of 2 Hz - //To-Do: adjust constant below based on update rate - _vel_error.z = _vel_error.z + 0.20085f * ((vel_target_z - curr_vel.z) - _vel_error.z); - // feed forward acceleration based on change in the filtered desired speed. - z_target_speed_delta = 0.20085f * (vel_target_z - _vel_target_filt_z); - _vel_target_filt_z = _vel_target_filt_z + z_target_speed_delta; - desired_accel = z_target_speed_delta / _dt; + _vel_error.z = (_vel_target.z - curr_vel.z); } // calculate p p = _p_alt_rate.kP() * _vel_error.z; // consolidate and constrain target acceleration - desired_accel += p; + desired_accel = _accel_feedforward.z + p; desired_accel = constrain_int32(desired_accel, -32000, 32000); // set target for accel based throttle controller @@ -686,11 +697,15 @@ void AC_PosControl::rate_to_accel_xy(float dt) // feed forward desired acceleration calculation if (dt > 0.0f) { - _accel_target.x = (_vel_target.x - _vel_last.x)/dt; - _accel_target.y = (_vel_target.y - _vel_last.y)/dt; + if(!_limit.freeze_ff_xy) { + _accel_feedforward.x = (_vel_target.x - _vel_last.x)/dt; + _accel_feedforward.y = (_vel_target.y - _vel_last.y)/dt; + } else { + _limit.freeze_ff_xy = false; + } } else { - _accel_target.x = 0.0f; - _accel_target.y = 0.0f; + _accel_feedforward.x = 0.0f; + _accel_feedforward.y = 0.0f; } // store this iteration's velocities for the next iteration @@ -714,8 +729,8 @@ void AC_PosControl::rate_to_accel_xy(float dt) } // combine feed forward accel with PID output from velocity error - _accel_target.x += _pid_rate_lat.get_p(_vel_error.x) + lat_i + _pid_rate_lat.get_d(_vel_error.x, dt); - _accel_target.y += _pid_rate_lon.get_p(_vel_error.y) + lon_i + _pid_rate_lon.get_d(_vel_error.y, dt); + _accel_target.x = _accel_feedforward.x + _pid_rate_lat.get_p(_vel_error.x) + lat_i + _pid_rate_lat.get_d(_vel_error.x, dt); + _accel_target.y = _accel_feedforward.y + _pid_rate_lon.get_p(_vel_error.y) + lon_i + _pid_rate_lon.get_d(_vel_error.y, dt); // scale desired acceleration if it's beyond acceptable limit // To-Do: move this check down to the accel_to_lean_angle method? diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index c06a6e007b..3ccef56491 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -243,6 +243,8 @@ private: uint8_t vel_up : 1; // 1 if we have hit the vertical velocity limit going up uint8_t vel_down : 1; // 1 if we have hit the vertical velocity limit going down uint8_t accel_xy : 1; // 1 if we have hit the horizontal accel limit + uint8_t freeze_ff_xy: 1; // 1 use to freeze feed forward during step updates + uint8_t freeze_ff_z : 1; // 1 use to freeze feed forward during step updates } _limit; /// @@ -257,7 +259,7 @@ private: void pos_to_rate_z(); // rate_to_accel_z - calculates desired accel required to achieve the velocity target - void rate_to_accel_z(float vel_target_z); + void rate_to_accel_z(); // accel_to_throttle - alt hold's acceleration controller void accel_to_throttle(float accel_target_z); @@ -327,10 +329,10 @@ private: Vector2f _vel_desired; // desired velocity in cm/s in lat and lon directions (provided by external callers of move_target_at_rate() method) Vector3f _vel_target; // velocity target in cm/s calculated by pos_to_rate step Vector3f _vel_error; // error between desired and actual acceleration in cm/s - Vector2f _vel_last; // previous iterations velocity in cm/s - float _vel_target_filt_z; // filtered target vertical velocity + Vector3f _vel_last; // previous iterations velocity in cm/s Vector3f _accel_target; // desired acceleration in cm/s/s // To-Do: are xy actually required? Vector3f _accel_error; // desired acceleration in cm/s/s // To-Do: are xy actually required? + Vector3f _accel_feedforward; // feedforward acceleration in cm/s/s float _alt_max; // max altitude - should be updated from the main code with altitude limit from fence float _distance_to_target; // distance to position target - for reporting only uint8_t _xy_step; // used to decide which portion of horizontal position controller to run during this iteration