Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: add interface to override horizontal velocity process variable

This commit is contained in:
Jonathan Challinger 2016-07-20 11:38:37 -07:00 committed by Randy Mackay
parent 25a14fe0dc
commit 5a8db4f271
2 changed files with 18 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -237,7 +237,7 @@ void AC_PosControl::relax_alt_hold_controllers(float throttle_setting)
_pos_target.z = _inav.get_altitude(); _pos_target.z = _inav.get_altitude();
_vel_desired.z = 0.0f; _vel_desired.z = 0.0f;
_flags.use_desvel_ff_z = false; _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(); _vel_last.z = _inav.get_velocity_z();
_accel_feedforward.z = 0.0f; _accel_feedforward.z = 0.0f;
_accel_last_z_cms = 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 /// set_leash_length() should have been called before this method
void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const
{ {
const Vector3f curr_pos = _inav.get_position(); const Vector3f curr_pos = _inav.get_position();
Vector3f curr_vel = _inav.get_velocity(); Vector3f curr_vel = _inav.get_velocity();
float linear_distance; // the distance at which we swap from a linear to sqrt response 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 linear_velocity; // the velocity above which we swap from a linear to sqrt response
float stopping_dist; // the distance within the vehicle can stop 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 /// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
void AC_PosControl::rate_to_accel_xy(float dt, float ekfNavVelGainScaler) 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; Vector2f vel_xy_p, vel_xy_i;
// reset last velocity target to current target // 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; _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 // feed forward desired acceleration calculation
if (dt > 0.0f) { if (dt > 0.0f) {
if (!_flags.freeze_ff_xy) { 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; _vel_last.y = _vel_target.y;
// calculate velocity error // calculate velocity error
_vel_error.x = _vel_target.x - vel_curr.x; _vel_error.x = _vel_target.x - _vehicle_horiz_vel.x;
_vel_error.y = _vel_target.y - vel_curr.y; _vel_error.y = _vel_target.y - _vehicle_horiz_vel.y;
// call pi controller // call pi controller
_pi_vel_xy.set_input(_vel_error); _pi_vel_xy.set_input(_vel_error);

5
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -229,6 +229,9 @@ public:
/// when update_vel_controller_xyz is next called the position target is moved based on the desired velocity /// 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(); } 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 /// freeze_ff_z - used to stop the feed forward being calculated during a known discontinuity
void freeze_ff_z() { _flags.freeze_ff_z = true; } 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_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 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 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; } _flags;
// limit flags structure // 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_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_error; // desired acceleration in cm/s/s // To-Do: are xy actually required?
Vector3f _accel_feedforward; // feedforward acceleration in cm/s/s 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 _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 float _distance_to_target; // distance to position target - for reporting only
LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error