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

Mission feasibility fixes

Browse Source 
- Fixes https://github.com/PX4/Firmware/issues/3656
- Fixes https://github.com/PX4/Firmware/issues/3648
- Corrected spelling on method
This commit is contained in:
sander 2016-02-04 20:44:35 +01:00
parent 0bde2b6fa3
commit fdcfb7c7c6
5 changed files with 41 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -80,7 +80,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
_mission_type(MISSION_TYPE_NONE),
_inited(false),
_home_inited(false),
_missionFeasiblityChecker(),
_missionFeasibilityChecker(),
_min_current_sp_distance_xy(FLT_MAX),
_mission_item_previous_alt(NAN),
_on_arrival_yaw(NAN),
@ -255,7 +255,7 @@ Mission::update_offboard_mission()
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
failed = !_missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt, _navigator->home_position_valid(),
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
@ -841,7 +841,7 @@ Mission::check_mission_valid()
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
_navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
_navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt, _navigator->home_position_valid(),
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,

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

@ -196,7 +196,7 @@ private:
bool _inited;
bool _home_inited;
MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */
MissionFeasibilityChecker _missionFeasibilityChecker; /**< class that checks if a mission is feasible */
float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint */
float _mission_item_previous_alt; /**< holds the altitude of the previous mission item,

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

@ -36,6 +36,7 @@
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Sander Smeets <sander@droneslab.com>
*/
#include "mission_feasibility_checker.h"
@ -185,25 +186,32 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
return false;
}
/* always reject relative alt without home set */
if (missionitem.altitude_is_relative && !home_valid) {
mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i);
/* reject relative alt without home set */
if (missionitem.altitude_is_relative && !home_valid && isPositionCommand(missionitem.nav_cmd)) {
warning_issued = true;
return false;
if (throw_error) {
mavlink_log_critical(_mavlink_fd, "Rejecting mission: No home pos, WP %d uses rel alt", i+1);
return false;
} else {
mavlink_log_critical(_mavlink_fd, "Warning: No home pos, WP %d uses rel alt", i+1);
return true;
}
}
/* calculate the global waypoint altitude */
float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
if (home_alt > wp_alt) {
if (home_alt > wp_alt && isPositionCommand(missionitem.nav_cmd)) {
warning_issued = true;
if (throw_error) {
mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i);
mavlink_log_critical(_mavlink_fd, "Rejecting mission: Waypoint %d below home", i+1);
return false;
} else {
mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i);
mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i+1);
return true;
}
}
@ -235,8 +243,8 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
missionitem.nav_cmd != NAV_CMD_PATHPLANNING &&
missionitem.nav_cmd != NAV_CMD_DO_JUMP &&
missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO &&
missionitem.nav_cmd != vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL &&
missionitem.nav_cmd != vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) {
missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL &&
missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1));
return false;
@ -357,12 +365,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
}
}
/* check only items with valid lat/lon */
else if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
mission_item.nav_cmd == NAV_CMD_PATHPLANNING) {
else if (isPositionCommand(mission_item.nav_cmd)) {
/* check distance from current position to item */
float dist_to_1wp = get_distance_to_next_waypoint(
@ -401,6 +404,22 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
}
}
bool
MissionFeasibilityChecker::isPositionCommand(unsigned cmd){
if( cmd == NAV_CMD_WAYPOINT ||
cmd == NAV_CMD_LOITER_TIME_LIMIT ||
cmd == NAV_CMD_LOITER_TURN_COUNT ||
cmd == NAV_CMD_LOITER_UNLIMITED ||
cmd == NAV_CMD_TAKEOFF ||
cmd == NAV_CMD_LAND ||
cmd == NAV_CMD_PATHPLANNING) {
return true;
} else {
return false;
}
}
void MissionFeasibilityChecker::updateNavigationCapabilities()
{
(void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);

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

@ -36,6 +36,7 @@
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Sander Smeets <sander@droneslab.com>
*/
#ifndef MISSION_FEASIBILITY_CHECKER_H_
@ -65,6 +66,7 @@ private:
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false);
bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed);
bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued);
bool isPositionCommand(unsigned cmd);
/* Checks specific to fixedwing airframes */
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);

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

@ -64,6 +64,7 @@ enum NAV_CMD {
NAV_CMD_DO_JUMP = 177,
NAV_CMD_DO_SET_SERVO=183,
NAV_CMD_DO_REPEAT_SERVO=184,
NAV_CMD_DO_DIGICAM_CONTROL=203,
NAV_CMD_DO_VTOL_TRANSITION=3000,
NAV_CMD_INVALID=UINT16_MAX /* ensure that casting a large number results in a specific error */
};