diff --git a/libraries/AP_Winch/AP_Winch.cpp b/libraries/AP_Winch/AP_Winch.cpp
new file mode 100644
index 0000000000..31b4d7f424
--- /dev/null
+++ b/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)
diff --git a/libraries/AP_Winch/AP_Winch.h b/libraries/AP_Winch/AP_Winch.h
new file mode 100644
index 0000000000..0b7956bd01
--- /dev/null
+++ b/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 .
+ */
+
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+
+// 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;
+};
diff --git a/libraries/AP_Winch/AP_Winch_Backend.h b/libraries/AP_Winch/AP_Winch_Backend.h
new file mode 100644
index 0000000000..9daf8686d7
--- /dev/null
+++ b/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 .
+ */
+
+#pragma once
+
+#include
+
+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;
+};
diff --git a/libraries/AP_Winch/AP_Winch_Servo.cpp b/libraries/AP_Winch/AP_Winch_Servo.cpp
new file mode 100644
index 0000000000..86cafcff5b
--- /dev/null
+++ b/libraries/AP_Winch/AP_Winch_Servo.cpp
@@ -0,0 +1,92 @@
+#include
+
+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);
+}
diff --git a/libraries/AP_Winch/AP_Winch_Servo.h b/libraries/AP_Winch/AP_Winch_Servo.h
new file mode 100644
index 0000000000..634339ecc4
--- /dev/null
+++ b/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 .
+ */
+
+#pragma once
+
+#include
+#include
+
+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
+};