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

AP_MotorsUGV: add limit flags

Browse Source 
These flags become true when the steering servo or motors hit their
limits.  used to stop I-term build-up in higher level controllers.
This commit is contained in:
Randy Mackay 2017-08-09 16:20:44 +09:00
parent d8e260874d
commit a5783939ef
2 changed files with 39 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
APMrover2/AP_MotorsUGV.cpp
View File

@ -100,6 +100,9 @@ void AP_MotorsUGV::output(bool armed, float dt)
slew_limit_throttle(dt);
// 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);
// output for regular steering/throttle style frames
output_regular(armed, _steering, _throttle);
@ -137,6 +140,10 @@ void AP_MotorsUGV::output_regular(bool armed, float steering, float throttle)
// 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) {
@ -162,6 +169,17 @@ void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float thrott
if (saturation_value > 1.0f) {
steering_scaled = steering_scaled / saturation_value;
throttle_scaled = throttle_scaled / saturation_value;
// set limits
if (is_negative(steering)) {
limit.steer_left = true;
} else {
limit.steer_right = true;
}
if (is_negative(throttle)) {
limit.throttle_lower = true;
} else {
limit.throttle_upper = true;
}
}
// add in throttle
@ -245,6 +263,16 @@ void AP_MotorsUGV::slew_limit_throttle(float dt)
}
}
// set limits based on steering and throttle input
void AP_MotorsUGV::set_limits_from_input(bool armed, float steering, float throttle)
{
// set limits based on inputs
limit.steer_left = !armed || (steering <= -4500.0f);
limit.steer_right = !armed || (steering >= 4500.0f);
limit.throttle_lower = !armed || (throttle <= -100.0f);
limit.throttle_upper = !armed || (throttle >= 100.0f);
}
// setup servo output
void AP_MotorsUGV::setup_servo_output()
{

11
APMrover2/AP_MotorsUGV.h
View File

@ -57,6 +57,14 @@ public:
// test steering or throttle output using a pwm value
bool output_test_pwm(motor_test_order motor_seq, float pwm);
// structure for holding motor limit flags
struct AP_MotorsUGV_limit {
uint8_t steer_left : 1; // we have reached the steering controller's left most limit
uint8_t steer_right : 1; // we have reached the steering controller's right most limit
uint8_t throttle_lower : 1; // we have reached throttle's lower limit
uint8_t throttle_upper : 1; // we have reached throttle's upper limit
} limit;
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
@ -77,6 +85,9 @@ protected:
// slew limit throttle for one iteration
void slew_limit_throttle(float dt);
// set limits based on steering and throttle input
void set_limits_from_input(bool armed, float steering, float throttle);
// external references
AP_ServoRelayEvents &_relayEvents;