Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsUGV: add output_throttle to handle scaling and relay 

this function hides the difference in scaling between the regular throttle
(-100 to +100) and the throttleLeft and throttleRight (-1000 to +1000)
Also handles the setting of the relay for brushed-with-relay motors
This commit is contained in:
khancyr 2017-07-12 10:31:56 +02:00 committed by Randy Mackay
parent 8223743df8
commit 8421575f05
2 changed files with 49 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
APMrover2/AP_MotorsUGV.cpp
View File

@ -184,16 +184,53 @@ void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float thrott
motor_left += dir * steering_scaled;
}
}
if (_pwm_type == PWM_TYPE_BRUSHED) {
const bool dirLeft = is_positive(motor_left);
const bool dirRight = is_positive(motor_right);
_relayEvents.do_set_relay(0, dirLeft);
_relayEvents.do_set_relay(1, dirRight);
motor_left = fabsf(motor_left);
motor_right = fabsf(motor_right);
// 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 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)
{
// sanity check servo function
if (function != SRV_Channel::k_throttle && function != SRV_Channel::k_throttleLeft && function != SRV_Channel::k_throttleRight) {
return;
}
// constrain output
throttle = constrain_float(throttle, -100.0f, 100.0f);
// set relay if necessary
if (_pwm_type == PWM_TYPE_BRUSHED) {
switch (function) {
case SRV_Channel::k_throttle:
case SRV_Channel::k_throttleLeft:
_relayEvents.do_set_relay(0, is_negative(throttle));
break;
case SRV_Channel::k_throttleRight:
_relayEvents.do_set_relay(1, is_negative(throttle));
break;
default:
// do nothing
break;
}
throttle = fabsf(throttle);
}
// output to servo channel
switch (function) {
case SRV_Channel::k_throttle:
SRV_Channels::set_output_scaled(function, throttle);
break;
case SRV_Channel::k_throttleLeft:
case SRV_Channel::k_throttleRight:
SRV_Channels::set_output_scaled(function, throttle*10.0f);
break;
default:
// do nothing
break;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 1000.0f * motor_left);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 1000.0f * motor_right);
}
// slew limit throttle for one iteration

3
APMrover2/AP_MotorsUGV.h
View File

@ -57,6 +57,9 @@ protected:
// output to skid steering channels
void output_skid_steering(bool armed, float steering, float throttle);
// output throttle (-100 ~ +100) to a throttle channel. Sets relays if required
void output_throttle(SRV_Channel::Aux_servo_function_t function, float throttle);
// slew limit throttle for one iteration
void slew_limit_throttle(float dt);