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

Rover: circle checks max speed and min radius

This commit is contained in:
Randy Mackay 2023-06-28 10:15:16 +09:00 committed by Andrew Tridgell
parent b93bc2767f
commit dc9e1a2bc3
2 changed files with 43 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
Rover/mode.h
View File

@ -451,6 +451,14 @@ protected:
// if there is no current position estimate target_yaw_rad is set to vehicle yaw
void init_target_yaw_rad();
// limit config speed so that lateral acceleration is within limits
// outputs warning to user if speed is reduced
void check_config_speed();
// ensure config radius is no smaller then vehicle's TURN_RADIUS
// radius is increased if necessary and warning is output to the user
void check_config_radius();
// enum for Direction parameter
enum class Direction {
CW = 0,

35
Rover/mode_circle.cpp
View File

@ -56,6 +56,7 @@ bool ModeCircle::set_center(const Location& center_loc, float radius_m, bool dir
// set radius
config.radius = MAX(fabsf(radius_m), AR_CIRCLE_RADIUS_MIN);
check_config_radius();
// set direction
config.dir = dir_ccw ? Direction::CCW : Direction::CW;
@ -77,11 +78,16 @@ bool ModeCircle::_enter()
return false;
}
config.radius = MAX(fabsf(radius), AR_CIRCLE_RADIUS_MIN);
check_config_radius();
config.dir = (direction == 1) ? Direction::CCW : Direction::CW;
config.speed = is_positive(speed) ? speed : g2.wp_nav.get_default_speed();
target.yaw_rad = AP::ahrs().get_yaw();
target.speed = 0;
// check speed around circle does not lead to excessive lateral acceleration
check_config_speed();
// 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());
@ -219,6 +225,9 @@ bool ModeCircle::set_desired_speed(float speed_ms)
if (is_positive(speed_ms)) {
config.speed = speed_ms;
// check desired speed does not lead to excessive lateral acceleration
check_config_speed();
// update position controller limits if required
if (config.speed > g2.pos_control.get_speed_max()) {
g2.pos_control.set_limits(config.speed, g2.pos_control.get_accel_max(), g2.pos_control.get_lat_accel_max(), g2.pos_control.get_jerk_max());
@ -235,3 +244,29 @@ bool ModeCircle::get_desired_location(Location& destination) const
destination = config.center_loc;
return true;
}
// limit config speed so that lateral acceleration is within limits
// assumes that config.radius and attitude controller lat accel max have been set
// outputs warning to user if speed is reduced
void ModeCircle::check_config_speed()
{
// calculate maximum speed based on radius and max lateral acceleration max
const float speed_max = MAX(safe_sqrt(g2.attitude_control.get_turn_lat_accel_max() * config.radius), 0);
if (config.speed > speed_max) {
config.speed = speed_max;
gcs().send_text(MAV_SEVERITY_WARNING, "Circle: max speed is %4.1f", (double)config.speed);
}
}
// ensure config radius is no smaller then vehicle's TURN_RADIUS
// assumes that config.radius has been set
// radius is increased if necessary and warning is output to the user
void ModeCircle::check_config_radius()
{
// ensure radius is at least as large as vehicle's turn radius
if (config.radius < rover.g2.turn_radius) {
config.radius = rover.g2.turn_radius;
gcs().send_text(MAV_SEVERITY_WARNING, "Circle: radius increased to TURN_RADIUS (%4.1f)", (double)rover.g2.turn_radius);
}
}