From ee3daaa7c906cab8139f04f885f8a2ec7e7f5b72 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 26 Oct 2017 14:58:10 +0900 Subject: [PATCH] AR_AttitudeControl: reset input filter for steering and throttle controllers Also only set dt to non-zero value --- libraries/APM_Control/AR_AttitudeControl.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index 3e43ae3fc8..2ff38efc02 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -173,9 +173,11 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool skid_st float dt = (now - _steer_turn_last_ms) / 1000.0f; if (_steer_turn_last_ms == 0 || dt > 0.1) { dt = 0.0f; + _steer_rate_pid.reset_filter(); + } else { + _steer_rate_pid.set_dt(dt); } _steer_turn_last_ms = now; - _steer_rate_pid.set_dt(dt); // get speed forward float speed; @@ -252,6 +254,7 @@ float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool skid_ float dt = (now - _speed_last_ms) / 1000.0f; if (_speed_last_ms == 0 || dt > 0.1) { dt = 0.0f; + _throttle_speed_pid.reset_filter(); } _speed_last_ms = now;