From 640332113c99c871d58c92e32daf367c356f9cf6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 10 Jan 2016 09:42:25 +1100 Subject: [PATCH] AP_L1_Control: calculate dt for crosstracking --- libraries/AP_L1_Control/AP_L1_Control.cpp | 11 ++++++++--- libraries/AP_L1_Control/AP_L1_Control.h | 2 +- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index 8546de2c9c..2c9bd8effe 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -142,7 +142,14 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct float Nu; float xtrackVel; float ltrackVel; - + + uint32_t now = AP_HAL::micros(); + float dt = (now - _last_update_waypoint_us) * 1.0e-6f; + if (dt > 0.1) { + dt = 0.1; + } + _last_update_waypoint_us = now; + // Calculate L1 gain required for specified damping float K_L1 = 4.0f * _L1_damping * _L1_damping; @@ -220,8 +227,6 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct _L1_xtrack_i = 0; _L1_xtrack_i_gain_prev = _L1_xtrack_i_gain; } else if (fabsf(Nu1) < radians(5)) { - - const float dt = 0.1f; // 10Hz _L1_xtrack_i += Nu1 * _L1_xtrack_i_gain * dt; // an AHRS_TRIM_X=0.1 will drift to about 0.08 so 0.1 is a good worst-case to clip at diff --git a/libraries/AP_L1_Control/AP_L1_Control.h b/libraries/AP_L1_Control/AP_L1_Control.h index 0997fc95fa..b0720444aa 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.h +++ b/libraries/AP_L1_Control/AP_L1_Control.h @@ -104,7 +104,7 @@ private: float _L1_xtrack_i = 0; AP_Float _L1_xtrack_i_gain; float _L1_xtrack_i_gain_prev = 0; - + uint32_t _last_update_waypoint_us; };