mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 23:48:31 -04:00
AC_PosControl: add interface to override horizontal velocity process variable
This commit is contained in:
parent
25a14fe0dc
commit
5a8db4f271
libraries/AC_AttitudeControl
@ -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);
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user