diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-28 10:04:13 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-28 10:04:13 +0400 |
commit | 7b7539fbbd256165603d545e2c4c99daaf719e3e (patch) | |
tree | 209a98f04a4093ec9d827a5ad61c1d4afc336d07 /src/lib/ecl/l1 | |
parent | 153114aec8571a9105541b1fef473d36c4099519 (diff) | |
parent | 72d9c80ce954d2289282f5df01aef7e5e8914acc (diff) | |
download | px4-firmware-7b7539fbbd256165603d545e2c4c99daaf719e3e.tar.gz px4-firmware-7b7539fbbd256165603d545e2c4c99daaf719e3e.tar.bz2 px4-firmware-7b7539fbbd256165603d545e2c4c99daaf719e3e.zip |
Merge branch 'navigator_new' into navigator_new_vector, WIP
Diffstat (limited to 'src/lib/ecl/l1')
-rw-r--r-- | src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 4 |
1 files changed, 2 insertions, 2 deletions
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index fb7ed486d..3b68a0a4e 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::max(wp_radius, _L1_distance); + return math::min(wp_radius, _L1_distance); } bool ECL_L1_Pos_Controller::reached_loiter_target(void) @@ -280,7 +280,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con */ // XXX check switch over - if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) | + if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) || (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) { _lateral_accel = lateral_accel_sp_center; _circle_mode = false; |