Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 15:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_L1_Control: calculate dt for crosstracking

Browse Source
This commit is contained in:
Andrew Tridgell 2016-01-10 09:42:25 +11:00
parent 4adda34439
commit 640332113c
2 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -143,6 +143,13 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
float xtrackVel; float xtrackVel;
float ltrackVel; 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 // Calculate L1 gain required for specified damping
float K_L1 = 4.0f * _L1_damping * _L1_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 = 0;
_L1_xtrack_i_gain_prev = _L1_xtrack_i_gain; _L1_xtrack_i_gain_prev = _L1_xtrack_i_gain;
} else if (fabsf(Nu1) < radians(5)) { } else if (fabsf(Nu1) < radians(5)) {
const float dt = 0.1f; // 10Hz
_L1_xtrack_i += Nu1 * _L1_xtrack_i_gain * dt; _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 // an AHRS_TRIM_X=0.1 will drift to about 0.08 so 0.1 is a good worst-case to clip at

2
libraries/AP_L1_Control/AP_L1_Control.h
View File

@ -104,7 +104,7 @@ private:
float _L1_xtrack_i = 0; float _L1_xtrack_i = 0;
AP_Float _L1_xtrack_i_gain; AP_Float _L1_xtrack_i_gain;
float _L1_xtrack_i_gain_prev = 0; float _L1_xtrack_i_gain_prev = 0;
uint32_t _last_update_waypoint_us;
}; };