AP_Winch: library to control winch

fixes after peer review:
renamed disable to relaxed
make parameters private
default type to 1
enabled method checks backend created
servo range initialised in init method
contrain rate-desired during position control
use set-output-limit instead of set-safety-limit
release_length accepts rate

libraries/AP_Winch/AP_Winch.cpp

@@ -0,0 +1,139 @@
+#include "AP_Winch.h"
+#include "AP_Winch_Servo.h"
+
+extern const AP_HAL::HAL& hal;
+
+const AP_Param::GroupInfo AP_Winch::var_info[] = {
+    // @Param: ENABLE
+    // @DisplayName: Winch enable/disable
+    // @Description: Winch enable/disable
+    // @User: Standard
+    // @Values: 0:Disabled, 1:Enabled
+    AP_GROUPINFO_FLAGS("_ENABLE", 0, AP_Winch, _enabled, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: TYPE
+    // @DisplayName: Winch Type
+    // @Description: Winch Type
+    // @User: Standard
+    // @Values: 1:Servo with encoder
+    AP_GROUPINFO("_TYPE", 1, AP_Winch, config.type, 1),
+
+    // @Param: _RATE_MAX
+    // @DisplayName: Winch deploy or retract rate maximum
+    // @Description: Winch deploy or retract rate maximum.  Set to maximum rate with no load.
+    // @User: Standard
+    // @Range: 0 10
+    // @Units: m/s
+    AP_GROUPINFO("_RATE_MAX", 2, AP_Winch, config.rate_max, 1.0f),
+
+    // @Param: _POS_P
+    // @DisplayName: Winch control position error P gain
+    // @Description: Winch control position error P gain
+    // @Range: 0.01 10.0
+    // @User: Standard
+    AP_GROUPINFO("_POS_P", 3, AP_Winch, config.pos_p, AP_WINCH_POS_P),
+
+    // @Param: _RATE_P
+    // @DisplayName: Winch control rate P gain
+    // @Description: Winch control rate P gain.  Converts rate error (in radians/sec) to pwm output (in the range -1 to +1)
+    // @Range: 0.100 2.000
+    // @User: Standard
+
+    // @Param: _RATE_I
+    // @DisplayName: Winch control I gain
+    // @Description: Winch control I gain.  Corrects long term error between the desired rate (in rad/s) and actual
+    // @Range: 0.000 2.000
+    // @User: Standard
+
+    // @Param: _RATE_IMAX
+    // @DisplayName: Winch control I gain maximum
+    // @Description: Winch control I gain maximum.  Constrains the output (range -1 to +1) that the I term will generate
+    // @Range: 0.000 1.000
+    // @User: Standard
+
+    // @Param: _RATE_D
+    // @DisplayName: Winch control D gain
+    // @Description: Winch control D gain.  Compensates for short-term change in desired rate vs actual
+    // @Range: 0.000 0.400
+    // @User: Standard
+
+    // @Param: _RATE_FILT
+    // @DisplayName: Winch control filter frequency
+    // @Description: Winch control input filter.  Lower values reduce noise but add delay.
+    // @Range: 1.000 100.000
+    // @Units: Hz
+    // @User: Standard
+    AP_SUBGROUPINFO(config.rate_pid, "_RATE_", 4, AP_Winch, AC_PID),
+
+    AP_GROUPEND
+};
+
+AP_Winch::AP_Winch()
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+// indicate whether this module is enabled
+bool AP_Winch::enabled() const
+{
+    if (!_enabled) {
+        return false;
+    }
+
+    return ((config.type > 0) && (backend != nullptr));
+}
+
+void AP_Winch::init(const AP_WheelEncoder *wheel_encoder)
+{
+    // return immediately if not enabled
+    if (!_enabled.get()) {
+        return;
+    }
+
+    switch(config.type.get()) {
+    case 0:
+        break;
+    case 1:
+        backend = new AP_Winch_Servo(config);
+        break;
+    default:
+        break;
+    }
+    if (backend != nullptr) {
+        backend->init(wheel_encoder);
+    }
+}
+
+// release specified length of cable (in meters) at the specified rate
+// if rate is zero, the RATE_MAX parameter value will be used
+void AP_Winch::release_length(float length, float rate)
+{
+    config.length_desired = config.length_curr + length;
+    config.state = STATE_POSITION;
+    if (is_zero(rate)) {
+        config.rate_desired = config.rate_max;
+    } else {
+        config.rate_desired = constrain_float(fabsf(rate), -config.rate_max, config.rate_max);
+    }
+}
+
+// deploy line at specified speed in m/s (+ve deploys line, -ve retracts line, 0 stops)
+void AP_Winch::set_desired_rate(float rate)
+{
+    config.rate_desired = constrain_float(rate, -get_rate_max(), get_rate_max());
+    config.state = STATE_RATE;
+}
+
+// update - should be called at at least 10hz
+#define PASS_TO_BACKEND(function_name) \
+    void AP_Winch::function_name()   \
+    {                                  \
+        if (!enabled()) {              \
+            return;                    \
+        }                              \
+        if (backend != nullptr) {      \
+            backend->function_name();  \
+        }                              \
+    }
+
+PASS_TO_BACKEND(update)

libraries/AP_Winch/AP_Winch.h

@@ -0,0 +1,93 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_Param/AP_Param.h>
+#include <AC_PID/AC_PID.h>
+#include <AP_WheelEncoder/AP_WheelEncoder.h>
+
+// winch rate control default gains
+#define AP_WINCH_POS_P      1.00f
+#define AP_WINCH_RATE_P     1.00f
+#define AP_WINCH_RATE_I     0.50f
+#define AP_WINCH_RATE_IMAX  1.00f
+#define AP_WINCH_RATE_D     0.00f
+#define AP_WINCH_RATE_FILT  5.00f
+#define AP_WINCH_RATE_DT    0.10f
+
+class AP_Winch_Backend;
+
+class AP_Winch {
+    friend class AP_Winch_Backend;
+    friend class AP_Winch_Servo;
+
+public:
+    AP_Winch();
+
+    // indicate whether this module is enabled
+    bool enabled() const;
+
+    // initialise the winch
+    void init(const AP_WheelEncoder *wheel_encoder = nullptr);
+
+    // update the winch
+    void update();
+
+    // relax the winch so it does not attempt to maintain length or rate
+    void relax() { config.state = STATE_RELAXED; }
+
+    // get current line length
+    float get_line_length() const { return config.length_curr; }
+
+    // release specified length of cable (in meters) at the specified rate
+    // if rate is zero, the RATE_MAX parameter value will be used
+    void release_length(float length, float rate = 0.0f);
+
+    // deploy line at specified speed in m/s (+ve deploys line, -ve retracts line, 0 stops)
+    void set_desired_rate(float rate);
+
+    // get rate maximum in m/s
+    float get_rate_max() const { return MAX(config.rate_max, 0.0f); }
+
+    static const struct AP_Param::GroupInfo        var_info[];
+
+private:
+
+    // parameters
+    AP_Int8     _enabled;               // grabber enable/disable
+
+    // winch states
+    typedef enum {
+        STATE_RELAXED = 0,              // winch is not operating
+        STATE_POSITION,                 // moving or maintaining a target length
+        STATE_RATE,                     // deploying or retracting at a target rate
+    }  winch_state;
+
+    struct Backend_Config {
+        AP_Int8     type;               // winch type
+        AP_Float    rate_max;           // deploy or retract rate maximum (in m/s).
+        AP_Float    pos_p;              // position error P gain
+        AC_PID      rate_pid = AC_PID(AP_WINCH_RATE_P, AP_WINCH_RATE_I, AP_WINCH_RATE_D, AP_WINCH_RATE_IMAX, AP_WINCH_RATE_FILT, AP_WINCH_RATE_DT);           // rate control PID
+        winch_state state;              // state of winch control (using target position or target rate)
+        float       length_curr;        // current length of the line (in meters) that has been deployed
+        float       length_desired;     // target desired length (in meters)
+        float       rate_desired;       // target deploy rate (in m/s, +ve = deploying, -ve = retracting)
+    } config;
+
+    AP_Winch_Backend *backend;
+};

libraries/AP_Winch/AP_Winch_Backend.h

@@ -0,0 +1,34 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <AP_Winch/AP_Winch.h>
+
+class AP_Winch_Backend {
+public:
+    AP_Winch_Backend(struct AP_Winch::Backend_Config &_config) :
+        config(_config) { }
+
+    // initialise the backend
+    virtual void init(const AP_WheelEncoder* wheel_encoder) = 0;
+
+    // update - should be called at at least 10hz
+    virtual void update() = 0;
+
+protected:
+
+    struct AP_Winch::Backend_Config &config;
+};

libraries/AP_Winch/AP_Winch_Servo.cpp

@@ -0,0 +1,92 @@
+#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::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 - _last_distance) / dt;
+    }
+    _last_distance = distance;
+
+    // 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 rate error and pass to pid controller
+    float rate_error = rate_desired - rate;
+    config.rate_pid.set_input_filter_all(rate_error);
+
+    // get p
+    float p = config.rate_pid.get_p();
+
+    // get i unless winch hit limit on last iteration
+    float i = config.rate_pid.get_integrator();
+    if (((is_negative(rate_error) && !limit_low) || (is_positive(rate_error) && !limit_high))) {
+        i = config.rate_pid.get_i();
+    }
+
+    // get d
+    float d = config.rate_pid.get_d();
+
+    // 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 + p + i + d;
+    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);
+}

libraries/AP_Winch/AP_Winch_Servo.h

@@ -0,0 +1,41 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <AP_Winch/AP_Winch_Backend.h>
+#include <SRV_Channel/SRV_Channel.h>
+
+class AP_Winch_Servo : public AP_Winch_Backend {
+public:
+
+    AP_Winch_Servo(struct AP_Winch::Backend_Config &_config) :
+        AP_Winch_Backend(_config) { }
+
+    // initialise the winch
+    void init(const AP_WheelEncoder* wheel_encoder) override;
+
+    // control the winch
+    void update() override;
+
+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
+};