diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index d20d549698..7c025e0009 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -532,6 +532,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: ../libraries/APM_Control/AR_AttitudeControl.cpp AP_SUBGROUPINFO(attitude_control, "ATC", 10, ParametersG2, AR_AttitudeControl), + // @Param: TURN_RADIUS + // @DisplayName: Turn radius of vehicle + // @Description: Turn radius of vehicle in meters while at low speeds. Lower values produce tighter turns in steering mode + // @Units: m + // @Range: 0 10 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("TURN_RADIUS", 11, ParametersG2, turn_radius, 0.9), + AP_GROUPEND }; diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 7fc90f7657..7cc7d1e9e9 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -326,6 +326,9 @@ public: // steering and throttle controller AR_AttitudeControl attitude_control; + + // turn radius of vehicle (only used in steering mode) + AP_Float turn_radius; }; extern const AP_Param::Info var_info[]; diff --git a/APMrover2/mode_steering.cpp b/APMrover2/mode_steering.cpp index ba559c1c10..6d14ee7770 100644 --- a/APMrover2/mode_steering.cpp +++ b/APMrover2/mode_steering.cpp @@ -12,7 +12,7 @@ void ModeSteering::update() // acceleration at full steering lock for this speed. That is V^2/R where R is the radius of turn. // We get the radius of turn from half the STEER2SRV_P. const float ground_speed = rover.ground_speed; - float max_g_force = ground_speed * ground_speed / rover.steerController.get_turn_radius(); + float max_g_force = ground_speed * ground_speed / MAX(g2.turn_radius, 0.1f); // constrain to user set TURN_MAX_G max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);