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_active() { /* 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 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); } } }
bool Mission::set_current_offboard_mission_index(unsigned index) { if (index < _offboard_mission.count) { _current_offboard_mission_index = index; set_current_offboard_mission_item(); // update mission items if already in active mission if (_navigator->is_planned_mission()) { // prevent following "previous - current" line _navigator->get_position_setpoint_triplet()->previous.valid = false; _navigator->get_position_setpoint_triplet()->current.valid = false; set_mission_items(); } return true; } return false; }
void Mission::on_activation() { set_mission_items(); }
void Delivery::set_delivery_items() { // set the parameters needed for getting to the destination if (delivery_status == DELIV_ENROUTE) { set_mission_items(); } else if (delivery_status == DELIV_DROPOFF) { mavlink_log_critical(_navigator->get_mavlink_fd(), "Setting descent mission item"); // setting the mission items for a descent to the _drop_alt struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* make sure we have the latest params */ updateParams(); set_previous_pos_setpoint(); _navigator->set_can_loiter_at_sp(false); // mission_item to descend to _drop_alt _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; _mission_item.altitude_is_relative = false; // .altitude needs to be referenced to location altitude, not home altitude _mission_item.altitude = _navigator->get_home_position()->alt + _drop_alt; _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 10.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = false; _mission_item.origin = ORIGIN_ONBOARD; mavlink_log_critical(_navigator->get_mavlink_fd(), "DELIVERY: descend to %d m (%d m above destination)", (int)(_mission_item.altitude), (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); reset_mission_item_reached(); /* convert mission item to current position setpoint and make it valid */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; _navigator->set_position_setpoint_triplet_updated(); } else if (delivery_status == DELIV_RETURN) { // setting the proper _rtl_state for current position if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get()) { _rtl_state = RTL_STATE_CLIMB; /* otherwise go straight to return */ } else { /* set altitude setpoint to current altitude */ _rtl_state = RTL_STATE_RETURN; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_global_position()->alt; } set_rtl_item(); } _first_run = false; _complete = false; }
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_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(); } }