diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-10-06 21:04:59 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-10-06 21:04:59 +0200 |
commit | 81e9c06129ff96508377777ab3a405977c873357 (patch) | |
tree | e60f1904d626321e0a44bf0cb59510f0a79115f5 /src/lib/ecl/l1 | |
parent | d1871bd7bb9ae9eefdf1ada56fc3f57428e689eb (diff) | |
download | px4-firmware-81e9c06129ff96508377777ab3a405977c873357.tar.gz px4-firmware-81e9c06129ff96508377777ab3a405977c873357.tar.bz2 px4-firmware-81e9c06129ff96508377777ab3a405977c873357.zip |
Robustified flight close to waypoints
Diffstat (limited to 'src/lib/ecl/l1')
-rw-r--r-- | src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 16 | ||||
-rw-r--r-- | src/lib/ecl/l1/ecl_l1_pos_controller.h | 8 |
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 |