mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-31 10:41:35 +08:00
Navigator: output new mission status
This commit is contained in:
@@ -78,6 +78,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
|
|||||||
_takeoff(false),
|
_takeoff(false),
|
||||||
_mission_type(MISSION_TYPE_NONE),
|
_mission_type(MISSION_TYPE_NONE),
|
||||||
_inited(false),
|
_inited(false),
|
||||||
|
_home_inited(false),
|
||||||
_dist_1wp_ok(false),
|
_dist_1wp_ok(false),
|
||||||
_missionFeasiblityChecker(),
|
_missionFeasiblityChecker(),
|
||||||
_min_current_sp_distance_xy(FLT_MAX),
|
_min_current_sp_distance_xy(FLT_MAX),
|
||||||
@@ -110,6 +111,22 @@ Mission::on_inactive()
|
|||||||
update_offboard_mission();
|
update_offboard_mission();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* check if the home position became valid in the meantime */
|
||||||
|
if ((_mission_type == MISSION_TYPE_NONE || _mission_type == MISSION_TYPE_OFFBOARD) &&
|
||||||
|
!_home_inited && _navigator->home_position_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,
|
||||||
|
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
|
||||||
|
_navigator->get_home_position()->alt, _navigator->home_position_valid());
|
||||||
|
|
||||||
|
_navigator->increment_mission_instance_count();
|
||||||
|
_navigator->set_mission_result_updated();
|
||||||
|
|
||||||
|
_home_inited = true;
|
||||||
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
/* read mission topics on initialization */
|
/* read mission topics on initialization */
|
||||||
_inited = true;
|
_inited = true;
|
||||||
@@ -176,6 +193,7 @@ Mission::on_active()
|
|||||||
&& _mission_type != MISSION_TYPE_NONE) {
|
&& _mission_type != MISSION_TYPE_NONE) {
|
||||||
heading_sp_update();
|
heading_sp_update();
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
|||||||
@@ -186,6 +186,7 @@ private:
|
|||||||
} _mission_type;
|
} _mission_type;
|
||||||
|
|
||||||
bool _inited;
|
bool _inited;
|
||||||
|
bool _home_inited;
|
||||||
bool _dist_1wp_ok;
|
bool _dist_1wp_ok;
|
||||||
|
|
||||||
MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */
|
MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */
|
||||||
|
|||||||
Reference in New Issue
Block a user