diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-06-01 09:53:24 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-06-01 09:53:24 +0200 |
commit | 0c943b30d4a26f4d8bf1059a3ee491988bcd8dac (patch) | |
tree | 845a4a23e7a4ceed1737517c24941d6178ee2552 | |
parent | bb87cb8c612bb733f0d37226c2157eb080aaf07a (diff) | |
parent | dc13307a8e662c34e1a1b6fa7ae47258db1465a0 (diff) | |
download | px4-firmware-0c943b30d4a26f4d8bf1059a3ee491988bcd8dac.tar.gz px4-firmware-0c943b30d4a26f4d8bf1059a3ee491988bcd8dac.tar.bz2 px4-firmware-0c943b30d4a26f4d8bf1059a3ee491988bcd8dac.zip |
Merge remote-tracking branch 'upstream/master' into mtecs
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.mc_defaults | 1 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 42 |
2 files changed, 25 insertions, 18 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index b6b0a5b4e..65f1e38c6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -41,6 +41,7 @@ then param set PE_POSNE_NOISE 0.5 param set PE_POSD_NOISE 1.0 + param set NAV_ACCEPT_RAD 2.0 fi set PWM_RATE 400 diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 401d50f7e..06e0c5904 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1458,7 +1458,6 @@ Navigator::check_mission_item_reached() /* XXX TODO count turns */ if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && _mission_item.loiter_radius > 0.01f) { @@ -1477,27 +1476,27 @@ Navigator::check_mission_item_reached() acceptance_radius = _parameters.acceptance_radius; } - float dist = -1.0f; - float dist_xy = -1.0f; - float dist_z = -1.0f; - - /* calculate AMSL altitude for this waypoint */ - float wp_alt_amsl = _mission_item.altitude; - - if (_mission_item.altitude_is_relative) - wp_alt_amsl += _home_pos.alt; - - dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, - &dist_xy, &dist_z); - if (_do_takeoff) { - if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { - /* require only altitude for takeoff */ + /* require only altitude for takeoff */ + if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) { _waypoint_position_reached = true; } } else { + float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + + /* calculate AMSL altitude for this waypoint */ + float wp_alt_amsl = _mission_item.altitude; + + if (_mission_item.altitude_is_relative) + wp_alt_amsl += _home_pos.alt; + + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, + (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, + &dist_xy, &dist_z); + if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; } @@ -1567,7 +1566,14 @@ Navigator::on_mission_item_reached() } if (_mission.current_mission_available()) { - set_mission_item(); + if (_mission_item.autocontinue) { + /* continue mission */ + set_mission_item(); + + } else { + /* autocontinue disabled for this item */ + request_loiter_or_ready(); + } } else { /* if no more mission items available then finish mission */ |