Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: add keep_xy_I_terms method

Browse Source 
Stops horizontal PID's I terms from being reset when the controller is
next updated
This commit is contained in:
Randy Mackay 2014-04-23 14:40:06 +09:00
parent ba94fc9796
commit 098f8169b0
2 changed files with 10 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -64,6 +64,7 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
#endif #endif
_flags.recalc_leash_xy = true; _flags.recalc_leash_xy = true;
_flags.recalc_leash_z = true; _flags.recalc_leash_z = true;
_flags.keep_xy_I_terms = false;
} }
/// ///
@ -428,17 +429,19 @@ float AC_PosControl::get_distance_to_target() const
} }
/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher /// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
void AC_PosControl::update_xy_controller(bool use_desired_velocity, bool reset_I) void AC_PosControl::update_xy_controller(bool use_desired_velocity)
{ {
// catch if we've just been started // catch if we've just been started
uint32_t now = hal.scheduler->millis(); uint32_t now = hal.scheduler->millis();
if ((now - _last_update_ms) >= 1000) { if ((now - _last_update_ms) >= 1000) {
_last_update_ms = now; _last_update_ms = now;
if (reset_I) { if (!_flags.keep_xy_I_terms) {
reset_I_xy(); reset_I_xy();
} }
_xy_step = 0; _xy_step = 0;
} }
// reset keep_xy_I_term flag in case it has been set
_flags.keep_xy_I_terms = false;
// check if xy leash needs to be recalculated // check if xy leash needs to be recalculated
calc_leash_length_xy(); calc_leash_length_xy();

6
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -165,7 +165,7 @@ public:
/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher /// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
/// when use_desired_velocity is true the desired velocity (i.e. feed forward) is incorporated at the pos_to_rate step /// when use_desired_velocity is true the desired velocity (i.e. feed forward) is incorporated at the pos_to_rate step
void update_xy_controller(bool use_desired_velocity, bool reset_I=true); void update_xy_controller(bool use_desired_velocity);
/// get_stopping_point_xy - calculates stopping point based on current position, velocity, vehicle acceleration /// get_stopping_point_xy - calculates stopping point based on current position, velocity, vehicle acceleration
/// distance_max allows limiting distance to stopping point /// distance_max allows limiting distance to stopping point
@ -187,6 +187,9 @@ public:
/// get_pos_xy_kP - returns xy position controller's kP gain /// get_pos_xy_kP - returns xy position controller's kP gain
float get_pos_xy_kP() const { return _p_pos_xy.kP(); } float get_pos_xy_kP() const { return _p_pos_xy.kP(); }
/// keep_xy_I_terms - ensure xy position controller's PID's I terms are not cleared when the xy controller is next run. Reset automatically back to zero in update_xy_controller.
void keep_xy_I_terms() { _flags.keep_xy_I_terms = true; }
/// accessors for reporting /// accessors for reporting
const Vector3f& get_vel_target() const { return _vel_target; } const Vector3f& get_vel_target() const { return _vel_target; }
const Vector3f& get_accel_target() const { return _accel_target; } const Vector3f& get_accel_target() const { return _accel_target; }
@ -201,6 +204,7 @@ private:
uint8_t recalc_leash_xy : 1; // 1 if we should recalculate the xy axis leash length uint8_t recalc_leash_xy : 1; // 1 if we should recalculate the xy axis leash length
uint8_t force_recalc_xy : 1; // 1 if we want the xy position controller to run at it's next possible time. set by loiter and wp controllers after they have updated the target position. uint8_t force_recalc_xy : 1; // 1 if we want the xy position controller to run at it's next possible time. set by loiter and wp controllers after they have updated the target position.
uint8_t slow_cpu : 1; // 1 if we are running on a slow_cpu machine. xy position control is broken up into multiple steps uint8_t slow_cpu : 1; // 1 if we are running on a slow_cpu machine. xy position control is broken up into multiple steps
uint8_t keep_xy_I_terms : 1; // 1 if we are to keep I terms when the position controller is next run. Reset automatically back to zero in update_xy_controller.
} _flags; } _flags;
// limit flags structure // limit flags structure