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

AP_L1_Control: calculate dt for crosstracking

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

11
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

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

2
libraries/AP_L1_Control/AP_L1_Control.h
View File

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