mirror of https://github.com/ArduPilot/ardupilot
74 lines
2.5 KiB
C++
74 lines
2.5 KiB
C++
#include <AP_Winch/AP_Winch_Backend.h>
|
|
|
|
#if AP_WINCH_ENABLED
|
|
|
|
#include <RC_Channel/RC_Channel.h>
|
|
#include <SRV_Channel/SRV_Channel.h>
|
|
|
|
// setup rc input and output
|
|
void AP_Winch_Backend::init()
|
|
{
|
|
// 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();
|
|
}
|
|
|
|
// if significant change in rc input switch to rate mode
|
|
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;
|
|
}
|
|
|
|
#endif // AP_WINCH_ENABLED
|