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

AC_WPNav: reset_I flag moved to position controller

Browse Source
This commit is contained in:
Randy Mackay 2014-04-23 14:40:55 +09:00
parent 098f8169b0
commit 7c02a02bd8
2 changed files with 7 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -99,8 +99,10 @@ AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, AC_PosContro
/// set_loiter_target in cm from home /// set_loiter_target in cm from home
void AC_WPNav::set_loiter_target(const Vector3f& position, bool reset_I) void AC_WPNav::set_loiter_target(const Vector3f& position, bool reset_I)
{ {
// flag in case of loiter x_y I_term should not be reset => reset_I=false. default is true // if reset_I is false we warn position controller not to reset I terms
_reset_I = reset_I; if (!reset_I) {
_pos_control.keep_xy_I_terms();
}
// set target position // set target position
_pos_control.set_pos_target(position); _pos_control.set_pos_target(position);
@ -121,11 +123,8 @@ void AC_WPNav::set_loiter_target(const Vector3f& position, bool reset_I)
} }
/// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity /// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
void AC_WPNav::init_loiter_target(bool reset_I) void AC_WPNav::init_loiter_target()
{ {
// flag in case of loiter x_y I_term should not be reset => reset_I=false. default is true
_reset_I = reset_I;
Vector3f curr_vel = _inav->get_velocity(); Vector3f curr_vel = _inav->get_velocity();
// set target position // set target position
@ -256,7 +255,7 @@ void AC_WPNav::update_loiter()
_pos_control.trigger_xy(); _pos_control.trigger_xy();
}else{ }else{
// run horizontal position controller // run horizontal position controller
_pos_control.update_xy_controller(true, _reset_I); _pos_control.update_xy_controller(true);
} }
} }

2
libraries/AC_WPNav/AC_WPNav.h
View File

@ -53,6 +53,7 @@ public:
/// ///
/// set_loiter_target in cm from home /// set_loiter_target in cm from home
/// caller can set reset_I to false to preserve I term since previous time loiter controller ran. Should only be false when caller is sure that not too much time has passed to invalidate the I terms
void set_loiter_target(const Vector3f& position, bool reset_I=true); void set_loiter_target(const Vector3f& position, bool reset_I=true);
/// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity /// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
@ -235,7 +236,6 @@ protected:
AP_Float _wp_accel_cms; // acceleration in cm/s/s during missions AP_Float _wp_accel_cms; // acceleration in cm/s/s during missions
// loiter controller internal variables // loiter controller internal variables
bool _reset_I; // if true, reset x_y_I_terms of loiter controller
uint32_t _loiter_last_update; // time of last update_loiter call uint32_t _loiter_last_update; // time of last update_loiter call
uint8_t _loiter_step; // used to decide which portion of loiter controller to run during this iteration uint8_t _loiter_step; // used to decide which portion of loiter controller to run during this iteration
int16_t _pilot_accel_fwd_cms; // pilot's desired acceleration forward (body-frame) int16_t _pilot_accel_fwd_cms; // pilot's desired acceleration forward (body-frame)