Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AC_Avoidance: track status Rover: sailboats tack on fence #12117

Open
wants to merge 2 commits into
base: master
from
Open
Changes from all commits
Commits
File filter...
Filter file types
Jump to…
Jump to file or symbol
Failed to load files and symbols.

Always

Just for now

@@ -269,6 +269,10 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled)
// apply object avoidance to desired speed using half vehicle's maximum deceleration
if (avoidance_enabled) {
g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_decel_max(), ahrs.yaw, target_speed, rover.G_Dt);
if (g2.sailboat.enabled() && g2.avoid.limits_active()) {
// we are a sailboat trying to avoid fence, try a tack
rover.control_mode->handle_tack_request();

This comment has been minimized.

Copy link
@IamPete1

IamPete1 Aug 25, 2019

Author Contributor

this should be in auto modes only, auto tacking in acro is annoying as it turns out

}
}

// call throttle controller and convert output to -100 to +100 range
@@ -199,6 +199,7 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c
// limit climb rate
const float max_speed = get_max_speed(kP, accel_cmss_limited, alt_diff*100.0f, dt);
climb_rate_cms = MIN(max_speed, climb_rate_cms);
_last_limit_time = AP_HAL::millis();
}
}

@@ -257,14 +258,15 @@ void AC_Avoid::adjust_roll_pitch(float &roll, float &pitch, float veh_angle_max)
* Uses velocity adjustment idea from Randy's second email on this thread:
* https://groups.google.com/forum/#!searchin/drones-discuss/obstacle/drones-discuss/QwUXz__WuqY/qo3G8iTLSJAJ
*/
void AC_Avoid::limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel_cms, const Vector2f& limit_direction, float limit_distance_cm, float dt) const
void AC_Avoid::limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel_cms, const Vector2f& limit_direction, float limit_distance_cm, float dt)
{
const float max_speed = get_max_speed(kP, accel_cmss, limit_distance_cm, dt);
// project onto limit direction
const float speed = desired_vel_cms * limit_direction;
if (speed > max_speed) {
// subtract difference between desired speed and maximum acceptable speed
desired_vel_cms += limit_direction*(max_speed - speed);
_last_limit_time = AP_HAL::millis();
}
}

@@ -349,6 +351,7 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f
const float distance_to_target = target_direction.length();
const float max_speed = get_max_speed(kP, accel_cmss, distance_to_target, dt);
desired_vel_cms = target_direction * (MIN(desired_speed,max_speed) / distance_to_target);
_last_limit_time = AP_HAL::millis();
} else {
// implement stopping behaviour
// calculate stopping point plus a margin so we look forward far enough to intersect with circular fence
@@ -360,6 +363,7 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f
// otherwise user is backing away from fence so do not apply limits
if (stopping_point_plus_margin_dist_from_home >= dist_from_home) {
desired_vel_cms.zero();
_last_limit_time = AP_HAL::millis();
}
} else {
// shorten vector without adjusting its direction
@@ -369,6 +373,7 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f
const float max_speed = get_max_speed(kP, accel_cmss, distance_to_target, dt);
if (max_speed < desired_speed) {
desired_vel_cms *= MAX(max_speed, 0.0f) / desired_speed;
_last_limit_time = AP_HAL::millis();
}
}
}
@@ -18,6 +18,8 @@
#define AC_AVOID_NONGPS_DIST_MAX_DEFAULT 5.0f // objects over 5m away are ignored (default value for DIST_MAX parameter)
#define AC_AVOID_ANGLE_MAX_PERCENT 0.75f // object avoidance max lean angle as a percentage (expressed in 0 ~ 1 range) of total vehicle max lean angle

#define AC_AVOID_ACTIVE_LIMIT_TIMEOUT_MS 500 // if limiting is active if last limit is happend in the last x ms

/*
* This class prevents the vehicle from leaving a polygon fence in
* 2 dimensions by limiting velocity (adjust_velocity).
@@ -69,13 +71,16 @@ class AC_Avoid {
// limit_direction to be at most the maximum speed permitted by the limit_distance_cm.
// uses velocity adjustment idea from Randy's second email on this thread:
// https://groups.google.com/forum/#!searchin/drones-discuss/obstacle/drones-discuss/QwUXz__WuqY/qo3G8iTLSJAJ
void limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel_cms, const Vector2f& limit_direction, float limit_distance_cm, float dt) const;
void limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel_cms, const Vector2f& limit_direction, float limit_distance_cm, float dt);

// compute the speed such that the stopping distance of the vehicle will
// be exactly the input distance.
// kP should be non-zero for Copter which has a non-linear response
float get_max_speed(float kP, float accel_cmss, float distance_cm, float dt) const;

// return true if limiting is active
bool limits_active() const {return (AP_HAL::millis() - _last_limit_time) < AC_AVOID_ACTIVE_LIMIT_TIMEOUT_MS;};

static const struct AP_Param::GroupInfo var_info[];

private:
@@ -135,6 +140,7 @@ class AC_Avoid {
AP_Int8 _behavior; // avoidance behaviour (slide or stop)

bool _proximity_enabled = true; // true if proximity sensor based avoidance is enabled (used to allow pilot to enable/disable)
uint32_t _last_limit_time; // the last time a limit was active

static AC_Avoid *_singleton;
};
ProTip! Use n and p to navigate between commits in a pull request.
You can’t perform that action at this time.