aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_MotorsUGV: add motor output test

Browse Source
This commit is contained in:
khancyr 2017-07-15 11:57:38 +09:00 committed by Randy Mackay
parent 8421575f05
commit 2cd3dd5cba
2 changed files with 58 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
APMrover2/AP_MotorsUGV.cpp
View File

@ -312,3 +312,52 @@ void AP_MotorsUGV::setup_safety_output()
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);
}
bool AP_MotorsUGV::output_test(motor_test_order motor_seq)
{
// check if the motor_seq is valid
if (motor_seq > THROTTLE_RIGHT) {
return false;
}
_throttle = constrain_float(_throttle, -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);
break;
}
case STEERING: {
if (!SRV_Channels::function_assigned(SRV_Channel::k_steering)) {
return false;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, _throttle);
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);
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);
break;
}
default:
return false;
}
SRV_Channels::calc_pwm();
hal.rcout->cork();
SRV_Channels::output_ch_all();
hal.rcout->push();
return true;
}

9
APMrover2/AP_MotorsUGV.h
View File

@ -18,6 +18,13 @@ public:
PWM_TYPE_BRUSHEDBIPOLAR = 4,
};
enum motor_test_order {
THROTTLE = 0,
STEERING = 1,
THROTTLE_LEFT = 2,
THROTTLE_RIGHT = 3,
};
// initialise motors
void init();
@ -43,6 +50,8 @@ 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);
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];