fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 14:13:35 +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 { } else {
_triplet_target = tmp_target; _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))) { if (!PX4_ISFINITE(_triplet_target(0)) || !PX4_ISFINITE(_triplet_target(1))) {
// Horizontal target is not finite. // 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 // We only adjust yaw if vehicle is outside of acceptance radius. Once we enter acceptance
// radius, lock yaw to current yaw. // radius, lock yaw to current yaw.
// This prevents excessive yawing. // 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); _compute_heading_from_2D_vector(_yaw_setpoint, v);
_yaw_lock = false; _yaw_lock = false;
@@ -331,7 +332,7 @@ void FlightTaskAuto::_checkAvoidanceProgress()
const float pos_to_target_z = fabsf(_triplet_target(2) - _position(2)); 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 // vehicle above or below the target waypoint
pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f; 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 // 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 && 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() angle = Vector2f(_target - _prev_wp).unit_or_zero()
* Vector2f(_target - _next_wp).unit_or_zero() * Vector2f(_target - _next_wp).unit_or_zero()
@@ -492,6 +493,7 @@ void FlightTaskAuto::_updateInternalWaypoints()
case State::previous_infront: { case State::previous_infront: {
_next_wp = _triplet_target; _next_wp = _triplet_target;
_target = _triplet_prev_wp; _target = _triplet_prev_wp;
_target_acceptance_radius = _sub_triplet_setpoint->get().previous.acceptance_radius;
_prev_wp = _position; _prev_wp = _position;
float angle = 2.0f; float angle = 2.0f;
@@ -500,7 +502,7 @@ void FlightTaskAuto::_updateInternalWaypoints()
// angle = cos(x) + 1.0 // angle = cos(x) + 1.0
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*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 && 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() angle = Vector2f(_target - _prev_wp).unit_or_zero()
* Vector2f(_target - _next_wp).unit_or_zero() * Vector2f(_target - _next_wp).unit_or_zero()
@@ -521,7 +523,7 @@ void FlightTaskAuto::_updateInternalWaypoints()
// angle = cos(x) + 1.0 // angle = cos(x) + 1.0
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*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 && 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() angle = Vector2f(_target - _prev_wp).unit_or_zero()
* Vector2f(_target - _next_wp).unit_or_zero() * Vector2f(_target - _next_wp).unit_or_zero()
@@ -542,7 +544,7 @@ void FlightTaskAuto::_updateInternalWaypoints()
// angle = cos(x) + 1.0 // angle = cos(x) + 1.0
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*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 && 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 = angle =
Vector2f(_target - _prev_wp).unit_or_zero() Vector2f(_target - _prev_wp).unit_or_zero()
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
+1 -1
View File
@@ -104,11 +104,11 @@ protected:
State _current_state{State::none}; State _current_state{State::none};
float _speed_at_target = 0.0f; /**< Desired velocity at target. */ float _speed_at_target = 0.0f; /**< Desired velocity at target. */
float _target_acceptance_radius = 0.0f; /**< Acceptances radius of the target */
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_XY_CRUISE>) MPC_XY_CRUISE, (ParamFloat<px4::params::MPC_XY_CRUISE>) MPC_XY_CRUISE,
(ParamFloat<px4::params::MPC_CRUISE_90>) MPC_CRUISE_90, // speed at corner when angle is 90 degrees move to line (ParamFloat<px4::params::MPC_CRUISE_90>) MPC_CRUISE_90, // speed at corner when angle is 90 degrees move to line
(ParamFloat<px4::params::NAV_ACC_RAD>) NAV_ACC_RAD, // acceptance radius at which waypoints are updated move to line
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) NAV_MC_ALT_RAD, //vertical acceptance radius at which waypoints are updated (ParamFloat<px4::params::NAV_MC_ALT_RAD>) NAV_MC_ALT_RAD, //vertical acceptance radius at which waypoints are updated
(ParamInt<px4::params::MPC_YAW_MODE>) MPC_YAW_MODE, // defines how heading is executed, (ParamInt<px4::params::MPC_YAW_MODE>) MPC_YAW_MODE, // defines how heading is executed,
(ParamInt<px4::params::MPC_OBS_AVOID>) MPC_OBS_AVOID // obstacle avoidance active (ParamInt<px4::params::MPC_OBS_AVOID>) MPC_OBS_AVOID // obstacle avoidance active
src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp
+7 -7
View File
@@ -63,10 +63,10 @@ void FlightTaskAutoLine::_generateHeadingAlongTrack()
void FlightTaskAutoLine::_generateXYsetpoints() void FlightTaskAutoLine::_generateXYsetpoints()
{ {
Vector2f pos_sp_to_dest(_target - _position_setpoint); Vector2f pos_sp_to_dest(_target - _position_setpoint);
const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < NAV_ACC_RAD.get(); const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < _target_acceptance_radius;
if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < NAV_ACC_RAD.get()) || if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < _target_acceptance_radius) ||
(!has_reached_altitude && pos_sp_to_dest.length() < NAV_ACC_RAD.get())) { (!has_reached_altitude && pos_sp_to_dest.length() < _target_acceptance_radius)) {
// Vehicle reached target in xy and no passing required. Lock position */ // Vehicle reached target in xy and no passing required. Lock position */
_position_setpoint(0) = _target(0); _position_setpoint(0) = _target(0);
@@ -96,9 +96,9 @@ void FlightTaskAutoLine::_generateXYsetpoints()
} }
// Compute maximum speed at target threshold */ // Compute maximum speed at target threshold */
if (threshold_max > NAV_ACC_RAD.get()) { if (threshold_max > _target_acceptance_radius) {
float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - NAV_ACC_RAD.get()); float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - _target_acceptance_radius);
speed_threshold = m * (target_threshold - NAV_ACC_RAD.get()) + _speed_at_target; // speed at transition speed_threshold = m * (target_threshold - _target_acceptance_radius) + _speed_at_target; // speed at transition
} }
// Either accelerate or decelerate // Either accelerate or decelerate
@@ -111,7 +111,7 @@ void FlightTaskAutoLine::_generateXYsetpoints()
_speed_at_target = 0.0f; _speed_at_target = 0.0f;
} }
float acceptance_radius = NAV_ACC_RAD.get(); float acceptance_radius = _target_acceptance_radius;
if (_speed_at_target < 0.01f) { if (_speed_at_target < 0.01f) {
// If vehicle wants to stop at the target, then set acceptance radius to zero as well. // If vehicle wants to stop at the target, then set acceptance radius to zero as well.