Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

FlightTaskAuto: Respect altitude with offtrack state #18953

Merged
merged 1 commit into from
Jan 7, 2022
Merged

FlightTaskAuto: Respect altitude with offtrack state #18953

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 8 additions & 7 deletions src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;

Expand All @@ -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) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

longerThan ?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I made a follow up with more of these cases here: #18985

// Vehicle is more than cruise speed off track.
return_state = State::offtrack;

Expand Down Expand Up @@ -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;

Expand Down
2 changes: 1 addition & 1 deletion src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -194,7 +194,7 @@ class FlightTaskAuto : public FlightTask
_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 */

map_projection_reference_s _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */
float _reference_altitude{NAN}; /**< Altitude relative to ground. */
Expand Down