From 6528bde62aaf5ddbddcf49d3b61034120dd9fa01 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Mon, 8 Jul 2024 11:24:34 +0200 Subject: [PATCH 1/2] initial working --- .../tasks/Auto/FlightTaskAuto.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 606776aea8c4..2f30bf08004b 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -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.length() < 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 (pos_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 (Vector3f(_position - _closest_pt).longerThan(_target_acceptance_radius)) { // Vehicle too far from the track return_state = State::offtrack; From 50d3bb5e48d8f50dec362d08960e9c873403637f Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Wed, 10 Jul 2024 13:26:05 +0200 Subject: [PATCH 2/2] incoperated review --- .../flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 2f30bf08004b..dad05e3c31db 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -640,11 +640,11 @@ State FlightTaskAuto::_getCurrentState() State return_state = State::none; - if (u_prev_to_target.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 (pos_to_target * pos_to_target < 0.0f) { + } else if (u_prev_to_target * pos_to_target < 0.0f) { // Target is behind return_state = State::target_behind; @@ -652,7 +652,7 @@ State FlightTaskAuto::_getCurrentState() // Previous is in front return_state = State::previous_infront; - } else if (Vector3f(_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;