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

AC_AttitudeControl: AC_PosControl: Remove velocity override

This commit is contained in:
Leonard Hall 2022-02-01 01:15:17 +10:30 committed by Randy Mackay
parent 7369d18abf
commit 162dd6d2bc
2 changed files with 4 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -609,14 +609,9 @@ void AC_PosControl::update_xy_controller()
// Velocity Controller // Velocity Controller
// check if vehicle velocity is being overridden const Vector2f &curr_vel = _inav.get_velocity_xy_cms();
// todo: remove this and use input shaping Vector2f accel_target = _pid_vel_xy.update_all(_vel_target.xy(), curr_vel, _limit_vector.xy());
if (_flags.vehicle_horiz_vel_override) {
_flags.vehicle_horiz_vel_override = false;
} else {
_vehicle_horiz_vel = _inav.get_velocity_xy_cms();
}
Vector2f accel_target = _pid_vel_xy.update_all(_vel_target.xy(), _vehicle_horiz_vel, _limit_vector.xy());
// acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise // acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
accel_target *= ahrsControlScaleXY; accel_target *= ahrsControlScaleXY;

10
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -354,9 +354,6 @@ public:
/// this prevents integrator buildup /// this prevents integrator buildup
void set_externally_limited_xy() { _limit_vector.x = _accel_target.x; _limit_vector.y = _accel_target.y; } void set_externally_limited_xy() { _limit_vector.x = _accel_target.x; _limit_vector.y = _accel_target.y; }
// 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; }
// lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s // lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
Vector3f lean_angles_to_accel(const Vector3f& att_target_euler) const; Vector3f lean_angles_to_accel(const Vector3f& att_target_euler) const;
@ -387,11 +384,6 @@ public:
protected: protected:
// general purpose flags
struct poscontrol_flags {
uint16_t vehicle_horiz_vel_override : 1; // 1 if we should use _vehicle_horiz_vel as our velocity process variable for one timestep
} _flags;
// get throttle using vibration-resistant calculation (uses feed forward with manually calculated gain) // get throttle using vibration-resistant calculation (uses feed forward with manually calculated gain)
float get_throttle_with_vibration_override(); float get_throttle_with_vibration_override();
@ -458,7 +450,7 @@ protected:
Vector3f _accel_desired; // desired acceleration in NEU cm/s/s (feed forward) Vector3f _accel_desired; // desired acceleration in NEU cm/s/s (feed forward)
Vector3f _accel_target; // acceleration target in NEU cm/s/s Vector3f _accel_target; // acceleration target in NEU cm/s/s
Vector3f _limit_vector; // the direction that the position controller is limited, zero when not limited Vector3f _limit_vector; // the direction that the position controller is limited, zero when not limited
Vector2f _vehicle_horiz_vel; // velocity to use if _flags.vehicle_horiz_vel_override is set
float _pos_offset_target_z; // vertical position offset target, frame NEU in cm relative to the EKF origin float _pos_offset_target_z; // vertical position offset target, frame NEU in cm relative to the EKF origin
float _pos_offset_z; // vertical position offset, frame NEU in cm relative to the EKF origin float _pos_offset_z; // vertical position offset, frame NEU in cm relative to the EKF origin
float _vel_offset_z; // vertical velocity offset in NEU cm/s calculated by pos_to_rate step float _vel_offset_z; // vertical velocity offset in NEU cm/s calculated by pos_to_rate step