Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_AttitudeControl: fix get_desired_speed timeout

Browse Source
This commit is contained in:
Randy Mackay 2017-12-09 15:20:11 +09:00
parent 5d7e3d31c9
commit f00f4ce79b
2 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/APM_Control/AR_AttitudeControl.cpp
View File

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

2
libraries/APM_Control/AR_AttitudeControl.h
View File

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