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

use altitude acceptance radius for MC takeoff check

Browse Source 
When checking a mission, the takeoff altitude is being checked
against the waypoint acceptance radius to ensure the MAV being
clear from ground before heading to the next waypoint.
However, until now the _horizontal_ acceptance radius was being
used, instead of the altitude reference value.

Targets PX4/Firmware/#7379
This commit is contained in:
Lasse
2017-10-22 22:05:56 +02:00
committed by Lorenz Meier
parent 812f9ea11d
commit e2a359143d
2 changed files with 7 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_feasibility_checker.cpp
+6 -4
View File
@@ -67,6 +67,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
bool &warning_issued = _navigator->get_mission_result()->warning; bool &warning_issued = _navigator->get_mission_result()->warning;
const float default_acceptance_rad = _navigator->get_default_acceptance_radius(); const float default_acceptance_rad = _navigator->get_default_acceptance_radius();
const float default_altitude_acceptance_rad = _navigator->get_altitude_acceptance_radius();
const bool landed = _navigator->get_land_detected()->landed; const bool landed = _navigator->get_land_detected()->landed;
bool failed = false; bool failed = false;
@@ -92,7 +93,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
if (isRotarywing) { if (isRotarywing) {
failed = failed failed = failed
|| !checkRotarywing(dm_current, nMissionItems, home_alt, home_valid, default_acceptance_rad); || !checkRotarywing(dm_current, nMissionItems, home_alt, home_valid, default_altitude_acceptance_rad);
} else { } else {
failed = failed failed = failed
@@ -105,7 +106,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
bool bool
MissionFeasibilityChecker::checkRotarywing(dm_item_t dm_current, size_t nMissionItems, MissionFeasibilityChecker::checkRotarywing(dm_item_t dm_current, size_t nMissionItems,
float home_alt, bool home_valid, float default_acceptance_rad) float home_alt, bool home_valid, float default_altitude_acceptance_rad)
{ {
for (size_t i = 0; i < nMissionItems; i++) { for (size_t i = 0; i < nMissionItems; i++) {
struct mission_item_s missionitem = {}; struct mission_item_s missionitem = {};
@@ -118,13 +119,14 @@ MissionFeasibilityChecker::checkRotarywing(dm_item_t dm_current, size_t nMission
// look for a takeoff waypoint // look for a takeoff waypoint
if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) { if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) {
// make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius // make sure that the altitude of the waypoint is at least one meter larger than the altitude acceptance radius
// this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air // this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air
float takeoff_alt = missionitem.altitude_is_relative ? missionitem.altitude : missionitem.altitude - home_alt; float takeoff_alt = missionitem.altitude_is_relative ? missionitem.altitude : missionitem.altitude - home_alt;
// check if we should use default acceptance radius // check if we should use default acceptance radius
float acceptance_radius = default_acceptance_rad; float acceptance_radius = default_altitude_acceptance_rad;
// if a specific acceptance radius has been defined, use that one instead
if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) { if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) {
acceptance_radius = missionitem.acceptance_radius; acceptance_radius = missionitem.acceptance_radius;
} }
src/modules/navigator/mission_feasibility_checker.h
+1 -1
View File
@@ -77,7 +77,7 @@ private:
/* Checks specific to rotarywing airframes */ /* Checks specific to rotarywing airframes */
bool checkRotarywing(dm_item_t dm_current, size_t nMissionItems, bool checkRotarywing(dm_item_t dm_current, size_t nMissionItems,
float home_alt, bool home_valid, float default_acceptance_rad); float home_alt, bool home_valid, float default_altitude_acceptance_rad);
public: public:
MissionFeasibilityChecker(Navigator *navigator) : _navigator(navigator) {} MissionFeasibilityChecker(Navigator *navigator) : _navigator(navigator) {}