Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Loiter: consolidate sanity checks

Browse Source
This commit is contained in:
Randy Mackay 2018-04-04 09:34:50 +09:00
parent defdeaed95
commit 757a35f3ba
2 changed files with 13 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AC_WPNav/AC_Loiter.cpp
View File

@ -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)

3
libraries/AC_WPNav/AC_Loiter.h
View File

@ -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);