Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 14:24:21 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

Survey - fix of survey tracking problem on steep slopes (#23371)

Browse Source 
* initial working

* incoperated review
This commit is contained in:
Claudio Chies
2024-07-11 14:54:22 +02:00
committed by GitHub
parent 9124a7b471
commit 33be5d8356
1 changed files with 8 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+8 -10
View File
@@ -632,29 +632,27 @@ bool FlightTaskAuto::_evaluateGlobalReference()
State FlightTaskAuto::_getCurrentState()
{
// Calculate the vehicle current state based on the Navigator triplets and the current position.
const Vector2f u_prev_to_target_xy = Vector2f(_triplet_target - _triplet_prev_wp).unit_or_zero();
const Vector2f pos_to_target_xy = Vector2f(_triplet_target - _position);
const Vector2f prev_to_pos_xy = Vector2f(_position - _triplet_prev_wp);
const Vector3f u_prev_to_target = (_triplet_target - _triplet_prev_wp).unit_or_zero();
const Vector3f prev_to_pos = _position - _triplet_prev_wp;
const Vector3f pos_to_target = _triplet_target - _position;
// Calculate the closest point to the vehicle position on the line prev_wp - target
const Vector2f closest_pt_xy = Vector2f(_triplet_prev_wp) + u_prev_to_target_xy * (prev_to_pos_xy *
u_prev_to_target_xy);
_closest_pt = Vector3f(closest_pt_xy(0), closest_pt_xy(1), _triplet_target(2));
_closest_pt = _triplet_prev_wp + u_prev_to_target * (prev_to_pos * u_prev_to_target);
State return_state = State::none;
if (u_prev_to_target_xy.length() < FLT_EPSILON) {
if (!u_prev_to_target.longerThan(FLT_EPSILON)) {
// Previous and target are the same point, so we better don't try to do any special line following
return_state = State::none;
} else if (u_prev_to_target_xy * pos_to_target_xy < 0.0f) {
} else if (u_prev_to_target * pos_to_target < 0.0f) {
// Target is behind
return_state = State::target_behind;
} else if (u_prev_to_target_xy * prev_to_pos_xy < 0.0f && prev_to_pos_xy.longerThan(_target_acceptance_radius)) {
} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.longerThan(_target_acceptance_radius)) {
// Previous is in front
return_state = State::previous_infront;
} else if (Vector2f(_position - _closest_pt).longerThan(_target_acceptance_radius)) {
} else if ((_position - _closest_pt).longerThan(_target_acceptance_radius)) {
// Vehicle too far from the track
return_state = State::offtrack;