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

navigator: mission check refactor 

The mission feasability checker was called with the same arguments
twice which made it hard to understand when a mission is marked valid.

The mission check should run in these two cases:
- When initializing (if home comes up) if there is already a mission saved.
- When the mission gets updated.
This commit is contained in:
Julian Oes 2016-08-04 16:46:03 +02:00 committed by Lorenz Meier
parent 8566cabd76
commit a956429c4c
2 changed files with 28 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -81,7 +81,6 @@ Mission::Mission(Navigator *navigator, const char *name) :
_need_takeoff(true),
_mission_type(MISSION_TYPE_NONE),
_inited(false),
_home_inited(false),
_need_mission_reset(false),
_missionFeasibilityChecker(),
_min_current_sp_distance_xy(FLT_MAX),
@ -100,6 +99,11 @@ Mission::~Mission()
void
Mission::on_inactive()
{
/* Without home a mission can't be valid yet anyway, let's wait. */
if (!_navigator->home_position_valid()) {
return;
}
if (_inited) {
/* check if missions have changed so that feedback to ground station is given */
bool onboard_updated = false;
@ -138,11 +142,12 @@ Mission::on_inactive()
_current_offboard_mission_index = mission_state.current_seq;
}
/* On init let's check the mission, maybe there is already one available. */
check_mission_valid();
_inited = true;
}
check_mission_valid();
/* require takeoff after non-loiter or landing */
if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) {
_need_takeoff = true;
@ -274,24 +279,14 @@ Mission::update_offboard_mission()
/* otherwise, just leave it */
}
/* Check mission feasibility, for now do not handle the return value,
* 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);
check_mission_valid();
failed = !_missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
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,
_param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_default_acceptance_radius(),
_navigator->get_land_detected()->landed);
failed = !_navigator->get_mission_result()->valid;
_navigator->get_mission_result()->valid = !failed;
if (!failed) {
/* reset mission failure if we have an updated valid mission */
_navigator->get_mission_result()->mission_failure = false;
}
_navigator->increment_mission_instance_count();
_navigator->set_mission_result_updated();
} else {
PX4_WARN("offboard mission update failed, handle: %d", _navigator->get_offboard_mission_sub());
@ -1131,28 +1126,28 @@ Mission::set_mission_finished()
_navigator->set_mission_result_updated();
}
bool
void
Mission::check_mission_valid()
{
/* check if the home position became valid in the meantime */
if (!_home_inited && _navigator->home_position_valid()) {
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
_navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
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,
_param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_default_acceptance_radius(),
_navigator->get_land_detected()->landed);
_navigator->get_mission_result()->valid =
_missionFeasibilityChecker.checkMissionFeasible(
_navigator->get_mavlink_log_pub(),
(_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
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,
_param_dist_1wp.get(),
_navigator->get_mission_result()->warning,
_navigator->get_default_acceptance_radius(),
_navigator->get_land_detected()->landed);
_navigator->increment_mission_instance_count();
_navigator->set_mission_result_updated();
_home_inited = true;
}
return _navigator->get_mission_result()->valid;
_navigator->increment_mission_instance_count();
_navigator->set_mission_result_updated();
}
void

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

@ -211,7 +211,7 @@ private:
/**
* Check wether a mission is ready to go
*/
bool check_mission_valid();
void check_mission_valid();
/**
* Reset offboard mission
@ -244,7 +244,6 @@ private:
} _mission_type;
bool _inited;
bool _home_inited;
bool _need_mission_reset;
MissionFeasibilityChecker _missionFeasibilityChecker; /**< class that checks if a mission is feasible */