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

Rover: implement new style skid steering

Browse Source 
use throttleLeft and throttleRight servo functions. This gives much
greater flexibility and control over motor trims and limits
This commit is contained in:
Andrew Tridgell 2017-06-20 15:17:24 +09:00 committed by Randy Mackay
parent 17893958bf
commit 79f7cc5779
3 changed files with 71 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/Rover.h
View File

@ -475,6 +475,8 @@ private:
void calc_throttle(float target_speed);
void calc_lateral_acceleration();
void calc_nav_steer();
bool have_skid_steering();
void mix_skid_steering();
void set_servos(void);
void set_auto_WP(const struct Location& loc);
void set_guided_WP(const struct Location& loc);

96
APMrover2/Steering.cpp
View File

@ -69,7 +69,7 @@ bool Rover::auto_check_trigger(void) {
work out if we are going to use pivot steering
*/
bool Rover::use_pivot_steering(void) {
if (control_mode >= AUTO && g.skid_steer_out && g.pivot_turn_angle != 0) {
if (control_mode >= AUTO && have_skid_steering() && g.pivot_turn_angle != 0) {
const int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
if (abs(bearing_error) > g.pivot_turn_angle) {
return true;
@ -239,12 +239,52 @@ void Rover::calc_nav_steer() {
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steerController.get_steering_out_lat_accel(lateral_acceleration));
}
/*
run the skid steering mixer
*/
void Rover::mix_skid_steering(void)
{
const float steering_scaled = SRV_Channels::get_output_scaled(SRV_Channel::k_steering) / 4500.0;
const float throttle_scaled = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 100.0;
float motor1 = throttle_scaled + 0.5f * steering_scaled;
float motor2 = throttle_scaled - 0.5f * steering_scaled;
if (fabsf(throttle_scaled) <= 0.01f) {
// Use full range for on spot turn
motor1 = steering_scaled;
motor2 = -steering_scaled;
}
// first new-style skid steering
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 1000 * motor1);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 1000 * motor2);
// now old style skid steering with skid_steer_out
if (g.skid_steer_out) {
// convert the two radio_out values to skid steering values
/*
mixing rule:
steering = motor1 - motor2
throttle = 0.5*(motor1 + motor2)
motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering
*/
if ((control_mode == MANUAL || control_mode == LEARNING) && g.skid_steer_in) {
// Mixage is already done by a controller so just pass the value to motor
motor1 = steering_scaled;
motor2 = throttle_scaled;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 4500 * motor1);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100 * motor2);
}
}
/*****************************************
Set the flight control servos based on the current calculated values
*****************************************/
void Rover::set_servos(void) {
static uint16_t last_throttle;
bool apply_skid_mix = true; // Normaly true, false when the mixage is done by the controler with skid_steer_in = 1
if (control_mode == MANUAL || control_mode == LEARNING) {
// do a direct pass through of radio values
@ -258,9 +298,6 @@ void Rover::set_servos(void) {
SRV_Channels::set_output_limit(SRV_Channel::k_steering, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
}
if (g.skid_steer_in) {
apply_skid_mix = false;
}
} else {
if (in_reverse) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
@ -276,7 +313,7 @@ void Rover::set_servos(void) {
// suppress throttle if in failsafe
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
// suppress steer if in failsafe and skid steer mode
if (g.skid_steer_out) {
if (have_skid_steering()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
}
}
@ -284,7 +321,7 @@ void Rover::set_servos(void) {
if (!hal.util->get_soft_armed()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
// suppress steer if in failsafe and skid steer mode
if (g.skid_steer_out) {
if (have_skid_steering()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
}
}
@ -296,32 +333,10 @@ void Rover::set_servos(void) {
// record last throttle before we apply skid steering
SRV_Channels::get_output_pwm(SRV_Channel::k_throttle, last_throttle);
if (g.skid_steer_out) {
// convert the two radio_out values to skid steering values
/*
mixing rule:
steering = motor1 - motor2
throttle = 0.5*(motor1 + motor2)
motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering
*/
const float steering_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_steering);
const float throttle_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_throttle);
float motor1 = throttle_scaled + 0.5f * steering_scaled;
float motor2 = throttle_scaled - 0.5f * steering_scaled;
if (!apply_skid_mix) { // Mixage is already done by a controller so just pass the value to motor
motor1 = steering_scaled;
motor2 = throttle_scaled;
} else if (fabsf(throttle_scaled) <= 0.01f) { // Use full range for on spot turn
motor1 = steering_scaled;
motor2 = -steering_scaled;
if (have_skid_steering()) {
mix_skid_steering();
}
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 4500 * motor1);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100 * motor2);
}
if (!arming.is_armed()) {
// Some ESCs get noisy (beep error msgs) if PWM == 0.
// This little segment aims to avoid this.
@ -333,6 +348,8 @@ void Rover::set_servos(void) {
case AP_Arming::YES_ZERO_PWM:
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
if (g.skid_steer_out) {
SRV_Channels::set_output_limit(SRV_Channel::k_steering, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
}
@ -341,6 +358,8 @@ void Rover::set_servos(void) {
case AP_Arming::YES_MIN_PWM:
default:
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
if (g.skid_steer_out) {
SRV_Channels::set_output_limit(SRV_Channel::k_steering, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
@ -357,4 +376,17 @@ void Rover::set_servos(void) {
#endif
}
/*
work out if skid steering is available
*/
bool Rover::have_skid_steering(void)
{
if (g.skid_steer_out) {
return true;
}
if (SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) &&
SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) {
return true;
}
return false;
}

6
APMrover2/radio.cpp
View File

@ -16,6 +16,10 @@ void Rover::set_control_channels(void)
SRV_Channels::set_angle(SRV_Channel::k_steering, SERVO_MAX);
SRV_Channels::set_angle(SRV_Channel::k_throttle, 100);
// 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);
// 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());
@ -94,7 +98,7 @@ void Rover::rudder_arm_disarm_check()
// not at full right rudder
rudder_arm_timer = 0;
}
} else if (!motor_active() & !g.skid_steer_out) {
} else if (!motor_active() & !have_skid_steering()) {
// 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