ardupilot/libraries/AP_Winch/AP_Winch_PWM.cpp

106 lines
3.3 KiB
C++
Raw Normal View History

2020-08-06 22:37:36 -03:00
#include "AP_Winch_PWM.h"
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
// true if winch is healthy
2020-08-06 22:37:36 -03:00
bool AP_Winch_PWM::healthy() const
{
// return immediately if no servo is assigned to control the winch
if (!SRV_Channels::function_assigned(SRV_Channel::k_winch)) {
return false;
}
return true;
}
2020-08-06 22:37:36 -03:00
void AP_Winch_PWM::update()
{
// return immediately if no servo is assigned to control the winch
if (!SRV_Channels::function_assigned(SRV_Channel::k_winch)) {
return;
}
// read pilot input
read_pilot_desired_rate();
// send outputs to winch
control_winch();
}
// update pwm outputs to control winch
2020-08-06 22:37:36 -03:00
void AP_Winch_PWM::control_winch()
{
const uint32_t now_ms = AP_HAL::millis();
float dt = (now_ms - control_update_ms) / 1000.0f;
if (dt > 1.0f) {
dt = 0.0f;
}
control_update_ms = now_ms;
// 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;
}
// if doing position control, calculate position error to desired rate
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);
}
// apply acceleration limited to rate
const float rate_limited = get_rate_limited_by_accel(config.rate_desired, dt);
// 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);
// update distance estimate assuming winch will move exactly as requested
line_length += config.rate_desired * dt;
}
//send generator status
2020-08-06 22:37:36 -03:00
void AP_Winch_PWM::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
2020-08-06 22:37:36 -03:00
void AP_Winch_PWM::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)
}