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_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 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 (_mission_item.nav_cmd == NAV_CMD_LAND && _waypoint_position_reached && _navigator->get_vstatus()->is_rotary_wing) { // the copter has reached the landing spot location // set the same landing item again but this time the vehicle will actually // descend and land set_mission_item_reached(); set_mission_items(); } 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 for rotary wing types */ if (_navigator->get_vstatus()->is_rotary_wing && _param_yawmode.get() != MISSION_YAWMODE_NONE && _param_yawmode.get() < MISSION_YAWMODE_MAX && _mission_type != MISSION_TYPE_NONE) { 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::update_offboard_mission() { bool failed = true; if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq); /* determine current index */ if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) { _current_offboard_mission_index = _offboard_mission.current_seq; } else { /* if less items available, reset to first item */ if (_current_offboard_mission_index >= (int)_offboard_mission.count) { _current_offboard_mission_index = 0; } else if (_current_offboard_mission_index < 0) { /* if not initialized, set it to 0 */ _current_offboard_mission_index = 0; } /* otherwise, just leave it */ } check_mission_valid(true); failed = !_navigator->get_mission_result()->valid; if (!failed) { /* reset mission failure if we have an updated valid mission */ _navigator->get_mission_result()->mission_failure = false; /* reset reached info as well */ _navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->seq_reached = 0; _navigator->get_mission_result()->seq_total = _offboard_mission.count; } } else { PX4_WARN("offboard mission update failed, handle: %d", _navigator->get_offboard_mission_sub()); } if (failed) { _offboard_mission.count = 0; _offboard_mission.current_seq = 0; _current_offboard_mission_index = 0; warnx("mission check failed"); } set_current_offboard_mission_item(); }
void Delivery::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(); } } 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(); /* reset RTL state only if setpoint moved */ if (!_navigator->get_can_loiter_at_sp()) { _rtl_state = RTL_STATE_NONE; } }
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(); } } 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_vstatus()->condition_landed) { _need_takeoff = true; } }
void Delivery::to_destination() { // set a mission to destination with takeoff enabled // Status = enroute ; change to Dropoff at completion if (_complete) { // Update status now that travel to destination is complete and reset _first_run for next stage _first_run = true; mavlink_log_critical(_navigator->get_mavlink_fd(), "Black Hawk at Location"); advance_delivery(); return; } 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 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(); mavlink_log_critical(_navigator->get_mavlink_fd(), "Black Hawk advancing mission"); } } 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); _complete = true; } } /* see if we need to update the current yaw heading for rotary wing types */ if (_navigator->get_vstatus()->is_rotary_wing && _param_yawmode.get() != MISSION_YAWMODE_NONE && _mission_type != MISSION_TYPE_NONE) { heading_sp_update(); } }
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(); } }