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: Respect altitude with offtrack state

Browse Source 
To avoid weird cases where the altitude is different enough and
the offtrack state flies to the target altitude instead of the closest
point on the track between the waypoints.
This commit is contained in:
Matthias Grob 2022-01-04 18:15:10 +01:00 committed by Daniel Agar
parent be0a5b4b32
commit bbad4a5397
2 changed files with 9 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

@ -455,7 +455,8 @@ bool FlightTaskAuto::_evaluateTriplets()
_sub_triplet_setpoint.get().next.yaw,
_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : (float)NAN,
_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type);
_obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt);
_obstacle_avoidance.checkAvoidanceProgress(
_position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt));
}
// set heading
@ -617,11 +618,11 @@ Vector2f FlightTaskAuto::_getTargetVelocityXY()
State FlightTaskAuto::_getCurrentState()
{
// Calculate the vehicle current state based on the Navigator triplets and the current position.
Vector2f u_prev_to_target = Vector2f(_triplet_target - _triplet_prev_wp).unit_or_zero();
Vector2f pos_to_target(_triplet_target - _position);
Vector2f prev_to_pos(_position - _triplet_prev_wp);
const Vector3f u_prev_to_target = (_triplet_target - _triplet_prev_wp).unit_or_zero();
const Vector3f pos_to_target(_triplet_target - _position);
const Vector3f prev_to_pos(_position - _triplet_prev_wp);
// Calculate the closest point to the vehicle position on the line prev_wp - target
_closest_pt = Vector2f(_triplet_prev_wp) + u_prev_to_target * (prev_to_pos * u_prev_to_target);
_closest_pt = _triplet_prev_wp + u_prev_to_target * (prev_to_pos * u_prev_to_target);
State return_state = State::none;
@ -633,7 +634,7 @@ State FlightTaskAuto::_getCurrentState()
// Current position is more than cruise speed in front of previous setpoint.
return_state = State::previous_infront;
} else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _target_acceptance_radius) {
} else if ((_position - _closest_pt).length() > _target_acceptance_radius) {
// Vehicle is more than cruise speed off track.
return_state = State::offtrack;
@ -664,7 +665,7 @@ void FlightTaskAuto::_updateInternalWaypoints()
case State::offtrack:
_next_wp = _triplet_target;
_target = matrix::Vector3f(_closest_pt(0), _closest_pt(1), _triplet_target(2));
_target = _closest_pt;
_prev_wp = _position;
break;

2
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
View File

@ -194,7 +194,7 @@ private:
_triplet_prev_wp; /**< previous triplet from navigator which may differ from the intenal one (_prev_wp) depending on the vehicle state.*/
matrix::Vector3f
_triplet_next_wp; /**< next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state.*/
matrix::Vector2f _closest_pt; /**< closest point to the vehicle position on the line previous - target */
matrix::Vector3f _closest_pt; /**< closest point to the vehicle position on the line previous - target */
MapProjection _reference_position{}; /**< Class used to project lat/lon setpoint into local frame. */
float _reference_altitude{NAN}; /**< Altitude relative to ground. */