From 54cc2d884ccaefe0188bee8397fa7cca3bc0f298 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 17 Nov 2015 15:48:05 +0900 Subject: [PATCH] AC_PosControl: default z-axis controller to 400hz No functional change as vehicle code always sets it explicitely --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 2 +- libraries/AC_AttitudeControl/AC_PosControl.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index ffc1f5482d..d92221809c 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -37,7 +37,7 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, _pid_accel_z(pid_accel_z), _p_pos_xy(p_pos_xy), _pi_vel_xy(pi_vel_xy), - _dt(POSCONTROL_DT_10HZ), + _dt(POSCONTROL_DT_400HZ), _dt_xy(POSCONTROL_DT_50HZ), _last_update_xy_ms(0), _last_update_z_ms(0), diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 56d6a783c4..25751a9a99 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -32,8 +32,8 @@ #define POSCONTROL_LEASH_LENGTH_MIN 100.0f // minimum leash lengths in cm -#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_DT_400HZ 0.0025f // time difference in seconds for 400hz update rate #define POSCONTROL_ACTIVE_TIMEOUT_MS 200 // position controller is considered active if it has been called within the past 0.2 seconds