diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index 85c7092042..2154f06df9 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -73,15 +73,13 @@ AC_Loiter::AC_Loiter(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_Po _attitude_control(attitude_control) { AP_Param::setup_object_defaults(this, var_info); - - // sanity check some parameters - _speed_cms = MAX(_speed_cms, LOITER_SPEED_MIN); - _accel_cmss = MIN(_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max() * 0.01f))); } /// init_target to a position in cm from ekf origin void AC_Loiter::init_target(const Vector3f& position) { + sanity_check_params(); + // initialise pos controller speed, acceleration _pos_control.set_speed_xy(LOITER_VEL_CORRECTION_MAX); _pos_control.set_accel_xy(_accel_cmss); @@ -108,8 +106,7 @@ void AC_Loiter::init_target() const Vector3f& curr_pos = _inav.get_position(); const Vector3f& curr_vel = _inav.get_velocity(); - // sanity check loiter speed - _speed_cms = MAX(_speed_cms, LOITER_SPEED_MIN); + sanity_check_params(); // initialise pos controller speed and acceleration _pos_control.set_speed_xy(LOITER_VEL_CORRECTION_MAX); @@ -211,6 +208,13 @@ void AC_Loiter::update(float ekfGndSpdLimit, float ekfNavVelGainScaler) _pos_control.update_xy_controller(ekfNavVelGainScaler); } +// sanity check parameters +void AC_Loiter::sanity_check_params() +{ + _speed_cms = MAX(_speed_cms, LOITER_SPEED_MIN); + _accel_cmss = MIN(_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max() * 0.01f))); +} + /// calc_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance /// updated velocity sent directly to position controller void AC_Loiter::calc_desired_velocity(float nav_dt, float ekfGndSpdLimit) diff --git a/libraries/AC_WPNav/AC_Loiter.h b/libraries/AC_WPNav/AC_Loiter.h index e6cf232f39..c066a0a741 100644 --- a/libraries/AC_WPNav/AC_Loiter.h +++ b/libraries/AC_WPNav/AC_Loiter.h @@ -71,6 +71,9 @@ public: protected: + // sanity check parameters + void sanity_check_params(); + /// updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance /// updated velocity sent directly to position controller void calc_desired_velocity(float nav_dt, float ekfGndSpdLimit);