Robustified flight close to waypoints

2 files changed, 24 insertions, 0 deletions

diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index 87eb99f16..0dadf56f3 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 a6a2c0156..5a17346cb 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.h
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h
@@ -131,6 +131,14 @@ public:
 
 
 	/**
+	 * Returns true if following a circle (loiter)
+	 */
+	bool circle_mode() {
+		return _circle_mode;
+	}
+
+
+	/**
 	 * Get the switch distance
 	 * 
 	 * This is the distance at which the system will