Beispiel #1
0
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();
	}

}
Beispiel #2
0
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();
	}

}
Beispiel #3
0
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;		
		}
	}
}
Beispiel #4
0
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();
}
Beispiel #5
0
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;
	}
}
Beispiel #6
0
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;
	}
}
Beispiel #7
0
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();
	}

}
Beispiel #8
0
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;
		}
	}
}
Beispiel #9
0
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();
	}

}