Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: add pivot_turn_rate to g2

Browse Source
This commit is contained in:
Ammarf 2018-05-18 17:38:06 +09:00 committed by Randy Mackay
parent a9ee949c32
commit 2d64a47f90
4 changed files with 16 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
APMrover2/Parameters.cpp
View File

@ -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
};

3
APMrover2/Parameters.h
View File

@ -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[];

5
APMrover2/mode.cpp
View File

@ -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);

2
APMrover2/mode.h
View File

@ -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