Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: fix dt sanity checking

This commit is contained in:
Jonathan Challinger 2015-03-10 00:44:29 -07:00 committed by Randy Mackay
parent e7efe23fb5
commit 9871b95586
2 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -528,7 +528,7 @@ void AC_PosControl::update_xy_controller(xy_mode mode, float ekfNavVelGainScaler
_last_update_xy_ms = now;
// sanity check dt - expect to be called faster than ~5hz
if (dt > POSCONTROL_ACTIVE_TIMEOUT_MS) {
if (dt > POSCONTROL_ACTIVE_TIMEOUT_SEC) {
dt = 0.0f;
}
@ -589,7 +589,7 @@ void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler)
float dt = (now - _last_update_xy_ms) / 1000.0f;
// sanity check dt - expect to be called faster than ~5hz
if (dt >= POSCONTROL_ACTIVE_TIMEOUT_MS) {
if (dt >= POSCONTROL_ACTIVE_TIMEOUT_SEC) {
dt = 0.0f;
}

2
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -36,7 +36,7 @@
#define POSCONTROL_DT_10HZ 0.10f // time difference in seconds for 10hz update rate
#define POSCONTROL_DT_50HZ 0.02f // time difference in seconds for 50hz update rate
#define POSCONTROL_ACTIVE_TIMEOUT_MS 200 // position controller is considered active if it has been called within the past 200ms (0.2 seconds)
#define POSCONTROL_ACTIVE_TIMEOUT_SEC 0.2f // position controller is considered active if it has been called within the past 0.2 seconds
#define POSCONTROL_VEL_ERROR_CUTOFF_FREQ 4.0 // 4hz low-pass filter on velocity error
#define POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ 2.0 // 2hz low-pass filter on accel error