Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsUGV: re-order implementation to match declaration

Browse Source 
non-functional change
This commit is contained in:
Randy Mackay 2017-08-16 10:00:40 +09:00
parent 47968cda09
commit c61f965828
2 changed files with 42 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
APMrover2/AP_MotorsUGV.cpp
View File

@ -75,6 +75,47 @@ void AP_MotorsUGV::init()
setup_safety_output(); setup_safety_output();
} }
// setup output in case of main CPU failure
void AP_MotorsUGV::setup_safety_output()
{
if (_pwm_type == PWM_TYPE_BRUSHED) {
// set trim to min to set duty cycle range (0 - 100%) to servo range
SRV_Channels::set_trim_to_min_for(SRV_Channel::k_throttleLeft);
SRV_Channels::set_trim_to_min_for(SRV_Channel::k_throttleRight);
}
if (_disarm_disable_pwm) {
// throttle channels output zero pwm (i.e. no signal)
SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_safety_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_safety_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
} else {
// throttle channels output trim values (because rovers will go backwards if set to MIN)
SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_safety_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_safety_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
// stop sending pwm if main CPU fails
SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
}
// setup servo output ranges
void AP_MotorsUGV::setup_servo_output()
{
// k_steering are limited to -45;45 degree
SRV_Channels::set_angle(SRV_Channel::k_steering, SERVO_MAX);
// k_throttle are in power percent so -100 ... 100
SRV_Channels::set_angle(SRV_Channel::k_throttle, 100);
// skid steering left/right throttle as -1000 to 1000 values
SRV_Channels::set_angle(SRV_Channel::k_throttleLeft, 1000);
SRV_Channels::set_angle(SRV_Channel::k_throttleRight, 1000);
}
/* /*
work out if skid steering is available work out if skid steering is available
*/ */
@ -273,20 +314,6 @@ void AP_MotorsUGV::set_limits_from_input(bool armed, float steering, float throt
limit.throttle_upper = !armed || (throttle >= 100.0f); limit.throttle_upper = !armed || (throttle >= 100.0f);
} }
// setup servo output
void AP_MotorsUGV::setup_servo_output()
{
// k_steering are limited to -45;45 degree
SRV_Channels::set_angle(SRV_Channel::k_steering, SERVO_MAX);
// k_throttle are in power percent so -100 ... 100
SRV_Channels::set_angle(SRV_Channel::k_throttle, 100);
// skid steering left/right throttle as -1000 to 1000 values
SRV_Channels::set_angle(SRV_Channel::k_throttleLeft, 1000);
SRV_Channels::set_angle(SRV_Channel::k_throttleRight, 1000);
}
// setup pwm output type // setup pwm output type
void AP_MotorsUGV::setup_pwm_type() void AP_MotorsUGV::setup_pwm_type()
{ {
@ -314,33 +341,6 @@ void AP_MotorsUGV::setup_pwm_type()
} }
} }
// setup output in case of main CPU failure
void AP_MotorsUGV::setup_safety_output()
{
if (_pwm_type == PWM_TYPE_BRUSHED) {
// set trim to min to set duty cycle range (0 - 100%) to servo range
SRV_Channels::set_trim_to_min_for(SRV_Channel::k_throttleLeft);
SRV_Channels::set_trim_to_min_for(SRV_Channel::k_throttleRight);
}
if (_disarm_disable_pwm) {
// throttle channels output zero pwm (i.e. no signal)
SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_safety_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_safety_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
} else {
// throttle channels output trim values (because rovers will go backwards if set to MIN)
SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_safety_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_safety_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
// stop sending pwm if main CPU fails
SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
}
// test steering or throttle output as a percentage of the total (range -100 to +100) // test steering or throttle output as a percentage of the total (range -100 to +100)
// used in response to DO_MOTOR_TEST mavlink command // used in response to DO_MOTOR_TEST mavlink command
bool AP_MotorsUGV::output_test_pct(motor_test_order motor_seq, float pct) bool AP_MotorsUGV::output_test_pct(motor_test_order motor_seq, float pct)

1
APMrover2/AP_MotorsUGV.h
View File

@ -31,6 +31,7 @@ public:
// setup output in case of main CPU failure // setup output in case of main CPU failure
void setup_safety_output(); void setup_safety_output();
// setup servo output ranges
void setup_servo_output(); void setup_servo_output();
// set steering as a value from -4500 to +4500 // set steering as a value from -4500 to +4500