Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Randy Mackay 2017-10-05 11:19:22 +09:00
parent 8b4345f29d
commit 2dfb6a94f2
5 changed files with 399 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

139
libraries/AP_Winch/AP_Winch.cpp Normal file
View File

@ -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)

93
libraries/AP_Winch/AP_Winch.h Normal file
View File

@ -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;
};

34
libraries/AP_Winch/AP_Winch_Backend.h Normal file
View File

@ -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;
};

92
libraries/AP_Winch/AP_Winch_Servo.cpp Normal file
View File

@ -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);
}

41
libraries/AP_Winch/AP_Winch_Servo.h Normal file
View File

@ -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
};