diff --git a/libraries/AP_Winch/AP_Winch.cpp b/libraries/AP_Winch/AP_Winch.cpp index c7cc461433..51d9c62f1e 100644 --- a/libraries/AP_Winch/AP_Winch.cpp +++ b/libraries/AP_Winch/AP_Winch.cpp @@ -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(); +} + +}; diff --git a/libraries/AP_Winch/AP_Winch.h b/libraries/AP_Winch/AP_Winch.h index bc89c1506f..458ab7c9a5 100644 --- a/libraries/AP_Winch/AP_Winch.h +++ b/libraries/AP_Winch/AP_Winch.h @@ -19,16 +19,6 @@ #include #include #include -#include - -// 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(); }; diff --git a/libraries/AP_Winch/AP_Winch_Backend.cpp b/libraries/AP_Winch/AP_Winch_Backend.cpp new file mode 100644 index 0000000000..b8d534f16d --- /dev/null +++ b/libraries/AP_Winch/AP_Winch_Backend.cpp @@ -0,0 +1,68 @@ +#include +#include + +// 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; +} diff --git a/libraries/AP_Winch/AP_Winch_Backend.h b/libraries/AP_Winch/AP_Winch_Backend.h index 9daf8686d7..5e71367e03 100644 --- a/libraries/AP_Winch/AP_Winch_Backend.h +++ b/libraries/AP_Winch/AP_Winch_Backend.h @@ -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 }; diff --git a/libraries/AP_Winch/AP_Winch_Servo.cpp b/libraries/AP_Winch/AP_Winch_Servo.cpp index 9bade117b3..7dbac8d3ed 100644 --- a/libraries/AP_Winch/AP_Winch_Servo.cpp +++ b/libraries/AP_Winch/AP_Winch_Servo.cpp @@ -1,13 +1,23 @@ #include +#include 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::quiet_NaN(), // tension + std::numeric_limits::quiet_NaN(), // voltage + std::numeric_limits::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) } diff --git a/libraries/AP_Winch/AP_Winch_Servo.h b/libraries/AP_Winch/AP_Winch_Servo.h index 47a83156f4..7590b8392f 100644 --- a/libraries/AP_Winch/AP_Winch_Servo.h +++ b/libraries/AP_Winch/AP_Winch_Servo.h @@ -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 };