ardupilot/libraries/AP_Winch/AP_Winch_Servo.cpp

76 lines
2.2 KiB
C++

#include <AP_Winch/AP_Winch_Servo.h>
extern const AP_HAL::HAL& hal;
void AP_Winch_Servo::init(const AP_WheelEncoder* wheel_encoder)
{
_wheel_encoder = wheel_encoder;
// set servo output range
SRV_Channels::set_angle(SRV_Channel::k_winch, 1000);
}
void AP_Winch_Servo::update()
{
// return immediately if no servo is assigned to control the winch
if (!SRV_Channels::function_assigned(SRV_Channel::k_winch)) {
return;
}
// return immediately if no wheel encoder
if (_wheel_encoder == nullptr) {
return;
}
// if not doing any control output trim value
if (config.state == AP_Winch::STATE_RELAXED) {
SRV_Channels::set_output_limit(SRV_Channel::k_winch, SRV_Channel::Limit::TRIM);
return;
}
// calculate dt since last iteration
uint32_t now = AP_HAL::millis();
float dt = (now - last_update_ms) / 1000.0f;
if (dt > 1.0f) {
dt = 0.0f;
}
last_update_ms = now;
// calculate latest rate
float distance = _wheel_encoder->get_distance(0);
float rate = 0.0f;
if (is_positive(dt)) {
rate = (distance - config.length_curr) / dt;
}
// update distance from wheel encoder
config.length_curr = distance;
// if doing position control, calculate position error to desired rate
float rate_desired = 0.0f;
if (config.state == AP_Winch::STATE_POSITION) {
float position_error = config.length_desired - config.length_curr;
rate_desired = constrain_float(position_error * config.pos_p, -config.rate_desired, config.rate_desired);
}
// if doing rate control, set desired rate
if (config.state == AP_Winch::STATE_RATE) {
rate_desired = config.rate_desired;
}
// calculate base output
float base = 0.0f;
if (is_positive(config.rate_max)) {
base = rate_desired / config.rate_max;
}
// constrain and set limit flags
float output = base + config.rate_pid.update_all(rate_desired, rate, (limit_low || limit_high));
limit_low = (output <= -1.0f);
limit_high = (output >= 1.0f);
output = constrain_float(output, -1.0f, 1.0f);
// output to servo
SRV_Channels::set_output_scaled(SRV_Channel::k_winch, output * 1000);
}