Browse Source

AP_Winch: remove redundant member

Also use get_rate_max method instead of config.rate_max member
master
Randy Mackay 7 years ago
parent
commit
2614d2112a
  1. 2
      libraries/AP_Winch/AP_Winch.cpp
  2. 3
      libraries/AP_Winch/AP_Winch_Servo.cpp
  3. 1
      libraries/AP_Winch/AP_Winch_Servo.h

2
libraries/AP_Winch/AP_Winch.cpp

@ -113,7 +113,7 @@ void AP_Winch::release_length(float length, float rate) @@ -113,7 +113,7 @@ void AP_Winch::release_length(float length, float rate)
if (is_zero(rate)) {
config.rate_desired = config.rate_max;
} else {
config.rate_desired = constrain_float(fabsf(rate), -config.rate_max, config.rate_max);
config.rate_desired = constrain_float(fabsf(rate), -get_rate_max(), get_rate_max());
}
}

3
libraries/AP_Winch/AP_Winch_Servo.cpp

@ -40,9 +40,8 @@ void AP_Winch_Servo::update() @@ -40,9 +40,8 @@ void AP_Winch_Servo::update()
float distance = _wheel_encoder->get_distance(0);
float rate = 0.0f;
if (is_positive(dt)) {
rate = (distance - _last_distance) / dt;
rate = (distance - config.length_curr) / dt;
}
_last_distance = distance;
// update distance from wheel encoder
config.length_curr = distance;

1
libraries/AP_Winch/AP_Winch_Servo.h

@ -34,7 +34,6 @@ private: @@ -34,7 +34,6 @@ private:
// external reference
const AP_WheelEncoder* _wheel_encoder;
float _last_distance; // wheel encoder total distance from previous iteration (used to calculate rate)
uint32_t last_update_ms; // last time update was called
bool limit_high; // output hit limit on last iteration
bool limit_low; // output hit lower limit on last iteration

Loading…
Cancel
Save