void
Mission::on_active()
{
check_mission_valid();
/* check if anything has changed */
bool onboard_updated = false;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
if (onboard_updated) {
update_onboard_mission();
}
bool offboard_updated = false;
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
if (offboard_updated) {
update_offboard_mission();
}
/* reset the current offboard mission if needed */
if (need_to_reset_mission(true)) {
reset_offboard_mission(_offboard_mission);
update_offboard_mission();
offboard_updated = true;
}
/* reset mission items if needed */
if (onboard_updated || offboard_updated) {
set_mission_items();
}
/* lets check if we reached the current mission item */
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
set_mission_item_reached();
if (_mission_item.autocontinue) {
/* switch to next waypoint if 'autocontinue' flag set */
advance_mission();
set_mission_items();
}
} else if (_mission_type != MISSION_TYPE_NONE && _param_altmode.get() == MISSION_ALTMODE_FOH) {
altitude_sp_foh_update();
} else {
/* if waypoint position reached allow loiter on the setpoint */
if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
_navigator->set_can_loiter_at_sp(true);
}
}
/* see if we need to update the current yaw heading */
if ((_param_yawmode.get() != MISSION_YAWMODE_NONE
&& _param_yawmode.get() < MISSION_YAWMODE_MAX
&& _mission_type != MISSION_TYPE_NONE)
|| _navigator->get_vstatus()->is_vtol) {
heading_sp_update();
}
}
void
Mission::on_inactive()
{
if (_inited) {
/* check if missions have changed so that feedback to ground station is given */
bool onboard_updated = false;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
if (onboard_updated) {
update_onboard_mission();
}
bool offboard_updated = false;
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
if (offboard_updated) {
update_offboard_mission();
}
/* reset the current offboard mission if needed */
if (need_to_reset_mission(false)) {
reset_offboard_mission(_offboard_mission);
update_offboard_mission();
}
} else {
/* load missions from storage */
mission_s mission_state;
dm_lock(DM_KEY_MISSION_STATE);
/* read current state */
int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
dm_unlock(DM_KEY_MISSION_STATE);
if (read_res == sizeof(mission_s)) {
_offboard_mission.dataman_id = mission_state.dataman_id;
_offboard_mission.count = mission_state.count;
_current_offboard_mission_index = mission_state.current_seq;
}
_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;
/* Reset work item type to default if auto take-off has been paused or aborted,
and we landed in manual mode. */
if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
}
}
void
Mission::on_inactive()
{
/* We need to reset the mission cruising speed, otherwise the
* mission velocity which might have been set using mission items
* is used for missions such as RTL. */
_navigator->set_cruising_speed();
/* 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;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
if (onboard_updated) {
update_onboard_mission();
}
bool offboard_updated = false;
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
if (offboard_updated) {
update_offboard_mission();
}
/* reset the current offboard mission if needed */
if (need_to_reset_mission(false)) {
reset_offboard_mission(_offboard_mission);
update_offboard_mission();
}
} else {
/* load missions from storage */
mission_s mission_state;
dm_lock(DM_KEY_MISSION_STATE);
/* read current state */
int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
dm_unlock(DM_KEY_MISSION_STATE);
if (read_res == sizeof(mission_s)) {
_offboard_mission.dataman_id = mission_state.dataman_id;
_offboard_mission.count = mission_state.count;
_current_offboard_mission_index = mission_state.current_seq;
}
/* On init let's check the mission, maybe there is already one available. */
check_mission_valid(false);
_inited = true;
}
/* require takeoff after non-loiter or landing */
if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) {
_need_takeoff = true;
/* Reset work item type to default if auto take-off has been paused or aborted,
and we landed in manual mode. */
if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
}
}
void
Mission::on_active()
{
check_mission_valid(false);
/* check if anything has changed */
bool onboard_updated = false;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
if (onboard_updated) {
update_onboard_mission();
}
bool offboard_updated = false;
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
if (offboard_updated) {
update_offboard_mission();
}
/* reset the current offboard mission if needed */
if (need_to_reset_mission(true)) {
reset_offboard_mission(_offboard_mission);
update_offboard_mission();
offboard_updated = true;
}
/* reset mission items if needed */
if (onboard_updated || offboard_updated) {
set_mission_items();
}
/* lets check if we reached the current mission item */
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
/* If we just completed a takeoff which was inserted before the right waypoint,
there is no need to report that we reached it because we didn't. */
if (_work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
set_mission_item_reached();
}
if (_mission_item.autocontinue) {
/* switch to next waypoint if 'autocontinue' flag set */
advance_mission();
set_mission_items();
}
} else if (_mission_type != MISSION_TYPE_NONE && _param_altmode.get() == MISSION_ALTMODE_FOH) {
altitude_sp_foh_update();
} else {
/* if waypoint position reached allow loiter on the setpoint */
if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
_navigator->set_can_loiter_at_sp(true);
}
}
/* see if we need to update the current yaw heading */
if ((_param_yawmode.get() != MISSION_YAWMODE_NONE
&& _param_yawmode.get() < MISSION_YAWMODE_MAX
&& _mission_type != MISSION_TYPE_NONE)
|| _navigator->get_vstatus()->is_vtol) {
heading_sp_update();
}
/* check if landing needs to be aborted */
if ((_mission_item.nav_cmd == NAV_CMD_LAND)
&& (_navigator->abort_landing())) {
do_abort_landing();
}
}