diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index ea753047ef..9194345b80 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -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; diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index 2e50966cb4..48227e756f 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -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 diff --git a/APMrover2/Steering.pde b/APMrover2/Steering.pde index ffbd21e31f..df933ef3cb 100644 --- a/APMrover2/Steering.pde +++ b/APMrover2/Steering.pde @@ -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) { - // 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; + 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 = 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(); + } } diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 5f7006fc8c..4cccb3d8f0 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -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 diff --git a/APMrover2/radio.pde b/APMrover2/radio.pde index b228f44007..479ade690d 100644 --- a/APMrover2/radio.pde +++ b/APMrover2/radio.pde @@ -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; } - /* - 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); - */ + 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 + */ + + 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)