From f00f4ce79b5b4316e9f684bd0a5abc9203064e3d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 9 Dec 2017 15:20:11 +0900 Subject: [PATCH] AR_AttitudeControl: fix get_desired_speed timeout --- libraries/APM_Control/AR_AttitudeControl.cpp | 8 ++++---- libraries/APM_Control/AR_AttitudeControl.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index 3ba5fd3d86..7146d93777 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -183,7 +183,7 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool skid_st // calculate dt const uint32_t now = AP_HAL::millis(); float dt = (now - _steer_turn_last_ms) / 1000.0f; - if (_steer_turn_last_ms == 0 || dt > AR_ATTCONTROL_TIMEOUT) { + if ((_steer_turn_last_ms == 0) || (dt > AR_ATTCONTROL_TIMEOUT_MS)) { dt = 0.0f; _steer_rate_pid.reset_filter(); } else { @@ -250,7 +250,7 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool skid_st float AR_AttitudeControl::get_desired_turn_rate() const { // return zero if no recent calls to turn rate controller - if ((_steer_turn_last_ms == 0) || ((AP_HAL::millis() - _steer_turn_last_ms) > AR_ATTCONTROL_TIMEOUT)) { + if ((_steer_turn_last_ms == 0) || ((AP_HAL::millis() - _steer_turn_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) { return 0.0f; } return _desired_turn_rate; @@ -260,7 +260,7 @@ float AR_AttitudeControl::get_desired_turn_rate() const float AR_AttitudeControl::get_desired_lat_accel() const { // return zero if no recent calls to lateral acceleration controller - if ((_steer_lat_accel_last_ms == 0) || ((AP_HAL::millis() - _steer_lat_accel_last_ms) > AR_ATTCONTROL_TIMEOUT)) { + if ((_steer_lat_accel_last_ms == 0) || ((AP_HAL::millis() - _steer_lat_accel_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) { return 0.0f; } return _desired_lat_accel; @@ -424,7 +424,7 @@ bool AR_AttitudeControl::get_forward_speed(float &speed) const float AR_AttitudeControl::get_desired_speed() const { // return zero if no recent calls to speed controller - if ((_speed_last_ms == 0) || ((AP_HAL::millis() - _speed_last_ms) > AR_ATTCONTROL_TIMEOUT)) { + if ((_speed_last_ms == 0) || ((AP_HAL::millis() - _speed_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) { return 0.0f; } return _desired_speed; diff --git a/libraries/APM_Control/AR_AttitudeControl.h b/libraries/APM_Control/AR_AttitudeControl.h index c6b4c8cfa9..847cd654f0 100644 --- a/libraries/APM_Control/AR_AttitudeControl.h +++ b/libraries/APM_Control/AR_AttitudeControl.h @@ -18,7 +18,7 @@ #define AR_ATTCONTROL_THR_SPEED_D 0.00f #define AR_ATTCONTROL_THR_SPEED_FILT 5.00f #define AR_ATTCONTROL_DT 0.02f -#define AR_ATTCONTROL_TIMEOUT 0.1f +#define AR_ATTCONTROL_TIMEOUT_MS 100 // throttle/speed control maximum acceleration/deceleration (in m/s) (_ACCEL_MAX parameter default) #define AR_ATTCONTROL_THR_ACCEL_MAX 5.00f