diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 76cdeba1f5..c8fc0b2542 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -555,6 +555,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp AP_SUBGROUPINFO(avoid, "AVOID_", 19, ParametersG2, AC_Avoid), + // @Param: PIVOT_TURN_RATE + // @DisplayName: Pivot turn rate + // @Description: Desired pivot turn rate in deg/s. + // @Units: deg/s + // @Range: 0 360 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("PIVOT_TURN_RATE", 20, ParametersG2, pivot_turn_rate, 90), + AP_GROUPEND }; diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 86298872ef..7cc4fa9176 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -341,6 +341,9 @@ public: // avoidance library AC_Avoid avoid; + + // pivot turn rate + AP_Int16 pivot_turn_rate; }; extern const AP_Param::Info var_info[]; diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index c9c0d0b030..5e96d24961 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -363,7 +363,7 @@ void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct if (rover.use_pivot_steering(_yaw_error_cd)) { // for pivot turns use heading controller - calc_steering_to_heading(desired_heading); + calc_steering_to_heading(desired_heading, g2.pivot_turn_rate); } else { // call lateral acceleration to steering controller calc_steering_from_lateral_acceleration(desired_lat_accel, reversed); @@ -392,10 +392,11 @@ void Mode::calc_steering_from_lateral_acceleration(float lat_accel, bool reverse } // calculate steering output to drive towards desired heading -void Mode::calc_steering_to_heading(float desired_heading_cd, bool reversed) +void Mode::calc_steering_to_heading(float desired_heading_cd, float rate_max, bool reversed) { // calculate yaw error (in radians) and pass to steering angle controller const float steering_out = attitude_control.get_steering_out_heading(radians(desired_heading_cd*0.01f), + rate_max, g2.motors.limit.steer_left, g2.motors.limit.steer_right); g2.motors.set_steering(steering_out * 4500.0f); diff --git a/APMrover2/mode.h b/APMrover2/mode.h index b71b9e049b..520235edb3 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -116,7 +116,7 @@ protected: void calc_steering_from_lateral_acceleration(float lat_accel, bool reversed = false); // calculate steering output to drive towards desired heading - void calc_steering_to_heading(float desired_heading_cd, bool reversed = false); + void calc_steering_to_heading(float desired_heading_cd, float rate_max, bool reversed = false); // calculates the amount of throttle that should be output based // on things like proximity to corners and current speed