// 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 skid steering channels
void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float throttle)
{
if (!have_skid_steering()) {
return;
}
// clear and set limits based on input
set_limits_from_input(armed, steering, throttle);
// constrain steering
steering = constrain_float(steering, -4500.0f, 4500.0f);
// handle simpler disarmed case
if (!armed) {
if (_disarm_disable_pwm) {
SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
} else {
SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
return;
}
// skid steering mixer
float steering_scaled = steering / 4500.0f; // steering scaled -1 to +1
float throttle_scaled = throttle / 100.0f; // throttle scaled -1 to +1
// apply constraints
steering_scaled = constrain_float(steering_scaled, -1.0f, 1.0f);
throttle_scaled = constrain_float(throttle_scaled, -1.0f, 1.0f);
// check for saturation and scale back throttle and steering proportionally
const float saturation_value = fabsf(steering_scaled) + fabsf(throttle_scaled);
if (saturation_value > 1.0f) {
steering_scaled = steering_scaled / saturation_value;
throttle_scaled = throttle_scaled / saturation_value;
}
// add in throttle and steering
const float motor_left = throttle_scaled + steering_scaled;
const float motor_right = throttle_scaled - steering_scaled;
// send pwm value to each motor
output_throttle(SRV_Channel::k_throttleLeft, 100.0f * motor_left);
output_throttle(SRV_Channel::k_throttleRight, 100.0f * motor_right);
}
// 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 skid steering channels
void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float throttle)
{
if (!have_skid_steering()) {
return;
}
// handle simpler disarmed case
if (!armed) {
if (_disarm_disable_pwm) {
SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
} else {
SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
return;
}
// skid steering mixer
float steering_scaled = steering / 4500.0f; // steering scaled -1 to +1
float throttle_scaled = throttle / 100.0f; // throttle scaled -1 to +1
// apply constraints
steering_scaled = constrain_float(steering_scaled, -1.0f, 1.0f);
throttle_scaled = constrain_float(throttle_scaled, -1.0f, 1.0f);
// check for saturation and scale back throttle and steering proportionally
const float saturation_value = fabsf(steering_scaled) + fabsf(throttle_scaled);
if (saturation_value > 1.0f) {
steering_scaled = steering_scaled / saturation_value;
throttle_scaled = throttle_scaled / saturation_value;
}
// reverse steering direction if throttle is negative to mimic regular rovers
const float steering_dir = is_negative(throttle_scaled) ? -1.0f : 1.0f;
// add in throttle and steering
const float motor_left = throttle_scaled + (steering_dir * steering_scaled);
const float motor_right = throttle_scaled - (steering_dir * steering_scaled);
// send pwm value to each motor
output_throttle(SRV_Channel::k_throttleLeft, 100.0f * motor_left);
output_throttle(SRV_Channel::k_throttleRight, 100.0f * motor_right);
}
// return true if motors are moving
bool AP_MotorsUGV::active() const
{
// if soft disarmed, motors not active
if (!hal.util->get_soft_armed()) {
return false;
}
// check throttle is active
if (!is_zero(get_throttle())) {
return true;
}
// skid-steering vehicles active when steering
if (have_skid_steering() && !is_zero(get_steering())) {
return true;
}
return false;
}
