fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-05 05:48:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskAuto: replace NAV_ACC_RAD with acceptance radius from triplet

Browse Source
This commit is contained in:
Martina
2018-10-09 12:01:45 -04:00
committed by Daniel Agar
parent 18f4144e5a
commit 280cb34a77
3 changed files with 16 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+8 -6
View File
@@ -160,6 +160,7 @@ bool FlightTaskAuto::_evaluateTriplets()
} else {
_triplet_target = tmp_target;
_target_acceptance_radius = _sub_triplet_setpoint->get().current.acceptance_radius;
if (!PX4_ISFINITE(_triplet_target(0)) || !PX4_ISFINITE(_triplet_target(1))) {
// Horizontal target is not finite.
@@ -268,7 +269,7 @@ void FlightTaskAuto::_set_heading_from_mode()
// We only adjust yaw if vehicle is outside of acceptance radius. Once we enter acceptance
// radius, lock yaw to current yaw.
// This prevents excessive yawing.
if (v.length() > NAV_ACC_RAD.get()) {
if (v.length() > _target_acceptance_radius) {
_compute_heading_from_2D_vector(_yaw_setpoint, v);
_yaw_lock = false;
@@ -331,7 +332,7 @@ void FlightTaskAuto::_checkAvoidanceProgress()
const float pos_to_target_z = fabsf(_triplet_target(2) - _position(2));
if (pos_to_target.length() < NAV_ACC_RAD.get() && pos_to_target_z > NAV_MC_ALT_RAD.get()) {
if (pos_to_target.length() < _target_acceptance_radius && pos_to_target_z > NAV_MC_ALT_RAD.get()) {
// vehicle above or below the target waypoint
pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f;
}
@@ -479,7 +480,7 @@ void FlightTaskAuto::_updateInternalWaypoints()
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
if (Vector2f(_target - _next_wp).length() > 0.001f &&
(Vector2f(_target - _prev_wp).length() > NAV_ACC_RAD.get())) {
(Vector2f(_target - _prev_wp).length() > _target_acceptance_radius)) {
angle = Vector2f(_target - _prev_wp).unit_or_zero()
* Vector2f(_target - _next_wp).unit_or_zero()
@@ -492,6 +493,7 @@ void FlightTaskAuto::_updateInternalWaypoints()
case State::previous_infront: {
_next_wp = _triplet_target;
_target = _triplet_prev_wp;
_target_acceptance_radius = _sub_triplet_setpoint->get().previous.acceptance_radius;
_prev_wp = _position;
float angle = 2.0f;
@@ -500,7 +502,7 @@ void FlightTaskAuto::_updateInternalWaypoints()
// angle = cos(x) + 1.0
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
if (Vector2f(_target - _next_wp).length() > 0.001f &&
(Vector2f(_target - _prev_wp).length() > NAV_ACC_RAD.get())) {
(Vector2f(_target - _prev_wp).length() > _target_acceptance_radius)) {
angle = Vector2f(_target - _prev_wp).unit_or_zero()
* Vector2f(_target - _next_wp).unit_or_zero()
@@ -521,7 +523,7 @@ void FlightTaskAuto::_updateInternalWaypoints()
// angle = cos(x) + 1.0
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
if (Vector2f(_target - _next_wp).length() > 0.001f &&
(Vector2f(_target - _prev_wp).length() > NAV_ACC_RAD.get())) {
(Vector2f(_target - _prev_wp).length() > _target_acceptance_radius)) {
angle = Vector2f(_target - _prev_wp).unit_or_zero()
* Vector2f(_target - _next_wp).unit_or_zero()
@@ -542,7 +544,7 @@ void FlightTaskAuto::_updateInternalWaypoints()
// angle = cos(x) + 1.0
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
if (Vector2f(_target - _next_wp).length() > 0.001f &&
(Vector2f(_target - _prev_wp).length() > NAV_ACC_RAD.get())) {
(Vector2f(_target - _prev_wp).length() > _target_acceptance_radius)) {
angle =
Vector2f(_target - _prev_wp).unit_or_zero()