diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 60b4ae07d0..96ecdeac56 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -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 diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 65d712ccbc..6f5722e07d 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -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; diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 02f7e74599..e7b33fff03 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -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 diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index a29c90dc57..8ac54f7d0a 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -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) { diff --git a/APMrover2/mode.h b/APMrover2/mode.h index a0efd9d780..56bb07b991 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -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); diff --git a/APMrover2/mode_manual.cpp b/APMrover2/mode_manual.cpp index 8e2d18db4d..d49c88dc6b 100644 --- a/APMrover2/mode_manual.cpp +++ b/APMrover2/mode_manual.cpp @@ -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 diff --git a/APMrover2/mode_steering.cpp b/APMrover2/mode_steering.cpp index e7a847e1e7..783ea75b3c 100644 --- a/APMrover2/mode_steering.cpp +++ b/APMrover2/mode_steering.cpp @@ -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; diff --git a/APMrover2/radio.cpp b/APMrover2/radio.cpp index 58394b25b7..1e34b57e59 100644 --- a/APMrover2/radio.cpp +++ b/APMrover2/radio.cpp @@ -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(); }