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

mission feasibility full home position isn't always needed

Browse Source
This commit is contained in:
Daniel Agar
2017-12-21 15:28:45 +08:00
committed by Lorenz Meier
parent b7189012dc
commit 7dab5d4380
3 changed files with 17 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_feasibility_checker.cpp
+11 -10
View File
@@ -64,6 +64,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
fw_pos_ctrl_status_s *fw_pos_ctrl_status = _navigator->get_fw_pos_ctrl_status();
const float home_alt = _navigator->get_home_position()->alt;
const bool home_valid = _navigator->home_position_valid();
const bool home_alt_valid = _navigator->home_alt_valid();
bool &warning_issued = _navigator->get_mission_result()->warning;
const float default_acceptance_rad = _navigator->get_default_acceptance_radius();
@@ -74,7 +75,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
bool warned = false;
// first check if we have a valid position
if (!home_valid) {
if (!home_alt_valid) {
failed = true;
warned = true;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.");
@@ -89,15 +90,15 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
failed = failed
|| !checkDistancesBetweenWaypoints(dm_current, nMissionItems, max_distance_between_waypoints, warning_issued);
failed = failed || !checkGeofence(dm_current, nMissionItems, geofence, home_alt, home_valid);
failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned);
failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_alt_valid, warned);
if (isRotarywing) {
failed = failed
|| !checkRotarywing(dm_current, nMissionItems, home_alt, home_valid, default_altitude_acceptance_rad);
|| !checkRotarywing(dm_current, nMissionItems, home_alt, home_alt_valid, default_altitude_acceptance_rad);
} else {
failed = failed
|| !checkFixedwing(dm_current, nMissionItems, fw_pos_ctrl_status, home_alt, home_valid,
|| !checkFixedwing(dm_current, nMissionItems, fw_pos_ctrl_status, home_alt, home_alt_valid,
default_acceptance_rad, land_start_req);
}
@@ -106,7 +107,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_altitude_acceptance_rad)
float home_alt, bool home_alt_valid, float default_altitude_acceptance_rad)
{
for (size_t i = 0; i < nMissionItems; i++) {
struct mission_item_s missionitem = {};
@@ -144,11 +145,11 @@ MissionFeasibilityChecker::checkRotarywing(dm_item_t dm_current, size_t nMission
bool
MissionFeasibilityChecker::checkFixedwing(dm_item_t dm_current, size_t nMissionItems,
fw_pos_ctrl_status_s *fw_pos_ctrl_status, float home_alt, bool home_valid,
fw_pos_ctrl_status_s *fw_pos_ctrl_status, float home_alt, bool home_alt_valid,
float default_acceptance_rad, bool land_start_req)
{
/* Perform checks and issue feedback to the user for all checks */
bool resTakeoff = checkFixedWingTakeoff(dm_current, nMissionItems, home_alt, home_valid, land_start_req);
bool resTakeoff = checkFixedWingTakeoff(dm_current, nMissionItems, home_alt, home_alt_valid, land_start_req);
bool resLanding = checkFixedWingLanding(dm_current, nMissionItems, fw_pos_ctrl_status, land_start_req);
/* Mission is only marked as feasible if all checks return true */
@@ -198,7 +199,7 @@ MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionIt
bool
MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems,
float home_alt, bool home_valid, bool &warning_issued, bool throw_error)
float home_alt, bool home_alt_valid, bool &warning_issued, bool throw_error)
{
/* Check if all waypoints are above the home altitude */
for (size_t i = 0; i < nMissionItems; i++) {
@@ -212,7 +213,7 @@ MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_
}
/* reject relative alt without home set */
if (missionitem.altitude_is_relative && !home_valid && MissionBlock::item_contains_position(missionitem)) {
if (missionitem.altitude_is_relative && !home_alt_valid && MissionBlock::item_contains_position(missionitem)) {
warning_issued = true;
@@ -328,7 +329,7 @@ MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t
bool
MissionFeasibilityChecker::checkFixedWingTakeoff(dm_item_t dm_current, size_t nMissionItems,
float home_alt, bool home_valid, float default_acceptance_rad)
float home_alt, bool home_alt_valid, float default_acceptance_rad)
{
for (size_t i = 0; i < nMissionItems; i++) {
struct mission_item_s missionitem = {};
src/modules/navigator/mission_feasibility_checker.h
+4 -4
View File
@@ -57,7 +57,7 @@ private:
/* Checks for all airframes */
bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid,
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_alt_valid,
bool &warning_issued, bool throw_error = false);
bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed);
@@ -68,16 +68,16 @@ private:
/* Checks specific to fixedwing airframes */
bool checkFixedwing(dm_item_t dm_current, size_t nMissionItems, fw_pos_ctrl_status_s *fw_pos_ctrl_status,
float home_alt, bool home_valid, float default_acceptance_rad, bool land_start_req);
float home_alt, bool home_alt_valid, float default_acceptance_rad, bool land_start_req);
bool checkFixedWingTakeoff(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid,
bool checkFixedWingTakeoff(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_alt_valid,
float default_acceptance_rad);
bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems, fw_pos_ctrl_status_s *fw_pos_ctrl_status,
bool land_start_req);
/* Checks specific to rotarywing airframes */
bool checkRotarywing(dm_item_t dm_current, size_t nMissionItems,
float home_alt, bool home_valid, float default_altitude_acceptance_rad);
float home_alt, bool home_alt_valid, float default_altitude_acceptance_rad);
public:
MissionFeasibilityChecker(Navigator *navigator) : _navigator(navigator) {}
src/modules/navigator/navigator.h
+2 -1
View File
@@ -150,7 +150,8 @@ public:
const vehicle_roi_s &get_vroi() { return _vroi; }
bool home_position_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_hpos && _home_pos.valid_alt); }
bool home_alt_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt); }
bool home_position_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt && _home_pos.valid_hpos); }
int get_onboard_mission_sub() { return _onboard_mission_sub; }
int get_offboard_mission_sub() { return _offboard_mission_sub; }