fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-06-08 11:52:46 +03:00 committed by Lorenz Meier
parent 812f9ea11d
commit e2a359143d
2 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -67,6 +67,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
bool &warning_issued = _navigator->get_mission_result()->warning;
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;
bool failed = false;
@ -92,7 +93,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
if (isRotarywing) {
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 {
failed = failed
@ -105,7 +106,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
bool
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++) {
struct mission_item_s missionitem = {};
@ -118,13 +119,14 @@ MissionFeasibilityChecker::checkRotarywing(dm_item_t dm_current, size_t nMission
// look for a takeoff waypoint
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
float takeoff_alt = missionitem.altitude_is_relative ? missionitem.altitude : missionitem.altitude - home_alt;
// 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) {
acceptance_radius = missionitem.acceptance_radius;
}

2
src/modules/navigator/mission_feasibility_checker.h
View File

@ -77,7 +77,7 @@ private:
/* Checks specific to rotarywing airframes */
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:
MissionFeasibilityChecker(Navigator *navigator) : _navigator(navigator) {}