AP_MotorsUGV: steering limits and constraint done by each output handler

each motor output handler (regular, skid, omni) becomes responsible for constraining the steering input (if required) and setting the limits flags
it is possible to reduce duplicate code but I think it might be more important to keep it clear which level is responsible for the limiting and constraining of steering
This commit is contained in:
Randy Mackay 2018-06-08 10:50:12 +09:00
parent 0333c3f3fa
commit 374c77d9c8
1 changed files with 21 additions and 4 deletions

View File

@ -161,7 +161,7 @@ void AP_MotorsUGV::setup_servo_output()
// no scaling by speed or angle should be performed
void AP_MotorsUGV::set_steering(float steering, bool apply_scaling)
{
_steering = constrain_float(steering, -4500.0f, 4500.0f);
_steering = steering;
_scale_steering = apply_scaling;
}
@ -230,9 +230,6 @@ void AP_MotorsUGV::output(bool armed, float ground_speed, float dt)
// 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);
@ -465,6 +462,13 @@ void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering
}
}
// 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);
}
@ -475,6 +479,13 @@ void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle, float
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;
@ -527,6 +538,12 @@ void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float thrott
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) {