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

Rover: PILOT_STEER_TYPE replaces SKID_STEER_IN 

new options allow controlling vehicle's heading while reversing
skid-steering vehicles rotate in opposite direction when backing up
This commit is contained in:
Randy Mackay 2017-11-27 22:11:45 +09:00
parent c6689fd2e1
commit 26f50f6055
8 changed files with 70 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
APMrover2/Parameters.cpp
View File

@ -142,12 +142,12 @@ const AP_Param::Info Rover::var_info[] = {
// @User: Standard
GSCALAR(throttle_cruise, "CRUISE_THROTTLE", 50),
// @Param: SKID_STEER_IN
// @DisplayName: Skid steering input
// @Param: PILOT_STEER_TYPE
// @DisplayName: Pilot input steering type
// @Description: Set this to 1 for skid steering input rovers (tank track style in RC controller). When enabled, servo1 is used for the left track control, servo3 is used for right track control
// @Values: 0:Disabled, 1:SkidSteeringInput
// @Values: 0:Default,1:Two Paddles Input,2:Direction reversed when backing up,3:Direction unchanged when backing up
// @User: Standard
GSCALAR(skid_steer_in, "SKID_STEER_IN", 0),
GSCALAR(pilot_steer_type, "PILOT_STEER_TYPE", 0),
// @Param: FS_ACTION
// @DisplayName: Failsafe Action

4
APMrover2/Parameters.h
View File

@ -129,7 +129,7 @@ public:
k_param_throttle_cruise,
k_param_throttle_slewrate_old, // unused
k_param_throttle_reduction, // unused
k_param_skid_steer_in,
k_param_pilot_steer_type,
k_param_skid_steer_out_old, // unused
// failsafe control
@ -238,7 +238,7 @@ public:
// Throttle
//
AP_Int8 throttle_cruise;
AP_Int8 skid_steer_in;
AP_Int8 pilot_steer_type;
// failsafe control
AP_Int8 fs_action;

7
APMrover2/defines.h
View File

@ -121,5 +121,12 @@ enum aux_switch_pos {
AUX_SWITCH_HIGH
};
enum pilot_steer_type_t {
PILOT_STEER_TYPE_DEFAULT = 0,
PILOT_STEER_TYPE_TWO_PADDLES = 1,
PILOT_STEER_TYPE_DIR_REVERSED_WHEN_REVERSING = 2,
PILOT_STEER_TYPE_DIR_UNCHANGED_WHEN_REVERSING = 3,
};
#define AUX_SWITCH_PWM_TRIGGER_HIGH 1800 // pwm value above which the ch7 or ch8 option will be invoked
#define AUX_SWITCH_PWM_TRIGGER_LOW 1200 // pwm value below which the ch7 or ch8 option will be disabled

43
APMrover2/mode.cpp
View File

@ -24,6 +24,49 @@ bool Mode::enter()
return _enter();
}
void Mode::get_pilot_desired_steering_and_throttle(float &steering_out, float &throttle_out)
{
// apply RC skid steer mixing
switch ((enum pilot_steer_type_t)rover.g.pilot_steer_type.get())
{
case PILOT_STEER_TYPE_DEFAULT:
default: {
// by default regular and skid-steering vehicles reverse their rotation direction when backing up
// (this is the same as PILOT_STEER_TYPE_DIR_REVERSED_WHEN_REVERSING below)
throttle_out = rover.channel_throttle->get_control_in();
steering_out = rover.channel_steer->get_control_in();
break;
}
case PILOT_STEER_TYPE_TWO_PADDLES: {
// convert the two radio_in values from skid steering values
// left paddle from steering input channel, right paddle from throttle input channel
// steering = left-paddle - right-paddle
// throttle = average(left-paddle, right-paddle)
const float left_paddle = rover.channel_steer->norm_input();
const float right_paddle = rover.channel_throttle->norm_input();
throttle_out = 0.5f * (left_paddle + right_paddle) * 100.0f;
const float steering_dir = is_negative(throttle_out) ? -1 : 1;
steering_out = steering_dir * (left_paddle - right_paddle) * 4500.0f;
break;
}
case PILOT_STEER_TYPE_DIR_REVERSED_WHEN_REVERSING:
throttle_out = rover.channel_throttle->get_control_in();
steering_out = rover.channel_steer->get_control_in();
break;
case PILOT_STEER_TYPE_DIR_UNCHANGED_WHEN_REVERSING: {
throttle_out = rover.channel_throttle->get_control_in();
const float steering_dir = is_negative(throttle_out) ? -1 : 1;
steering_out = steering_dir * rover.channel_steer->get_control_in();
break;
}
}
}
// set desired location
void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
{

4
APMrover2/mode.h
View File

@ -91,6 +91,10 @@ protected:
// subclasses override this to perform any required cleanup when exiting the mode
virtual void _exit() { return; }
// decode pilot steering held in channel_steer, channel_throttle and return in steer_out and throttle_out arguments
// steering_out is in the range -4500 ~ +4500, throttle_out is in the range -100 ~ +100
void get_pilot_desired_steering_and_throttle(float &steering_out, float &throttle_out);
// calculate steering angle given a desired lateral acceleration
void calc_steering_from_lateral_acceleration(bool reversed = false);

6
APMrover2/mode_manual.cpp
View File

@ -8,9 +8,11 @@ void ModeManual::update()
g2.motors.set_throttle(0.0f);
g2.motors.set_steering(0.0f);
} else {
float desired_steering, desired_throttle;
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
// copy RC scaled inputs to outputs
g2.motors.set_throttle(channel_throttle->get_control_in());
g2.motors.set_steering(channel_steer->get_control_in());
g2.motors.set_throttle(desired_throttle);
g2.motors.set_steering(desired_steering);
}
// mark us as in_reverse when using a negative throttle to stop AHRS getting off

9
APMrover2/mode_steering.cpp
View File

@ -3,8 +3,11 @@
void ModeSteering::update()
{
float desired_steering, desired_throttle;
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
// convert pilot throttle input to desired speed (up to twice the cruise speed)
float target_speed = channel_throttle->get_control_in() * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
float target_speed = desired_steering * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
// get speed forward
float speed;
@ -17,7 +20,7 @@ void ModeSteering::update()
}
// determine if pilot is requesting pivot turn
bool is_pivot_turning = g2.motors.have_skid_steering() && is_zero(target_speed) && (channel_steer->get_control_in() != 0);
bool is_pivot_turning = g2.motors.have_skid_steering() && is_zero(target_speed) && (!is_zero(desired_steering));
// In steering mode we control lateral acceleration directly.
// For pivot steering vehicles we use the TURN_MAX_G parameter
@ -33,7 +36,7 @@ void ModeSteering::update()
max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);
// convert pilot steering input to desired lateral acceleration
lateral_acceleration = max_g_force * (channel_steer->get_control_in() / 4500.0f);
lateral_acceleration = max_g_force * (desired_steering / 4500.0f);
// reverse target lateral acceleration if backing up
bool reversed = false;

37
APMrover2/radio.cpp
View File

@ -113,43 +113,6 @@ void Rover::read_radio()
// check that RC value are valid
control_failsafe(channel_throttle->get_radio_in());
// apply RC skid steer mixing
if (g.skid_steer_in) {
// convert the two radio_in values from 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 left_input = channel_steer->norm_input();
const float right_input = channel_throttle->norm_input();
const float throttle_scaled = 0.5f * (left_input + right_input);
float steering_scaled = constrain_float(left_input - right_input, -1.0f, 1.0f);
// flip the steering direction if requesting the vehicle reverse (to be consistent with separate steering-throttle frames)
if (is_negative(throttle_scaled)) {
steering_scaled = -steering_scaled;
}
int16_t steer = channel_steer->get_radio_trim();
int16_t thr = channel_throttle->get_radio_trim();
if (steering_scaled > 0.0f) {
steer += steering_scaled * (channel_steer->get_radio_max() - channel_steer->get_radio_trim());
} else {
steer += steering_scaled * (channel_steer->get_radio_trim() - channel_steer->get_radio_min());
}
if (throttle_scaled > 0.0f) {
thr += throttle_scaled * (channel_throttle->get_radio_max() - channel_throttle->get_radio_trim());
} else {
thr += throttle_scaled * (channel_throttle->get_radio_trim() - channel_throttle->get_radio_min());
}
channel_steer->set_pwm(steer);
channel_throttle->set_pwm(thr);
}
// check if we try to do RC arm/disarm
rudder_arm_disarm_check();
}