diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-10-08 09:13:41 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-10-08 09:13:41 +0200 |
commit | 5bc7d7c00f1f572825ce0db5f7fb24b8d77872a3 (patch) | |
tree | d323afbf4c4e440f04b528bcecd822b08fec1850 /src/lib/ecl/l1/ecl_l1_pos_controller.cpp | |
parent | 69fda8a05908a4871756b91ba68d84ff18a37bcc (diff) | |
download | px4-firmware-5bc7d7c00f1f572825ce0db5f7fb24b8d77872a3.tar.gz px4-firmware-5bc7d7c00f1f572825ce0db5f7fb24b8d77872a3.tar.bz2 px4-firmware-5bc7d7c00f1f572825ce0db5f7fb24b8d77872a3.zip |
Fixed turn radius return value
Diffstat (limited to 'src/lib/ecl/l1/ecl_l1_pos_controller.cpp')
-rw-r--r-- | src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 25 |
1 files changed, 15 insertions, 10 deletions
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 0dadf56f3..c5c0c7a3c 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -70,7 +70,7 @@ float ECL_L1_Pos_Controller::target_bearing() float ECL_L1_Pos_Controller::switch_distance(float wp_radius) { /* following [2], switching on L1 distance */ - return math::min(wp_radius, _L1_distance); + return math::max(wp_radius, _L1_distance); } bool ECL_L1_Pos_Controller::reached_loiter_target(void) @@ -117,7 +117,6 @@ 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; @@ -128,10 +127,13 @@ 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] */ + /* estimate airplane position WRT to B */ + math::Vector2f vector_B_to_airplane_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + float bearing_wp_b = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX()); + + /* extension from [2], fly directly to A */ if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) { /* calculate eta to fly to waypoint A */ @@ -146,19 +148,21 @@ 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) { +// XXX this can be useful as last-resort guard, but is currently not needed +#if 0 + } else if (absf(bearing_wp_b) > math::radians(80.0f)) { + /* extension, fly back to waypoint */ - /* fly directly to B */ - /* unit vector from waypoint B to current position */ - math::Vector2f vector_B_to_airplane_unit = vector_B_to_airplane.normalized(); + /* calculate eta to fly to waypoint B */ + /* 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()); - + _nav_bearing = bearing_wp_b; +#endif } else { /* calculate eta to fly along the line between A and B */ @@ -178,6 +182,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c eta = eta1 + eta2; /* bearing from current position to L1 point */ _nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1; + } /* limit angle to +-90 degrees */ |