You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
159 lines
3.7 KiB
159 lines
3.7 KiB
#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(); |
|
} |
|
|
|
};
|
|
|