fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 03:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into mtecs

Browse Source
This commit is contained in:
Thomas Gubler
2014-06-01 09:53:24 +02:00
parent bb87cb8c61 dc13307a8e
commit 0c943b30d4
2 changed files with 25 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rc.mc_defaults
+1
View File
@@ -41,6 +41,7 @@ then
param set PE_POSNE_NOISE 0.5 param set PE_POSNE_NOISE 0.5
param set PE_POSD_NOISE 1.0 param set PE_POSD_NOISE 1.0
param set NAV_ACCEPT_RAD 2.0
fi fi
set PWM_RATE 400 set PWM_RATE 400
src/modules/navigator/navigator_main.cpp
+24 -18
View File
@@ -1458,7 +1458,6 @@ Navigator::check_mission_item_reached()
/* XXX TODO count turns */ /* XXX TODO count turns */
if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || 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.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
_mission_item.loiter_radius > 0.01f) { _mission_item.loiter_radius > 0.01f) {
@@ -1477,27 +1476,27 @@ Navigator::check_mission_item_reached()
acceptance_radius = _parameters.acceptance_radius; 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 (_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; _waypoint_position_reached = true;
} }
} else { } 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) { if (dist >= 0.0f && dist <= acceptance_radius) {
_waypoint_position_reached = true; _waypoint_position_reached = true;
} }
@@ -1567,7 +1566,14 @@ Navigator::on_mission_item_reached()
} }
if (_mission.current_mission_available()) { 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 { } else {
/* if no more mission items available then finish mission */ /* if no more mission items available then finish mission */