From 81e9c06129ff96508377777ab3a405977c873357 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Oct 2013 21:04:59 +0200 Subject: [PATCH] Robustified flight close to waypoints --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 16 ++++++++++++++++ src/lib/ecl/l1/ecl_l1_pos_controller.h | 8 ++++++++ 2 files changed, 24 insertions(+) diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 87eb99f16d..0dadf56f39 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -86,6 +86,7 @@ float ECL_L1_Pos_Controller::crosstrack_error(void) void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, const math::Vector2f &ground_speed_vector) { + /* this follows the logic presented in [1] */ float eta; @@ -116,6 +117,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* calculate the vector from waypoint A to the aircraft */ math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + math::Vector2f vector_B_to_airplane = get_local_planar_vector(vector_B, vector_curr_position); /* calculate crosstrack error (output only) */ _crosstrack_error = vector_AB % vector_A_to_airplane; @@ -126,6 +128,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c * If the aircraft is already between A and B normal L1 logic is applied. */ float distance_A_to_airplane = vector_A_to_airplane.length(); + float distance_B_to_airplane = vector_B_to_airplane.length(); float alongTrackDist = vector_A_to_airplane * vector_AB; /* extension from [2] */ @@ -143,6 +146,19 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* bearing from current position to L1 point */ _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + } else if (distance_B_to_airplane < alongTrackDist || distance_B_to_airplane < _L1_distance) { + + /* fly directly to B */ + /* unit vector from waypoint B to current position */ + math::Vector2f vector_B_to_airplane_unit = vector_B_to_airplane.normalized(); + /* velocity across / orthogonal to line */ + xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit); + /* velocity along line */ + ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit); + eta = atan2f(xtrack_vel, ltrack_vel); + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX()); + } else { /* calculate eta to fly along the line between A and B */ diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h index a6a2c01563..5a17346cb1 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.h +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h @@ -130,6 +130,14 @@ public: bool reached_loiter_target(); + /** + * Returns true if following a circle (loiter) + */ + bool circle_mode() { + return _circle_mode; + } + + /** * Get the switch distance *