fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 10:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'px4/master' into navigator_rewrite

Browse Source 
Conflicts:
	src/modules/navigator/navigator_main.cpp
This commit is contained in:
Julian Oes
2014-06-03 16:04:39 +02:00
parent 854bb7fe08 da5b60adab
commit d1d03c34b9
30 changed files with 1060 additions and 181 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_feasibility_checker.cpp
+38 -8
View File
@@ -63,7 +63,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab
}
bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
{
/* Init if not done yet */
init();
@@ -76,24 +76,24 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
if (isRotarywing)
return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence);
return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt);
else
return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence);
return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt);
}
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
{
return checkGeofence(dm_current, nMissionItems, geofence);
return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
}
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
{
/* Update fixed wing navigation capabilites */
updateNavigationCapabilities();
// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence));
return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
}
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
@@ -109,7 +109,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
return false;
}
if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { //xxx: handle relative altitude
if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) {
mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i);
return false;
}
@@ -119,6 +119,36 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
return true;
}
bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error)
{
/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
for (size_t i = 0; i < nMissionItems; i++) {
static struct mission_item_s missionitem;
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
if (throw_error) {
return false;
} else {
return true;
}
}
if (home_alt > missionitem.altitude) {
if (throw_error) {
mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i);
return false;
} else {
mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i);
return true;
}
}
}
return true;
}
bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems)
{
/* Go through all mission items and search for a landing waypoint
src/modules/navigator/mission_feasibility_checker.h
+4 -3
View File
@@ -61,14 +61,15 @@ private:
/* Checks for all airframes */
bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false);
/* Checks specific to fixedwing airframes */
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems);
void updateNavigationCapabilities();
/* Checks specific to rotarywing airframes */
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
public:
MissionFeasibilityChecker();
@@ -77,7 +78,7 @@ public:
/*
* Returns true if mission is feasible and false otherwise
*/
bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
};
src/modules/navigator/navigator_main.cpp
+20 -20
View File
@@ -674,11 +674,10 @@ Navigator::check_mission_item_reached()
return _vstatus.condition_landed;
}
/* 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) {
/* XXX TODO count turns */
if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
_mission_item.loiter_radius > 0.01f) {
// return false;
// }
@@ -695,24 +694,26 @@ Navigator::check_mission_item_reached()
acceptance_radius = _parameters.acceptance_radius;
}
float dist = -1.0f;
float dist_xy = -1.0f;
float dist_z = -1.0f;
float altitude_amsl = _mission_item.altitude_is_relative
? _mission_item.altitude + _home_pos.alt : _mission_item.altitude;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
_global_pos.lat, _global_pos.lon, _global_pos.alt,
&dist_xy, &dist_z);
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
if (_do_takeoff) {
/* require only altitude for takeoff */
if (_global_pos.alt > altitude_amsl - acceptance_radius) {
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;
}
@@ -765,7 +766,6 @@ Navigator::reset_reached()
_waypoint_yaw_reached = false;
}
#endif
void
Navigator::publish_position_setpoint_triplet()
{