dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Navigator: Reject missions with relative altitude if no home was set before arming

This commit is contained in:
Lorenz Meier 2015-06-11 12:24:26 +02:00
parent f2b81ce69a
commit bc48634101
4 changed files with 21 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/navigator/mission.cpp
View File

@ -232,7 +232,7 @@ Mission::update_offboard_mission()
failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), 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 { } else {
warnx("offboard mission update failed"); warnx("offboard mission update failed");

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

@ -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; bool failed = false;
/* Init if not done yet */ /* Init if not done yet */
@ -80,25 +80,25 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
if (isRotarywing) if (isRotarywing)
failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid);
else else
failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
return !failed; 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 */ /* Perform checks and issue feedback to the user for all checks */
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); 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 */ /* Mission is only marked as feasible if all checks return true */
return (resGeofence && resHomeAltitude); 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 */ /* Update fixed wing navigation capabilites */
updateNavigationCapabilities(); updateNavigationCapabilities();
@ -107,7 +107,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
/* Perform checks and issue feedback to the user for all checks */ /* Perform checks and issue feedback to the user for all checks */
bool resLanding = checkFixedWingLanding(dm_current, nMissionItems); bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); 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 */ /* Mission is only marked as feasible if all checks return true */
return (resLanding && resGeofence && resHomeAltitude); return (resLanding && resGeofence && resHomeAltitude);
@ -136,7 +136,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
return true; 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 */ /* 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++) { 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 */ /* calculate the global waypoint altitude */
float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;

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

@ -61,16 +61,16 @@ private:
/* Checks for all airframes */ /* Checks for all airframes */
bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); 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); bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems);
/* Checks specific to fixedwing airframes */ /* 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); bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems);
void updateNavigationCapabilities(); void updateNavigationCapabilities();
/* Checks specific to rotarywing airframes */ /* 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: public:
MissionFeasibilityChecker(); MissionFeasibilityChecker();
@ -80,7 +80,7 @@ public:
* Returns true if mission is feasible and false otherwise * Returns true if mission is feasible and false otherwise
*/ */
bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
float home_alt); float home_alt, bool home_valid);
}; };

1
src/modules/navigator/navigator.h
View File

@ -134,6 +134,7 @@ public:
struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; } struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; }
struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; } struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; }
struct home_position_s* get_home_position() { return &_home_pos; } 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 position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct mission_result_s* get_mission_result() { return &_mission_result; } struct mission_result_s* get_mission_result() { return &_mission_result; }
struct geofence_result_s* get_geofence_result() { return &_geofence_result; } struct geofence_result_s* get_geofence_result() { return &_geofence_result; }