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

Rover: initial implementation of skid steering for APMrover2 

this adds SKID_STEER_IN and SKID_STEER_OUT parameters for controlling
skid skeering control and output
This commit is contained in:
Andrew Tridgell 2013-03-15 11:04:33 +11:00
parent 8d8e30156f
commit 0dde0b3551
5 changed files with 73 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
APMrover2/Parameters.h
View File

@ -88,6 +88,8 @@ public:
k_param_throttle_cruise,
k_param_throttle_slewrate,
k_param_throttle_reduction,
k_param_skid_steer_in,
k_param_skid_steer_out,
// failsafe control
k_param_fs_action = 180,
@ -192,6 +194,8 @@ public:
AP_Int8 throttle_max;
AP_Int8 throttle_cruise;
AP_Int8 throttle_slewrate;
AP_Int8 skid_steer_in;
AP_Int8 skid_steer_out;
// failsafe control
AP_Int8 fs_action;

14
APMrover2/Parameters.pde
View File

@ -208,6 +208,20 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GSCALAR(throttle_slewrate, "THR_SLEWRATE", 0),
// @Param: SKID_STEER_OUT
// @DisplayName: Skid steering output
// @Description: Set this to 1 for skid steering controlled rovers (tank track style). When enabled, servo1 is used for the left track control, servo3 is used for right track control
// @Values: 0:Disabled, 1:SkidSteeringOutput
// @User: Standard
GSCALAR(skid_steer_out, "SKID_STEER_OUT", 0),
// @Param: SKID_STEER_IN
// @DisplayName: Skid steering input
// @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:SkidSteeringOutput
// @User: Standard
GSCALAR(skid_steer_in, "SKID_STEER_IN", 0),
// @Param: FS_ACTION
// @DisplayName: Failsafe Action
// @Description: What to do on a failsafe event

26
APMrover2/Steering.pde
View File

@ -91,10 +91,11 @@ static void set_servos(void)
{
int16_t last_throttle = g.channel_throttle.radio_out;
if (control_mode == MANUAL || control_mode == LEARNING) {
if ((control_mode == MANUAL || control_mode == LEARNING) &&
(g.skid_steer_out == g.skid_steer_in)) {
// do a direct pass through of radio values
g.channel_steer.radio_out = g.channel_steer.radio_in;
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
g.channel_steer.radio_out = hal.rcin->read(CH_STEER);
g.channel_throttle.radio_out = hal.rcin->read(CH_THROTTLE);
} else {
g.channel_steer.calc_pwm();
g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out,
@ -105,6 +106,25 @@ static void set_servos(void)
// limit throttle movement speed
throttle_slew_limit(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
*/
float steering_scaled = g.channel_steer.norm_output();
float throttle_scaled = g.channel_throttle.norm_output();
float motor1 = throttle_scaled + 0.5*steering_scaled;
float motor2 = throttle_scaled - 0.5*steering_scaled;
g.channel_steer.servo_out = 4500*motor1;
g.channel_throttle.servo_out = 100*motor2;
g.channel_steer.calc_pwm();
g.channel_throttle.calc_pwm();
}
}

7
APMrover2/defines.h
View File

@ -52,13 +52,8 @@ enum ch7_option {
#define GPS_PROTOCOL_MTK19 6
#define GPS_PROTOCOL_AUTO 7
#define CH_ROLL CH_1
#define CH_PITCH CH_2
#define CH_STEER CH_1
#define CH_THROTTLE CH_3
#define CH_RUDDER CH_4
#define CH_YAW CH_4
#define CH_AIL1 CH_5
#define CH_AIL2 CH_6
// HIL enumerations
#define HIL_MODE_DISABLED 0

34
APMrover2/radio.pde
View File

@ -54,7 +54,7 @@ static void init_rc_out()
static void read_radio()
{
g.channel_steer.set_pwm(hal.rcin->read(CH_ROLL));
g.channel_steer.set_pwm(hal.rcin->read(CH_STEER));
g.channel_throttle.set_pwm(hal.rcin->read(CH_3));
g.rc_5.set_pwm(hal.rcin->read(CH_5));
@ -72,13 +72,35 @@ static void read_radio()
throttle_nudge = 0;
}
if (g.skid_steer_in) {
// convert the two radio_in values from skid steering values
/*
cliSerial->printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
g.rc_1.control_in,
g.rc_2.control_in,
g.rc_3.control_in,
g.rc_4.control_in);
mixing rule:
steering = motor1 - motor2
throttle = 0.5*(motor1 + motor2)
motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering
*/
float motor1 = g.channel_steer.norm_input();
float motor2 = g.channel_throttle.norm_input();
float steering_scaled = motor2 - motor1;
float throttle_scaled = 0.5f*(motor1 + motor2);
int16_t steer = g.channel_steer.radio_trim;
int16_t thr = g.channel_throttle.radio_trim;
if (steering_scaled > 0.0f) {
steer += steering_scaled*(g.channel_steer.radio_max-g.channel_steer.radio_trim);
} else {
steer += steering_scaled*(g.channel_steer.radio_trim-g.channel_steer.radio_min);
}
if (throttle_scaled > 0.0f) {
thr += throttle_scaled*(g.channel_throttle.radio_max-g.channel_throttle.radio_trim);
} else {
thr += throttle_scaled*(g.channel_throttle.radio_trim-g.channel_throttle.radio_min);
}
g.channel_steer.set_pwm(steer);
g.channel_throttle.set_pwm(thr);
}
}
static void control_failsafe(uint16_t pwm)