Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: remove reset_I_xy

Browse Source 
lean_angles_to_accel is now used to load the position rate I terms based
on vehicle's initial roll and pitch angle
This commit is contained in:
Randy Mackay 2014-05-07 11:46:39 +09:00
parent cd8b1f278c
commit c13db680b7
2 changed files with 11 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -320,9 +320,6 @@ void AC_PosControl::rate_to_accel_z(float vel_target_z)
desired_accel += p; desired_accel += p;
desired_accel = constrain_int32(desired_accel, -32000, 32000); desired_accel = constrain_int32(desired_accel, -32000, 32000);
// To-Do: re-enable PID logging?
// TO-DO: ensure throttle cruise is updated some other way in the main code or attitude control
// set target for accel based throttle controller // set target for accel based throttle controller
accel_to_throttle(desired_accel); accel_to_throttle(desired_accel);
} }
@ -483,6 +480,9 @@ bool AC_PosControl::is_active_xy() const
} }
/// init_xy_controller - initialise the xy controller /// init_xy_controller - initialise the xy controller
/// sets target roll angle, pitch angle and I terms based on vehicle current lean angles
/// should be called once whenever significant changes to the position target are made
/// this does not update the xy target
void AC_PosControl::init_xy_controller() void AC_PosControl::init_xy_controller()
{ {
// reset xy controller to first step // reset xy controller to first step
@ -492,19 +492,17 @@ void AC_PosControl::init_xy_controller()
_roll_target = _ahrs.roll_sensor; _roll_target = _ahrs.roll_sensor;
_pitch_target = _ahrs.pitch_sensor; _pitch_target = _ahrs.pitch_sensor;
// initialise I terms // initialise I terms from lean angles
if (!_flags.keep_xy_I_terms) { if (!_flags.keep_xy_I_terms) {
reset_I_xy(); // reset last velocity if this controller has just been engaged or dt is zero
lean_angles_to_accel(_accel_target.x, _accel_target.y);
_pid_rate_lat.set_integrator(_accel_target.x);
_pid_rate_lon.set_integrator(_accel_target.y);
} else { } else {
// reset keep_xy_I_term flag in case it has been set // reset keep_xy_I_term flag in case it has been set
_flags.keep_xy_I_terms = false; _flags.keep_xy_I_terms = false;
} }
// reset last velocity if this controller has just been engaged or dt is zero
lean_angles_to_accel(_accel_target.x, _accel_target.y);
_pid_rate_lat.set_integrator(_accel_target.x);
_pid_rate_lon.set_integrator(_accel_target.y);
// flag reset required in rate to accel step // flag reset required in rate to accel step
_flags.reset_desired_vel_to_pos = true; _flags.reset_desired_vel_to_pos = true;
_flags.reset_rate_to_accel_xy = true; _flags.reset_rate_to_accel_xy = true;
@ -748,13 +746,6 @@ void AC_PosControl::lean_angles_to_accel(float& accel_x_cmss, float& accel_y_cms
accel_y_cmss = (GRAVITY_MSS * 100) * (-(_ahrs.sin_yaw() * _ahrs.sin_pitch() / max(_ahrs.cos_pitch(),0.5)) + _ahrs.cos_yaw() * _ahrs.sin_roll() / max(_ahrs.cos_roll(),0.5)); accel_y_cmss = (GRAVITY_MSS * 100) * (-(_ahrs.sin_yaw() * _ahrs.sin_pitch() / max(_ahrs.cos_pitch(),0.5)) + _ahrs.cos_yaw() * _ahrs.sin_roll() / max(_ahrs.cos_roll(),0.5));
} }
/// reset_I_xy - clears I terms from loiter PID controller
void AC_PosControl::reset_I_xy()
{
_pid_rate_lon.reset_I();
_pid_rate_lat.reset_I();
}
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain /// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
float AC_PosControl::calc_leash_length(float speed_cms, float accel_cms, float kP) const float AC_PosControl::calc_leash_length(float speed_cms, float accel_cms, float kP) const
{ {

6
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -137,6 +137,9 @@ public:
/// ///
/// init_xy_controller - initialise the xy controller /// init_xy_controller - initialise the xy controller
/// sets target roll angle, pitch angle and I terms based on vehicle current lean angles
/// should be called once whenever significant changes to the position target are made
/// this does not update the xy target
void init_xy_controller(); void init_xy_controller();
/// set_accel_xy - set horizontal acceleration in cm/s/s /// set_accel_xy - set horizontal acceleration in cm/s/s
@ -278,9 +281,6 @@ private:
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles /// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void accel_to_lean_angles(); void accel_to_lean_angles();
/// reset_I_xy - clears I terms from horizontal position PID controller
void reset_I_xy();
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain /// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
float calc_leash_length(float speed_cms, float accel_cms, float kP) const; float calc_leash_length(float speed_cms, float accel_cms, float kP) const;