mirror of https://github.com/ArduPilot/ardupilot
160 lines
3.7 KiB
C++
160 lines
3.7 KiB
C++
#include "AP_Winch.h"
|
|
#include "AP_Winch_PWM.h"
|
|
#include "AP_Winch_Daiwa.h"
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
const AP_Param::GroupInfo AP_Winch::var_info[] = {
|
|
// 0 was ENABLE
|
|
|
|
// @Param: _TYPE
|
|
// @DisplayName: Winch Type
|
|
// @Description: Winch Type
|
|
// @User: Standard
|
|
// @Values: 0:None, 1:PWM, 2:Daiwa
|
|
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
|
|
// @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, 1.0f),
|
|
|
|
// 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
|
|
{
|
|
return ((config.type > 0) && (backend != nullptr));
|
|
}
|
|
|
|
// true if winch is healthy
|
|
bool AP_Winch::healthy() const
|
|
{
|
|
if (backend != nullptr) {
|
|
return backend->healthy();
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void AP_Winch::init()
|
|
{
|
|
switch ((WinchType)config.type.get()) {
|
|
case WinchType::NONE:
|
|
break;
|
|
case WinchType::PWM:
|
|
backend = new AP_Winch_PWM(config);
|
|
break;
|
|
case WinchType::DAIWA:
|
|
backend = new AP_Winch_Daiwa(config);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
if (backend != nullptr) {
|
|
backend->init();
|
|
}
|
|
}
|
|
|
|
// release specified length of cable (in meters)
|
|
void AP_Winch::release_length(float length)
|
|
{
|
|
if (backend == nullptr) {
|
|
return;
|
|
}
|
|
config.length_desired = backend->get_current_length() + length;
|
|
config.control_mode = 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.control_mode = 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()) {
|
|
hal.util->snprintf(failmsg, failmsg_len, "winch unhealthy");
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
// 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)
|
|
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();
|
|
}
|
|
|
|
};
|