From 5a8db4f2716530f3c4ad9e4b92bb870329fbc12c Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 20 Jul 2016 11:38:37 -0700 Subject: [PATCH] AC_PosControl: add interface to override horizontal velocity process variable --- .../AC_AttitudeControl/AC_PosControl.cpp | 19 +++++++++++++------ libraries/AC_AttitudeControl/AC_PosControl.h | 5 +++++ 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index f28b5d80f7..8ea033d49b 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -237,7 +237,7 @@ void AC_PosControl::relax_alt_hold_controllers(float throttle_setting) _pos_target.z = _inav.get_altitude(); _vel_desired.z = 0.0f; _flags.use_desvel_ff_z = false; - _vel_target.z= _inav.get_velocity_z(); + _vel_target.z = _inav.get_velocity_z(); _vel_last.z = _inav.get_velocity_z(); _accel_feedforward.z = 0.0f; _accel_last_z_cms = 0.0f; @@ -568,8 +568,8 @@ void AC_PosControl::set_target_to_stopping_point_xy() /// set_leash_length() should have been called before this method void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const { - const Vector3f curr_pos = _inav.get_position(); - Vector3f curr_vel = _inav.get_velocity(); + const Vector3f curr_pos = _inav.get_position(); + Vector3f curr_vel = _inav.get_velocity(); float linear_distance; // the distance at which we swap from a linear to sqrt response float linear_velocity; // the velocity above which we swap from a linear to sqrt response float stopping_dist; // the distance within the vehicle can stop @@ -866,7 +866,6 @@ void AC_PosControl::pos_to_rate_xy(xy_mode mode, float dt, float ekfNavVelGainSc /// converts desired velocities in lat/lon directions to accelerations in lat/lon frame void AC_PosControl::rate_to_accel_xy(float dt, float ekfNavVelGainScaler) { - const Vector3f &vel_curr = _inav.get_velocity(); // current velocity in cm/s Vector2f vel_xy_p, vel_xy_i; // reset last velocity target to current target @@ -876,6 +875,14 @@ void AC_PosControl::rate_to_accel_xy(float dt, float ekfNavVelGainScaler) _flags.reset_rate_to_accel_xy = false; } + // check if vehicle velocity is being overridden + if (_flags.vehicle_horiz_vel_override) { + _flags.vehicle_horiz_vel_override = false; + } else { + _vehicle_horiz_vel.x = _inav.get_velocity().x; + _vehicle_horiz_vel.y = _inav.get_velocity().y; + } + // feed forward desired acceleration calculation if (dt > 0.0f) { if (!_flags.freeze_ff_xy) { @@ -895,8 +902,8 @@ void AC_PosControl::rate_to_accel_xy(float dt, float ekfNavVelGainScaler) _vel_last.y = _vel_target.y; // calculate velocity error - _vel_error.x = _vel_target.x - vel_curr.x; - _vel_error.y = _vel_target.y - vel_curr.y; + _vel_error.x = _vel_target.x - _vehicle_horiz_vel.x; + _vel_error.y = _vel_target.y - _vehicle_horiz_vel.y; // call pi controller _pi_vel_xy.set_input(_vel_error); diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index abbaa8988d..318e833104 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -229,6 +229,9 @@ public: /// when update_vel_controller_xyz is next called the position target is moved based on the desired velocity void set_desired_velocity(const Vector3f &des_vel) { _vel_desired = des_vel; freeze_ff_xy(); } + // overrides the velocity process variable for one timestep + void override_vehicle_velocity_xy(const Vector2f& vel_xy) { _vehicle_horiz_vel = vel_xy; _flags.vehicle_horiz_vel_override = true; } + /// freeze_ff_z - used to stop the feed forward being calculated during a known discontinuity void freeze_ff_z() { _flags.freeze_ff_z = true; } @@ -302,6 +305,7 @@ private: uint16_t freeze_ff_xy : 1; // 1 use to freeze feed forward during step updates 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 } _flags; // limit flags structure @@ -401,6 +405,7 @@ private: 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 + Vector2f _vehicle_horiz_vel; // velocity to use if _flags.vehicle_horiz_vel_override is set 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 LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error