Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsUGV: use wheel-rate-control for skid steering vehicles

Browse Source
This commit is contained in:
Randy Mackay 2018-08-07 16:15:05 +09:00
parent 785da8add9
commit 69541fc89c
2 changed files with 36 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
APMrover2/AP_MotorsUGV.cpp
View File

@ -321,7 +321,7 @@ void AP_MotorsUGV::output(bool armed, float ground_speed, float dt)
output_regular(armed, ground_speed, _steering, _throttle);
// output for skid steering style frames
output_skid_steering(armed, _steering, _throttle);
output_skid_steering(armed, _steering, _throttle, dt);
// output for frames with vectored and custom motor configurations
output_custom_config(armed, _steering, _throttle, _lateral);
@ -582,7 +582,7 @@ void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering
}
// output to skid steering channels
void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float throttle)
void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float throttle, float dt)
{
if (!have_skid_steering()) {
return;
@ -626,8 +626,8 @@ void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float thrott
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_throttle(SRV_Channel::k_throttleLeft, 100.0f * motor_left, dt);
output_throttle(SRV_Channel::k_throttleRight, 100.0f * motor_right, dt);
}
// output for custom configurations
@ -680,7 +680,7 @@ void AP_MotorsUGV::output_custom_config(bool armed, float steering, float thrott
}
// output throttle value to main throttle channel, left throttle or right throttle. throttle should be scaled from -100 to 100
void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, float throttle)
void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, float throttle, float dt)
{
// sanity check servo function
if (function != SRV_Channel::k_throttle && function != SRV_Channel::k_throttleLeft && function != SRV_Channel::k_throttleRight && function != SRV_Channel::k_motor1 && function != SRV_Channel::k_motor2 && function != SRV_Channel::k_motor3 && function!= SRV_Channel::k_motor4) {
@ -690,6 +690,9 @@ void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, f
// constrain and scale output
throttle = get_scaled_throttle(throttle);
// apply rate control
throttle = get_rate_controlled_throttle(function, throttle, dt);
// set relay if necessary
if (_pwm_type == PWM_TYPE_BRUSHED_WITH_RELAY) {
// find the output channel, if not found return
@ -794,6 +797,28 @@ float AP_MotorsUGV::get_scaled_throttle(float throttle) const
return 100.0f * sign * ((_thrust_curve_expo - 1.0f) + safe_sqrt((1.0f - _thrust_curve_expo) * (1.0f - _thrust_curve_expo) + 4.0f * _thrust_curve_expo * fabsf(throttle_pct))) / (2.0f * _thrust_curve_expo);
}
// use rate controller to achieve desired throttle
float AP_MotorsUGV::get_rate_controlled_throttle(SRV_Channel::Aux_servo_function_t function, float throttle, float dt)
{
// require non-zero dt
if (!is_positive(dt)) {
return throttle;
}
// attempt to rate control left throttle
if ((function == SRV_Channel::k_throttleLeft) && rover.get_wheel_rate_control().enabled(0)) {
return rover.get_wheel_rate_control().get_rate_controlled_throttle(0, throttle, dt);
}
// rate control right throttle
if ((function == SRV_Channel::k_throttleRight) && rover.get_wheel_rate_control().enabled(1)) {
return rover.get_wheel_rate_control().get_rate_controlled_throttle(1, throttle, dt);
}
// return throttle unchanged
return throttle;
}
// return true if motors are moving
bool AP_MotorsUGV::active() const
{

8
APMrover2/AP_MotorsUGV.h
View File

@ -119,13 +119,14 @@ protected:
void output_regular(bool armed, float ground_speed, float steering, float throttle);
// output to skid steering channels
void output_skid_steering(bool armed, float steering, float throttle);
void output_skid_steering(bool armed, float steering, float throttle, float dt);
// output for vectored and custom motors configuration
void output_custom_config(bool armed, float steering, float throttle, float lateral);
// output throttle (-100 ~ +100) to a throttle channel. Sets relays if required
void output_throttle(SRV_Channel::Aux_servo_function_t function, float throttle);
// dt is the main loop time interval and is required when rate control is required
void output_throttle(SRV_Channel::Aux_servo_function_t function, float throttle, float dt = 0.0f);
// slew limit throttle for one iteration
void slew_limit_throttle(float dt);
@ -136,6 +137,9 @@ protected:
// scale a throttle using the _thrust_curve_expo parameter. throttle should be in the range -100 to +100
float get_scaled_throttle(float throttle) const;
// use rate controller to achieve desired throttle
float get_rate_controlled_throttle(SRV_Channel::Aux_servo_function_t function, float throttle, float dt);
// external references
AP_ServoRelayEvents &_relayEvents;