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

AP_MotorsUGV: add motor_output_test_pwm 

this allows the ground station to test a motor by requesting a specific pwm value
This commit is contained in:
Randy Mackay 2017-07-15 16:21:13 +09:00
parent 2cd3dd5cba
commit 7bfb79a4b7
2 changed files with 61 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
APMrover2/AP_MotorsUGV.cpp
View File

@ -313,43 +313,43 @@ void AP_MotorsUGV::setup_safety_output()
SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
}
bool AP_MotorsUGV::output_test(motor_test_order motor_seq)
// test steering or throttle output as a percentage of the total (range -100 to +100)
// used in response to DO_MOTOR_TEST mavlink command
bool AP_MotorsUGV::output_test_pct(motor_test_order motor_seq, float pct)
{
// check if the motor_seq is valid
if (motor_seq > THROTTLE_RIGHT) {
return false;
}
_throttle = constrain_float(_throttle, -100.0f, 100.0f);
pct = constrain_float(pct, -100.0f, 100.0f);
switch (motor_seq) {
case THROTTLE: {
if (!SRV_Channels::function_assigned(SRV_Channel::k_throttle)) {
return false;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, _throttle);
output_throttle(SRV_Channel::k_throttle, pct);
break;
}
case STEERING: {
if (!SRV_Channels::function_assigned(SRV_Channel::k_steering)) {
return false;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, _throttle);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, pct * 45.0f);
break;
}
case THROTTLE_LEFT: {
if (!SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft)) {
return false;
}
const float motorLeft = brushed_scaler((_throttle * 0.01f), _throttleLeft_servo);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 1000.0f * motorLeft);
output_throttle(SRV_Channel::k_throttleLeft, pct);
break;
}
case THROTTLE_RIGHT: {
if (!SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) {
return false;
}
const float motorRight = brushed_scaler((_throttle * 0.01f), _throttleRight_servo);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 1000.0f * motorRight);
output_throttle(SRV_Channel::k_throttleRight, pct);
break;
}
default:
@ -360,4 +360,50 @@ bool AP_MotorsUGV::output_test(motor_test_order motor_seq)
SRV_Channels::output_ch_all();
hal.rcout->push();
return true;
}
}
// test steering or throttle output using a pwm value
bool AP_MotorsUGV::output_test_pwm(motor_test_order motor_seq, float pwm)
{
// check if the motor_seq is valid
if (motor_seq > THROTTLE_RIGHT) {
return false;
}
switch (motor_seq) {
case THROTTLE: {
if (!SRV_Channels::function_assigned(SRV_Channel::k_throttle)) {
return false;
}
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, pwm);
break;
}
case STEERING: {
if (!SRV_Channels::function_assigned(SRV_Channel::k_steering)) {
return false;
}
SRV_Channels::set_output_pwm(SRV_Channel::k_steering, pwm);
break;
}
case THROTTLE_LEFT: {
if (!SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft)) {
return false;
}
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, pwm);
break;
}
case THROTTLE_RIGHT: {
if (!SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) {
return false;
}
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, pwm);
break;
}
default:
return false;
}
SRV_Channels::calc_pwm();
hal.rcout->cork();
SRV_Channels::output_ch_all();
hal.rcout->push();
return true;
}

7
APMrover2/AP_MotorsUGV.h
View File

@ -50,7 +50,12 @@ public:
// set when to use slew rate limiter
void slew_limit_throttle(bool value) { _use_slew_rate = value; }
bool output_test(motor_test_order motor_seq);
// test steering or throttle output as a percentage of the total (range -100 to +100)
// used in response to DO_MOTOR_TEST mavlink command
bool output_test_pct(motor_test_order motor_seq, float pct);
// test steering or throttle output using a pwm value
bool output_test_pwm(motor_test_order motor_seq, float pwm);
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];