5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-02 22:18:28 -04:00

Rover: Skid steering disarming fixes

I have disabled steering disarming for skid steering rovers.  Its
perfectly reasonable for a skid steering rover to go hard left on the
spot without any throttle and the user wouldn't want the rover to
disarm during this procedure.
If you disarm from the GCS for a skid steering rover we also ensure
the steering channel is disabled.
This commit is contained in:
Grant Morphett 2016-06-01 10:12:02 +10:00
parent 501284f65a
commit 094d571196
2 changed files with 15 additions and 1 deletions

View File

@ -18,6 +18,9 @@ void Rover::set_control_channels(void)
// For a rover safety is TRIM throttle
if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) {
hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->get_radio_trim());
if (g.skid_steer_out) {
hal.rcout->set_safety_pwm(1UL<<(rcmap.roll()-1), channel_steer->get_radio_trim());
}
}
// setup correct scaling for ESCs like the UAVCAN PX4ESC which
@ -42,6 +45,9 @@ void Rover::init_rc_out()
if (arming.arming_required() != AP_Arming::YES_ZERO_PWM) {
channel_throttle->enable_out();
if (g.skid_steer_out) {
channel_steer->enable_out();
}
}
RC_Channel::output_trim_all();
@ -55,6 +61,9 @@ void Rover::init_rc_out()
// full speed backward.
if (arming.arming_required() == AP_Arming::YES_MIN_PWM) {
hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->get_radio_trim());
if (g.skid_steer_out) {
hal.rcout->set_safety_pwm(1UL<<(rcmap.roll()-1), channel_steer->get_radio_trim());
}
}
}
@ -96,8 +105,10 @@ void Rover::rudder_arm_disarm_check()
// not at full right rudder
rudder_arm_timer = 0;
}
} else if (!motor_active()) {
} else if (!motor_active() & !g.skid_steer_out) {
// when armed and motor not active (not moving), full left rudder starts disarming counter
// This is disabled for skid steering otherwise when tring to turn a skid steering rover around
// the rover would disarm
if (channel_steer->get_control_in() < -4000) {
uint32_t now = millis();

View File

@ -528,6 +528,9 @@ bool Rover::disarm_motors(void)
}
if (arming.arming_required() == AP_Arming::YES_ZERO_PWM) {
channel_throttle->disable_out();
if (g.skid_steer_out) {
channel_steer->disable_out();
}
}
if (control_mode != AUTO) {
// reset the mission on disarm if we are not in auto