mirror of https://github.com/ArduPilot/ardupilot
AP_Winch: rewrite driver
includes the following changes move WINCH_ENABLED to library add singleton add pre_arm_check backend gets init_input_and_output and read_pilot_desired_rate remove use of wheel encoder rename servo-with-encoder to just servo add write_log rename control_mode add rc input processing add acceleration limiting
This commit is contained in:
Randy Mackay
parent
feb852a9fc
commit
738480884d
|
|
@ -4,19 +4,14 @@
|
|||
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),
|
||||
// 0 was 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),
|
||||
// @Values: 0:None, 1:Servo
|
||||
AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Winch, config.type, (int8_t)WinchType::NONE, AP_PARAM_FLAG_ENABLE),
|
||||
|
||||
// @Param: _RATE_MAX
|
||||
// @DisplayName: Winch deploy or retract rate maximum
|
||||
|
|
@ -31,97 +26,98 @@ const AP_Param::GroupInfo AP_Winch::var_info[] = {
|
|||
// @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),
|
||||
AP_GROUPINFO("_POS_P", 3, AP_Winch, config.pos_p, 1.0f),
|
||||
|
||||
// @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),
|
||||
// 4 was _RATE_PID
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
AP_Winch::AP_Winch()
|
||||
{
|
||||
if (_singleton) {
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
AP_HAL::panic("Too many winches");
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
_singleton = this;
|
||||
|
||||
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));
|
||||
return ((config.type > 0) && (backend != nullptr));
|
||||
}
|
||||
|
||||
void AP_Winch::init(const AP_WheelEncoder *wheel_encoder)
|
||||
// true if winch is healthy
|
||||
bool AP_Winch::healthy() const
|
||||
{
|
||||
// return immediately if not enabled
|
||||
if (!_enabled.get()) {
|
||||
return;
|
||||
if (backend != nullptr) {
|
||||
return backend->healthy();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
switch(config.type.get()) {
|
||||
case 0:
|
||||
void AP_Winch::init()
|
||||
{
|
||||
switch ((WinchType)config.type.get()) {
|
||||
case WinchType::NONE:
|
||||
break;
|
||||
case 1:
|
||||
case WinchType::SERVO:
|
||||
backend = new AP_Winch_Servo(config);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
if (backend != nullptr) {
|
||||
backend->init(wheel_encoder);
|
||||
backend->init();
|
||||
}
|
||||
}
|
||||
|
||||
// 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)
|
||||
void AP_Winch::release_length(float length)
|
||||
{
|
||||
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), -get_rate_max(), get_rate_max());
|
||||
if (backend == nullptr) {
|
||||
return;
|
||||
}
|
||||
config.length_desired = backend->get_current_length() + length;
|
||||
config.control_mode = AP_Winch::ControlMode::POSITION;
|
||||
}
|
||||
|
||||
// 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;
|
||||
config.control_mode = AP_Winch::ControlMode::RATE;
|
||||
}
|
||||
|
||||
// send status to ground station
|
||||
void AP_Winch::send_status(const GCS_MAVLINK &channel)
|
||||
{
|
||||
if (backend != nullptr) {
|
||||
backend->send_status(channel);
|
||||
}
|
||||
}
|
||||
|
||||
// returns true if pre arm checks have passed
|
||||
bool AP_Winch::pre_arm_check(char *failmsg, uint8_t failmsg_len) const
|
||||
{
|
||||
// succeed if winch is disabled
|
||||
if ((WinchType)config.type.get() == WinchType::NONE) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// fail if unhealthy
|
||||
if (!healthy()) {
|
||||
snprintf(failmsg, failmsg_len, "winch unhealthy");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// update - should be called at at least 10hz
|
||||
|
|
@ -137,5 +133,24 @@ void AP_Winch::set_desired_rate(float rate)
|
|||
}
|
||||
|
||||
PASS_TO_BACKEND(update)
|
||||
PASS_TO_BACKEND(write_log)
|
||||
|
||||
#undef PASS_TO_BACKEND
|
||||
|
||||
/*
|
||||
* Get the AP_Winch singleton
|
||||
*/
|
||||
AP_Winch *AP_Winch::_singleton;
|
||||
AP_Winch *AP_Winch::get_singleton()
|
||||
{
|
||||
return _singleton;
|
||||
}
|
||||
|
||||
namespace AP {
|
||||
|
||||
AP_Winch *winch()
|
||||
{
|
||||
return AP_Winch::get_singleton();
|
||||
}
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -19,16 +19,6 @@
|
|||
#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;
|
||||
|
||||
|
|
@ -39,24 +29,27 @@ class AP_Winch {
|
|||
public:
|
||||
AP_Winch();
|
||||
|
||||
// Do not allow copies
|
||||
AP_Winch(const AP_Winch &other) = delete;
|
||||
AP_Winch &operator=(const AP_Winch&) = delete;
|
||||
|
||||
// indicate whether this module is enabled
|
||||
bool enabled() const;
|
||||
|
||||
// true if winch is healthy
|
||||
bool healthy() const;
|
||||
|
||||
// initialise the winch
|
||||
void init(const AP_WheelEncoder *wheel_encoder = nullptr);
|
||||
void init();
|
||||
|
||||
// update the winch
|
||||
void update();
|
||||
|
||||
// relax the winch so it does not attempt to maintain length or rate
|
||||
void relax() { config.state = STATE_RELAXED; }
|
||||
void relax() { config.control_mode = AP_Winch::ControlMode::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);
|
||||
// release specified length of cable (in meters)
|
||||
void release_length(float length);
|
||||
|
||||
// deploy line at specified speed in m/s (+ve deploys line, -ve retracts line, 0 stops)
|
||||
void set_desired_rate(float rate);
|
||||
|
|
@ -64,30 +57,48 @@ public:
|
|||
// get rate maximum in m/s
|
||||
float get_rate_max() const { return MAX(config.rate_max, 0.0f); }
|
||||
|
||||
// send status to ground station
|
||||
void send_status(const GCS_MAVLINK &channel);
|
||||
|
||||
// write log
|
||||
void write_log();
|
||||
|
||||
// returns true if pre arm checks have passed
|
||||
bool pre_arm_check(char *failmsg, uint8_t failmsg_len) const;
|
||||
|
||||
static AP_Winch *get_singleton();
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
private:
|
||||
|
||||
// parameters
|
||||
AP_Int8 _enabled; // grabber enable/disable
|
||||
enum class WinchType {
|
||||
NONE = 0,
|
||||
SERVO = 1,
|
||||
};
|
||||
|
||||
// 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;
|
||||
enum class ControlMode : uint8_t {
|
||||
RELAXED = 0, // winch is realxed
|
||||
POSITION, // moving or maintaining a target length (from an external source)
|
||||
RATE, // extending or retracting at a target rate (from an external source)
|
||||
RATE_FROM_RC // extending or retracting at a target rate (from RC input)
|
||||
};
|
||||
|
||||
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, 0.0f, AP_WINCH_RATE_IMAX, 0.0f, AP_WINCH_RATE_FILT, 0.0f, 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
|
||||
ControlMode control_mode; // state of winch control (using target position or target rate)
|
||||
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;
|
||||
|
||||
static AP_Winch *_singleton;
|
||||
};
|
||||
|
||||
namespace AP {
|
||||
AP_Winch *winch();
|
||||
};
|
||||
|
|
|
|||
|
|
@ -0,0 +1,68 @@
|
|||
#include <AP_Winch/AP_Winch_Backend.h>
|
||||
#include <RC_Channel/RC_Channel.h>
|
||||
|
||||
// setup rc input and output
|
||||
void AP_Winch_Backend::init_input_and_output()
|
||||
{
|
||||
|
||||
// set servo output range
|
||||
SRV_Channels::set_angle(SRV_Channel::k_winch, 1000);
|
||||
|
||||
RC_Channel *rc_input = rc().find_channel_for_option(RC_Channel::AUX_FUNC::WINCH_CONTROL);
|
||||
if (rc_input != nullptr) {
|
||||
// rc input deadzone is required or pilot input will always override autonomous control of winch
|
||||
rc_input->set_default_dead_zone(30);
|
||||
}
|
||||
}
|
||||
|
||||
// calculate the pilot desired rate (+ve deploys line, -ve retracts line, 0 stops) from rc input
|
||||
// may change the state to RATE and update config.rate_desired
|
||||
void AP_Winch_Backend::read_pilot_desired_rate()
|
||||
{
|
||||
// fail if no input channel defined
|
||||
const RC_Channel *rc_input = rc().find_channel_for_option(RC_Channel::AUX_FUNC::WINCH_CONTROL);
|
||||
if (rc_input == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
// if pilot is controlling winch, set rate to zero during RC failsafe
|
||||
if (!rc().has_valid_input() && (config.control_mode == AP_Winch::ControlMode::RATE_FROM_RC)) {
|
||||
config.rate_desired = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
// initialise previous_radio_in
|
||||
if (previous_radio_in == -1) {
|
||||
previous_radio_in = rc_input->get_radio_in();
|
||||
}
|
||||
|
||||
// check for a significant change in rc input
|
||||
const int16_t radio_in = rc_input->get_radio_in();
|
||||
if (config.control_mode != AP_Winch::ControlMode::RATE_FROM_RC) {
|
||||
if (abs(radio_in - previous_radio_in) > rc_input->get_dead_zone()) {
|
||||
config.control_mode = AP_Winch::ControlMode::RATE_FROM_RC;
|
||||
}
|
||||
}
|
||||
|
||||
// update desired rate
|
||||
if (config.control_mode == AP_Winch::ControlMode::RATE_FROM_RC) {
|
||||
config.rate_desired = rc_input->norm_input_dz() * config.rate_max;
|
||||
previous_radio_in = radio_in;
|
||||
}
|
||||
}
|
||||
|
||||
// returns the rate limited by the maximum acceleration
|
||||
// this also updates the previous rate so should not be called more than once per loop
|
||||
float AP_Winch_Backend::get_rate_limited_by_accel(float rate, float dt)
|
||||
{
|
||||
// use maximum rate x 2 for maximum acceleration
|
||||
const float rate_change_max = MAX(config.rate_max * 2.0f, 0.1f) * dt;
|
||||
|
||||
// apply acceleration limit
|
||||
const float rate_limited = constrain_float(rate, previous_rate - rate_change_max, previous_rate + rate_change_max);
|
||||
|
||||
// update previous rate for next time
|
||||
set_previous_rate(rate_limited);
|
||||
|
||||
return rate_limited;
|
||||
}
|
||||
|
|
@ -22,13 +22,42 @@ public:
|
|||
AP_Winch_Backend(struct AP_Winch::Backend_Config &_config) :
|
||||
config(_config) { }
|
||||
|
||||
// true if winch is healthy
|
||||
virtual bool healthy() const = 0;
|
||||
|
||||
// initialise the backend
|
||||
virtual void init(const AP_WheelEncoder* wheel_encoder) = 0;
|
||||
virtual void init() = 0;
|
||||
|
||||
// update - should be called at at least 10hz
|
||||
virtual void update() = 0;
|
||||
|
||||
// returns current length of line deployed
|
||||
virtual float get_current_length() const = 0;
|
||||
|
||||
// send status to ground station
|
||||
virtual void send_status(const GCS_MAVLINK &channel) = 0;
|
||||
|
||||
// write log
|
||||
virtual void write_log() = 0;
|
||||
|
||||
protected:
|
||||
|
||||
// setup rc input and output
|
||||
void init_input_and_output();
|
||||
|
||||
// calculate the pilot desired rate (+ve deploys line, -ve retracts line, 0 stops) from rc input
|
||||
// may change the state to RATE and update config.rate_desired
|
||||
void read_pilot_desired_rate();
|
||||
|
||||
// returns the rate limited by the maximum acceleration
|
||||
// this also updates the previous rate so should not be called more than once per loop
|
||||
float get_rate_limited_by_accel(float rate, float dt) WARN_IF_UNUSED;
|
||||
|
||||
// set the rate used for acceleration limiting
|
||||
void set_previous_rate(float rate) { previous_rate = rate; }
|
||||
|
||||
struct AP_Winch::Backend_Config &config;
|
||||
|
||||
int16_t previous_radio_in = -1; // previous RC input from pilot, used to ignore small changes
|
||||
float previous_rate; // previous rate used for acceleration limiting
|
||||
};
|
||||
|
|
|
|||
|
|
@ -1,13 +1,23 @@
|
|||
#include <AP_Winch/AP_Winch_Servo.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
void AP_Winch_Servo::init(const AP_WheelEncoder* wheel_encoder)
|
||||
// true if winch is healthy
|
||||
bool AP_Winch_Servo::healthy() const
|
||||
{
|
||||
_wheel_encoder = wheel_encoder;
|
||||
// return immediately if no servo is assigned to control the winch
|
||||
if (!SRV_Channels::function_assigned(SRV_Channel::k_winch)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// set servo output range
|
||||
SRV_Channels::set_angle(SRV_Channel::k_winch, 1000);
|
||||
return true;
|
||||
}
|
||||
|
||||
void AP_Winch_Servo::init()
|
||||
{
|
||||
// initialise rc input and output
|
||||
init_input_and_output();
|
||||
}
|
||||
|
||||
void AP_Winch_Servo::update()
|
||||
|
|
@ -17,59 +27,85 @@ void AP_Winch_Servo::update()
|
|||
return;
|
||||
}
|
||||
|
||||
// return immediately if no wheel encoder
|
||||
if (_wheel_encoder == nullptr) {
|
||||
return;
|
||||
}
|
||||
// read pilot input
|
||||
read_pilot_desired_rate();
|
||||
|
||||
// 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;
|
||||
}
|
||||
// send outputs to winch
|
||||
control_winch();
|
||||
}
|
||||
|
||||
// calculate dt since last iteration
|
||||
uint32_t now = AP_HAL::millis();
|
||||
float dt = (now - last_update_ms) / 1000.0f;
|
||||
// update pwm outputs to control winch
|
||||
void AP_Winch_Servo::control_winch()
|
||||
{
|
||||
uint32_t now_ms = AP_HAL::millis();
|
||||
float dt = (now_ms - control_update_ms) / 1000.0f;
|
||||
if (dt > 1.0f) {
|
||||
dt = 0.0f;
|
||||
}
|
||||
last_update_ms = now;
|
||||
control_update_ms = now_ms;
|
||||
|
||||
// calculate latest rate
|
||||
float distance = _wheel_encoder->get_distance(0);
|
||||
float rate = 0.0f;
|
||||
if (is_positive(dt)) {
|
||||
rate = (distance - config.length_curr) / dt;
|
||||
// if relaxed stop outputting pwm signals
|
||||
if (config.control_mode == AP_Winch::ControlMode::RELAXED) {
|
||||
// if not doing any control stop sending pwm to winch
|
||||
SRV_Channels::set_output_pwm(SRV_Channel::k_winch, 0);
|
||||
|
||||
// rate used for acceleration limiting reset to zero
|
||||
set_previous_rate(0.0f);
|
||||
return;
|
||||
}
|
||||
|
||||
// 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 ((config.control_mode == AP_Winch::ControlMode::POSITION) && healthy()) {
|
||||
float position_error = config.length_desired - line_length;
|
||||
config.rate_desired = constrain_float(position_error * config.pos_p, -config.rate_max, config.rate_max);
|
||||
}
|
||||
|
||||
// if doing rate control, set desired rate
|
||||
if (config.state == AP_Winch::STATE_RATE) {
|
||||
rate_desired = config.rate_desired;
|
||||
}
|
||||
// apply acceleration limited to rate
|
||||
const float rate_limited = get_rate_limited_by_accel(config.rate_desired, dt);
|
||||
|
||||
// calculate base output
|
||||
float base = 0.0f;
|
||||
if (is_positive(config.rate_max)) {
|
||||
base = rate_desired / config.rate_max;
|
||||
}
|
||||
// use linear interpolation to calculate output to move winch at desired rate
|
||||
const int16_t scaled_output = linear_interpolate(-1000, 1000, rate_limited, -config.rate_max, config.rate_max);
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_winch, scaled_output);
|
||||
|
||||
// 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);
|
||||
// update distance estimate assuming winch will move exactly as requested
|
||||
line_length += config.rate_desired * dt;
|
||||
}
|
||||
|
||||
//send generator status
|
||||
void AP_Winch_Servo::send_status(const GCS_MAVLINK &channel)
|
||||
{
|
||||
// prepare status bitmask
|
||||
uint32_t status_bitmask = 0;
|
||||
if (healthy()) {
|
||||
status_bitmask |= MAV_WINCH_STATUS_HEALTHY;
|
||||
}
|
||||
|
||||
// send status
|
||||
mavlink_msg_winch_status_send(
|
||||
channel.get_chan(),
|
||||
AP_HAL::micros64(), // time_usec
|
||||
line_length, // line_length
|
||||
config.rate_desired, // speed
|
||||
std::numeric_limits<double>::quiet_NaN(), // tension
|
||||
std::numeric_limits<double>::quiet_NaN(), // voltage
|
||||
std::numeric_limits<double>::quiet_NaN(), // current
|
||||
INT16_MAX, // temperature
|
||||
status_bitmask); // status flags
|
||||
}
|
||||
|
||||
// write log
|
||||
void AP_Winch_Servo::write_log()
|
||||
{
|
||||
AP::logger().Write_Winch(
|
||||
healthy(),
|
||||
0, // thread end (unsupported)
|
||||
0, // moving (unsupported)
|
||||
0, // clutch (unsupported)
|
||||
(uint8_t)config.control_mode,
|
||||
config.length_desired,
|
||||
get_current_length(),
|
||||
config.rate_desired,
|
||||
0, // tension (unsupported)
|
||||
0, // voltage (unsupported)
|
||||
0); // temp (unsupported)
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,17 +24,29 @@ public:
|
|||
AP_Winch_Servo(struct AP_Winch::Backend_Config &_config) :
|
||||
AP_Winch_Backend(_config) { }
|
||||
|
||||
// true if winch is healthy
|
||||
bool healthy() const override;
|
||||
|
||||
// initialise the winch
|
||||
void init(const AP_WheelEncoder* wheel_encoder) override;
|
||||
void init() override;
|
||||
|
||||
// control the winch
|
||||
void update() override;
|
||||
|
||||
private:
|
||||
// external reference
|
||||
const AP_WheelEncoder* _wheel_encoder;
|
||||
// returns current length of line deployed
|
||||
float get_current_length() const override { return line_length; }
|
||||
|
||||
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
|
||||
// send status to ground station
|
||||
void send_status(const GCS_MAVLINK &channel) override;
|
||||
|
||||
// write log
|
||||
void write_log() override;
|
||||
|
||||
private:
|
||||
|
||||
// update pwm outputs to control winch
|
||||
void control_winch();
|
||||
|
||||
uint32_t control_update_ms; // last time control_winch was called
|
||||
float line_length; // estimated length of line in meters
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue