Revert "cleanup"

This reverts commit b00efcd966.
This commit is contained in:
Roman Bapst
2023-02-16 13:28:49 +03:00
committed by GitHub
parent b00efcd966
commit 759d33d00e
4 changed files with 41 additions and 161 deletions
@@ -150,22 +150,13 @@ void FeasibilityChecker::updateData()
} }
} }
bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int current_index, const int total_count) void FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int current_index, const int total_count)
{ {
if (current_index == 0) { if (current_index == 0) {
reset(); reset();
updateData(); updateData();
} }
if (!_mission_validity_failed) {
_mission_validity_failed = !checkMissionItemValidity(mission_item, current_index);
}
if (_mission_validity_failed) {
// if a mission item is not valid then abort the other checks
return false;
}
doCommonChecks(mission_item, current_index); doCommonChecks(mission_item, current_index);
if (_vehicle_type == VehicleType::Vtol) { if (_vehicle_type == VehicleType::Vtol) {
@@ -183,12 +174,13 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int
} }
_mission_item_previous = mission_item; _mission_item_previous = mission_item;
return true;
} }
void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int current_index) void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int current_index)
{ {
if (!_mission_validity_failed) {
_mission_validity_failed = !checkMissionItemValidity(mission_item, current_index);
}
if (!_distance_between_waypoints_failed) { if (!_distance_between_waypoints_failed) {
_distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item); _distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item);
@@ -235,20 +227,6 @@ void FeasibilityChecker::doMulticopterChecks(mission_item_s &mission_item, const
bool FeasibilityChecker::checkMissionItemValidity(mission_item_s &mission_item, const int current_index) bool FeasibilityChecker::checkMissionItemValidity(mission_item_s &mission_item, const int current_index)
{ {
/* reject relative alt without home set */
if (mission_item.altitude_is_relative && !PX4_ISFINITE(_home_alt_msl)
&& MissionBlock::item_contains_position(mission_item)) {
mavlink_log_critical(_mavlink_log_pub, "Mission rejected: No home pos, WP %d uses rel alt\t", current_index + 1);
events::send<int16_t>(events::ID("navigator_mis_no_home_rel_alt"), {events::Log::Error, events::LogInternal::Info},
"Mission rejected: No home position, waypoint {1} uses relative altitude",
current_index + 1);
return false;
}
// check if we find unsupported items and reject mission if so // check if we find unsupported items and reject mission if so
if (mission_item.nav_cmd != NAV_CMD_IDLE && if (mission_item.nav_cmd != NAV_CMD_IDLE &&
mission_item.nav_cmd != NAV_CMD_WAYPOINT && mission_item.nav_cmd != NAV_CMD_WAYPOINT &&
@@ -339,16 +317,23 @@ bool FeasibilityChecker::checkTakeoff(mission_item_s &mission_item)
// look for a takeoff waypoint // look for a takeoff waypoint
if (mission_item.nav_cmd == NAV_CMD_TAKEOFF || mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) { if (mission_item.nav_cmd == NAV_CMD_TAKEOFF || mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
// make sure that the altitude of the waypoint is above the home altitude // make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius
const float takeoff_alt = mission_item.altitude_is_relative // this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air
? mission_item.altitude
: mission_item.altitude - _home_alt_msl;
if (takeoff_alt < FLT_EPSILON) { float takeoff_alt = mission_item.altitude_is_relative
mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Takeoff altitude below home altitude!\t"); ? mission_item.altitude
: mission_item.altitude - _home_alt_msl;
float acceptance_radius = _param_nav_acc_rad;
if (mission_item.acceptance_radius > NAV_EPSILON_POSITION) {
acceptance_radius = mission_item.acceptance_radius;
}
if (takeoff_alt - 1.0f < acceptance_radius) {
mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Takeoff altitude too low!\t");
events::send<float>(events::ID("navigator_mis_takeoff_too_low"), {events::Log::Error, events::LogInternal::Info}, events::send<float>(events::ID("navigator_mis_takeoff_too_low"), {events::Log::Error, events::LogInternal::Info},
"Mission rejected: takeoff altitude too low! Minimum: {1:.1m_v}", "Mission rejected: takeoff altitude too low! Minimum: {1:.1m_v}", acceptance_radius + 1.f);
mission_item.altitude_is_relative ? 0.0f : _home_alt_msl);
return false; return false;
} }
@@ -725,6 +710,21 @@ bool FeasibilityChecker::checkDistancesBetweenWaypoints(const mission_item_s &mi
bool FeasibilityChecker::checkIfBelowHomeAltitude(const mission_item_s &mission_item, const int current_index) bool FeasibilityChecker::checkIfBelowHomeAltitude(const mission_item_s &mission_item, const int current_index)
{ {
/* reject relative alt without home set */
if (mission_item.altitude_is_relative && !PX4_ISFINITE(_home_alt_msl)
&& MissionBlock::item_contains_position(mission_item)) {
mavlink_log_critical(_mavlink_log_pub, "Mission rejected: No home pos, WP %d uses rel alt\t", current_index + 1);
events::send<int16_t>(events::ID("navigator_mis_no_home_rel_alt"), {events::Log::Error, events::LogInternal::Info},
"Mission rejected: No home position, waypoint {1} uses relative altitude",
current_index + 1);
return false;
}
/* calculate the global waypoint altitude */ /* calculate the global waypoint altitude */
float wp_alt = (mission_item.altitude_is_relative) ? mission_item.altitude + _home_alt_msl : mission_item.altitude; float wp_alt = (mission_item.altitude_is_relative) ? mission_item.altitude + _home_alt_msl : mission_item.altitude;
@@ -54,25 +54,13 @@ public:
Other Other
}; };
/** void processNextItem(mission_item_s &mission_item, const int current_index, const int total_count);
* @brief Run validity checks for mission item
*
*
* @param mission_item The mission item to run the checks on
* @param current_index The index of the current mission item in the mission
* @param total_count The total number of mission items in the mission
* @return False if fatal error occured and no other checks should be run.
*/
bool processNextItem(mission_item_s &mission_item, const int current_index, const int total_count);
void setMavlinkLogPub(orb_advert_t *mavlink_log_pub) void setMavlinkLogPub(orb_advert_t *mavlink_log_pub)
{ {
_mavlink_log_pub = mavlink_log_pub; _mavlink_log_pub = mavlink_log_pub;
} }
/**
* @return True At least one check failed.
*/
bool someCheckFailed() bool someCheckFailed()
{ {
return _takeoff_failed || return _takeoff_failed ||
@@ -85,9 +73,6 @@ public:
_takeoff_land_available_failed; _takeoff_land_available_failed;
} }
/**
* @brief Reset all data
*/
void reset(); void reset();
private: private:
@@ -138,113 +123,22 @@ private:
double _last_lon{(double)NAN}; double _last_lon{(double)NAN};
int _last_cmd{-1}; int _last_cmd{-1};
/**
* @brief Update data from external topics, e.g home position
*/
void updateData(); void updateData();
// methods which are called for each mission item // methods which are called for each mission item
/**
* @brief Check general mission item validity, e.g. supported commands.
*
* @param mission_item The current mission item
* @return False if the check failed.
*/
bool checkMissionItemValidity(mission_item_s &mission_item, const int current_index); bool checkMissionItemValidity(mission_item_s &mission_item, const int current_index);
/**
* @brief Check if takeoff is valid
*
* @param mission_item The current mission item
* @return False if the check failed.
*/
bool checkTakeoff(mission_item_s &mission_item); bool checkTakeoff(mission_item_s &mission_item);
/**
* @brief Check validity of landing pattern (fixed wing & vtol)
*
* @param mission_item The current mission item
* @param current_index The current mission index
* @param last_index The last index of the mission
* @return False if the check failed.
*/
bool checkLandPatternValidity(mission_item_s &mission_item, const int current_index, const int last_index); bool checkLandPatternValidity(mission_item_s &mission_item, const int current_index, const int last_index);
/**
* @brief Check distance to first waypoint.
*
* @param mission_item The current mission item
* @return False if the check failed.
*/
bool checkDistanceToFirstWaypoint(mission_item_s &mission_item); bool checkDistanceToFirstWaypoint(mission_item_s &mission_item);
/**
* @brief Check distances between waypoints
*
* @param mission_item The current mission item
* @return False if the check failed.
*/
bool checkDistancesBetweenWaypoints(const mission_item_s &mission_item); bool checkDistancesBetweenWaypoints(const mission_item_s &mission_item);
/**
* @brief Check if any waypoint is below the home altitude. Issues warning only.
*
* @param mission_item The current mission item
* @param current_index The current mission index
* @return Always returns true, only issues warning.
*/
bool checkIfBelowHomeAltitude(const mission_item_s &mission_item, const int current_index); bool checkIfBelowHomeAltitude(const mission_item_s &mission_item, const int current_index);
/**
* @brief Check fixed wing land approach (fixed wing only)
*
* @param mission_item The current mission item
* @param current_index The current mission index
* @return False if the check failed.
*/
bool checkFixedWindLandApproach(mission_item_s &mission_item, const int current_index); bool checkFixedWindLandApproach(mission_item_s &mission_item, const int current_index);
// methods which are called once at the end // methods which are called once at the end
/**
* @brief Check if takeoff/landing are available according to MIS_TKO_LAND_REQ parameter
*
* @return False if the check failed.
*/
bool checkTakeoffLandAvailable(); bool checkTakeoffLandAvailable();
/**
* @brief Run checks which are common for all vehicle types
*
* @param mission_item The current mission item
* @param current_index The current mission index
*/
void doCommonChecks(mission_item_s &mission_item, const int current_index); void doCommonChecks(mission_item_s &mission_item, const int current_index);
/**
* @brief Run checks which are only related to VTOL vehicles.
*
* @param mission_item The current mission item
* @param current_index The current mission index
*/
void doVtolChecks(mission_item_s &mission_item, const int current_index, const int last_index); void doVtolChecks(mission_item_s &mission_item, const int current_index, const int last_index);
/**
* @brief Run checks which are only related to fixed wing vehicles.
*
* @param mission_item The current mission item
* @param current_index The current mission index
* @param last_index The last mission index
* @return False if the check failed.
*/
void doFixedWingChecks(mission_item_s &mission_item, const int current_index, const int last_index); void doFixedWingChecks(mission_item_s &mission_item, const int current_index, const int last_index);
/**
* @brief Run checks which are only related to multirotor vehicles.
*
* @param mission_item The current mission item
* @param current_index The current mission index
* @return False if the check failed.
*/
void doMulticopterChecks(mission_item_s &mission_item, const int current_index); void doMulticopterChecks(mission_item_s &mission_item, const int current_index);
}; };
@@ -105,22 +105,10 @@ TEST_F(FeasibilityCheckerTest, mission_item_validity)
// when landed the first item cannot be a land item // when landed the first item cannot be a land item
checker.reset(); checker.reset();
checker.publishHomePosition(0, 0, 100.f);
checker.publishLanded(true); checker.publishLanded(true);
mission_item.nav_cmd = NAV_CMD_LAND; mission_item.nav_cmd = NAV_CMD_LAND;
bool ret = checker.processNextItem(mission_item, 0, 1); checker.processNextItem(mission_item, 0, 1);
ASSERT_EQ(checker.someCheckFailed(), true); ASSERT_EQ(checker.someCheckFailed(), true);
// mission item validity failed
ASSERT_EQ(ret, false);
checker.reset();
mission_item.nav_cmd = NAV_CMD_TAKEOFF;
mission_item.altitude_is_relative = true;
ret = checker.processNextItem(mission_item, 0, 5);
// home alt is not valid but we have a relative mission item, should fail immediately
ASSERT_EQ(ret, false);
} }
TEST_F(FeasibilityCheckerTest, max_dist_between_waypoints) TEST_F(FeasibilityCheckerTest, max_dist_between_waypoints)
@@ -226,8 +214,9 @@ TEST_F(FeasibilityCheckerTest, check_takeoff)
// takeoff altitude is smaller than acceptance radius, should fail // takeoff altitude is smaller than acceptance radius, should fail
mission_item_s mission_item = {}; mission_item_s mission_item = {};
mission_item.nav_cmd = NAV_CMD_TAKEOFF; mission_item.nav_cmd = NAV_CMD_TAKEOFF;
mission_item.altitude = -5.0f; mission_item.altitude = 5.0f;
mission_item.altitude_is_relative = true; mission_item.altitude_is_relative = true;
mission_item.acceptance_radius = 10.0f;
checker.processNextItem(mission_item, 0, 1); checker.processNextItem(mission_item, 0, 1);
@@ -238,8 +227,8 @@ TEST_F(FeasibilityCheckerTest, check_takeoff)
checker.publishHomePosition(0, 0, 100); checker.publishHomePosition(0, 0, 100);
// takeoff altitude is larger than home altitude, should pass // takeoff altitude is larger than acceptance radius, should pass
mission_item.altitude = 0.1f; mission_item.acceptance_radius = 3.0f;
checker.processNextItem(mission_item, 0, 1); checker.processNextItem(mission_item, 0, 1);
ASSERT_EQ(checker.someCheckFailed(), false); ASSERT_EQ(checker.someCheckFailed(), false);
@@ -86,10 +86,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission)
return false; return false;
} }
if (!_feasibility_checker.processNextItem(missionitem, i, mission.count)) { _feasibility_checker.processNextItem(missionitem, i, mission.count);
failed = true;
break;
}
} }