Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: slow vehicle if unable to track in circle mode

This commit is contained in:
Stephen Dade 2024-09-02 19:21:55 +10:00 committed by Peter Barker
parent e0e79a6287
commit a3548b6a21
2 changed files with 22 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Rover/mode.h
View File

@ -499,6 +499,7 @@ protected:
float angle_total_rad; // total angle in radians that vehicle has circled
bool reached_edge; // true once vehicle has reached edge of circle
float dist_to_edge_m; // distance to edge of circle in meters (equivalent to crosstrack error)
bool tracking_back; // true if the vehicle is trying to track back onto the circle
};
class ModeGuided : public Mode

28
Rover/mode_circle.cpp
View File

@ -91,6 +91,9 @@ bool ModeCircle::_enter()
// check speed around circle does not lead to excessive lateral acceleration
check_config_speed();
// reset tracking_back
tracking_back = false;
// calculate speed, accel and jerk limits
// otherwise the vehicle uses wp_nav default speed limit
float atc_accel_max = MIN(g2.attitude_control.get_accel_max(), g2.attitude_control.get_decel_max());
@ -155,7 +158,14 @@ void ModeCircle::update()
// Update depending on stage
if (!reached_edge) {
update_drive_to_radius();
} else if (dist_to_edge_m > 2 * MAX(g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST) && !tracking_back) {
// if more than 2* turn_radius outside circle radius, slow vehicle by 20%
config.speed = 0.8 * config.speed;
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Circle: cannot keep up, slowing to %.1fm/s", config.speed);
tracking_back = true;
} else if (dist_to_edge_m < MAX(g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST) && tracking_back) {
// if within turn_radius, call the vehicle back on track
tracking_back = false;
} else {
update_circling();
}
@ -193,12 +203,16 @@ void ModeCircle::update_circling()
const float accel_fb = constrain_float(config.speed - target.speed, -speed_change_max, speed_change_max);
target.speed += accel_fb;
// calculate angular rate and update target angle
const float circumference = 2.0 * M_PI * config.radius;
const float angular_rate_rad = (target.speed / circumference) * M_2PI * (config.dir == Direction::CW ? 1.0 : -1.0);
const float angle_dt = angular_rate_rad * rover.G_Dt;
target.yaw_rad = wrap_PI(target.yaw_rad + angle_dt);
angle_total_rad += angle_dt;
// calculate angular rate and update target angle, if the vehicle is not trying to track back
if (!tracking_back) {
const float circumference = 2.0 * M_PI * config.radius;
const float angular_rate_rad = (target.speed / circumference) * M_2PI * (config.dir == Direction::CW ? 1.0 : -1.0);
const float angle_dt = angular_rate_rad * rover.G_Dt;
target.yaw_rad = wrap_PI(target.yaw_rad + angle_dt);
angle_total_rad += angle_dt;
} else {
init_target_yaw_rad();
}
// calculate target point's position, velocity and acceleration
target.pos = config.center_pos.topostype();