// output to regular steering and throttle channels
void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering, float throttle)
{
// output to throttle channels
if (armed) {
if (_scale_steering) {
// vectored thrust handling
if (have_vectored_thrust()) {
if (fabsf(throttle) > _vector_throttle_base) {
// scale steering down linearly as throttle increases above _vector_throttle_base
steering *= constrain_float(_vector_throttle_base / fabsf(throttle), 0.0f, 1.0f);
}
} else {
// scale steering down as speed increase above MOT_SPD_SCA_BASE (1 m/s default)
if (is_positive(_speed_scale_base) && (fabsf(ground_speed) > _speed_scale_base)) {
steering *= (_speed_scale_base / fabsf(ground_speed));
} else {
// regular steering rover at low speed so set limits to stop I-term build-up in controllers
if (!have_skid_steering()) {
limit.steer_left = true;
limit.steer_right = true;
}
}
// reverse steering direction when backing up
if (is_negative(ground_speed)) {
steering *= -1.0f;
}
}
} else {
// reverse steering direction when backing up
if (is_negative(throttle)) {
steering *= -1.0f;
}
}
output_throttle(SRV_Channel::k_throttle, throttle);
} else {
// handle disarmed case
if (_disarm_disable_pwm) {
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
} else {
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
}
// clear and set limits based on input
// we do this here because vectored thrust or speed scaling may have reduced steering request
set_limits_from_input(armed, steering, throttle);
// constrain steering
steering = constrain_float(steering, -4500.0f, 4500.0f);
// always allow steering to move
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
}
// output to regular steering and throttle channels
void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering, float throttle)
{
// output to throttle channels
if (armed) {
if (_scale_steering) {
// vectored thrust handling
if (have_vectored_thrust()) {
if (fabsf(throttle) > _vector_throttle_base) {
// scale steering down linearly as throttle increases above _vector_throttle_base
steering *= constrain_float(_vector_throttle_base / fabsf(throttle), 0.0f, 1.0f);
}
} else {
// scale steering down as speed increase above 1m/s
if (fabsf(ground_speed) > 1.0f) {
steering *= (1.0f / fabsf(ground_speed));
} else {
// regular steering rover at low speed so set limits to stop I-term build-up in controllers
if (!have_skid_steering()) {
limit.steer_left = true;
limit.steer_right = true;
}
}
// reverse steering output if backing up
if (is_negative(ground_speed)) {
steering *= -1.0f;
}
}
} else {
// reverse steering direction when backing up
if (is_negative(throttle)) {
steering *= -1.0f;
}
}
output_throttle(SRV_Channel::k_throttle, throttle);
} else {
// handle disarmed case
if (_disarm_disable_pwm) {
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
} else {
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
}
// always allow steering to move
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
}
// output to regular steering and throttle channels
void AP_MotorsUGV::output_regular(bool armed, float steering, float throttle)
{
// output to throttle channels
if (armed) {
// vectored thrust handling
if (have_vectored_thrust() && (fabsf(throttle) > _vector_throttle_base)) {
// scale steering down linearly as throttle increases above _vector_throttle_base
const float steering_scalar = constrain_float(_vector_throttle_base / fabsf(throttle), 0.0f, 1.0f);
steering *= steering_scalar;
}
output_throttle(SRV_Channel::k_throttle, throttle);
} else {
// handle disarmed case
if (_disarm_disable_pwm) {
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
} else {
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
}
// always allow steering to move
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
}
