Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: SCurve: Increase corner speeds

Browse Source
This commit is contained in:
Leonard Hall 2022-03-05 09:31:58 +10:30 committed by Andrew Tridgell
parent 594fc3f02d
commit d7f283d3b5
2 changed files with 45 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_Math/SCurve.cpp
View File

@ -26,9 +26,11 @@ extern const AP_HAL::HAL &hal;
#define SEG_INIT 0 #define SEG_INIT 0
#define SEG_ACCEL_MAX 4 #define SEG_ACCEL_MAX 4
#define SEG_TURN_IN 4
#define SEG_ACCEL_END 7 #define SEG_ACCEL_END 7
#define SEG_SPEED_CHANGE_END 14 #define SEG_SPEED_CHANGE_END 14
#define SEG_CONST 15 #define SEG_CONST 15
#define SEG_TURN_OUT 15
#define SEG_DECEL_END 22 #define SEG_DECEL_END 22
// constructor // constructor
@ -264,7 +266,6 @@ void SCurve::set_speed_max(float speed_xy, float speed_up, float speed_down)
if (!is_equal(vel_max, segment[SEG_ACCEL_END].end_vel)) { if (!is_equal(vel_max, segment[SEG_ACCEL_END].end_vel)) {
// add velocity adjustment // add velocity adjustment
// check there is enough time to make velocity change // check there is enough time to make velocity change
// we use the approximation that the time will be distance/max_vel and 8 jerk segments // we use the approximation that the time will be distance/max_vel and 8 jerk segments
const float L = segment[SEG_CONST].end_pos - segment[SEG_ACCEL_END].end_pos; const float L = segment[SEG_CONST].end_pos - segment[SEG_ACCEL_END].end_pos;
float Jm = 0; float Jm = 0;
@ -481,7 +482,7 @@ void SCurve::set_destination_speed_max(float speed)
// target_pos should be set to this segment's origin and it will be updated to the current position target // target_pos should be set to this segment's origin and it will be updated to the current position target
// target_vel and target_accel are updated with new targets // target_vel and target_accel are updated with new targets
// returns true if vehicle has passed the apex of the corner // returns true if vehicle has passed the apex of the corner
bool SCurve::advance_target_along_track(SCurve &prev_leg, SCurve &next_leg, float wp_radius, bool fast_waypoint, float dt, Vector3f &target_pos, Vector3f &target_vel, Vector3f &target_accel) bool SCurve::advance_target_along_track(SCurve &prev_leg, SCurve &next_leg, float wp_radius, float accel_corner, bool fast_waypoint, float dt, Vector3f &target_pos, Vector3f &target_vel, Vector3f &target_accel)
{ {
prev_leg.move_to_pos_vel_accel(dt, target_pos, target_vel, target_accel); prev_leg.move_to_pos_vel_accel(dt, target_pos, target_vel, target_accel);
move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel); move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel);
@ -489,16 +490,19 @@ bool SCurve::advance_target_along_track(SCurve &prev_leg, SCurve &next_leg, floa
// check for change of leg on fast waypoint // check for change of leg on fast waypoint
const float time_to_destination = get_time_remaining(); const float time_to_destination = get_time_remaining();
if (fast_waypoint && braking() && is_zero(next_leg.get_time_elapsed()) && (time_to_destination <= next_leg.get_accel_finished_time())) { if (fast_waypoint
&& is_zero(next_leg.get_time_elapsed())
&& (get_time_elapsed() >= time_turn_out() - next_leg.time_turn_in())
&& (position_sq >= 0.25 * track.length_squared())) {
Vector3f turn_pos = -get_track(); Vector3f turn_pos = -get_track();
Vector3f turn_vel, turn_accel; Vector3f turn_vel, turn_accel;
move_from_time_pos_vel_accel(get_time_elapsed() + time_to_destination * 0.5f, turn_pos, turn_vel, turn_accel); move_from_time_pos_vel_accel(get_time_elapsed() + time_to_destination * 0.5f, turn_pos, turn_vel, turn_accel);
next_leg.move_from_time_pos_vel_accel(time_to_destination * 0.5f, turn_pos, turn_vel, turn_accel); next_leg.move_from_time_pos_vel_accel(time_to_destination * 0.5f, turn_pos, turn_vel, turn_accel);
const float speed_min = MIN(get_speed_along_track(), next_leg.get_speed_along_track()); const float speed_min = MIN(get_speed_along_track(), next_leg.get_speed_along_track());
const float accel_min = MIN(get_accel_along_track(), next_leg.get_accel_along_track());
if ((get_time_remaining() < next_leg.time_end() * 0.5f) && (turn_pos.length() < wp_radius) && if ((get_time_remaining() < next_leg.time_end() * 0.5f) && (turn_pos.length() < wp_radius) &&
(Vector2f{turn_vel.x, turn_vel.y}.length() < speed_min) && (Vector2f{turn_vel.x, turn_vel.y}.length() < speed_min) &&
(Vector2f{turn_accel.x, turn_accel.y}.length() < 2.0f*accel_min)) { (Vector2f{turn_accel.x, turn_accel.y}.length() < accel_corner)) {
next_leg.move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel); next_leg.move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel);
} }
} else if (!is_zero(next_leg.get_time_elapsed())) { } else if (!is_zero(next_leg.get_time_elapsed())) {
@ -559,7 +563,7 @@ void SCurve::move_from_time_pos_vel_accel(float time_now, Vector3f &pos, Vector3
float SCurve::time_end() const float SCurve::time_end() const
{ {
if (num_segs != segments_max) { if (num_segs != segments_max) {
return 0.0f; return 0.0;
} }
return segment[SEG_DECEL_END].end_time; return segment[SEG_DECEL_END].end_time;
} }
@ -568,7 +572,7 @@ float SCurve::time_end() const
float SCurve::get_time_remaining() const float SCurve::get_time_remaining() const
{ {
if (num_segs != segments_max) { if (num_segs != segments_max) {
return 0.0f; return 0.0;
} }
return segment[SEG_DECEL_END].end_time - time; return segment[SEG_DECEL_END].end_time - time;
} }
@ -577,7 +581,7 @@ float SCurve::get_time_remaining() const
float SCurve::get_accel_finished_time() const float SCurve::get_accel_finished_time() const
{ {
if (num_segs != segments_max) { if (num_segs != segments_max) {
return 0.0f; return 0.0;
} }
return segment[SEG_ACCEL_END].end_time; return segment[SEG_ACCEL_END].end_time;
} }
@ -591,6 +595,24 @@ bool SCurve::braking() const
return time >= segment[SEG_CONST].end_time; return time >= segment[SEG_CONST].end_time;
} }
// return time offset used to initiate the turn onto leg
float SCurve::time_turn_in() const
{
if (num_segs != segments_max) {
return 0.0;
}
return segment[SEG_TURN_IN].end_time;
}
// return time offset used to initiate the turn from leg
float SCurve::time_turn_out() const
{
if (num_segs != segments_max) {
return 0.0;
}
return segment[SEG_TURN_OUT].end_time;
}
// increment the internal time // increment the internal time
void SCurve::advance_time(float dt) void SCurve::advance_time(float dt)
{ {

23
libraries/AP_Math/SCurve.h
View File

@ -81,14 +81,15 @@ public:
void set_destination_speed_max(float speed); void set_destination_speed_max(float speed);
// move target location along path from origin to destination // move target location along path from origin to destination
// prev_leg and next_leg are the paths before and after this path // prev_leg and next_leg - the paths before and after this path
// wp_radius is max distance from the waypoint at the apex of the turn // wp_radius - max distance from the waypoint at the apex of the turn
// fast_waypoint should be true if vehicle will not stop at end of this leg // accel_corner - max acceleration that the aircraft may use during the corner
// dt is the time increment the vehicle will move along the path // fast_waypoint - true if vehicle will not stop at end of this leg
// target_pos should be set to this segment's origin and it will be updated to the current position target // dt - the time increment the vehicle will move along the path
// target_vel and target_accel are updated with new targets // target_pos - set to this segment's origin and it will be updated to the current position target
// returns true if vehicle has passed the apex of the corner // target_vel and target_accel - updated with new targets
bool advance_target_along_track(SCurve &prev_leg, SCurve &next_leg, float wp_radius, bool fast_waypoint, float dt, Vector3f &target_pos, Vector3f &target_vel, Vector3f &target_accel) WARN_IF_UNUSED; // advance_target_along_track returns true if vehicle has passed the apex of the corner
bool advance_target_along_track(SCurve &prev_leg, SCurve &next_leg, float wp_radius, float accel_corner, bool fast_waypoint, float dt, Vector3f &target_pos, Vector3f &target_vel, Vector3f &target_accel) WARN_IF_UNUSED;
// time has reached the end of the sequence // time has reached the end of the sequence
bool finished() const WARN_IF_UNUSED; bool finished() const WARN_IF_UNUSED;
@ -128,6 +129,12 @@ private:
// return true if the sequence is braking to a stop // return true if the sequence is braking to a stop
bool braking() const WARN_IF_UNUSED; bool braking() const WARN_IF_UNUSED;
// return time offset used to initiate the turn onto leg
float time_turn_in() const WARN_IF_UNUSED;
// return time offset used to initiate the turn from leg
float time_turn_out() const WARN_IF_UNUSED;
// increment the internal time // increment the internal time
void advance_time(float dt); void advance_time(float dt);