forked from Archive/PX4-Autopilot
Navigator: Reject missions with relative altitude if no home was set before arming
This commit is contained in:
parent
f2b81ce69a
commit
bc48634101
|
@ -232,7 +232,7 @@ Mission::update_offboard_mission()
|
|||
|
||||
failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
|
||||
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
|
||||
_navigator->get_home_position()->alt);
|
||||
_navigator->get_home_position()->alt, _navigator->home_position_valid());
|
||||
|
||||
} else {
|
||||
warnx("offboard mission update failed");
|
||||
|
|
|
@ -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, float home_alt)
|
||||
bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
|
||||
{
|
||||
bool failed = false;
|
||||
/* Init if not done yet */
|
||||
|
@ -80,25 +80,25 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
|
|||
|
||||
|
||||
if (isRotarywing)
|
||||
failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt);
|
||||
failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid);
|
||||
else
|
||||
failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt);
|
||||
failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
|
||||
|
||||
return !failed;
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
|
||||
{
|
||||
|
||||
/* Perform checks and issue feedback to the user for all checks */
|
||||
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
|
||||
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
|
||||
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid);
|
||||
|
||||
/* Mission is only marked as feasible if all checks return true */
|
||||
return (resGeofence && resHomeAltitude);
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
|
||||
{
|
||||
/* Update fixed wing navigation capabilites */
|
||||
updateNavigationCapabilities();
|
||||
|
@ -107,7 +107,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
|
|||
/* Perform checks and issue feedback to the user for all checks */
|
||||
bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
|
||||
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
|
||||
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
|
||||
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid);
|
||||
|
||||
/* Mission is only marked as feasible if all checks return true */
|
||||
return (resLanding && resGeofence && resHomeAltitude);
|
||||
|
@ -136,7 +136,7 @@ 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)
|
||||
bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, 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++) {
|
||||
|
@ -152,6 +152,13 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
|
|||
}
|
||||
}
|
||||
|
||||
if (missionitem.altitude_is_relative && !home_valid) {
|
||||
if (throw_error) {
|
||||
mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/* calculate the global waypoint altitude */
|
||||
float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
|
||||
|
||||
|
|
|
@ -61,16 +61,16 @@ 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);
|
||||
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error = false);
|
||||
bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems);
|
||||
|
||||
/* Checks specific to fixedwing airframes */
|
||||
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
|
||||
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
|
||||
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, float home_alt);
|
||||
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
|
||||
public:
|
||||
|
||||
MissionFeasibilityChecker();
|
||||
|
@ -80,7 +80,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,
|
||||
float home_alt);
|
||||
float home_alt, bool home_valid);
|
||||
|
||||
};
|
||||
|
||||
|
|
|
@ -134,6 +134,7 @@ public:
|
|||
struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; }
|
||||
struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; }
|
||||
struct home_position_s* get_home_position() { return &_home_pos; }
|
||||
bool home_position_valid() { return _home_position_set; }
|
||||
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
|
||||
struct mission_result_s* get_mission_result() { return &_mission_result; }
|
||||
struct geofence_result_s* get_geofence_result() { return &_geofence_result; }
|
||||
|
|
Loading…
Reference in New Issue