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

AP_MotorsUGV: support vectored thrust

Browse Source 
steering scaled down as throttle increases
This commit is contained in:
Randy Mackay 2018-04-09 21:09:49 +09:00
parent 7a4ad9ff6f
commit 32ef6e85eb
2 changed files with 24 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
APMrover2/AP_MotorsUGV.cpp
View File

@ -79,6 +79,14 @@ const AP_Param::GroupInfo AP_MotorsUGV::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("THST_EXPO", 9, AP_MotorsUGV, _thrust_curve_expo, 0.0f), AP_GROUPINFO("THST_EXPO", 9, AP_MotorsUGV, _thrust_curve_expo, 0.0f),
// @Param: VEC_THR_BASE
// @DisplayName: Vector thrust throttle base
// @Description: Throttle level above which steering is scaled down when using vector thrust. zero to disable vectored thrust
// @Units: %
// @Range: 0 100
// @User: Advanced
AP_GROUPINFO("VEC_THR_BASE", 10, AP_MotorsUGV, _vector_throttle_base, 0.0f),
AP_GROUPEND AP_GROUPEND
}; };
@ -98,6 +106,9 @@ void AP_MotorsUGV::init()
// set safety output // set safety output
setup_safety_output(); setup_safety_output();
// sanity check parameters
_vector_throttle_base = constrain_float(_vector_throttle_base, 0.0f, 100.0f);
} }
// setup output in case of main CPU failure // setup output in case of main CPU failure
@ -353,12 +364,14 @@ void AP_MotorsUGV::setup_pwm_type()
// output to regular steering and throttle channels // output to regular steering and throttle channels
void AP_MotorsUGV::output_regular(bool armed, float steering, float throttle) void AP_MotorsUGV::output_regular(bool armed, float steering, float throttle)
{ {
// always allow steering to move
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
// output to throttle channels // output to throttle channels
if (armed) { if (armed) {
// handle armed case // 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); output_throttle(SRV_Channel::k_throttle, throttle);
} else { } else {
// handle disarmed case // handle disarmed case
@ -368,6 +381,9 @@ void AP_MotorsUGV::output_regular(bool armed, float steering, float throttle)
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); 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 // output to skid steering channels

4
APMrover2/AP_MotorsUGV.h
View File

@ -45,6 +45,9 @@ public:
// true if vehicle is capable of skid steering // true if vehicle is capable of skid steering
bool have_skid_steering() const; bool have_skid_steering() const;
// true if vehicle has vectored thrust (i.e. boat with motor on steering servo)
bool have_vectored_thrust() const { return is_positive(_vector_throttle_base); }
// output to motors and steering servos // output to motors and steering servos
void output(bool armed, float dt); void output(bool armed, float dt);
@ -103,6 +106,7 @@ protected:
AP_Int8 _throttle_min; // throttle minimum percentage AP_Int8 _throttle_min; // throttle minimum percentage
AP_Int8 _throttle_max; // throttle maximum percentage AP_Int8 _throttle_max; // throttle maximum percentage
AP_Float _thrust_curve_expo; // thrust curve exponent from -1 to +1 with 0 being linear AP_Float _thrust_curve_expo; // thrust curve exponent from -1 to +1 with 0 being linear
AP_Float _vector_throttle_base; // throttle level above which steering is scaled down when using vector thrust. zero to disable vectored thrust
// internal variables // internal variables
float _steering; // requested steering as a value from -4500 to +4500 float _steering; // requested steering as a value from -4500 to +4500