void AP_MotorsUGV::output(bool armed, float ground_speed, float dt)
{
// soft-armed overrides passed in armed status
if (!hal.util->get_soft_armed()) {
armed = false;
_throttle = 0.0f;
}
// sanity check parameters
sanity_check_parameters();
// clear and set limits based on input (limit flags may be set again by output_regular or output_skid_steering methods)
set_limits_from_input(armed, _steering, _throttle);
// slew limit throttle
slew_limit_throttle(dt);
// output for regular steering/throttle style frames
output_regular(armed, ground_speed, _steering, _throttle);
// output for omni style frames
output_omni(armed, _steering, _throttle);
// output for skid steering style frames
output_skid_steering(armed, _steering, _throttle);
// send values to the PWM timers for output
SRV_Channels::calc_pwm();
SRV_Channels::cork();
SRV_Channels::output_ch_all();
SRV_Channels::push();
}
// output for omni style frames
void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle, float lateral)
{
if (!has_lateral_control()) {
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);
if (armed) {
// scale throttle, steering and lateral to -1 ~ 1
const float scaled_throttle = throttle / 100.0f;
const float scaled_steering = steering / 4500.0f;
const float scaled_lateral = lateral / 100.0f;
// calculate desired vehicle speed and direction
const float magnitude = safe_sqrt((scaled_throttle*scaled_throttle)+(scaled_lateral*scaled_lateral));
const float theta = atan2f(scaled_throttle,scaled_lateral);
// calculate X and Y vectors using the following the equations: vx = cos(theta) * magnitude and vy = sin(theta) * magnitude
const float Vx = -(cosf(theta)*magnitude);
const float Vy = -(sinf(theta)*magnitude);
// calculate output throttle for each motor. Output is multiplied by 0.5 to bring the range generally within -1 ~ 1
// First wheel (motor 1) moves only parallel to x-axis so only X component is taken. Normal range is -2 ~ 2 with the steering
// motor_2 and motor_3 utilizes both X and Y components.
// safe_sqrt((3)/2) used because the motors are 120 degrees apart in the frame, this setup is mandatory
float motor_1 = 0.5 * ((-Vx) + scaled_steering);
float motor_2 = 0.5 * (((0.5*Vx)-((safe_sqrt(3)/2)*Vy)) + scaled_steering);
float motor_3 = 0.5 * (((0.5*Vx)+((safe_sqrt(3)/2)*Vy)) + scaled_steering);
// apply constraints
motor_1 = constrain_float(motor_1, -1.0f, 1.0f);
motor_2 = constrain_float(motor_2, -1.0f, 1.0f);
motor_3 = constrain_float(motor_3, -1.0f, 1.0f);
// scale back and send pwm value to each motor
output_throttle(SRV_Channel::k_motor1, 100.0f * motor_1);
output_throttle(SRV_Channel::k_motor2, 100.0f * motor_2);
output_throttle(SRV_Channel::k_motor3, 100.0f * motor_3);
} else {
// handle disarmed case
if (_disarm_disable_pwm) {
SRV_Channels::set_output_limit(SRV_Channel::k_motor1, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_motor2, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_motor3, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
} else {
SRV_Channels::set_output_limit(SRV_Channel::k_motor1, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_motor2, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_motor3, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
}
}
// 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 for custom configurations
void AP_MotorsUGV::output_custom_config(bool armed, float steering, float throttle, float lateral)
{
// exit immediately if the frame type is set to UNDEFINED
if (rover.get_frame_type() == FRAME_TYPE_UNDEFINED) {
return;
}
if (armed) {
// clear and set limits based on input
set_limits_from_input(armed, steering, throttle);
// constrain steering
steering = constrain_float(steering, -4500.0f, 4500.0f);
// scale throttle, steering and lateral inputs to -1 to 1
const float scaled_throttle = throttle / 100.0f;
const float scaled_steering = steering / 4500.0f;
const float scaled_lateral = lateral / 100.0f;
float thr_str_ltr_out;
float thr_str_ltr_max = 1;
for (uint8_t i=0; i<AP_MOTORS_NUM_MOTORS_MAX; i++) {
thr_str_ltr_out = (scaled_throttle * _throttle_factor[i]) +
(scaled_steering * _steering_factor[i]) +
(scaled_lateral * _lateral_factor[i]);
if (fabsf(thr_str_ltr_out) > thr_str_ltr_max) {
thr_str_ltr_max = fabsf(thr_str_ltr_out);
}
float output_vectored = (thr_str_ltr_out / thr_str_ltr_max);
// send output for each motor
output_throttle(SRV_Channels::get_motor_function(i), 100.0f * output_vectored);
}
} else {
// handle disarmed case
if (_disarm_disable_pwm) {
for (uint8_t i=0; i<_motors_num; i++) {
SRV_Channels::set_output_limit(SRV_Channels::get_motor_function(i), SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
}
} else {
for (uint8_t i=0; i<_motors_num; i++) {
SRV_Channels::set_output_limit(SRV_Channels::get_motor_function(i), SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
}
}
}
