fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-09-24 16:47:39 +02:00 committed by Daniel Agar
parent 18f4144e5a
commit 280cb34a77
3 changed files with 16 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
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()

2
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
View File

@ -104,11 +104,11 @@ protected:
State _current_state{State::none};
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,
(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::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
(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

14
src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp
View File

@ -63,10 +63,10 @@ void FlightTaskAutoLine::_generateHeadingAlongTrack()
void FlightTaskAutoLine::_generateXYsetpoints()
{
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()) ||
(!has_reached_altitude && 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() < _target_acceptance_radius)) {
// Vehicle reached target in xy and no passing required. Lock position */
_position_setpoint(0) = _target(0);
@ -96,9 +96,9 @@ void FlightTaskAutoLine::_generateXYsetpoints()
}
// Compute maximum speed at target threshold */
if (threshold_max > NAV_ACC_RAD.get()) {
float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - NAV_ACC_RAD.get());
speed_threshold = m * (target_threshold - NAV_ACC_RAD.get()) + _speed_at_target; // speed at transition
if (threshold_max > _target_acceptance_radius) {
float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - _target_acceptance_radius);
speed_threshold = m * (target_threshold - _target_acceptance_radius) + _speed_at_target; // speed at transition
}
// Either accelerate or decelerate
@ -111,7 +111,7 @@ void FlightTaskAutoLine::_generateXYsetpoints()
_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 vehicle wants to stop at the target, then set acceptance radius to zero as well.