zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AR_AttitudeControl: fix get_desired_speed timeout

mission-4.1.18
Randy Mackay 7 years ago
parent
5d7e3d31c9
commit
f00f4ce79b
2 changed files with 5 additions and 5 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      libraries/APM_Control/AR_AttitudeControl.cpp
  2. 2
      libraries/APM_Control/AR_AttitudeControl.h

8
libraries/APM_Control/AR_AttitudeControl.cpp
Unescape View File

@ -183,7 +183,7 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool skid_st
// calculate dt // calculate dt
const uint32_t now = AP_HAL::millis(); const uint32_t now = AP_HAL::millis();
float dt = (now - _steer_turn_last_ms) / 1000.0f; 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; dt = 0.0f;
_steer_rate_pid.reset_filter(); _steer_rate_pid.reset_filter();
} else { } 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 float AR_AttitudeControl::get_desired_turn_rate() const
{ {
// return zero if no recent calls to turn rate controller // 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 0.0f;
} }
return _desired_turn_rate; return _desired_turn_rate;
@ -260,7 +260,7 @@ float AR_AttitudeControl::get_desired_turn_rate() const
float AR_AttitudeControl::get_desired_lat_accel() const float AR_AttitudeControl::get_desired_lat_accel() const
{ {
// return zero if no recent calls to lateral acceleration controller // 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 0.0f;
} }
return _desired_lat_accel; return _desired_lat_accel;
@ -424,7 +424,7 @@ bool AR_AttitudeControl::get_forward_speed(float &speed) const
float AR_AttitudeControl::get_desired_speed() const float AR_AttitudeControl::get_desired_speed() const
{ {
// return zero if no recent calls to speed controller // 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 0.0f;
} }
return _desired_speed; return _desired_speed;

2
libraries/APM_Control/AR_AttitudeControl.h
Unescape View File

@ -18,7 +18,7 @@
#define AR_ATTCONTROL_THR_SPEED_D 0.00f #define AR_ATTCONTROL_THR_SPEED_D 0.00f
#define AR_ATTCONTROL_THR_SPEED_FILT 5.00f #define AR_ATTCONTROL_THR_SPEED_FILT 5.00f
#define AR_ATTCONTROL_DT 0.02f #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) // throttle/speed control maximum acceleration/deceleration (in m/s) (_ACCEL_MAX parameter default)
#define AR_ATTCONTROL_THR_ACCEL_MAX 5.00f #define AR_ATTCONTROL_THR_ACCEL_MAX 5.00f

Write Preview
Loading…
Cancel
Save