From a7bfbefdd3cde0f4745bd56508ef36e9dc94569b Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 23 Oct 2025 17:34:45 +1030 Subject: [PATCH] AC_Avoidance: Naming Change No compiler change --- libraries/AC_Avoidance/AC_Avoid.cpp | 821 +++++++++++---------- libraries/AC_Avoidance/AC_Avoid.h | 93 ++- libraries/AC_Avoidance/AP_OABendyRuler.cpp | 8 +- 3 files changed, 463 insertions(+), 459 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 4a2b0cbfa5e..7ef78316de6 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -62,7 +62,7 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { // @Units: m // @Range: 1 30 // @User: Standard - AP_GROUPINFO_FRAME("DIST_MAX", 3, AC_Avoid, _dist_max, AC_AVOID_NONGPS_DIST_MAX_DEFAULT, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER), + AP_GROUPINFO_FRAME("DIST_MAX", 3, AC_Avoid, _dist_max_m, AC_AVOID_NONGPS_DIST_MAX_DEFAULT, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER), // @Param: MARGIN // @DisplayName: Avoidance distance margin in GPS modes @@ -70,7 +70,7 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { // @Units: m // @Range: 1 10 // @User: Standard - AP_GROUPINFO("MARGIN", 4, AC_Avoid, _margin, 2.0f), + AP_GROUPINFO("MARGIN", 4, AC_Avoid, _margin_m, 2.0f), // @Param{Copter, Rover}: BEHAVE // @DisplayName: Avoidance behaviour @@ -85,7 +85,7 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { // @Units: m/s // @Range: 0 2 // @User: Standard - AP_GROUPINFO("BACKUP_SPD", 6, AC_Avoid, _backup_speed_xy_max, 0.75f), + AP_GROUPINFO("BACKUP_SPD", 6, AC_Avoid, _backup_speed_max_ne_ms, 0.75f), // @Param{Copter}: ALT_MIN // @DisplayName: Avoidance minimum altitude @@ -93,7 +93,7 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { // @Units: m // @Range: 0 6 // @User: Standard - AP_GROUPINFO_FRAME("ALT_MIN", 7, AC_Avoid, _alt_min, 0.0f, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER), + AP_GROUPINFO_FRAME("ALT_MIN", 7, AC_Avoid, _alt_min_m, 0.0f, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER), // @Param: ACCEL_MAX // @DisplayName: Avoidance maximum acceleration @@ -101,7 +101,7 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { // @Units: m/s/s // @Range: 0 9 // @User: Standard - AP_GROUPINFO("ACCEL_MAX", 8, AC_Avoid, _accel_max, 3.0f), + AP_GROUPINFO("ACCEL_MAX", 8, AC_Avoid, _accel_max_mss, 3.0f), // @Param: BACKUP_DZ // @DisplayName: Avoidance deadzone between stopping and backing away from obstacle @@ -109,7 +109,7 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { // @Units: m // @Range: 0 2 // @User: Standard - AP_GROUPINFO("BACKUP_DZ", 9, AC_Avoid, _backup_deadzone, 0.10f), + AP_GROUPINFO("BACKUP_DZ", 9, AC_Avoid, _backup_deadzone_m, 0.10f), // @Param: BACKZ_SPD // @DisplayName: Avoidance maximum vertical backup speed @@ -117,7 +117,7 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { // @Units: m/s // @Range: 0 2 // @User: Standard - AP_GROUPINFO("BACKZ_SPD", 10, AC_Avoid, _backup_speed_z_max, 0.75), + AP_GROUPINFO("BACKZ_SPD", 10, AC_Avoid, _backup_speed_max_u_ms, 0.75), AP_GROUPEND }; @@ -134,148 +134,148 @@ AC_Avoid::AC_Avoid() * This method limits velocity and calculates backaway velocity from various supported fences * Also limits vertical velocity using adjust_velocity_z method */ -void AC_Avoid::adjust_velocity_fence(float kP, float accel_cmss, Vector3f &desired_vel_cms, Vector3f &backup_vel, float kP_z, float accel_cmss_z, float dt) +void AC_Avoid::adjust_velocity_fence(float kP, float accel_cmss, Vector3f &desired_vel_neu_cms, Vector3f &backup_vel_neu_cms, float kP_z, float accel_z_cmss, float dt) { // Only horizontal component needed for most fences, since fences are 2D - Vector2f desired_velocity_xy_cms{desired_vel_cms.x, desired_vel_cms.y}; + Vector2f desired_velocity_ne_cms{desired_vel_neu_cms.x, desired_vel_neu_cms.y}; #if AP_FENCE_ENABLED || AP_BEACON_ENABLED // limit acceleration - const float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX); + const float accel_limited_cmss = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX); #endif // maximum component of desired backup velocity in each quadrant - Vector2f quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel; + Vector2f quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms; #if AP_FENCE_ENABLED if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0) { // Store velocity needed to back away from fence - Vector2f backup_vel_fence; + Vector2f backup_vel_fence_ne_cms; - adjust_velocity_circle_fence(kP, accel_cmss_limited, desired_velocity_xy_cms, backup_vel_fence, dt); - find_max_quadrant_velocity(backup_vel_fence, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel); + adjust_velocity_circle_fence(kP, accel_limited_cmss, desired_velocity_ne_cms, backup_vel_fence_ne_cms, dt); + find_max_quadrant_velocity(backup_vel_fence_ne_cms, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms); - // backup_vel_fence is set to zero after each fence in case the velocity is unset from previous methods - backup_vel_fence.zero(); - adjust_velocity_inclusion_and_exclusion_polygons(kP, accel_cmss_limited, desired_velocity_xy_cms, backup_vel_fence, dt); - find_max_quadrant_velocity(backup_vel_fence, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel); + // backup_vel_fence_ne_cms is set to zero after each fence in case the velocity is unset from previous methods + backup_vel_fence_ne_cms.zero(); + adjust_velocity_inclusion_and_exclusion_polygons(kP, accel_limited_cmss, desired_velocity_ne_cms, backup_vel_fence_ne_cms, dt); + find_max_quadrant_velocity(backup_vel_fence_ne_cms, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms); - backup_vel_fence.zero(); - adjust_velocity_inclusion_circles(kP, accel_cmss_limited, desired_velocity_xy_cms, backup_vel_fence, dt); - find_max_quadrant_velocity(backup_vel_fence, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel); + backup_vel_fence_ne_cms.zero(); + adjust_velocity_inclusion_circles(kP, accel_limited_cmss, desired_velocity_ne_cms, backup_vel_fence_ne_cms, dt); + find_max_quadrant_velocity(backup_vel_fence_ne_cms, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms); - backup_vel_fence.zero(); - adjust_velocity_exclusion_circles(kP, accel_cmss_limited, desired_velocity_xy_cms, backup_vel_fence, dt); - find_max_quadrant_velocity(backup_vel_fence, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel); + backup_vel_fence_ne_cms.zero(); + adjust_velocity_exclusion_circles(kP, accel_limited_cmss, desired_velocity_ne_cms, backup_vel_fence_ne_cms, dt); + find_max_quadrant_velocity(backup_vel_fence_ne_cms, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms); } #endif // AP_FENCE_ENABLED #if AP_BEACON_ENABLED if ((_enabled & AC_AVOID_STOP_AT_BEACON_FENCE) > 0) { // Store velocity needed to back away from beacon fence - Vector2f backup_vel_beacon; - adjust_velocity_beacon_fence(kP, accel_cmss_limited, desired_velocity_xy_cms, backup_vel_beacon, dt); - find_max_quadrant_velocity(backup_vel_beacon, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel); + Vector2f backup_vel_beacon_ne_cms; + adjust_velocity_beacon_fence(kP, accel_limited_cmss, desired_velocity_ne_cms, backup_vel_beacon_ne_cms, dt); + find_max_quadrant_velocity(backup_vel_beacon_ne_cms, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms); } #endif // AP_BEACON_ENABLED // check for vertical fence - float desired_velocity_z_cms = desired_vel_cms.z; - float desired_backup_vel_z = 0.0f; - adjust_velocity_z(kP_z, accel_cmss_z, desired_velocity_z_cms, desired_backup_vel_z, dt); + float desired_velocity_z_cms = desired_vel_neu_cms.z; + float desired_backup_vel_u_cms = 0.0f; + adjust_velocity_z(kP_z, accel_z_cmss, desired_velocity_z_cms, desired_backup_vel_u_cms, dt); // Desired backup velocity is sum of maximum velocity component in each quadrant - const Vector2f desired_backup_vel_xy = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; - backup_vel = Vector3f{desired_backup_vel_xy.x, desired_backup_vel_xy.y, desired_backup_vel_z}; - desired_vel_cms = Vector3f{desired_velocity_xy_cms.x, desired_velocity_xy_cms.y, desired_velocity_z_cms}; + const Vector2f desired_backup_vel_ne_cms = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms; + backup_vel_neu_cms = Vector3f{desired_backup_vel_ne_cms.x, desired_backup_vel_ne_cms.y, desired_backup_vel_u_cms}; + desired_vel_neu_cms = Vector3f{desired_velocity_ne_cms.x, desired_velocity_ne_cms.y, desired_velocity_z_cms}; } /* * Adjusts the desired velocity so that the vehicle can stop * before the fence/object. * kP, accel_cmss are for the horizontal axis -* kP_z, accel_cmss_z are for vertical axis +* kP_z, accel_z_cmss are for vertical axis */ -void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, float kP, float accel_cmss, float kP_z, float accel_cmss_z, float dt) +void AC_Avoid::adjust_velocity(Vector3f &desired_vel_neu_cms, bool &backing_up, float kP, float accel_cmss, float kP_z, float accel_z_cmss, float dt) { // exit immediately if disabled if (_enabled == AC_AVOID_DISABLED) { return; } - // make a copy of input velocity, because desired_vel_cms might be changed - const Vector3f desired_vel_cms_original = desired_vel_cms; + // make a copy of input velocity, because desired_vel_neu_cms might be changed + const Vector3f desired_vel_original_neu_cms = desired_vel_neu_cms; // limit acceleration - const float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX); + const float accel_limited_cmss = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX); // maximum component of horizontal desired backup velocity in each quadrant - Vector2f quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel; - float back_vel_up = 0.0f; - float back_vel_down = 0.0f; + Vector2f quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms; + float back_vel_up_cms = 0.0f; + float back_vel_down_cms = 0.0f; // Avoidance in response to proximity sensor if (proximity_avoidance_enabled() && _proximity_alt_enabled) { // Store velocity needed to back away from physical obstacles - Vector3f backup_vel_proximity; - adjust_velocity_proximity(kP, accel_cmss_limited, desired_vel_cms, backup_vel_proximity, kP_z,accel_cmss_z, dt); - find_max_quadrant_velocity_3D(backup_vel_proximity, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel, back_vel_up, back_vel_down); + Vector3f backup_vel_proximity_neu_cms; + adjust_velocity_proximity(kP, accel_limited_cmss, desired_vel_neu_cms, backup_vel_proximity_neu_cms, kP_z,accel_z_cmss, dt); + find_max_quadrant_velocity_3D(backup_vel_proximity_neu_cms, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms, back_vel_up_cms, back_vel_down_cms); } // Avoidance in response to various fences - Vector3f backup_vel_fence; - adjust_velocity_fence(kP, accel_cmss, desired_vel_cms, backup_vel_fence, kP_z, accel_cmss_z, dt); - find_max_quadrant_velocity_3D(backup_vel_fence , quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel, back_vel_up, back_vel_down); + Vector3f backup_vel_fence_neu_cms; + adjust_velocity_fence(kP, accel_cmss, desired_vel_neu_cms, backup_vel_fence_neu_cms, kP_z, accel_z_cmss, dt); + find_max_quadrant_velocity_3D(backup_vel_fence_neu_cms , quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms, back_vel_up_cms, back_vel_down_cms); // Desired backup velocity is sum of maximum velocity component in each quadrant - const Vector2f desired_backup_vel_xy = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; - const float desired_backup_vel_z = back_vel_down + back_vel_up; - Vector3f desired_backup_vel{desired_backup_vel_xy.x, desired_backup_vel_xy.y, desired_backup_vel_z}; + const Vector2f desired_backup_vel_ne_cms = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms; + const float desired_backup_vel_u_cms = back_vel_down_cms + back_vel_up_cms; + Vector3f desired_backup_vel_neu_cm{desired_backup_vel_ne_cms.x, desired_backup_vel_ne_cms.y, desired_backup_vel_u_cms}; - const float max_back_spd_xy_cms = _backup_speed_xy_max * 100.0; - if (!desired_backup_vel.xy().is_zero() && is_positive(max_back_spd_xy_cms)) { + const float backup_speed_max_ne_cms = _backup_speed_max_ne_ms * 100.0; + if (!desired_backup_vel_neu_cm.xy().is_zero() && is_positive(backup_speed_max_ne_cms)) { backing_up = true; // Constrain horizontal backing away speed - desired_backup_vel.xy().limit_length(max_back_spd_xy_cms); + desired_backup_vel_neu_cm.xy().limit_length(backup_speed_max_ne_cms); // let user take control if they are backing away at a greater speed than what we have calculated // this has to be done for x,y,z separately. For eg, user is doing fine in "x" direction but might need backing up in "y". - if (!is_zero(desired_backup_vel.x)) { - if (is_positive(desired_backup_vel.x)) { - desired_vel_cms.x = MAX(desired_vel_cms.x, desired_backup_vel.x); + if (!is_zero(desired_backup_vel_neu_cm.x)) { + if (is_positive(desired_backup_vel_neu_cm.x)) { + desired_vel_neu_cms.x = MAX(desired_vel_neu_cms.x, desired_backup_vel_neu_cm.x); } else { - desired_vel_cms.x = MIN(desired_vel_cms.x, desired_backup_vel.x); + desired_vel_neu_cms.x = MIN(desired_vel_neu_cms.x, desired_backup_vel_neu_cm.x); } } - if (!is_zero(desired_backup_vel.y)) { - if (is_positive(desired_backup_vel.y)) { - desired_vel_cms.y = MAX(desired_vel_cms.y, desired_backup_vel.y); + if (!is_zero(desired_backup_vel_neu_cm.y)) { + if (is_positive(desired_backup_vel_neu_cm.y)) { + desired_vel_neu_cms.y = MAX(desired_vel_neu_cms.y, desired_backup_vel_neu_cm.y); } else { - desired_vel_cms.y = MIN(desired_vel_cms.y, desired_backup_vel.y); + desired_vel_neu_cms.y = MIN(desired_vel_neu_cms.y, desired_backup_vel_neu_cm.y); } } } - const float max_back_spd_z_cms = _backup_speed_z_max * 100.0; - if (!is_zero(desired_backup_vel.z) && is_positive(max_back_spd_z_cms)) { + const float backup_speed_max_u_cms = _backup_speed_max_u_ms * 100.0; + if (!is_zero(desired_backup_vel_neu_cm.z) && is_positive(backup_speed_max_u_cms)) { backing_up = true; // Constrain vertical backing away speed - desired_backup_vel.z = constrain_float(desired_backup_vel.z, -max_back_spd_z_cms, max_back_spd_z_cms); + desired_backup_vel_neu_cm.z = constrain_float(desired_backup_vel_neu_cm.z, -backup_speed_max_u_cms, backup_speed_max_u_cms); - if (!is_zero(desired_backup_vel.z)) { - if (is_positive(desired_backup_vel.z)) { - desired_vel_cms.z = MAX(desired_vel_cms.z, desired_backup_vel.z); + if (!is_zero(desired_backup_vel_neu_cm.z)) { + if (is_positive(desired_backup_vel_neu_cm.z)) { + desired_vel_neu_cms.z = MAX(desired_vel_neu_cms.z, desired_backup_vel_neu_cm.z); } else { - desired_vel_cms.z = MIN(desired_vel_cms.z, desired_backup_vel.z); + desired_vel_neu_cms.z = MIN(desired_vel_neu_cms.z, desired_backup_vel_neu_cm.z); } } } // limit acceleration - limit_accel(desired_vel_cms_original, desired_vel_cms, dt); + limit_accel_NEU_cm(desired_vel_original_neu_cms, desired_vel_neu_cms, dt); - if (desired_vel_cms_original != desired_vel_cms) { + if (desired_vel_original_neu_cms != desired_vel_neu_cms) { _last_limit_time = AP_HAL::millis(); } @@ -285,13 +285,13 @@ void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, floa uint32_t now = AP_HAL::millis(); if ((now - _last_log_ms) > 100) { _last_log_ms = now; - Write_SimpleAvoidance(true, desired_vel_cms_original, desired_vel_cms, backing_up); + Write_SimpleAvoidance(true, desired_vel_original_neu_cms, desired_vel_neu_cms, backing_up); } } else { // avoidance isn't active anymore // log once so that it registers in logs if (_last_log_ms) { - Write_SimpleAvoidance(false, desired_vel_cms_original, desired_vel_cms, backing_up); + Write_SimpleAvoidance(false, desired_vel_original_neu_cms, desired_vel_neu_cms, backing_up); // this makes sure logging won't run again till it is active _last_log_ms = 0; } @@ -303,31 +303,31 @@ void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, floa * Limit acceleration so that change of velocity output by avoidance library is controlled * This helps reduce jerks and sudden movements in the vehicle */ -void AC_Avoid::limit_accel(const Vector3f &original_vel, Vector3f &modified_vel, float dt) +void AC_Avoid::limit_accel_NEU_cm(const Vector3f &original_vel_neu_cms, Vector3f &modified_vel_neu_cms, float dt) { - if (original_vel == modified_vel || is_zero(_accel_max) || !is_positive(dt)) { + if (original_vel_neu_cms == modified_vel_neu_cms || is_zero(_accel_max_mss) || !is_positive(dt)) { // we can't limit accel if any of these conditions are true return; } if (AP_HAL::millis() - _last_limit_time > AC_AVOID_ACCEL_TIMEOUT_MS) { // reset this velocity because its been a long time since avoidance was active - _prev_avoid_vel = original_vel; + _prev_avoid_vel_neu_cms = original_vel_neu_cms; } // acceleration demanded by avoidance - const Vector3f accel = (modified_vel - _prev_avoid_vel)/dt; + const Vector3f accel_neu_cmss = (modified_vel_neu_cms - _prev_avoid_vel_neu_cms)/dt; // max accel in cm - const float max_accel_cm = _accel_max * 100.0f; + const float accel_max_cmss = _accel_max_mss * 100.0f; - if (accel.length() > max_accel_cm) { + if (accel_neu_cmss.length() > accel_max_cmss) { // pull back on the acceleration - const Vector3f accel_direction = accel.normalized(); - modified_vel = (accel_direction * max_accel_cm) * dt + _prev_avoid_vel; + const Vector3f accel_direction_neu = accel_neu_cmss.normalized(); + modified_vel_neu_cms = (accel_direction_neu * accel_max_cmss) * dt + _prev_avoid_vel_neu_cms; } - _prev_avoid_vel = modified_vel; + _prev_avoid_vel_neu_cms = modified_vel_neu_cms; return; } @@ -337,53 +337,53 @@ void AC_Avoid::limit_accel(const Vector3f &original_vel, Vector3f &modified_vel, // heading is in radians // speed is in m/s // kP should be zero for linear response, non-zero for non-linear response -void AC_Avoid::adjust_speed(float kP, float accel, float heading, float &speed, float dt) +void AC_Avoid::adjust_speed(float kP, float accel_mss, float heading_rad, float &speed_ms, float dt) { // convert heading and speed into velocity vector - Vector3f vel{ - cosf(heading) * speed * 100.0f, - sinf(heading) * speed * 100.0f, + Vector3f vel_neu_cms{ + cosf(heading_rad) * speed_ms * 100.0f, + sinf(heading_rad) * speed_ms * 100.0f, 0.0f }; bool backing_up = false; - adjust_velocity(vel, backing_up, kP, accel * 100.0f, 0, 0, dt); - const Vector2f vel_xy{vel.x, vel.y}; + adjust_velocity(vel_neu_cms, backing_up, kP, accel_mss * 100.0f, 0, 0, dt); + const Vector2f vel_ne_cms{vel_neu_cms.x, vel_neu_cms.y}; if (backing_up) { // back up - if (fabsf(wrap_180(degrees(vel_xy.angle())) - degrees(heading)) > 90.0f) { + if (fabsf(wrap_180(degrees(vel_ne_cms.angle())) - degrees(heading_rad)) > 90.0f) { // Big difference between the direction of velocity vector and actual heading therefore we need to reverse the direction - speed = -vel_xy.length() * 0.01f; + speed_ms = -vel_ne_cms.length() * 0.01f; } else { - speed = vel_xy.length() * 0.01f; + speed_ms = vel_ne_cms.length() * 0.01f; } return; } // No need to back up so adjust speed towards zero if needed - if (is_negative(speed)) { - speed = -vel_xy.length() * 0.01f; + if (is_negative(speed_ms)) { + speed_ms = -vel_ne_cms.length() * 0.01f; } else { - speed = vel_xy.length() * 0.01f; + speed_ms = vel_ne_cms.length() * 0.01f; } } // adjust vertical climb rate so vehicle does not break the vertical fence void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt) { - float backup_speed = 0.0f; - adjust_velocity_z(kP, accel_cmss, climb_rate_cms, backup_speed, dt); - if (!is_zero(backup_speed)) { - if (is_negative(backup_speed)) { - climb_rate_cms = MIN(climb_rate_cms, backup_speed); + float backup_speed_cms = 0.0f; + adjust_velocity_z(kP, accel_cmss, climb_rate_cms, backup_speed_cms, dt); + if (!is_zero(backup_speed_cms)) { + if (is_negative(backup_speed_cms)) { + climb_rate_cms = MIN(climb_rate_cms, backup_speed_cms); } else { - climb_rate_cms = MAX(climb_rate_cms, backup_speed); + climb_rate_cms = MAX(climb_rate_cms, backup_speed_cms); } } } // adjust vertical climb rate so vehicle does not break the vertical fence -void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float& backup_speed, float dt) +void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float& backup_speed_cms, float dt) { #ifdef AP_AVOID_ENABLE_Z @@ -399,27 +399,27 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c const AP_AHRS &_ahrs = AP::ahrs(); // limit acceleration - const float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX); + const float accel_limited_cmss = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX); bool limit_min_alt = false; bool limit_max_alt = false; - float max_alt_diff = 0.0f; // distance from altitude limit to vehicle in metres (positive means vehicle is below limit) - float min_alt_diff = 0.0f; + float max_alt_diff_m = 0.0f; // distance from altitude limit to vehicle in metres (positive means vehicle is below limit) + float min_alt_diff_m = 0.0f; #if AP_FENCE_ENABLED // calculate distance below fence AC_Fence *fence = AP::fence(); if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && fence) { // calculate distance from vehicle to safe altitude - float veh_alt; - _ahrs.get_relative_position_D_home(veh_alt); + float veh_alt_m; + _ahrs.get_relative_position_D_home(veh_alt_m); if ((fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) > 0) { - // fence.get_safe_alt_max() is UP, veh_alt is DOWN: - min_alt_diff = -(fence->get_safe_alt_min() + veh_alt); + // fence.get_safe_alt_max_m() is UP, veh_alt_m is DOWN: + min_alt_diff_m = -(fence->get_safe_alt_min_m() + veh_alt_m); limit_min_alt = true; } if ((fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) { - // fence.get_safe_alt_max() is UP, veh_alt is DOWN: - max_alt_diff = fence->get_safe_alt_max() + veh_alt; + // fence.get_safe_alt_max_m() is UP, veh_alt_m is DOWN: + max_alt_diff_m = fence->get_safe_alt_max_m() + veh_alt_m; limit_max_alt = true; } } @@ -427,26 +427,26 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c // calculate distance to (e.g.) optical flow altitude limit // AHRS values are always in metres - float alt_limit; - float curr_alt; - if (_ahrs.get_hgt_ctrl_limit(alt_limit) && - _ahrs.get_relative_position_D_origin_float(curr_alt)) { - // alt_limit is UP, curr_alt is DOWN: - const float ctrl_alt_diff = alt_limit + curr_alt; - if (!limit_max_alt || ctrl_alt_diff < max_alt_diff) { - max_alt_diff = ctrl_alt_diff; + float alt_limit_m; + float curr_alt_m; + if (_ahrs.get_hgt_ctrl_limit(alt_limit_m) && + _ahrs.get_relative_position_D_origin_float(curr_alt_m)) { + // alt_limit_m is UP, curr_alt_m is DOWN: + const float ctrl_alt_diff_m = alt_limit_m + curr_alt_m; + if (!limit_max_alt || ctrl_alt_diff_m < max_alt_diff_m) { + max_alt_diff_m = ctrl_alt_diff_m; limit_max_alt = true; } } #if HAL_PROXIMITY_ENABLED // get distance from proximity sensor - float proximity_alt_diff; + float proximity_alt_diff_m; AP_Proximity *proximity = AP::proximity(); - if (proximity && proximity_avoidance_enabled() && proximity->get_upward_distance(proximity_alt_diff)) { - proximity_alt_diff -= _margin; - if (!limit_max_alt || proximity_alt_diff < max_alt_diff) { - max_alt_diff = proximity_alt_diff; + if (proximity && proximity_avoidance_enabled() && proximity->get_upward_distance(proximity_alt_diff_m)) { + proximity_alt_diff_m -= _margin_m; + if (!limit_max_alt || proximity_alt_diff_m < max_alt_diff_m) { + max_alt_diff_m = proximity_alt_diff_m; limit_max_alt = true; } } @@ -454,39 +454,39 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c // limit climb rate if (limit_max_alt || limit_min_alt) { - const float max_back_spd_cms = _backup_speed_z_max * 100.0; + const float max_back_spd_cms = _backup_speed_max_u_ms * 100.0; // do not allow climbing if we've breached the safe altitude - if (max_alt_diff <= 0.0f && limit_max_alt) { + if (max_alt_diff_m <= 0.0f && limit_max_alt) { climb_rate_cms = MIN(climb_rate_cms, 0.0f); // also calculate backup speed that will get us back to safe altitude if (is_positive(max_back_spd_cms)) { - backup_speed = -1*(get_max_speed(kP, accel_cmss_limited, -max_alt_diff*100.0f, dt)); + backup_speed_cms = -1*(get_max_speed(kP, accel_limited_cmss, -max_alt_diff_m * 100.0f, dt)); // Constrain to max backup speed - backup_speed = MAX(backup_speed, -max_back_spd_cms); + backup_speed_cms = MAX(backup_speed_cms, -max_back_spd_cms); } return; // do not allow descending if we've breached the safe altitude - } else if (min_alt_diff <= 0.0f && limit_min_alt) { + } else if (min_alt_diff_m <= 0.0f && limit_min_alt) { climb_rate_cms = MAX(climb_rate_cms, 0.0f); // also calculate backup speed that will get us back to safe altitude if (is_positive(max_back_spd_cms)) { - backup_speed = get_max_speed(kP, accel_cmss_limited, -min_alt_diff*100.0f, dt); + backup_speed_cms = get_max_speed(kP, accel_limited_cmss, -min_alt_diff_m * 100.0f, dt); // Constrain to max backup speed - backup_speed = MIN(backup_speed, max_back_spd_cms); + backup_speed_cms = MIN(backup_speed_cms, max_back_spd_cms); } return; } // limit climb rate if (limit_max_alt) { - const float max_alt_max_speed = get_max_speed(kP, accel_cmss_limited, max_alt_diff*100.0f, dt); - climb_rate_cms = MIN(max_alt_max_speed, climb_rate_cms); + const float max_alt_max_speed_cms = get_max_speed(kP, accel_limited_cmss, max_alt_diff_m * 100.0f, dt); + climb_rate_cms = MIN(max_alt_max_speed_cms, climb_rate_cms); } if (limit_min_alt) { - const float max_alt_min_speed = get_max_speed(kP, accel_cmss_limited, min_alt_diff*100.0f, dt); + const float max_alt_min_speed = get_max_speed(kP, accel_limited_cmss, min_alt_diff_m * 100.0f, dt); climb_rate_cms = MAX(-max_alt_min_speed, climb_rate_cms); } } @@ -496,7 +496,7 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c // adjust roll-pitch to push vehicle away from objects // roll and pitch value are in radians // veh_angle_max_rad is the user defined maximum lean angle for the vehicle in radians -void AC_Avoid::adjust_roll_pitch_rad(float &roll_rad, float &pitch_rad, float veh_angle_max_rad) +void AC_Avoid::adjust_roll_pitch_rad(float &roll_rad, float &pitch_rad, float veh_angle_max_rad) const { // exit immediately if proximity based avoidance is disabled if (!proximity_avoidance_enabled()) { @@ -508,16 +508,16 @@ void AC_Avoid::adjust_roll_pitch_rad(float &roll_rad, float &pitch_rad, float ve return; } - float roll_positive = 0.0f; // maximum positive roll value - float roll_negative = 0.0f; // minimum negative roll value - float pitch_positive = 0.0f; // maximum positive pitch value - float pitch_negative = 0.0f; // minimum negative pitch value + float roll_positive_norm = 0.0f; // maximum positive roll value + float roll_negative_norm = 0.0f; // minimum negative roll value + float pitch_positive_norm = 0.0f; // maximum positive pitch value + float pitch_negative_norm = 0.0f; // minimum negative pitch value // get maximum positive and negative roll and pitch percentages from proximity sensor - get_proximity_roll_pitch_norm(roll_positive, roll_negative, pitch_positive, pitch_negative); + get_proximity_roll_pitch_norm(roll_positive_norm, roll_negative_norm, pitch_positive_norm, pitch_negative_norm); // add maximum positive and negative percentages together for roll and pitch, convert to radians - Vector2f rp_out_rad((roll_positive + roll_negative) * radians(45.0), (pitch_positive + pitch_negative) * radians(45.0)); + Vector2f rp_out_rad((roll_positive_norm + roll_negative_norm) * radians(45.0), (pitch_positive_norm + pitch_negative_norm) * radians(45.0)); // apply avoidance angular limits // the object avoidance lean angle is never more than 75% of the total angle-limit to allow the pilot to override @@ -544,76 +544,82 @@ void AC_Avoid::adjust_roll_pitch_rad(float &roll_rad, float &pitch_rad, float ve /* * Note: This method is used to limit velocity horizontally only - * Limits the component of desired_vel_cms in the direction of the unit vector - * limit_direction to be at most the maximum speed permitted by the limit_distance_cm. + * Limits the component of desired_vel in the direction of the unit vector + * limit_direction_ne to be at most the maximum speed permitted by the limit_distance. + * + * The function is unit-agnostic — accel, desired_vel_ne, and limit_direction_ne + * must all use the same base unit (e.g. m, cm) for correct scaling. * * 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_2D(float kP, float accel_cmss, Vector2f &desired_vel_cms, const Vector2f& limit_direction, float limit_distance_cm, float dt) +void AC_Avoid::limit_velocity_NE(float kP, float accel, Vector2f &desired_vel_ne, const Vector2f& limit_direction_ne, float limit_distance, float dt) const { - const float max_speed = get_max_speed(kP, accel_cmss, limit_distance_cm, dt); + const float max_speed = get_max_speed(kP, accel, limit_distance, dt); // project onto limit direction - const float speed = desired_vel_cms * limit_direction; + const float speed = desired_vel_ne * limit_direction_ne; if (speed > max_speed) { // subtract difference between desired speed and maximum acceptable speed - desired_vel_cms += limit_direction*(max_speed - speed); + desired_vel_ne += limit_direction_ne * (max_speed - speed); } } /* * Note: This method is used to limit velocity horizontally and vertically given a 3D desired velocity vector - * Limits the component of desired_vel_cms in the direction of the obstacle_vector based on the passed value of "margin" + * Limits the component of desired_vel_neu in the direction of the obstacle_vector_neu based on the passed value of "margin" + * + * The function is unit-agnostic — accel, desired_vel_neu, obstacle_vector_neu, margin, and accel_u + * must all use the same base unit (e.g. m, cm) for correct scaling. */ -void AC_Avoid::limit_velocity_3D(float kP, float accel_cmss, Vector3f &desired_vel_cms, const Vector3f& obstacle_vector, float margin_cm, float kP_z, float accel_cmss_z, float dt) +void AC_Avoid::limit_velocity_NEU(float kP, float accel, Vector3f &desired_vel_neu, const Vector3f& obstacle_vector_neu, float margin, float kP_z, float accel_u, float dt) const { - if (desired_vel_cms.is_zero()) { + if (desired_vel_neu.is_zero()) { // nothing to limit return; } - // create a margin_cm length vector in the direction of desired_vel_cms + // create a margin length vector in the direction of desired_vel_cms // this will create larger margin towards the direction vehicle is travelling in - const Vector3f margin_vector = desired_vel_cms.normalized() * margin_cm; - const Vector2f limit_direction_xy{obstacle_vector.x, obstacle_vector.y}; + const Vector3f margin_vector_neu = desired_vel_neu.normalized() * margin; + const Vector2f limit_direction_ne{obstacle_vector_neu.x, obstacle_vector_neu.y}; - if (!limit_direction_xy.is_zero()) { - const float distance_from_fence_xy = MAX((limit_direction_xy.length() - Vector2f{margin_vector.x, margin_vector.y}.length()), 0.0f); - Vector2f velocity_xy{desired_vel_cms.x, desired_vel_cms.y}; - limit_velocity_2D(kP, accel_cmss, velocity_xy, limit_direction_xy.normalized(), distance_from_fence_xy, dt); - desired_vel_cms.x = velocity_xy.x; - desired_vel_cms.y = velocity_xy.y; + if (!limit_direction_ne.is_zero()) { + const float distance_from_fence_xy = MAX((limit_direction_ne.length() - Vector2f{margin_vector_neu.x, margin_vector_neu.y}.length()), 0.0f); + Vector2f velocity_ne{desired_vel_neu.x, desired_vel_neu.y}; + limit_velocity_NE(kP, accel, velocity_ne, limit_direction_ne.normalized(), distance_from_fence_xy, dt); + desired_vel_neu.x = velocity_ne.x; + desired_vel_neu.y = velocity_ne.y; } - if (is_zero(desired_vel_cms.z) || is_zero(obstacle_vector.z)) { + if (is_zero(desired_vel_neu.z) || is_zero(obstacle_vector_neu.z)) { // nothing to limit vertically if desired_vel_cms.z is zero - // if obstacle_vector.z is zero then the obstacle is probably horizontally located, and we can move vertically + // if obstacle_vector_neu.z is zero then the obstacle is probably horizontally located, and we can move vertically return; } - if (is_positive(desired_vel_cms.z) != is_positive(obstacle_vector.z)) { + if (is_positive(desired_vel_neu.z) != is_positive(obstacle_vector_neu.z)) { // why limit velocity vertically when we are going the opposite direction return; } // to check if Z velocity changes - const float velocity_z_original = desired_vel_cms.z; - const float z_speed = fabsf(desired_vel_cms.z); + const float velocity_original_u = desired_vel_neu.z; + const float speed_u = fabsf(desired_vel_neu.z); - // obstacle_vector.z and margin_vector.z should be in same direction as checked above - const float dist_z = MAX(fabsf(obstacle_vector.z) - fabsf(margin_vector.z), 0.0f); - if (is_zero(dist_z)) { + // obstacle_vector_neu.z and margin_vector_neu.z should be in same direction as checked above + const float dist_u = MAX(fabsf(obstacle_vector_neu.z) - fabsf(margin_vector_neu.z), 0.0f); + if (is_zero(dist_u)) { // eliminate any vertical velocity - desired_vel_cms.z = 0.0f; + desired_vel_neu.z = 0.0f; } else { - const float max_z_speed = get_max_speed(kP_z, accel_cmss_z, dist_z, dt); - desired_vel_cms.z = MIN(max_z_speed, z_speed); + const float max_z_speed = get_max_speed(kP_z, accel_u, dist_u, dt); + desired_vel_neu.z = MIN(max_z_speed, speed_u); } // make sure the direction of the Z velocity did not change // we are only limiting speed here, not changing directions // check if original z velocity is positive or negative - if (is_negative(velocity_z_original)) { - desired_vel_cms.z = desired_vel_cms.z * -1.0f; + if (is_negative(velocity_original_u)) { + desired_vel_neu.z = desired_vel_neu.z * -1.0f; } } @@ -623,22 +629,22 @@ void AC_Avoid::limit_velocity_3D(float kP, float accel_cmss, Vector3f &desired_v * It then calculates the desired backup velocity and passes it on to "find_max_quadrant_velocity" method to distribute the velocity vectors into respective quadrants * OUTPUT: The method then outputs four velocities (quad1/2/3/4_back_vel_cms), which correspond to the maximum horizontal desired backup velocity in each quadrant */ -void AC_Avoid::calc_backup_velocity_2D(float kP, float accel_cmss, Vector2f &quad1_back_vel_cms, Vector2f &quad2_back_vel_cms, Vector2f &quad3_back_vel_cms, Vector2f &quad4_back_vel_cms, float back_distance_cm, Vector2f limit_direction, float dt) +void AC_Avoid::calc_backup_velocity_2D(float kP, float accel_cmss, Vector2f &quad1_back_vel_cms, Vector2f &quad2_back_vel_cms, Vector2f &quad3_back_vel_cms, Vector2f &quad4_back_vel_cms, float back_distance_cm, Vector2f limit_direction, float dt) const { if (limit_direction.is_zero()) { // protect against divide by zero return; } // speed required to move away the exact distance that we have breached the margin with - const float back_speed = get_max_speed(kP, 0.4f * accel_cmss, fabsf(back_distance_cm), dt); + const float back_speed_cms = get_max_speed(kP, 0.4f * accel_cmss, fabsf(back_distance_cm), dt); // direction to the obstacle limit_direction.normalize(); // move in the opposite direction with the required speed - Vector2f back_direction_vel = limit_direction * (-back_speed); + Vector2f back_direction_vel_cms = limit_direction * (-back_speed_cms); // divide the vector into quadrants, find maximum velocity component in each quadrant - find_max_quadrant_velocity(back_direction_vel, quad1_back_vel_cms, quad2_back_vel_cms, quad3_back_vel_cms, quad4_back_vel_cms); + find_max_quadrant_velocity(back_direction_vel_cms, quad1_back_vel_cms, quad2_back_vel_cms, quad3_back_vel_cms, quad4_back_vel_cms); } /* @@ -647,28 +653,29 @@ void AC_Avoid::calc_backup_velocity_2D(float kP, float accel_cmss, Vector2f &qua * max_z_vel is >= 0, and stores the greatest velocity in the upwards direction * eventually max_z_vel + min_z_vel will give the final desired Z backaway velocity */ -void AC_Avoid::calc_backup_velocity_3D(float kP, float accel_cmss, Vector2f &quad1_back_vel_cms, Vector2f &quad2_back_vel_cms, Vector2f &quad3_back_vel_cms, Vector2f &quad4_back_vel_cms, float back_distance_cms, Vector3f limit_direction, float kp_z, float accel_cmss_z, float back_distance_z, float& min_z_vel, float& max_z_vel, float dt) +void AC_Avoid::calc_backup_velocity_3D(float kP, float accel_cmss, Vector2f &quad1_back_vel_cms, Vector2f &quad2_back_vel_cms, Vector2f &quad3_back_vel_cms, Vector2f &quad4_back_vel_cms, + float back_distance_cms, Vector3f limit_direction_neu, float kp_z, float accel_z_cmss, float back_distance_u_cm, float& min_vel_u_cms, float& max_vel_u_cms, float dt) const { // backup horizontally if (is_positive(back_distance_cms)) { - Vector2f limit_direction_2d{limit_direction.x, limit_direction.y}; - calc_backup_velocity_2D(kP, accel_cmss, quad1_back_vel_cms, quad2_back_vel_cms, quad3_back_vel_cms, quad4_back_vel_cms, back_distance_cms, limit_direction_2d, dt); + Vector2f limit_direction_ne{limit_direction_neu.x, limit_direction_neu.y}; + calc_backup_velocity_2D(kP, accel_cmss, quad1_back_vel_cms, quad2_back_vel_cms, quad3_back_vel_cms, quad4_back_vel_cms, back_distance_cms, limit_direction_ne, dt); } // backup vertically - if (!is_zero(back_distance_z)) { - float back_speed_z = get_max_speed(kp_z, 0.4f * accel_cmss_z, fabsf(back_distance_z), dt); + if (!is_zero(back_distance_u_cm)) { + float back_speed_z_cms = get_max_speed(kp_z, 0.4f * accel_z_cmss, fabsf(back_distance_u_cm), dt); // Down is positive - if (is_positive(back_distance_z)) { - back_speed_z *= -1.0f; + if (is_positive(back_distance_u_cm)) { + back_speed_z_cms *= -1.0f; } // store the z backup speed into min or max z if possible - if (back_speed_z < min_z_vel) { - min_z_vel = back_speed_z; + if (back_speed_z_cms < min_vel_u_cms) { + min_vel_u_cms = back_speed_z_cms; } - if (back_speed_z > max_z_vel) { - max_z_vel = back_speed_z; + if (back_speed_z_cms > max_vel_u_cms) { + max_vel_u_cms = back_speed_z_cms; } } } @@ -679,7 +686,7 @@ void AC_Avoid::calc_backup_velocity_3D(float kP, float accel_cmss, Vector2f &qua * The desired velocity is then fit into one of the 4 quadrant velocities as per the sign of its components * This ensures that if we have multiple backup velocities, we can get the maximum of all of those velocities in each quadrant */ -void AC_Avoid::find_max_quadrant_velocity(Vector2f &desired_vel, Vector2f &quad1_vel, Vector2f &quad2_vel, Vector2f &quad3_vel, Vector2f &quad4_vel) +void AC_Avoid::find_max_quadrant_velocity(Vector2f &desired_vel, Vector2f &quad1_vel, Vector2f &quad2_vel, Vector2f &quad3_vel, Vector2f &quad4_vel) const { if (desired_vel.is_zero()) { return; @@ -705,11 +712,11 @@ void AC_Avoid::find_max_quadrant_velocity(Vector2f &desired_vel, Vector2f &quad1 /* Calculate maximum velocity vector that can be formed in each quadrant and separately store max & min of vertical components */ -void AC_Avoid::find_max_quadrant_velocity_3D(Vector3f &desired_vel, Vector2f &quad1_vel, Vector2f &quad2_vel, Vector2f &quad3_vel, Vector2f &quad4_vel, float &max_z_vel, float &min_z_vel) +void AC_Avoid::find_max_quadrant_velocity_3D(Vector3f &desired_vel, Vector2f &quad1_vel, Vector2f &quad2_vel, Vector2f &quad3_vel, Vector2f &quad4_vel, float &max_z_vel, float &min_z_vel) const { // split into horizontal and vertical components - Vector2f velocity_xy{desired_vel.x, desired_vel.y}; - find_max_quadrant_velocity(velocity_xy, quad1_vel, quad2_vel, quad3_vel, quad4_vel); + Vector2f velocity_ne{desired_vel.x, desired_vel.y}; + find_max_quadrant_velocity(velocity_ne, quad1_vel, quad2_vel, quad3_vel, quad4_vel); // store maximum and minimum of z if (is_positive(desired_vel.z) && (desired_vel.z > max_z_vel)) { @@ -724,12 +731,12 @@ void AC_Avoid::find_max_quadrant_velocity_3D(Vector3f &desired_vel, Vector2f &qu * Computes the speed such that the stopping distance * of the vehicle will be exactly the input distance. */ -float AC_Avoid::get_max_speed(float kP, float accel_cmss, float distance_cm, float dt) const +float AC_Avoid::get_max_speed(float kP, float accel, float distance, float dt) const { if (is_zero(kP)) { - return safe_sqrt(2.0f * distance_cm * accel_cmss); + return safe_sqrt(2.0f * distance * accel); } else { - return sqrt_controller(distance_cm, kP, accel_cmss, dt); + return sqrt_controller(distance, kP, accel, dt); } } @@ -738,7 +745,7 @@ float AC_Avoid::get_max_speed(float kP, float accel_cmss, float distance_cm, flo /* * Adjusts the desired velocity for the circular fence. */ -void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt) +void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f &desired_vel_ne_cms, Vector2f &backup_vel_ne_cms, float dt) { AC_Fence *fence = AP::fence(); if (fence == nullptr) { @@ -758,8 +765,8 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f } // get desired speed - const float desired_speed = desired_vel_cms.length(); - if (is_zero(desired_speed)) { + const float desired_speed_cms = desired_vel_ne_cms.length(); + if (is_zero(desired_speed_cms)) { // no avoidance necessary when desired speed is zero return; } @@ -767,59 +774,59 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f const AP_AHRS &_ahrs = AP::ahrs(); // get position as a 2D offset from ahrs home - Vector2f position_xy; - if (!_ahrs.get_relative_position_NE_home(position_xy)) { + Vector2f position_ne_cm; + if (!_ahrs.get_relative_position_NE_home(position_ne_cm)) { // we have no idea where we are.... return; } - position_xy *= 100.0f; // m -> cm + position_ne_cm *= 100.0f; // m -> cm // get the fence radius in cm - const float fence_radius = _fence.get_radius() * 100.0f; + const float fence_radius_cm = _fence.get_radius_m() * 100.0f; // get the margin to the fence in cm - const float margin_cm = _fence.get_horizontal_margin() * 100.0f; + const float margin_cm = _fence.get_margin_ne_m() * 100.0f; - if (margin_cm > fence_radius) { + if (margin_cm > fence_radius_cm) { return; } // get vehicle distance from home - const float dist_from_home = position_xy.length(); - if (dist_from_home > fence_radius) { + const float dist_from_home_cm = position_ne_cm.length(); + if (dist_from_home_cm > fence_radius_cm) { // outside of circular fence, no velocity adjustments return; } - const float distance_to_boundary = fence_radius - dist_from_home; + const float distance_to_boundary_cm = fence_radius_cm - dist_from_home_cm; // for backing away - Vector2f quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel; + Vector2f quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms; // back away if vehicle has breached margin - if (is_negative(distance_to_boundary - margin_cm)) { - calc_backup_velocity_2D(kP, accel_cmss, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel, margin_cm - distance_to_boundary, position_xy, dt); + if (is_negative(distance_to_boundary_cm - margin_cm)) { + calc_backup_velocity_2D(kP, accel_cmss, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms, margin_cm - distance_to_boundary_cm, position_ne_cm, dt); } // desired backup velocity is sum of maximum velocity component in each quadrant - backup_vel = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; + backup_vel_ne_cms = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms; // vehicle is inside the circular fence switch (_behavior) { case BEHAVIOR_SLIDE: { // implement sliding behaviour - const Vector2f stopping_point = position_xy + desired_vel_cms*(get_stopping_distance(kP, accel_cmss, desired_speed)/desired_speed); - const float stopping_point_dist_from_home = stopping_point.length(); - if (stopping_point_dist_from_home <= fence_radius - margin_cm) { + const Vector2f stopping_point_ne_cm = position_ne_cm + desired_vel_ne_cms * (get_stopping_distance(kP, accel_cmss, desired_speed_cms) / desired_speed_cms); + const float stopping_point_dist_from_home_ne_cm = stopping_point_ne_cm.length(); + if (stopping_point_dist_from_home_ne_cm <= fence_radius_cm - margin_cm) { // stopping before before fence so no need to adjust return; } // unsafe desired velocity - will not be able to stop before reaching margin from fence // Project stopping point radially onto fence boundary // Adjusted velocity will point towards this projected point at a safe speed - const Vector2f target_offset = stopping_point * ((fence_radius - margin_cm) / stopping_point_dist_from_home); - const Vector2f target_direction = target_offset - position_xy; - const float distance_to_target = target_direction.length(); - if (is_positive(distance_to_target)) { - 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); + const Vector2f target_offset_ne_cm = stopping_point_ne_cm * ((fence_radius_cm - margin_cm) / stopping_point_dist_from_home_ne_cm); + const Vector2f target_direction_ne_cm = target_offset_ne_cm - position_ne_cm; + const float distance_to_target_cm = target_direction_ne_cm.length(); + if (is_positive(distance_to_target_cm)) { + const float max_speed_cms = get_max_speed(kP, accel_cmss, distance_to_target_cm, dt); + desired_vel_ne_cms = target_direction_ne_cm * (MIN(desired_speed_cms,max_speed_cms) / distance_to_target_cm); } break; } @@ -827,23 +834,23 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f case (BEHAVIOR_STOP): { // implement stopping behaviour // calculate stopping point plus a margin so we look forward far enough to intersect with circular fence - const Vector2f stopping_point_plus_margin = position_xy + desired_vel_cms*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, desired_speed))/desired_speed); - const float stopping_point_plus_margin_dist_from_home = stopping_point_plus_margin.length(); - if (dist_from_home >= fence_radius - margin_cm) { + const Vector2f stopping_point_plus_margin_ne_cm = position_ne_cm + desired_vel_ne_cms*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, desired_speed_cms))/desired_speed_cms); + const float stopping_point_plus_margin_dist_from_home_cm = stopping_point_plus_margin_ne_cm.length(); + if (dist_from_home_cm >= fence_radius_cm - margin_cm) { // vehicle has already breached margin around fence // if stopping point is even further from home (i.e. in wrong direction) then adjust speed to zero // 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(); + if (stopping_point_plus_margin_dist_from_home_cm >= dist_from_home_cm) { + desired_vel_ne_cms.zero(); } } else { // shorten vector without adjusting its direction - Vector2f intersection; - if (Vector2f::circle_segment_intersection(position_xy, stopping_point_plus_margin, Vector2f(0.0f,0.0f), fence_radius - margin_cm, intersection)) { - const float distance_to_target = (intersection - position_xy).length(); - 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; + Vector2f intersection_ne_cm; + if (Vector2f::circle_segment_intersection(position_ne_cm, stopping_point_plus_margin_ne_cm, Vector2f(0.0f,0.0f), fence_radius_cm - margin_cm, intersection_ne_cm)) { + const float distance_to_target_cm = (intersection_ne_cm - position_ne_cm).length(); + const float max_speed_cms = get_max_speed(kP, accel_cmss, distance_to_target_cm, dt); + if (max_speed_cms < desired_speed_cms) { + desired_vel_ne_cms *= MAX(max_speed_cms, 0.0f) / desired_speed_cms; } } } @@ -855,7 +862,7 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f /* * Adjusts the desired velocity for the exclusion polygons */ -void AC_Avoid::adjust_velocity_inclusion_and_exclusion_polygons(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt) +void AC_Avoid::adjust_velocity_inclusion_and_exclusion_polygons(float kP, float accel_cmss, Vector2f &desired_vel_ne_cms, Vector2f &backup_vel_ne_cms, float dt) { const AC_Fence *fence = AP::fence(); if (fence == nullptr) { @@ -868,17 +875,17 @@ void AC_Avoid::adjust_velocity_inclusion_and_exclusion_polygons(float kP, float } // for backing away - Vector2f quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel; + Vector2f quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms; // iterate through inclusion polygons const uint8_t num_inclusion_polygons = fence->polyfence().get_inclusion_polygon_count(); for (uint8_t i = 0; i < num_inclusion_polygons; i++) { uint16_t num_points; const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points); - Vector2f backup_vel_inc; + Vector2f backup_vel_inc_ne_cms; // adjust velocity - adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, backup_vel_inc, boundary, num_points, fence->get_horizontal_margin(), dt, true); - find_max_quadrant_velocity(backup_vel_inc, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel); + adjust_velocity_polygon(kP, accel_cmss, desired_vel_ne_cms, backup_vel_inc_ne_cms, boundary, num_points, fence->get_margin_ne_m(), dt, true); + find_max_quadrant_velocity(backup_vel_inc_ne_cms, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms); } // iterate through exclusion polygons @@ -886,19 +893,19 @@ void AC_Avoid::adjust_velocity_inclusion_and_exclusion_polygons(float kP, float for (uint8_t i = 0; i < num_exclusion_polygons; i++) { uint16_t num_points; const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points); - Vector2f backup_vel_exc; + Vector2f backup_vel_exc_ne_cms; // adjust velocity - adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, backup_vel_exc, boundary, num_points, fence->get_horizontal_margin(), dt, false); - find_max_quadrant_velocity(backup_vel_exc, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel); + adjust_velocity_polygon(kP, accel_cmss, desired_vel_ne_cms, backup_vel_exc_ne_cms, boundary, num_points, fence->get_margin_ne_m(), dt, false); + find_max_quadrant_velocity(backup_vel_exc_ne_cms, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms); } // desired backup velocity is sum of maximum velocity component in each quadrant - backup_vel = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; + backup_vel_ne_cms = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms; } /* * Adjusts the desired velocity for the inclusion circles */ -void AC_Avoid::adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt) +void AC_Avoid::adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_ne_cms, Vector2f &backup_vel_ne_cms, float dt) { const AC_Fence *fence = AP::fence(); if (fence == nullptr) { @@ -917,62 +924,62 @@ void AC_Avoid::adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vec } // get vehicle position - Vector2f position_NE; - if (!AP::ahrs().get_relative_position_NE_origin_float(position_NE)) { + Vector2f position_ne_cm; + if (!AP::ahrs().get_relative_position_NE_origin_float(position_ne_cm)) { // do not limit velocity if we don't have a position estimate return; } - position_NE = position_NE * 100.0f; // m to cm + position_ne_cm = position_ne_cm * 100.0f; // m to cm // get the margin to the fence in cm - const float margin_cm = fence->get_horizontal_margin() * 100.0f; + const float margin_cm = fence->get_margin_ne_m() * 100.0f; // get desired speed - const float desired_speed = desired_vel_cms.length(); + const float desired_speed_cms = desired_vel_ne_cms.length(); // get stopping distance as an offset from the vehicle - Vector2f stopping_offset; - if (!is_zero(desired_speed)) { + Vector2f stopping_offset_ne_cm; + if (!is_zero(desired_speed_cms)) { switch (_behavior) { case BEHAVIOR_SLIDE: - stopping_offset = desired_vel_cms*(get_stopping_distance(kP, accel_cmss, desired_speed)/desired_speed); + stopping_offset_ne_cm = desired_vel_ne_cms * (get_stopping_distance(kP, accel_cmss, desired_speed_cms) / desired_speed_cms); break; case BEHAVIOR_STOP: // calculate stopping point plus a margin so we look forward far enough to intersect with circular fence - stopping_offset = desired_vel_cms*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, desired_speed))/desired_speed); + stopping_offset_ne_cm = desired_vel_ne_cms * ((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, desired_speed_cms)) / desired_speed_cms); break; } } // for backing away - Vector2f quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel; + Vector2f quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms; // iterate through inclusion circles for (uint8_t i = 0; i < num_circles; i++) { - Vector2f center_pos_cm; - float radius; - if (fence->polyfence().get_inclusion_circle(i, center_pos_cm, radius)) { + Vector2f center_pos_ne_cm; + float radius_m; + if (fence->polyfence().get_inclusion_circle(i, center_pos_ne_cm, radius_m)) { // get position relative to circle's center - const Vector2f position_NE_rel = (position_NE - center_pos_cm); + const Vector2f position_rel_ne_cm = (position_ne_cm - center_pos_ne_cm); // if we are outside this circle do not limit velocity for this circle - const float dist_sq_cm = position_NE_rel.length_squared(); - const float radius_cm = (radius * 100.0f); + const float dist_sq_cm = position_rel_ne_cm.length_squared(); + const float radius_cm = (radius_m * 100.0f); if (dist_sq_cm > sq(radius_cm)) { continue; } - const float radius_with_margin = radius_cm - margin_cm; - if (is_negative(radius_with_margin)) { + const float radius_with_margin_cm = radius_cm - margin_cm; + if (is_negative(radius_with_margin_cm)) { return; } - const float margin_breach = radius_with_margin - safe_sqrt(dist_sq_cm); + const float margin_breach_cm = radius_with_margin_cm - safe_sqrt(dist_sq_cm); // back away if vehicle has breached margin - if (is_negative(margin_breach)) { - calc_backup_velocity_2D(kP, accel_cmss, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel, margin_breach, position_NE_rel, dt); + if (is_negative(margin_breach_cm)) { + calc_backup_velocity_2D(kP, accel_cmss, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms, margin_breach_cm, position_rel_ne_cm, dt); } - if (is_zero(desired_speed)) { + if (is_zero(desired_speed_cms)) { // no avoidance necessary when desired speed is zero continue; } @@ -980,46 +987,46 @@ void AC_Avoid::adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vec switch (_behavior) { case BEHAVIOR_SLIDE: { // implement sliding behaviour - const Vector2f stopping_point = position_NE_rel + stopping_offset; - const float stopping_point_dist = stopping_point.length(); - if (is_zero(stopping_point_dist) || (stopping_point_dist <= (radius_cm - margin_cm))) { + const Vector2f stopping_point_ne_cm = position_rel_ne_cm + stopping_offset_ne_cm; + const float stopping_point_dist_cm = stopping_point_ne_cm.length(); + if (is_zero(stopping_point_dist_cm) || (stopping_point_dist_cm <= (radius_cm - margin_cm))) { // stopping before before fence so no need to adjust for this circle continue; } // unsafe desired velocity - will not be able to stop before reaching margin from fence // project stopping point radially onto fence boundary // adjusted velocity will point towards this projected point at a safe speed - const Vector2f target_offset = stopping_point * ((radius_cm - margin_cm) / stopping_point_dist); - const Vector2f target_direction = target_offset - position_NE_rel; - const float distance_to_target = target_direction.length(); - if (is_positive(distance_to_target)) { - 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); + const Vector2f target_offset_ne_cm = stopping_point_ne_cm * ((radius_cm - margin_cm) / stopping_point_dist_cm); + const Vector2f target_direction_ne_cm = target_offset_ne_cm - position_rel_ne_cm; + const float distance_to_target_cm = target_direction_ne_cm.length(); + if (is_positive(distance_to_target_cm)) { + const float max_speed_cms = get_max_speed(kP, accel_cmss, distance_to_target_cm, dt); + desired_vel_ne_cms = target_direction_ne_cm * (MIN(desired_speed_cms, max_speed_cms) / distance_to_target_cm); } } break; case BEHAVIOR_STOP: { // implement stopping behaviour - const Vector2f stopping_point_plus_margin = position_NE_rel + stopping_offset; + const Vector2f stopping_point_plus_margin_ne_cm = position_rel_ne_cm + stopping_offset_ne_cm; const float dist_cm = safe_sqrt(dist_sq_cm); if (dist_cm >= radius_cm - margin_cm) { // vehicle has already breached margin around fence // if stopping point is even further from center (i.e. in wrong direction) then adjust speed to zero // otherwise user is backing away from fence so do not apply limits - if (stopping_point_plus_margin.length() >= dist_cm) { - desired_vel_cms.zero(); + if (stopping_point_plus_margin_ne_cm.length() >= dist_cm) { + desired_vel_ne_cms.zero(); // desired backup velocity is sum of maximum velocity component in each quadrant - backup_vel = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; + backup_vel_ne_cms = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms; return; } } else { // shorten vector without adjusting its direction - Vector2f intersection; - if (Vector2f::circle_segment_intersection(position_NE_rel, stopping_point_plus_margin, Vector2f(0.0f,0.0f), radius_cm - margin_cm, intersection)) { - const float distance_to_target = (intersection - position_NE_rel).length(); - 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; + Vector2f intersection_ne_cm; + if (Vector2f::circle_segment_intersection(position_rel_ne_cm, stopping_point_plus_margin_ne_cm, Vector2f(0.0f,0.0f), radius_cm - margin_cm, intersection_ne_cm)) { + const float distance_to_target_cm = (intersection_ne_cm - position_rel_ne_cm).length(); + const float max_speed_cms = get_max_speed(kP, accel_cmss, distance_to_target_cm, dt); + if (max_speed_cms < desired_speed_cms) { + desired_vel_ne_cms *= MAX(max_speed_cms, 0.0f) / desired_speed_cms; } } } @@ -1029,13 +1036,13 @@ void AC_Avoid::adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vec } } // desired backup velocity is sum of maximum velocity component in each quadrant - backup_vel = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; + backup_vel_ne_cms = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms; } /* * Adjusts the desired velocity for the exclusion circles */ -void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt) +void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_ne_cms, Vector2f &backup_vel_ne_cms, float dt) { const AC_Fence *fence = AP::fence(); if (fence == nullptr) { @@ -1054,41 +1061,41 @@ void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vec } // get vehicle position - Vector2f position_NE; - if (!AP::ahrs().get_relative_position_NE_origin_float(position_NE)) { + Vector2f position_ne_cm; + if (!AP::ahrs().get_relative_position_NE_origin_float(position_ne_cm)) { // do not limit velocity if we don't have a position estimate return; } - position_NE = position_NE * 100.0f; // m to cm + position_ne_cm = position_ne_cm * 100.0f; // m to cm // get the margin to the fence in cm - const float margin_cm = fence->get_horizontal_margin() * 100.0f; + const float margin_cm = fence->get_margin_ne_m() * 100.0f; // for backing away - Vector2f quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel; + Vector2f quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms; // get desired speed - const float desired_speed = desired_vel_cms.length(); + const float desired_speed_cms = desired_vel_ne_cms.length(); // calculate stopping distance as an offset from the vehicle (only used for BEHAVIOR_STOP) // add a margin so we look forward far enough to intersect with circular fence - Vector2f stopping_offset; - if (!is_zero(desired_speed)) { + Vector2f stopping_offset_ne_cm; + if (!is_zero(desired_speed_cms)) { if ((AC_Avoid::BehaviourType)_behavior.get() == BEHAVIOR_STOP) { - stopping_offset = desired_vel_cms*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, desired_speed))/desired_speed); + stopping_offset_ne_cm = desired_vel_ne_cms * ((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, desired_speed_cms)) / desired_speed_cms); } } // iterate through exclusion circles for (uint8_t i = 0; i < num_circles; i++) { - Vector2f center_pos_cm; - float radius; - if (fence->polyfence().get_exclusion_circle(i, center_pos_cm, radius)) { + Vector2f center_pos_ne_cm; + float radius_m; + if (fence->polyfence().get_exclusion_circle(i, center_pos_ne_cm, radius_m)) { // get position relative to circle's center - const Vector2f position_NE_rel = (position_NE - center_pos_cm); + const Vector2f position_rel_ne_cm = (position_ne_cm - center_pos_ne_cm); // if we are inside this circle do not limit velocity for this circle - const float dist_sq_cm = position_NE_rel.length_squared(); - const float radius_cm = (radius * 100.0f); + const float dist_sq_cm = position_rel_ne_cm.length_squared(); + const float radius_cm = (radius_m * 100.0f); if (radius_cm < margin_cm) { return; } @@ -1096,13 +1103,13 @@ void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vec continue; } - const Vector2f vector_to_center = center_pos_cm - position_NE; - const float dist_to_boundary = vector_to_center.length() - radius_cm; + const Vector2f vector_to_center_ne_cm = center_pos_ne_cm - position_ne_cm; + const float dist_to_boundary_cm = vector_to_center_ne_cm.length() - radius_cm; // back away if vehicle has breached margin - if (is_negative(dist_to_boundary - margin_cm)) { - calc_backup_velocity_2D(kP, accel_cmss, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel, margin_cm - dist_to_boundary, vector_to_center, dt); + if (is_negative(dist_to_boundary_cm - margin_cm)) { + calc_backup_velocity_2D(kP, accel_cmss, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms, margin_cm - dist_to_boundary_cm, vector_to_center_ne_cm, dt); } - if (is_zero(desired_speed)) { + if (is_zero(desired_speed_cms)) { // no avoidance necessary when desired speed is zero continue; } @@ -1110,43 +1117,43 @@ void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vec switch (_behavior) { case BEHAVIOR_SLIDE: { // vector from current position to circle's center - Vector2f limit_direction = vector_to_center; - if (limit_direction.is_zero()) { + Vector2f limit_direction_ne_cm = vector_to_center_ne_cm; + if (limit_direction_ne_cm.is_zero()) { // vehicle is exactly on circle center so do not limit velocity continue; } // calculate distance to edge of circle - const float limit_distance_cm = limit_direction.length() - radius_cm; + const float limit_distance_cm = limit_direction_ne_cm.length() - radius_cm; if (!is_positive(limit_distance_cm)) { // vehicle is within circle so do not limit velocity continue; } // vehicle is outside the circle, adjust velocity to stay outside - limit_direction.normalize(); - limit_velocity_2D(kP, accel_cmss, desired_vel_cms, limit_direction, MAX(limit_distance_cm - margin_cm, 0.0f), dt); + limit_direction_ne_cm.normalize(); + limit_velocity_NE(kP, accel_cmss, desired_vel_ne_cms, limit_direction_ne_cm, MAX(limit_distance_cm - margin_cm, 0.0f), dt); } break; case BEHAVIOR_STOP: { // implement stopping behaviour - const Vector2f stopping_point_plus_margin = position_NE_rel + stopping_offset; + const Vector2f stopping_point_plus_margin_ne_cm = position_rel_ne_cm + stopping_offset_ne_cm; const float dist_cm = safe_sqrt(dist_sq_cm); if (dist_cm < radius_cm + margin_cm) { // vehicle has already breached margin around fence // if stopping point is closer to center (i.e. in wrong direction) then adjust speed to zero // otherwise user is backing away from fence so do not apply limits - if (stopping_point_plus_margin.length() <= dist_cm) { - desired_vel_cms.zero(); - backup_vel = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; + if (stopping_point_plus_margin_ne_cm.length() <= dist_cm) { + desired_vel_ne_cms.zero(); + backup_vel_ne_cms = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms; return; } } else { // shorten vector without adjusting its direction - Vector2f intersection; - if (Vector2f::circle_segment_intersection(position_NE_rel, stopping_point_plus_margin, Vector2f(0.0f,0.0f), radius_cm + margin_cm, intersection)) { - const float distance_to_target = (intersection - position_NE_rel).length(); - 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; + Vector2f intersection_ne_cm; + if (Vector2f::circle_segment_intersection(position_rel_ne_cm, stopping_point_plus_margin_ne_cm, Vector2f(0.0f,0.0f), radius_cm + margin_cm, intersection_ne_cm)) { + const float distance_to_target_cm = (intersection_ne_cm - position_rel_ne_cm).length(); + const float max_speed_cms = get_max_speed(kP, accel_cmss, distance_to_target_cm, dt); + if (max_speed_cms < desired_speed_cms) { + desired_vel_ne_cms *= MAX(max_speed_cms, 0.0f) / desired_speed_cms; } } } @@ -1156,7 +1163,7 @@ void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vec } } // desired backup velocity is sum of maximum velocity component in each quadrant - backup_vel = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; + backup_vel_ne_cms = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms; } #endif // AP_FENCE_ENABLED @@ -1164,7 +1171,7 @@ void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vec /* * Adjusts the desired velocity for the beacon fence. */ -void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt) +void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f &desired_vel_ne_cms, Vector2f &backup_vel_ne_cms, float dt) { AP_Beacon *_beacon = AP::beacon(); @@ -1181,20 +1188,20 @@ void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f } // adjust velocity using beacon - float margin = 0; + float margin_m = 0; #if AP_FENCE_ENABLED if (AP::fence()) { - margin = AP::fence()->get_horizontal_margin(); + margin_m = AP::fence()->get_margin_ne_m(); } #endif - adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, backup_vel, boundary, num_points, margin, dt, true); + adjust_velocity_polygon(kP, accel_cmss, desired_vel_ne_cms, backup_vel_ne_cms, boundary, num_points, margin_m, dt, true); } #endif // AP_BEACON_ENABLED /* * Adjusts the desired velocity based on output from the proximity sensor */ -void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &desired_vel_cms, Vector3f &backup_vel, float kP_z, float accel_cmss_z, float dt) +void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &desired_vel_neu_cms, Vector3f &backup_vel_neu_cms, float kP_z, float accel_u_cmss, float dt) { #if HAL_PROXIMITY_ENABLED // exit immediately if proximity sensor is not present @@ -1214,54 +1221,54 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &d const AP_AHRS &_ahrs = AP::ahrs(); // for backing away - Vector2f quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel; - float max_back_vel_z = 0.0f; - float min_back_vel_z = 0.0f; + Vector2f quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms; + float max_back_vel_u_cms = 0.0f; + float min_back_vel_u_cms = 0.0f; // rotate velocity vector from earth frame to body-frame since obstacles are in body-frame - const Vector2f desired_vel_body_cms = _ahrs.earth_to_body2D(Vector2f{desired_vel_cms.x, desired_vel_cms.y}); + const Vector2f desired_vel_body_ne_cms = _ahrs.earth_to_body2D(Vector2f{desired_vel_neu_cms.x, desired_vel_neu_cms.y}); - // safe_vel will be adjusted to stay away from Proximity Obstacles - Vector3f safe_vel = Vector3f{desired_vel_body_cms.x, desired_vel_body_cms.y, desired_vel_cms.z}; - const Vector3f safe_vel_orig = safe_vel; + // safe_vel_ne_cms will be adjusted to stay away from Proximity Obstacles + Vector3f safe_vel_neu_cms = Vector3f{desired_vel_body_ne_cms.x, desired_vel_body_ne_cms.y, desired_vel_neu_cms.z}; + const Vector3f safe_vel_orig_neu_cms = safe_vel_neu_cms; // calc margin in cm - const float margin_cm = MAX(_margin * 100.0f, 0.0f); - Vector3f stopping_point_plus_margin; - if (!desired_vel_cms.is_zero()) { + const float margin_cm = MAX(_margin_m * 100.0f, 0.0f); + Vector3f stopping_point_plus_margin_neu_cm; + if (!desired_vel_neu_cms.is_zero()) { // only used for "stop mode". Pre-calculating the stopping point here makes sure we do not need to repeat the calculations under iterations. - const float speed = safe_vel.length(); - stopping_point_plus_margin = safe_vel * ((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, speed))/speed); + const float speed_cms = safe_vel_neu_cms.length(); + stopping_point_plus_margin_neu_cm = safe_vel_neu_cms * ((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, speed_cms)) / speed_cms); } for (uint8_t i = 0; i deadzone) { + const float deadzone_cm = MAX(0.0f, _backup_deadzone_m) * 100.0f; + if (breach_dist_cm > deadzone_cm) { // this vector will help us decide how much we have to back away horizontally and vertically - const Vector3f margin_vector = vector_to_obstacle.normalized() * breach_dist; - const float xy_back_dist = margin_vector.xy().length(); - const float z_back_dist = margin_vector.z; - calc_backup_velocity_3D(kP, accel_cmss, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel, xy_back_dist, vector_to_obstacle, kP_z, accel_cmss_z, z_back_dist, min_back_vel_z, max_back_vel_z, dt); + const Vector3f margin_vector_neu_cm = vector_to_obstacle_neu.normalized() * breach_dist_cm; + const float xy_back_dist = margin_vector_neu_cm.xy().length(); + const float z_back_dist = margin_vector_neu_cm.z; + calc_backup_velocity_3D(kP, accel_cmss, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms, xy_back_dist, vector_to_obstacle_neu, kP_z, accel_u_cmss, z_back_dist, min_back_vel_u_cms, max_back_vel_u_cms, dt); } } - if (desired_vel_cms.is_zero()) { + if (desired_vel_neu_cms.is_zero()) { // cannot limit velocity if there is nothing to limit // backing up (if needed) has already been done continue; @@ -1269,30 +1276,30 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &d switch (_behavior) { case BEHAVIOR_SLIDE: { - Vector3f limit_direction{vector_to_obstacle}; + Vector3f limit_direction_neu{vector_to_obstacle_neu}; // distance to closest point - const float limit_distance_cm = limit_direction.length(); + const float limit_distance_cm = limit_direction_neu.length(); if (is_zero(limit_distance_cm)) { // We are exactly on the edge, this should ideally never be possible // i.e. do not adjust velocity. continue; } // Adjust velocity to not violate margin. - limit_velocity_3D(kP, accel_cmss, safe_vel, limit_direction, margin_cm, kP_z, accel_cmss_z, dt); + limit_velocity_NEU(kP, accel_cmss, safe_vel_neu_cms, limit_direction_neu, margin_cm, kP_z, accel_u_cmss, dt); break; } case BEHAVIOR_STOP: { // vector from current position to obstacle - Vector3f limit_direction; + Vector3f limit_direction_neu; // find closest point with line segment // also see if the vehicle will "roughly" intersect the boundary with the projected stopping point - const bool intersect = _proximity.closest_point_from_segment_to_obstacle(i, Vector3f{}, stopping_point_plus_margin, limit_direction); + const bool intersect = _proximity.closest_point_from_segment_to_obstacle(i, Vector3f{}, stopping_point_plus_margin_neu_cm, limit_direction_neu); if (intersect) { // the vehicle is intersecting the plane formed by the boundary // distance to the closest point from the stopping point - float limit_distance_cm = limit_direction.length(); + float limit_distance_cm = limit_direction_neu.length(); if (is_zero(limit_distance_cm)) { // We are exactly on the edge, this should ideally never be possible // i.e. do not adjust velocity. @@ -1300,10 +1307,10 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &d } if (limit_distance_cm <= margin_cm) { // we are within the margin so stop vehicle - safe_vel.zero(); + safe_vel_neu_cms.zero(); } else { // vehicle inside the given edge, adjust velocity to not violate this edge - limit_velocity_3D(kP, accel_cmss, safe_vel, limit_direction, margin_cm, kP_z, accel_cmss_z, dt); + limit_velocity_NEU(kP, accel_cmss, safe_vel_neu_cms, limit_direction_neu, margin_cm, kP_z, accel_u_cmss, dt); } break; @@ -1313,28 +1320,28 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &d } // desired backup velocity is sum of maximum velocity component in each quadrant - const Vector2f desired_back_vel_cms_xy = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; - const float desired_back_vel_cms_z = max_back_vel_z + min_back_vel_z; + const Vector2f desired_back_vel_cms_xy = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms; + const float desired_back_vel_cms_z = max_back_vel_u_cms + min_back_vel_u_cms; - if (safe_vel == safe_vel_orig && desired_back_vel_cms_xy.is_zero() && is_zero(desired_back_vel_cms_z)) { + if (safe_vel_neu_cms == safe_vel_orig_neu_cms && desired_back_vel_cms_xy.is_zero() && is_zero(desired_back_vel_cms_z)) { // proximity avoidance did nothing, no point in doing the calculations below. Return early - backup_vel.zero(); + backup_vel_neu_cms.zero(); return; } // set modified desired velocity vector and back away velocity vector // vectors were in body-frame, rotate resulting vector back to earth-frame - const Vector2f safe_vel_2d = _ahrs.body_to_earth2D(Vector2f{safe_vel.x, safe_vel.y}); - desired_vel_cms = Vector3f{safe_vel_2d.x, safe_vel_2d.y, safe_vel.z}; - const Vector2f backup_vel_xy = _ahrs.body_to_earth2D(desired_back_vel_cms_xy); - backup_vel = Vector3f{backup_vel_xy.x, backup_vel_xy.y, desired_back_vel_cms_z}; + const Vector2f safe_vel_ne_cms = _ahrs.body_to_earth2D(Vector2f{safe_vel_neu_cms.x, safe_vel_neu_cms.y}); + desired_vel_neu_cms = Vector3f{safe_vel_ne_cms.x, safe_vel_ne_cms.y, safe_vel_neu_cms.z}; + const Vector2f backup_vel_ne_cms = _ahrs.body_to_earth2D(desired_back_vel_cms_xy); + backup_vel_neu_cms = Vector3f{backup_vel_ne_cms.x, backup_vel_ne_cms.y, desired_back_vel_cms_z}; #endif // HAL_PROXIMITY_ENABLED } /* * Adjusts the desired velocity for the polygon fence. */ -void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, const Vector2f* boundary, uint16_t num_points, float margin, float dt, bool stay_inside) +void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel_ne_cms, const Vector2f* boundary, uint16_t num_points, float margin, float dt, bool stay_inside) { // exit if there are no points if (boundary == nullptr || num_points == 0) { @@ -1344,17 +1351,17 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des const AP_AHRS &_ahrs = AP::ahrs(); // do not adjust velocity if vehicle is outside the polygon fence - Vector2f position_xy; - if (!_ahrs.get_relative_position_NE_origin_float(position_xy)) { + Vector2f position_ne_cm; + if (!_ahrs.get_relative_position_NE_origin_float(position_ne_cm)) { // boundary is in earth frame but we have no idea // where we are return; } - position_xy = position_xy * 100.0f; // m to cm + position_ne_cm = position_ne_cm * 100.0f; // m to cm // return if we have already breached polygon - const bool inside_polygon = !Polygon_outside(position_xy, boundary, num_points); + const bool inside_polygon = !Polygon_outside(position_ne_cm, boundary, num_points); if (inside_polygon != stay_inside) { return; } @@ -1362,21 +1369,21 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des // Safe_vel will be adjusted to remain within fence. // We need a separate vector in case adjustment fails, // e.g. if we are exactly on the boundary. - Vector2f safe_vel(desired_vel_cms); + Vector2f safe_vel_ne_cms(desired_vel_cms); Vector2f desired_back_vel_cms; // calc margin in cm const float margin_cm = MAX(margin * 100.0f, 0.0f); // for stopping - const float speed = safe_vel.length(); - Vector2f stopping_point_plus_margin; + const float speed = safe_vel_ne_cms.length(); + Vector2f stopping_point_plus_margin_ne_cm; if (!desired_vel_cms.is_zero()) { - stopping_point_plus_margin = position_xy + safe_vel*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, speed))/speed); + stopping_point_plus_margin_ne_cm = position_ne_cm + safe_vel_ne_cms*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, speed))/speed); } // for backing away - Vector2f quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel; + Vector2f quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms; for (uint16_t i=0; i= _dist_max || _dist_max <= 0.0f) { + if (dist_m < 0.0f || dist_m >= _dist_max_m || _dist_max_m <= 0.0f) { return 0.0f; } // inverted but linear response - return 1.0f - (dist_m / _dist_max); + return 1.0f - (dist_m / _dist_max_m); } // returns the maximum positive and negative roll and pitch percentages (in -1 ~ +1 range) based on the proximity sensor -void AC_Avoid::get_proximity_roll_pitch_norm(float &roll_positive, float &roll_negative, float &pitch_positive, float &pitch_negative) +void AC_Avoid::get_proximity_roll_pitch_norm(float &roll_positive_norm, float &roll_negative_norm, float &pitch_positive_norm, float &pitch_negative_norm) const { #if HAL_PROXIMITY_ENABLED AP_Proximity *proximity = AP::proximity(); @@ -1505,23 +1512,23 @@ void AC_Avoid::get_proximity_roll_pitch_norm(float &roll_positive, float &roll_n for (uint8_t i=0; i 0.0f) { - roll_positive = MAX(roll_positive, roll_pct); - } else if (roll_pct < 0.0f) { - roll_negative = MIN(roll_negative, roll_pct); + if (roll_norm > 0.0f) { + roll_positive_norm = MAX(roll_positive_norm, roll_norm); + } else if (roll_norm < 0.0f) { + roll_negative_norm = MIN(roll_negative_norm, roll_norm); } - if (pitch_pct > 0.0f) { - pitch_positive = MAX(pitch_positive, pitch_pct); - } else if (pitch_pct < 0.0f) { - pitch_negative = MIN(pitch_negative, pitch_pct); + if (pitch_norm > 0.0f) { + pitch_positive_norm = MAX(pitch_positive_norm, pitch_norm); + } else if (pitch_norm < 0.0f) { + pitch_negative_norm = MIN(pitch_negative_norm, pitch_norm); } } } diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index 4aa91a3c84c..80a5c5f7c3f 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -47,22 +47,22 @@ public: // Adjusts the desired velocity so that the vehicle can stop // before the fence/object. // kP, accel_cmss are for the horizontal axis - // kP_z, accel_cmss_z are for vertical axis - void adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, float kP, float accel_cmss, float kP_z, float accel_cmss_z, float dt); - void adjust_velocity(Vector3f &desired_vel_cms, float kP, float accel_cmss, float kP_z, float accel_cmss_z, float dt) { + // kP_z, accel_z_cmss are for vertical axis + void adjust_velocity(Vector3f &desired_vel_neu_cms, bool &backing_up, float kP, float accel_cmss, float kP_z, float accel_z_cmss, float dt); + void adjust_velocity(Vector3f &desired_vel_neu_cms, float kP, float accel_cmss, float kP_z, float accel_z_cmss, float dt) { bool backing_up = false; - adjust_velocity(desired_vel_cms, backing_up, kP, accel_cmss, kP_z, accel_cmss_z, dt); + adjust_velocity(desired_vel_neu_cms, backing_up, kP, accel_cmss, kP_z, accel_z_cmss, dt); } - void adjust_velocity_m(Vector3f &desired_vel_ms, float kP, float accel_mss, float kP_z, float accel_mss_z, float dt) { + void adjust_velocity_m(Vector3f &desired_vel_neu_ms, float kP, float accel_mss, float kP_z, float accel_z_mss, float dt) { bool backing_up = false; - Vector3f desired_vel_cms = desired_vel_ms * 100.0; - adjust_velocity(desired_vel_cms, backing_up, kP, accel_mss * 100.0, kP_z, accel_mss_z * 100.0, dt); - desired_vel_ms = desired_vel_cms * 0.01; + Vector3f desired_vel_neu_cms = desired_vel_neu_ms * 100.0; + adjust_velocity(desired_vel_neu_cms, backing_up, kP, accel_mss * 100.0, kP_z, accel_z_mss * 100.0, dt); + desired_vel_neu_ms = desired_vel_neu_cms * 0.01; } // This method limits velocity and calculates backaway velocity from various supported fences // Also limits vertical velocity using adjust_velocity_z method - void adjust_velocity_fence(float kP, float accel_cmss, Vector3f &desired_vel_cms, Vector3f &backup_vel, float kP_z, float accel_cmss_z, float dt); + void adjust_velocity_fence(float kP, float accel_cmss, Vector3f &desired_vel_neu_cms, Vector3f &backup_vel_cms, float kP_z, float accel_z_cmss, float dt); // adjust desired horizontal speed so that the vehicle stops before the fence or object // accel (maximum acceleration/deceleration) is in m/s/s @@ -70,16 +70,16 @@ public: // speed is in m/s // kP should be zero for linear response, non-zero for non-linear response // dt is the time since the last call in seconds - void adjust_speed(float kP, float accel, float heading, float &speed, float dt); + void adjust_speed(float kP, float accel_mss, float heading_rad, float &speed_ms, float dt); // adjust vertical climb rate so vehicle does not break the vertical fence - void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float& backup_speed, float dt); + void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float& backup_speed_cms, float dt); void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt); // adjust roll-pitch to push vehicle away from objects // roll and pitch value are in radians // veh_angle_max_rad is the user defined maximum lean angle for the vehicle in radians - void adjust_roll_pitch_rad(float &roll_rad, float &pitch_rad, float veh_angle_max_rad); + void adjust_roll_pitch_rad(float &roll_rad, float &pitch_rad, float veh_angle_max_rad) const; // enable/disable proximity based avoidance void proximity_avoidance_enable(bool on_off) { _proximity_enabled = on_off; } @@ -88,26 +88,23 @@ public: // helper functions - // Limits the component of desired_vel_cms in the direction of the unit vector + // Limits the component of desired_vel_neu_cms in the direction of the unit vector // 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_2D(float kP, float accel_cmss, Vector2f &desired_vel_cms, const Vector2f& limit_direction, float limit_distance_cm, float dt); + void limit_velocity_NE(float kP, float accel_cmss, Vector2f &desired_vel_neu_cms, const Vector2f& limit_direction, float limit_distance_cm, float dt) const; // Note: This method is used to limit velocity horizontally and vertically given a 3D desired velocity vector - // Limits the component of desired_vel_cms in the direction of the obstacle_vector based on the passed value of "margin" - void limit_velocity_3D(float kP, float accel_cmss, Vector3f &desired_vel_cms, const Vector3f& limit_direction, float limit_distance_cm, float kP_z, float accel_cmss_z ,float dt); + // Limits the component of desired_vel_neu_cms in the direction of the obstacle_vector based on the passed value of "margin" + void limit_velocity_NEU(float kP, float accel_cmss, Vector3f &desired_vel_neu_cms, const Vector3f& limit_direction, float limit_distance_cm, float kP_z, float accel_z_cmss ,float dt) const; // 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 margin (in meters) that the vehicle should stay from objects - float get_margin() const { return _margin; } + float get_max_speed(float kP, float accel, float distance, float dt) const; // return minimum alt (in meters) above which avoidance will be active - float get_min_alt() const { return _alt_min; } + float get_min_alt() const { return _alt_min_m; } // return true if limiting is active bool limits_active() const {return (AP_HAL::millis() - _last_limit_time) < AC_AVOID_ACTIVE_LIMIT_TIMEOUT_MS;}; @@ -125,33 +122,33 @@ private: * Limit acceleration so that change of velocity output by avoidance library is controlled * This helps reduce jerks and sudden movements in the vehicle */ - void limit_accel(const Vector3f &original_vel, Vector3f &modified_vel, float dt); + void limit_accel_NEU_cm(const Vector3f &original_vel, Vector3f &modified_vel, float dt); /* * Adjusts the desired velocity for the circular fence. */ - void adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt); + void adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f &desired_vel_neu_cms, Vector2f &backup_vel_cms, float dt); /* * Adjusts the desired velocity for inclusion and exclusion polygon fences */ - void adjust_velocity_inclusion_and_exclusion_polygons(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt); + void adjust_velocity_inclusion_and_exclusion_polygons(float kP, float accel_cmss, Vector2f &desired_vel_neu_cms, Vector2f &backup_vel, float dt); /* * Adjusts the desired velocity for the inclusion and exclusion circles */ - void adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt); - void adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt); + void adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_neu_cms, Vector2f &backup_vel, float dt); + void adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_neu_cms, Vector2f &backup_vel, float dt); /* * Adjusts the desired velocity for the beacon fence. */ - void adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt); + void adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f &desired_vel_neu_cms, Vector2f &backup_vel, float dt); /* * Adjusts the desired velocity based on output from the proximity sensor */ - void adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &desired_vel_cms, Vector3f &backup_vel, float kP_z, float accel_cmss_z, float dt); + void adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &desired_vel_neu_cms, Vector3f &backup_vel, float kP_z, float accel_z_cmss, float dt); /* * Adjusts the desired velocity given an array of boundary points @@ -159,7 +156,7 @@ private: * margin is the distance (in meters) that the vehicle should stop short of the polygon * stay_inside should be true for fences, false for exclusion polygons */ - void adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, const Vector2f* boundary, uint16_t num_points, float margin, float dt, bool stay_inside); + void adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel_neu_cms, Vector2f &backup_vel, const Vector2f* boundary, uint16_t num_points, float margin, float dt, bool stay_inside); /* * Computes distance required to stop, given current speed. @@ -172,7 +169,7 @@ private: * It then calculates the desired backup velocity and passes it on to "find_max_quadrant_velocity" method to distribute the velocity vector into respective quadrants * OUTPUT: The method then outputs four velocities (quad1/2/3/4_back_vel_cms), which correspond to the final desired backup velocity in each quadrant */ - void calc_backup_velocity_2D(float kP, float accel_cmss, Vector2f &quad1_back_vel_cms, Vector2f &qua2_back_vel_cms, Vector2f &quad3_back_vel_cms, Vector2f &quad4_back_vel_cms, float back_distance_cm, Vector2f limit_direction, float dt); + void calc_backup_velocity_2D(float kP, float accel_cmss, Vector2f &quad1_back_vel_cms, Vector2f &qua2_back_vel_cms, Vector2f &quad3_back_vel_cms, Vector2f &quad4_back_vel_cms, float back_distance_cm, Vector2f limit_direction, float dt) const; /* * Compute the back away velocity required to avoid breaching margin, including vertical component @@ -180,7 +177,7 @@ private: * max_z_vel is >= 0, and stores the greatest velocity in the upwards direction * eventually max_z_vel + min_z_vel will give the final desired Z backaway velocity */ - void calc_backup_velocity_3D(float kP, float accel_cmss, Vector2f &quad1_back_vel_cms, Vector2f &quad2_back_vel_cms, Vector2f &quad3_back_vel_cms, Vector2f &quad4_back_vel_cms, float back_distance_cms, Vector3f limit_direction, float kp_z, float accel_cmss_z, float back_distance_z, float& min_z_vel, float& max_z_vel, float dt); + void calc_backup_velocity_3D(float kP, float accel_cmss, Vector2f &quad1_back_vel_cms, Vector2f &quad2_back_vel_cms, Vector2f &quad3_back_vel_cms, Vector2f &quad4_back_vel_cms, float back_distance_cms, Vector3f limit_direction, float kp_z, float accel_z_cmss, float back_distance_z, float& min_z_vel, float& max_z_vel, float dt) const; /* * Calculate maximum velocity vector that can be formed in each quadrant @@ -188,43 +185,43 @@ private: * The desired velocity is then fit into one of the 4 quadrant velocities as per the sign of its components * This ensures that we have multiple backup velocities, we can get the maximum of all of those velocities in each quadrant */ - void find_max_quadrant_velocity(Vector2f &desired_vel, Vector2f &quad1_vel, Vector2f &quad2_vel, Vector2f &quad3_vel, Vector2f &quad4_vel); + void find_max_quadrant_velocity(Vector2f &desired_vel, Vector2f &quad1_vel, Vector2f &quad2_vel, Vector2f &quad3_vel, Vector2f &quad4_vel) const; /* * Calculate maximum velocity vector that can be formed in each quadrant and separately store max & min of vertical components */ - void find_max_quadrant_velocity_3D(Vector3f &desired_vel, Vector2f &quad1_vel, Vector2f &quad2_vel, Vector2f &quad3_vel, Vector2f &quad4_vel, float &max_z_vel, float &min_z_vel); + void find_max_quadrant_velocity_3D(Vector3f &desired_vel, Vector2f &quad1_vel, Vector2f &quad2_vel, Vector2f &quad3_vel, Vector2f &quad4_vel, float &max_z_vel, float &min_z_vel) const; /* * methods for avoidance in non-GPS flight modes */ // convert distance (in meters) to a lean percentage (in 0~1 range) for use in manual flight modes - float distance_to_lean_norm(float dist_m); + float distance_m_to_lean_norm(float dist_m) const; // returns the maximum positive and negative roll and pitch percentages (in -1 ~ +1 range) based on the proximity sensor - void get_proximity_roll_pitch_norm(float &roll_positive, float &roll_negative, float &pitch_positive, float &pitch_negative); + void get_proximity_roll_pitch_norm(float &roll_positive, float &roll_negative, float &pitch_positive, float &pitch_negative) const; // Logging function void Write_SimpleAvoidance(const uint8_t state, const Vector3f& desired_vel, const Vector3f& modified_vel, const bool back_up) const; // parameters AP_Int8 _enabled; - AP_Int16 _angle_max_cd; // maximum lean angle to avoid obstacles (only used in non-GPS flight modes) - AP_Float _dist_max; // distance (in meters) from object at which obstacle avoidance will begin in non-GPS modes - AP_Float _margin; // vehicle will attempt to stay this distance (in meters) from objects while in GPS modes - AP_Int8 _behavior; // avoidance behaviour (slide or stop) - AP_Float _backup_speed_xy_max; // Maximum speed that will be used to back away horizontally (in m/s) - AP_Float _backup_speed_z_max; // Maximum speed that will be used to back away verticality (in m/s) - AP_Float _alt_min; // alt below which Proximity based avoidance is turned off - AP_Float _accel_max; // maximum acceleration while simple avoidance is active - AP_Float _backup_deadzone; // distance beyond AVOID_MARGIN parameter, after which vehicle will backaway from obstacles + AP_Int16 _angle_max_cd; // maximum lean angle to avoid obstacles (only used in non-GPS flight modes) + AP_Float _dist_max_m; // distance (in meters) from object at which obstacle avoidance will begin in non-GPS modes + AP_Float _margin_m; // vehicle will attempt to stay this distance (in meters) from objects while in GPS modes + AP_Int8 _behavior; // avoidance behaviour (slide or stop) + AP_Float _backup_speed_max_ne_ms; // Maximum speed that will be used to back away horizontally (in m/s) + AP_Float _backup_speed_max_u_ms; // Maximum speed that will be used to back away verticality (in m/s) + AP_Float _alt_min_m; // alt below which Proximity based avoidance is turned off + AP_Float _accel_max_mss; // maximum acceleration while simple avoidance is active + AP_Float _backup_deadzone_m; // distance beyond AVOID_MARGIN parameter, after which vehicle will backaway from obstacles - bool _proximity_enabled = true; // true if proximity sensor based avoidance is enabled (used to allow pilot to enable/disable) + bool _proximity_enabled = true; // true if proximity sensor based avoidance is enabled (used to allow pilot to enable/disable) bool _proximity_alt_enabled = true; // true if proximity sensor based avoidance is enabled based on altitude - uint32_t _last_limit_time; // the last time a limit was active - uint32_t _last_log_ms; // the last time simple avoidance was logged - Vector3f _prev_avoid_vel; // copy of avoidance adjusted velocity + uint32_t _last_limit_time; // the last time a limit was active + uint32_t _last_log_ms; // the last time simple avoidance was logged + Vector3f _prev_avoid_vel_neu_cms; // copy of avoidance adjusted velocity static AC_Avoid *_singleton; }; diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.cpp b/libraries/AC_Avoidance/AP_OABendyRuler.cpp index 46983a3198f..efba4253be6 100644 --- a/libraries/AC_Avoidance/AP_OABendyRuler.cpp +++ b/libraries/AC_Avoidance/AP_OABendyRuler.cpp @@ -477,7 +477,7 @@ bool AP_OABendyRuler::calc_margin_from_circular_fence(const Location &start, con const float end_dist_sq = ahrs_home.get_distance_NE(end).length_squared(); // get circular fence radius + margin - const float fence_radius_plus_margin = fence->get_radius() - fence->get_horizontal_margin(); + const float fence_radius_plus_margin = fence->get_radius_m() - fence->get_margin_ne_m(); // margin is fence radius minus the longer of start or end distance margin = fence_radius_plus_margin - sqrtf(MAX(start_dist_sq, end_dist_sq)); @@ -510,7 +510,7 @@ bool AP_OABendyRuler::calc_margin_from_alt_fence(const Location &start, const Lo } // safe max alt = fence alt - fence margin - const float max_fence_alt = fence->get_safe_alt_max(); + const float max_fence_alt = fence->get_safe_alt_max_m(); const float margin_start = max_fence_alt - alt_above_home_cm_start * 0.01f; const float margin_end = max_fence_alt - alt_above_home_cm_end * 0.01f; @@ -553,7 +553,7 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Lo } // get fence margin - const float fence_margin = fence->get_horizontal_margin(); + const float fence_margin = fence->get_margin_ne_m(); // iterate through inclusion polygons and calculate minimum margin bool margin_updated = false; @@ -625,7 +625,7 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_circles(const Loc } // get fence margin - const float fence_margin = fence->get_horizontal_margin(); + const float fence_margin = fence->get_margin_ne_m(); // iterate through inclusion circles and calculate minimum margin bool margin_updated = false;